// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 Google Inc. All rights reserved.

// http://ceres-solver.org/

//

// Redistribution and use in source and binary forms, with or without

// modification, are permitted provided that the following conditions are met:

//

// * Redistributions of source code must retain the above copyright notice,

//   this list of conditions and the following disclaimer.

// * Redistributions in binary form must reproduce the above copyright notice,

//   this list of conditions and the following disclaimer in the documentation

//   and/or other materials provided with the distribution.

// * Neither the name of Google Inc. nor the names of its contributors may be

//   used to endorse or promote products derived from this software without

//   specific prior written permission.

//

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// POSSIBILITY OF SUCH DAMAGE.

//

// Author: sameeragarwal@google.com (Sameer Agarwal)

#include <cmath>

#include <limits>

#include "Eigen/Geometry"

#include "ceres/autodiff_local_parameterization.h"

#include "ceres/fpclassify.h"

#include "ceres/householder_vector.h"

#include "ceres/internal/autodiff.h"

#include "ceres/internal/eigen.h"

#include "ceres/local_parameterization.h"

#include "ceres/random.h"

#include "ceres/rotation.h"

#include "gtest/gtest.h"

namespace ceres {

namespace internal {

TEST(IdentityParameterization, EverythingTest) {

  IdentityParameterization parameterization(3);

  EXPECT_EQ(parameterization.GlobalSize(), 3);

  EXPECT_EQ(parameterization.LocalSize(), 3);

  double x[3] = {1.0, 2.0, 3.0};

  double delta[3] = {0.0, 1.0, 2.0};

  double x_plus_delta[3] = {0.0, 0.0, 0.0};

  parameterization.Plus(x, delta, x_plus_delta);

  EXPECT_EQ(x_plus_delta[0], 1.0);

  EXPECT_EQ(x_plus_delta[1], 3.0);

  EXPECT_EQ(x_plus_delta[2], 5.0);

  double jacobian[9];

  parameterization.ComputeJacobian(x, jacobian);

  int k = 0;

  for (int i = 0; i < 3; ++i) {

    for (int j = 0; j < 3; ++j, ++k) {

      EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0);

    }

  }

  Matrix global_matrix = Matrix::Ones(10, 3);

  Matrix local_matrix = Matrix::Zero(10, 3);

  parameterization.MultiplyByJacobian(x,

                                      10,

                                      global_matrix.data(),

                                      local_matrix.data());

  EXPECT_EQ((local_matrix - global_matrix).norm(), 0.0);

}

TEST(SubsetParameterization, NegativeParameterIndexDeathTest) {

  std::vector<int> constant_parameters;

  constant_parameters.push_back(-1);

  EXPECT_DEATH_IF_SUPPORTED(

      SubsetParameterization parameterization(2, constant_parameters),

      "greater than zero");

}

TEST(SubsetParameterization, GreaterThanSizeParameterIndexDeathTest) {

  std::vector<int> constant_parameters;

  constant_parameters.push_back(2);

  EXPECT_DEATH_IF_SUPPORTED(

      SubsetParameterization parameterization(2, constant_parameters),

      "less than the size");

}

TEST(SubsetParameterization, DuplicateParametersDeathTest) {

  std::vector<int> constant_parameters;

  constant_parameters.push_back(1);

  constant_parameters.push_back(1);

  EXPECT_DEATH_IF_SUPPORTED(

      SubsetParameterization parameterization(2, constant_parameters),

      "duplicates");

}

TEST(SubsetParameterization,

     ProductParameterizationWithZeroLocalSizeSubsetParameterization1) {

  std::vector<int> constant_parameters;

  constant_parameters.push_back(0);

  LocalParameterization* subset_param =

      new SubsetParameterization(1, constant_parameters);

  LocalParameterization* identity_param = new IdentityParameterization(2);

  ProductParameterization product_param(subset_param, identity_param);

  EXPECT_EQ(product_param.GlobalSize(), 3);

  EXPECT_EQ(product_param.LocalSize(), 2);

  double x[] = {1.0, 1.0, 1.0};

  double delta[] = {2.0, 3.0};

  double x_plus_delta[] = {0.0, 0.0, 0.0};

  EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta));

  EXPECT_EQ(x_plus_delta[0], x[0]);

  EXPECT_EQ(x_plus_delta[1], x[1] + delta[0]);

  EXPECT_EQ(x_plus_delta[2], x[2] + delta[1]);

  Matrix actual_jacobian(3, 2);

  EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data()));

}

TEST(SubsetParameterization,

     ProductParameterizationWithZeroLocalSizeSubsetParameterization2) {

  std::vector<int> constant_parameters;

  constant_parameters.push_back(0);

  LocalParameterization* subset_param =

      new SubsetParameterization(1, constant_parameters);

  LocalParameterization* identity_param = new IdentityParameterization(2);

  ProductParameterization product_param(identity_param, subset_param);

  EXPECT_EQ(product_param.GlobalSize(), 3);

  EXPECT_EQ(product_param.LocalSize(), 2);

  double x[] = {1.0, 1.0, 1.0};

  double delta[] = {2.0, 3.0};

  double x_plus_delta[] = {0.0, 0.0, 0.0};

  EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta));

  EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]);

  EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]);

  EXPECT_EQ(x_plus_delta[2], x[2]);

  Matrix actual_jacobian(3, 2);

  EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data()));

}

TEST(SubsetParameterization, NormalFunctionTest) {

  const int kGlobalSize = 4;

  const int kLocalSize = 3;

  double x[kGlobalSize] = {1.0, 2.0, 3.0, 4.0};

  for (int i = 0; i < kGlobalSize; ++i) {

    std::vector<int> constant_parameters;

    constant_parameters.push_back(i);

    SubsetParameterization parameterization(kGlobalSize, constant_parameters);

    double delta[kLocalSize] = {1.0, 2.0, 3.0};

    double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0};

    parameterization.Plus(x, delta, x_plus_delta);

    int k = 0;

    for (int j = 0; j < kGlobalSize; ++j) {

      if (j == i)  {

        EXPECT_EQ(x_plus_delta[j], x[j]);

      } else {

        EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]);

      }

    }

    double jacobian[kGlobalSize * kLocalSize];

    parameterization.ComputeJacobian(x, jacobian);

    int delta_cursor = 0;

    int jacobian_cursor = 0;

    for (int j = 0; j < kGlobalSize; ++j) {

      if (j != i) {

        for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) {

          EXPECT_EQ(jacobian[jacobian_cursor], delta_cursor == k ? 1.0 : 0.0);

        }

        ++delta_cursor;

      } else {

        for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) {

          EXPECT_EQ(jacobian[jacobian_cursor], 0.0);

        }

      }

    }

    Matrix global_matrix = Matrix::Ones(10, kGlobalSize);

    for (int row = 0; row < kGlobalSize; ++row) {

      for (int col = 0; col < kGlobalSize; ++col) {

        global_matrix(row, col) = col;

      }

    }

    Matrix local_matrix = Matrix::Zero(10, kLocalSize);

    parameterization.MultiplyByJacobian(x,

                                        10,

                                        global_matrix.data(),

                                        local_matrix.data());

    Matrix expected_local_matrix =

        global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize);

    EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0);

  }

}

// Functor needed to implement automatically differentiated Plus for

// quaternions.

struct QuaternionPlus {

  template<typename T>

  bool operator()(const T* x, const T* delta, T* x_plus_delta) const {

    const T squared_norm_delta =

        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];

    T q_delta[4];

    if (squared_norm_delta > T(0.0)) {

      T norm_delta = sqrt(squared_norm_delta);

      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;

      q_delta[0] = cos(norm_delta);

      q_delta[1] = sin_delta_by_delta * delta[0];

      q_delta[2] = sin_delta_by_delta * delta[1];

      q_delta[3] = sin_delta_by_delta * delta[2];

    } else {

      // We do not just use q_delta = [1,0,0,0] here because that is a

      // constant and when used for automatic differentiation will

      // lead to a zero derivative. Instead we take a first order

      // approximation and evaluate it at zero.

      q_delta[0] = T(1.0);

      q_delta[1] = delta[0];

      q_delta[2] = delta[1];

      q_delta[3] = delta[2];

    }

    QuaternionProduct(q_delta, x, x_plus_delta);

    return true;

  }

};

template<typename Parameterization, typename Plus>

void QuaternionParameterizationTestHelper(

    const double* x, const double* delta,

    const double* x_plus_delta_ref) {

  const int kGlobalSize = 4;

  const int kLocalSize = 3;

  const double kTolerance = 1e-14;

  double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0, 0.0};

  Parameterization parameterization;

  parameterization.Plus(x, delta, x_plus_delta);

  for (int i = 0; i < kGlobalSize; ++i) {

    EXPECT_NEAR(x_plus_delta[i], x_plus_delta[i], kTolerance);

  }

  const double x_plus_delta_norm =

      sqrt(x_plus_delta[0] * x_plus_delta[0] +

           x_plus_delta[1] * x_plus_delta[1] +

           x_plus_delta[2] * x_plus_delta[2] +

           x_plus_delta[3] * x_plus_delta[3]);

  EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance);

  double jacobian_ref[12];

  double zero_delta[kLocalSize] = {0.0, 0.0, 0.0};

  const double* parameters[2] = {x, zero_delta};

  double* jacobian_array[2] = { NULL, jacobian_ref };

  // Autodiff jacobian at delta_x = 0.

  internal::AutoDiff

                     double,

                     kGlobalSize,

                     kLocalSize>::Differentiate(Plus(),

                                                parameters,

                                                kGlobalSize,

                                                x_plus_delta,

                                                jacobian_array);

  double jacobian[12];

  parameterization.ComputeJacobian(x, jacobian);

  for (int i = 0; i < 12; ++i) {

    EXPECT_TRUE(IsFinite(jacobian[i]));

    EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance)

        << "Jacobian mismatch: i = " << i

        << "\n Expected \n"

        << ConstMatrixRef(jacobian_ref, kGlobalSize, kLocalSize)

        << "\n Actual \n"

        << ConstMatrixRef(jacobian, kGlobalSize, kLocalSize);

  }

  Matrix global_matrix = Matrix::Random(10, kGlobalSize);

  Matrix local_matrix = Matrix::Zero(10, kLocalSize);

  parameterization.MultiplyByJacobian(x,

                                      10,

                                      global_matrix.data(),

                                      local_matrix.data());

  Matrix expected_local_matrix =

      global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize);

  EXPECT_NEAR((local_matrix - expected_local_matrix).norm(),

              0.0,

              10.0 * std::numeric_limits<double>::epsilon());

}

template <int N>

void Normalize(double* x) {

  VectorRef(x, N).normalize();

}

TEST(QuaternionParameterization, ZeroTest) {

  double x[4] = {0.5, 0.5, 0.5, 0.5};

  double delta[3] = {0.0, 0.0, 0.0};

  double q_delta[4] = {1.0, 0.0, 0.0, 0.0};

  double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};

  QuaternionProduct(q_delta, x, x_plus_delta);

  QuaternionParameterizationTestHelper

                                       QuaternionPlus>(x, delta, x_plus_delta);

}

TEST(QuaternionParameterization, NearZeroTest) {

  double x[4] = {0.52, 0.25, 0.15, 0.45};

  Normalize<4>(x);

  double delta[3] = {0.24, 0.15, 0.10};

  for (int i = 0; i < 3; ++i) {

    delta[i] = delta[i] * 1e-14;

  }

  double q_delta[4];

  q_delta[0] = 1.0;

  q_delta[1] = delta[0];

  q_delta[2] = delta[1];

  q_delta[3] = delta[2];

  double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};

  QuaternionProduct(q_delta, x, x_plus_delta);

  QuaternionParameterizationTestHelper

                                       QuaternionPlus>(x, delta, x_plus_delta);

}

TEST(QuaternionParameterization, AwayFromZeroTest) {

  double x[4] = {0.52, 0.25, 0.15, 0.45};

  Normalize<4>(x);

  double delta[3] = {0.24, 0.15, 0.10};

  const double delta_norm = sqrt(delta[0] * delta[0] +

                                 delta[1] * delta[1] +

                                 delta[2] * delta[2]);

  double q_delta[4];

  q_delta[0] = cos(delta_norm);

  q_delta[1] = sin(delta_norm) / delta_norm * delta[0];

  q_delta[2] = sin(delta_norm) / delta_norm * delta[1];

  q_delta[3] = sin(delta_norm) / delta_norm * delta[2];

  double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};

  QuaternionProduct(q_delta, x, x_plus_delta);

  QuaternionParameterizationTestHelper

                                       QuaternionPlus>(x, delta, x_plus_delta);

}

// Functor needed to implement automatically differentiated Plus for

// Eigen's quaternion.

struct EigenQuaternionPlus {

  template<typename T>

  bool operator()(const T* x, const T* delta, T* x_plus_delta) const {

    const T norm_delta =

        sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);

    Eigen::Quaternion<T> q_delta;

    if (norm_delta > T(0.0)) {

      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;

      q_delta.coeffs() << sin_delta_by_delta * delta[0],

          sin_delta_by_delta * delta[1], sin_delta_by_delta * delta[2],

          cos(norm_delta);

    } else {

      // We do not just use q_delta = [0,0,0,1] here because that is a

      // constant and when used for automatic differentiation will

      // lead to a zero derivative. Instead we take a first order

      // approximation and evaluate it at zero.

      q_delta.coeffs() <<  delta[0], delta[1], delta[2], T(1.0);

    }

    Eigen::Map<Eigen::Quaternion<T> > x_plus_delta_ref(x_plus_delta);

    Eigen::Map<const Eigen::Quaternion<T> > x_ref(x);

    x_plus_delta_ref = q_delta * x_ref;

    return true;

  }

};

TEST(EigenQuaternionParameterization, ZeroTest) {

  Eigen::Quaterniond x(0.5, 0.5, 0.5, 0.5);

  double delta[3] = {0.0, 0.0, 0.0};

  Eigen::Quaterniond q_delta(1.0, 0.0, 0.0, 0.0);

  Eigen::Quaterniond x_plus_delta = q_delta * x;

  QuaternionParameterizationTestHelper

                                       EigenQuaternionPlus>(

      x.coeffs().data(), delta, x_plus_delta.coeffs().data());

}

TEST(EigenQuaternionParameterization, NearZeroTest) {

  Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45);

  x.normalize();

  double delta[3] = {0.24, 0.15, 0.10};

  for (int i = 0; i < 3; ++i) {

    delta[i] = delta[i] * 1e-14;

  }

  // Note: w is first in the constructor.

  Eigen::Quaterniond q_delta(1.0, delta[0], delta[1], delta[2]);

  Eigen::Quaterniond x_plus_delta = q_delta * x;

  QuaternionParameterizationTestHelper

                                       EigenQuaternionPlus>(

      x.coeffs().data(), delta, x_plus_delta.coeffs().data());

}

TEST(EigenQuaternionParameterization, AwayFromZeroTest) {

  Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45);

  x.normalize();

  double delta[3] = {0.24, 0.15, 0.10};

  const double delta_norm = sqrt(delta[0] * delta[0] +

                                 delta[1] * delta[1] +

                                 delta[2] * delta[2]);

  // Note: w is first in the constructor.

  Eigen::Quaterniond q_delta(cos(delta_norm),

                             sin(delta_norm) / delta_norm * delta[0],

                             sin(delta_norm) / delta_norm * delta[1],

                             sin(delta_norm) / delta_norm * delta[2]);

  Eigen::Quaterniond x_plus_delta = q_delta * x;

  QuaternionParameterizationTestHelper

                                       EigenQuaternionPlus>(

      x.coeffs().data(), delta, x_plus_delta.coeffs().data());

}

// Functor needed to implement automatically differentiated Plus for

// homogeneous vectors. Note this explicitly defined for vectors of size 4.

struct HomogeneousVectorParameterizationPlus {

  template<typename Scalar>

  bool operator()(const Scalar* p_x, const Scalar* p_delta,

                  Scalar* p_x_plus_delta) const {

    Eigen::Map<const Eigen::Matrix<Scalar, 4, 1> > x(p_x);

    Eigen::Map<const Eigen::Matrix<Scalar, 3, 1> > delta(p_delta);

    Eigen::Map<Eigen::Matrix<Scalar, 4, 1> > x_plus_delta(p_x_plus_delta);

    const Scalar squared_norm_delta =

        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];

    Eigen::Matrix<Scalar, 4, 1> y;

    Scalar one_half(0.5);

    if (squared_norm_delta > Scalar(0.0)) {

      Scalar norm_delta = sqrt(squared_norm_delta);

      Scalar norm_delta_div_2 = 0.5 * norm_delta;

      const Scalar sin_delta_by_delta = sin(norm_delta_div_2) /

          norm_delta_div_2;

      y[0] = sin_delta_by_delta * delta[0] * one_half;

      y[1] = sin_delta_by_delta * delta[1] * one_half;

      y[2] = sin_delta_by_delta * delta[2] * one_half;

      y[3] = cos(norm_delta_div_2);

    } else {

      // We do not just use y = [0,0,0,1] here because that is a

      // constant and when used for automatic differentiation will

      // lead to a zero derivative. Instead we take a first order

      // approximation and evaluate it at zero.

      y[0] = delta[0] * one_half;

      y[1] = delta[1] * one_half;

      y[2] = delta[2] * one_half;

      y[3] = Scalar(1.0);

    }

    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> v(4);

    Scalar beta;

    internal::ComputeHouseholderVector<Scalar>(x, &v, &beta);

    x_plus_delta = x.norm() * (y - v * (beta * v.dot(y)));

    return true;

  }

};

void HomogeneousVectorParameterizationHelper(const double* x,

                                             const double* delta) {

  const double kTolerance = 1e-14;

  HomogeneousVectorParameterization homogeneous_vector_parameterization(4);

  // Ensure the update maintains the norm.

  double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};

  homogeneous_vector_parameterization.Plus(x, delta, x_plus_delta);

  const double x_plus_delta_norm =

      sqrt(x_plus_delta[0] * x_plus_delta[0] +

           x_plus_delta[1] * x_plus_delta[1] +

           x_plus_delta[2] * x_plus_delta[2] +

           x_plus_delta[3] * x_plus_delta[3]);

  const double x_norm = sqrt(x[0] * x[0] + x[1] * x[1] +

                             x[2] * x[2] + x[3] * x[3]);

  EXPECT_NEAR(x_plus_delta_norm, x_norm, kTolerance);

  // Autodiff jacobian at delta_x = 0.

  AutoDiffLocalParameterization<HomogeneousVectorParameterizationPlus, 4, 3>

      autodiff_jacobian;

  double jacobian_autodiff[12];

  double jacobian_analytic[12];

  homogeneous_vector_parameterization.ComputeJacobian(x, jacobian_analytic);

  autodiff_jacobian.ComputeJacobian(x, jacobian_autodiff);

  for (int i = 0; i < 12; ++i) {

    EXPECT_TRUE(ceres::IsFinite(jacobian_analytic[i]));

    EXPECT_NEAR(jacobian_analytic[i], jacobian_autodiff[i], kTolerance)

        << "Jacobian mismatch: i = " << i << ", " << jacobian_analytic[i] << " "

        << jacobian_autodiff[i];

  }

}

TEST(HomogeneousVectorParameterization, ZeroTest) {

  double x[4] = {0.0, 0.0, 0.0, 1.0};

  Normalize<4>(x);

  double delta[3] = {0.0, 0.0, 0.0};

  HomogeneousVectorParameterizationHelper(x, delta);

}

TEST(HomogeneousVectorParameterization, NearZeroTest1) {

  double x[4] = {1e-5, 1e-5, 1e-5, 1.0};

  Normalize<4>(x);

  double delta[3] = {0.0, 1.0, 0.0};

  HomogeneousVectorParameterizationHelper(x, delta);

}

TEST(HomogeneousVectorParameterization, NearZeroTest2) {

  double x[4] = {0.001, 0.0, 0.0, 0.0};

  double delta[3] = {0.0, 1.0, 0.0};

  HomogeneousVectorParameterizationHelper(x, delta);

}

TEST(HomogeneousVectorParameterization, AwayFromZeroTest1) {

  double x[4] = {0.52, 0.25, 0.15, 0.45};

  Normalize<4>(x);

  double delta[3] = {0.0, 1.0, -0.5};

  HomogeneousVectorParameterizationHelper(x, delta);

}

TEST(HomogeneousVectorParameterization, AwayFromZeroTest2) {

  double x[4] = {0.87, -0.25, -0.34, 0.45};

  Normalize<4>(x);

  double delta[3] = {0.0, 0.0, -0.5};

  HomogeneousVectorParameterizationHelper(x, delta);

}

TEST(HomogeneousVectorParameterization, AwayFromZeroTest3) {

  double x[4] = {0.0, 0.0, 0.0, 2.0};

  double delta[3] = {0.0, 0.0, 0};

  HomogeneousVectorParameterizationHelper(x, delta);

}

TEST(HomogeneousVectorParameterization, AwayFromZeroTest4) {

  double x[4] = {0.2, -1.0, 0.0, 2.0};

  double delta[3] = {1.4, 0.0, -0.5};

  HomogeneousVectorParameterizationHelper(x, delta);

}

TEST(HomogeneousVectorParameterization, AwayFromZeroTest5) {

  double x[4] = {2.0, 0.0, 0.0, 0.0};

  double delta[3] = {1.4, 0.0, -0.5};

  HomogeneousVectorParameterizationHelper(x, delta);

}

TEST(HomogeneousVectorParameterization, DeathTests) {

  EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size");

}

class ProductParameterizationTest : public ::testing::Test {

 protected :

  virtual void SetUp() {

    const int global_size1 = 5;

    std::vector<int> constant_parameters1;

    constant_parameters1.push_back(2);

    param1_.reset(new SubsetParameterization(global_size1,

                                             constant_parameters1));

    const int global_size2 = 3;

    std::vector<int> constant_parameters2;

    constant_parameters2.push_back(0);

    constant_parameters2.push_back(1);

    param2_.reset(new SubsetParameterization(global_size2,

                                             constant_parameters2));

    const int global_size3 = 4;

    std::vector<int> constant_parameters3;

    constant_parameters3.push_back(1);

    param3_.reset(new SubsetParameterization(global_size3,

                                             constant_parameters3));

    const int global_size4 = 2;

    std::vector<int> constant_parameters4;

    constant_parameters4.push_back(1);

    param4_.reset(new SubsetParameterization(global_size4,

                                             constant_parameters4));

  }

  scoped_ptr<LocalParameterization> param1_;

  scoped_ptr<LocalParameterization> param2_;

  scoped_ptr<LocalParameterization> param3_;

  scoped_ptr<LocalParameterization> param4_;

};

TEST_F(ProductParameterizationTest, LocalAndGlobalSize2) {

  LocalParameterization* param1 = param1_.release();

  LocalParameterization* param2 = param2_.release();

  ProductParameterization product_param(param1, param2);

  EXPECT_EQ(product_param.LocalSize(),

            param1->LocalSize() + param2->LocalSize());

  EXPECT_EQ(product_param.GlobalSize(),

            param1->GlobalSize() + param2->GlobalSize());

}

TEST_F(ProductParameterizationTest, LocalAndGlobalSize3) {

  LocalParameterization* param1 = param1_.release();

  LocalParameterization* param2 = param2_.release();

  LocalParameterization* param3 = param3_.release();

  ProductParameterization product_param(param1, param2, param3);

  EXPECT_EQ(product_param.LocalSize(),

            param1->LocalSize() + param2->LocalSize() + param3->LocalSize());

  EXPECT_EQ(product_param.GlobalSize(),

            param1->GlobalSize() + param2->GlobalSize() + param3->GlobalSize());

}

TEST_F(ProductParameterizationTest, LocalAndGlobalSize4) {

  LocalParameterization* param1 = param1_.release();

  LocalParameterization* param2 = param2_.release();

  LocalParameterization* param3 = param3_.release();

  LocalParameterization* param4 = param4_.release();

  ProductParameterization product_param(param1, param2, param3, param4);

  EXPECT_EQ(product_param.LocalSize(),

            param1->LocalSize() +

            param2->LocalSize() +

            param3->LocalSize() +

            param4->LocalSize());

  EXPECT_EQ(product_param.GlobalSize(),

            param1->GlobalSize() +

            param2->GlobalSize() +

            param3->GlobalSize() +

            param4->GlobalSize());

}

TEST_F(ProductParameterizationTest, Plus) {

  LocalParameterization* param1 = param1_.release();

  LocalParameterization* param2 = param2_.release();

  LocalParameterization* param3 = param3_.release();

  LocalParameterization* param4 = param4_.release();

  ProductParameterization product_param(param1, param2, param3, param4);

  std::vector<double> x(product_param.GlobalSize(), 0.0);

  std::vector<double> delta(product_param.LocalSize(), 0.0);

  std::vector<double> x_plus_delta_expected(product_param.GlobalSize(), 0.0);

  std::vector<double> x_plus_delta(product_param.GlobalSize(), 0.0);

  for (int i = 0; i < product_param.GlobalSize(); ++i) {

    x[i] = RandNormal();

  }

  for (int i = 0; i < product_param.LocalSize(); ++i) {

    delta[i] = RandNormal();

  }

  EXPECT_TRUE(product_param.Plus(&x[0], &delta[0], &x_plus_delta_expected[0]));

  int x_cursor = 0;

  int delta_cursor = 0;

  EXPECT_TRUE(param1->Plus(&x[x_cursor],

                           &delta[delta_cursor],

                           &x_plus_delta[x_cursor]));

  x_cursor += param1->GlobalSize();

  delta_cursor += param1->LocalSize();

  EXPECT_TRUE(param2->Plus(&x[x_cursor],

                           &delta[delta_cursor],

                           &x_plus_delta[x_cursor]));

  x_cursor += param2->GlobalSize();

  delta_cursor += param2->LocalSize();

  EXPECT_TRUE(param3->Plus(&x[x_cursor],

                           &delta[delta_cursor],

                           &x_plus_delta[x_cursor]));

  x_cursor += param3->GlobalSize();

  delta_cursor += param3->LocalSize();

  EXPECT_TRUE(param4->Plus(&x[x_cursor],

                           &delta[delta_cursor],

                           &x_plus_delta[x_cursor]));

  x_cursor += param4->GlobalSize();

  delta_cursor += param4->LocalSize();

  for (int i = 0; i < x.size(); ++i) {

    EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]);

  }

}

TEST_F(ProductParameterizationTest, ComputeJacobian) {

  LocalParameterization* param1 = param1_.release();

  LocalParameterization* param2 = param2_.release();

  LocalParameterization* param3 = param3_.release();

  LocalParameterization* param4 = param4_.release();

  ProductParameterization product_param(param1, param2, param3, param4);

  std::vector<double> x(product_param.GlobalSize(), 0.0);

  for (int i = 0; i < product_param.GlobalSize(); ++i) {

    x[i] = RandNormal();

  }

  Matrix jacobian = Matrix::Random(product_param.GlobalSize(),

                                   product_param.LocalSize());

  EXPECT_TRUE(product_param.ComputeJacobian(&x[0], jacobian.data()));

  int x_cursor = 0;

  int delta_cursor = 0;

  Matrix jacobian1(param1->GlobalSize(), param1->LocalSize());

  EXPECT_TRUE(param1->ComputeJacobian(&x[x_cursor], jacobian1.data()));

  jacobian.block(x_cursor, delta_cursor,

                 param1->GlobalSize(),

                 param1->LocalSize())

      -= jacobian1;

  x_cursor += param1->GlobalSize();

  delta_cursor += param1->LocalSize();

  Matrix jacobian2(param2->GlobalSize(), param2->LocalSize());

  EXPECT_TRUE(param2->ComputeJacobian(&x[x_cursor], jacobian2.data()));

  jacobian.block(x_cursor, delta_cursor,

                 param2->GlobalSize(),

                 param2->LocalSize())

      -= jacobian2;

  x_cursor += param2->GlobalSize();

  delta_cursor += param2->LocalSize();

  Matrix jacobian3(param3->GlobalSize(), param3->LocalSize());

  EXPECT_TRUE(param3->ComputeJacobian(&x[x_cursor], jacobian3.data()));

  jacobian.block(x_cursor, delta_cursor,

                 param3->GlobalSize(),

                 param3->LocalSize())

      -= jacobian3;

  x_cursor += param3->GlobalSize();

  delta_cursor += param3->LocalSize();

  Matrix jacobian4(param4->GlobalSize(), param4->LocalSize());

  EXPECT_TRUE(param4->ComputeJacobian(&x[x_cursor], jacobian4.data()));

  jacobian.block(x_cursor, delta_cursor,

                 param4->GlobalSize(),

                 param4->LocalSize())

      -= jacobian4;

  x_cursor += param4->GlobalSize();

  delta_cursor += param4->LocalSize();

  EXPECT_NEAR(jacobian.norm(), 0.0, std::numeric_limits<double>::epsilon());

}

}  // namespace internal

}  // namespace ceres