internal/ceres/local_parameterization_test.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   4 //
   5 // Redistribution and use in source and binary forms, with or without
   6 // modification, are permitted provided that the following conditions are met:
   7 //
   8 // * Redistributions of source code must retain the above copyright notice,
   9 //   this list of conditions and the following disclaimer.
  10 // * Redistributions in binary form must reproduce the above copyright notice,
  11 //   this list of conditions and the following disclaimer in the documentation
  12 //   and/or other materials provided with the distribution.
  13 // * Neither the name of Google Inc. nor the names of its contributors may be
  14 //   used to endorse or promote products derived from this software without
  15 //   specific prior written permission.
  16 //
  17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27 // POSSIBILITY OF SUCH DAMAGE.
  28 //
  29 // Author: sameeragarwal@google.com (Sameer Agarwal)
  30
  31 #include <cmath>
  32 #include <limits>
  33 #include "Eigen/Geometry"
  34 #include "ceres/autodiff_local_parameterization.h"
  35 #include "ceres/fpclassify.h"
  36 #include "ceres/householder_vector.h"
  37 #include "ceres/internal/autodiff.h"
  38 #include "ceres/internal/eigen.h"
  39 #include "ceres/local_parameterization.h"
  40 #include "ceres/random.h"
  41 #include "ceres/rotation.h"
  42 #include "gtest/gtest.h"
  43
  44 namespace ceres {
  45 namespace internal {
  46
  47 TEST(IdentityParameterization, EverythingTest) {
  48   IdentityParameterization parameterization(3);
  49   EXPECT_EQ(parameterization.GlobalSize(), 3);
  50   EXPECT_EQ(parameterization.LocalSize(), 3);
  51
  52   double x[3] = {1.0, 2.0, 3.0};
  53   double delta[3] = {0.0, 1.0, 2.0};
  54   double x_plus_delta[3] = {0.0, 0.0, 0.0};
  55   parameterization.Plus(x, delta, x_plus_delta);
  56   EXPECT_EQ(x_plus_delta[0], 1.0);
  57   EXPECT_EQ(x_plus_delta[1], 3.0);
  58   EXPECT_EQ(x_plus_delta[2], 5.0);
  59
  60   double jacobian[9];
  61   parameterization.ComputeJacobian(x, jacobian);
  62   int k = 0;
  63   for (int i = 0; i < 3; ++i) {
  64     for (int j = 0; j < 3; ++j, ++k) {
  65       EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0);
  66     }
  67   }
  68
  69   Matrix global_matrix = Matrix::Ones(10, 3);
  70   Matrix local_matrix = Matrix::Zero(10, 3);
  71   parameterization.MultiplyByJacobian(x,
  72                                       10,
  73                                       global_matrix.data(),
  74                                       local_matrix.data());
  75   EXPECT_EQ((local_matrix - global_matrix).norm(), 0.0);
  76 }
  77
  78
  79 TEST(SubsetParameterization, NegativeParameterIndexDeathTest) {
  80   std::vector<int> constant_parameters;
  81   constant_parameters.push_back(-1);
  82   EXPECT_DEATH_IF_SUPPORTED(
  83       SubsetParameterization parameterization(2, constant_parameters),
  84       "greater than zero");
  85 }
  86
  87 TEST(SubsetParameterization, GreaterThanSizeParameterIndexDeathTest) {
  88   std::vector<int> constant_parameters;
  89   constant_parameters.push_back(2);
  90   EXPECT_DEATH_IF_SUPPORTED(
  91       SubsetParameterization parameterization(2, constant_parameters),
  92       "less than the size");
  93 }
  94
  95 TEST(SubsetParameterization, DuplicateParametersDeathTest) {
  96   std::vector<int> constant_parameters;
  97   constant_parameters.push_back(1);
  98   constant_parameters.push_back(1);
  99   EXPECT_DEATH_IF_SUPPORTED(
 100       SubsetParameterization parameterization(2, constant_parameters),
 101       "duplicates");
 102 }
 103
 104 TEST(SubsetParameterization,
 105      ProductParameterizationWithZeroLocalSizeSubsetParameterization1) {
 106   std::vector<int> constant_parameters;
 107   constant_parameters.push_back(0);
 108   LocalParameterization* subset_param =
 109       new SubsetParameterization(1, constant_parameters);
 110   LocalParameterization* identity_param = new IdentityParameterization(2);
 111   ProductParameterization product_param(subset_param, identity_param);
 112   EXPECT_EQ(product_param.GlobalSize(), 3);
 113   EXPECT_EQ(product_param.LocalSize(), 2);
 114   double x[] = {1.0, 1.0, 1.0};
 115   double delta[] = {2.0, 3.0};
 116   double x_plus_delta[] = {0.0, 0.0, 0.0};
 117   EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta));
 118   EXPECT_EQ(x_plus_delta[0], x[0]);
 119   EXPECT_EQ(x_plus_delta[1], x[1] + delta[0]);
 120   EXPECT_EQ(x_plus_delta[2], x[2] + delta[1]);
 121
 122   Matrix actual_jacobian(3, 2);
 123   EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data()));
 124 }
 125
 126 TEST(SubsetParameterization,
 127      ProductParameterizationWithZeroLocalSizeSubsetParameterization2) {
 128   std::vector<int> constant_parameters;
 129   constant_parameters.push_back(0);
 130   LocalParameterization* subset_param =
 131       new SubsetParameterization(1, constant_parameters);
 132   LocalParameterization* identity_param = new IdentityParameterization(2);
 133   ProductParameterization product_param(identity_param, subset_param);
 134   EXPECT_EQ(product_param.GlobalSize(), 3);
 135   EXPECT_EQ(product_param.LocalSize(), 2);
 136   double x[] = {1.0, 1.0, 1.0};
 137   double delta[] = {2.0, 3.0};
 138   double x_plus_delta[] = {0.0, 0.0, 0.0};
 139   EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta));
 140   EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]);
 141   EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]);
 142   EXPECT_EQ(x_plus_delta[2], x[2]);
 143
 144   Matrix actual_jacobian(3, 2);
 145   EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data()));
 146 }
 147
 148 TEST(SubsetParameterization, NormalFunctionTest) {
 149   const int kGlobalSize = 4;
 150   const int kLocalSize = 3;
 151
 152   double x[kGlobalSize] = {1.0, 2.0, 3.0, 4.0};
 153   for (int i = 0; i < kGlobalSize; ++i) {
 154     std::vector<int> constant_parameters;
 155     constant_parameters.push_back(i);
 156     SubsetParameterization parameterization(kGlobalSize, constant_parameters);
 157     double delta[kLocalSize] = {1.0, 2.0, 3.0};
 158     double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0};
 159
 160     parameterization.Plus(x, delta, x_plus_delta);
 161     int k = 0;
 162     for (int j = 0; j < kGlobalSize; ++j) {
 163       if (j == i)  {
 164         EXPECT_EQ(x_plus_delta[j], x[j]);
 165       } else {
 166         EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]);
 167       }
 168     }
 169
 170     double jacobian[kGlobalSize * kLocalSize];
 171     parameterization.ComputeJacobian(x, jacobian);
 172     int delta_cursor = 0;
 173     int jacobian_cursor = 0;
 174     for (int j = 0; j < kGlobalSize; ++j) {
 175       if (j != i) {
 176         for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) {
 177           EXPECT_EQ(jacobian[jacobian_cursor], delta_cursor == k ? 1.0 : 0.0);
 178         }
 179         ++delta_cursor;
 180       } else {
 181         for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) {
 182           EXPECT_EQ(jacobian[jacobian_cursor], 0.0);
 183         }
 184       }
 185     }
 186
 187     Matrix global_matrix = Matrix::Ones(10, kGlobalSize);
 188     for (int row = 0; row < kGlobalSize; ++row) {
 189       for (int col = 0; col < kGlobalSize; ++col) {
 190         global_matrix(row, col) = col;
 191       }
 192     }
 193
 194     Matrix local_matrix = Matrix::Zero(10, kLocalSize);
 195     parameterization.MultiplyByJacobian(x,
 196                                         10,
 197                                         global_matrix.data(),
 198                                         local_matrix.data());
 199     Matrix expected_local_matrix =
 200         global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize);
 201     EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0);
 202   }
 203 }
 204
 205 // Functor needed to implement automatically differentiated Plus for
 206 // quaternions.
 207 struct QuaternionPlus {
 208   template<typename T>
 209   bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
 210     const T squared_norm_delta =
 211         delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
 212
 213     T q_delta[4];
 214     if (squared_norm_delta > T(0.0)) {
 215       T norm_delta = sqrt(squared_norm_delta);
 216       const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
 217       q_delta[0] = cos(norm_delta);
 218       q_delta[1] = sin_delta_by_delta * delta[0];
 219       q_delta[2] = sin_delta_by_delta * delta[1];
 220       q_delta[3] = sin_delta_by_delta * delta[2];
 221     } else {
 222       // We do not just use q_delta = [1,0,0,0] here because that is a
 223       // constant and when used for automatic differentiation will
 224       // lead to a zero derivative. Instead we take a first order
 225       // approximation and evaluate it at zero.
 226       q_delta[0] = T(1.0);
 227       q_delta[1] = delta[0];
 228       q_delta[2] = delta[1];
 229       q_delta[3] = delta[2];
 230     }
 231
 232     QuaternionProduct(q_delta, x, x_plus_delta);
 233     return true;
 234   }
 235 };
 236
 237 template<typename Parameterization, typename Plus>
 238 void QuaternionParameterizationTestHelper(
 239     const double* x, const double* delta,
 240     const double* x_plus_delta_ref) {
 241   const int kGlobalSize = 4;
 242   const int kLocalSize = 3;
 243
 244   const double kTolerance = 1e-14;
 245
 246   double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0, 0.0};
 247   Parameterization parameterization;
 248   parameterization.Plus(x, delta, x_plus_delta);
 249   for (int i = 0; i < kGlobalSize; ++i) {
 250     EXPECT_NEAR(x_plus_delta[i], x_plus_delta[i], kTolerance);
 251   }
 252
 253   const double x_plus_delta_norm =
 254       sqrt(x_plus_delta[0] * x_plus_delta[0] +
 255            x_plus_delta[1] * x_plus_delta[1] +
 256            x_plus_delta[2] * x_plus_delta[2] +
 257            x_plus_delta[3] * x_plus_delta[3]);
 258
 259   EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance);
 260
 261   double jacobian_ref[12];
 262   double zero_delta[kLocalSize] = {0.0, 0.0, 0.0};
 263   const double* parameters[2] = {x, zero_delta};
 264   double* jacobian_array[2] = { NULL, jacobian_ref };
 265
 266   // Autodiff jacobian at delta_x = 0.
 267   internal::AutoDiff<Plus,
 268                      double,
 269                      kGlobalSize,
 270                      kLocalSize>::Differentiate(Plus(),
 271                                                 parameters,
 272                                                 kGlobalSize,
 273                                                 x_plus_delta,
 274                                                 jacobian_array);
 275
 276   double jacobian[12];
 277   parameterization.ComputeJacobian(x, jacobian);
 278   for (int i = 0; i < 12; ++i) {
 279     EXPECT_TRUE(IsFinite(jacobian[i]));
 280     EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance)
 281         << "Jacobian mismatch: i = " << i
 282         << "\n Expected \n"
 283         << ConstMatrixRef(jacobian_ref, kGlobalSize, kLocalSize)
 284         << "\n Actual \n"
 285         << ConstMatrixRef(jacobian, kGlobalSize, kLocalSize);
 286   }
 287
 288   Matrix global_matrix = Matrix::Random(10, kGlobalSize);
 289   Matrix local_matrix = Matrix::Zero(10, kLocalSize);
 290   parameterization.MultiplyByJacobian(x,
 291                                       10,
 292                                       global_matrix.data(),
 293                                       local_matrix.data());
 294   Matrix expected_local_matrix =
 295       global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize);
 296   EXPECT_NEAR((local_matrix - expected_local_matrix).norm(),
 297               0.0,
 298               10.0 * std::numeric_limits<double>::epsilon());
 299 }
 300
 301 template <int N>
 302 void Normalize(double* x) {
 303   VectorRef(x, N).normalize();
 304 }
 305
 306 TEST(QuaternionParameterization, ZeroTest) {
 307   double x[4] = {0.5, 0.5, 0.5, 0.5};
 308   double delta[3] = {0.0, 0.0, 0.0};
 309   double q_delta[4] = {1.0, 0.0, 0.0, 0.0};
 310   double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
 311   QuaternionProduct(q_delta, x, x_plus_delta);
 312   QuaternionParameterizationTestHelper<QuaternionParameterization,
 313                                        QuaternionPlus>(x, delta, x_plus_delta);
 314 }
 315
 316 TEST(QuaternionParameterization, NearZeroTest) {
 317   double x[4] = {0.52, 0.25, 0.15, 0.45};
 318   Normalize<4>(x);
 319
 320   double delta[3] = {0.24, 0.15, 0.10};
 321   for (int i = 0; i < 3; ++i) {
 322     delta[i] = delta[i] * 1e-14;
 323   }
 324
 325   double q_delta[4];
 326   q_delta[0] = 1.0;
 327   q_delta[1] = delta[0];
 328   q_delta[2] = delta[1];
 329   q_delta[3] = delta[2];
 330
 331   double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
 332   QuaternionProduct(q_delta, x, x_plus_delta);
 333   QuaternionParameterizationTestHelper<QuaternionParameterization,
 334                                        QuaternionPlus>(x, delta, x_plus_delta);
 335 }
 336
 337 TEST(QuaternionParameterization, AwayFromZeroTest) {
 338   double x[4] = {0.52, 0.25, 0.15, 0.45};
 339   Normalize<4>(x);
 340
 341   double delta[3] = {0.24, 0.15, 0.10};
 342   const double delta_norm = sqrt(delta[0] * delta[0] +
 343                                  delta[1] * delta[1] +
 344                                  delta[2] * delta[2]);
 345   double q_delta[4];
 346   q_delta[0] = cos(delta_norm);
 347   q_delta[1] = sin(delta_norm) / delta_norm * delta[0];
 348   q_delta[2] = sin(delta_norm) / delta_norm * delta[1];
 349   q_delta[3] = sin(delta_norm) / delta_norm * delta[2];
 350
 351   double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
 352   QuaternionProduct(q_delta, x, x_plus_delta);
 353   QuaternionParameterizationTestHelper<QuaternionParameterization,
 354                                        QuaternionPlus>(x, delta, x_plus_delta);
 355 }
 356
 357 // Functor needed to implement automatically differentiated Plus for
 358 // Eigen's quaternion.
 359 struct EigenQuaternionPlus {
 360   template<typename T>
 361   bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
 362     const T norm_delta =
 363         sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
 364
 365     Eigen::Quaternion<T> q_delta;
 366     if (norm_delta > T(0.0)) {
 367       const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
 368       q_delta.coeffs() << sin_delta_by_delta * delta[0],
 369           sin_delta_by_delta * delta[1], sin_delta_by_delta * delta[2],
 370           cos(norm_delta);
 371     } else {
 372       // We do not just use q_delta = [0,0,0,1] here because that is a
 373       // constant and when used for automatic differentiation will
 374       // lead to a zero derivative. Instead we take a first order
 375       // approximation and evaluate it at zero.
 376       q_delta.coeffs() <<  delta[0], delta[1], delta[2], T(1.0);
 377     }
 378
 379     Eigen::Map<Eigen::Quaternion<T> > x_plus_delta_ref(x_plus_delta);
 380     Eigen::Map<const Eigen::Quaternion<T> > x_ref(x);
 381     x_plus_delta_ref = q_delta * x_ref;
 382     return true;
 383   }
 384 };
 385
 386 TEST(EigenQuaternionParameterization, ZeroTest) {
 387   Eigen::Quaterniond x(0.5, 0.5, 0.5, 0.5);
 388   double delta[3] = {0.0, 0.0, 0.0};
 389   Eigen::Quaterniond q_delta(1.0, 0.0, 0.0, 0.0);
 390   Eigen::Quaterniond x_plus_delta = q_delta * x;
 391   QuaternionParameterizationTestHelper<EigenQuaternionParameterization,
 392                                        EigenQuaternionPlus>(
 393       x.coeffs().data(), delta, x_plus_delta.coeffs().data());
 394 }
 395
 396 TEST(EigenQuaternionParameterization, NearZeroTest) {
 397   Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45);
 398   x.normalize();
 399
 400   double delta[3] = {0.24, 0.15, 0.10};
 401   for (int i = 0; i < 3; ++i) {
 402     delta[i] = delta[i] * 1e-14;
 403   }
 404
 405   // Note: w is first in the constructor.
 406   Eigen::Quaterniond q_delta(1.0, delta[0], delta[1], delta[2]);
 407
 408   Eigen::Quaterniond x_plus_delta = q_delta * x;
 409   QuaternionParameterizationTestHelper<EigenQuaternionParameterization,
 410                                        EigenQuaternionPlus>(
 411       x.coeffs().data(), delta, x_plus_delta.coeffs().data());
 412 }
 413
 414 TEST(EigenQuaternionParameterization, AwayFromZeroTest) {
 415   Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45);
 416   x.normalize();
 417
 418   double delta[3] = {0.24, 0.15, 0.10};
 419   const double delta_norm = sqrt(delta[0] * delta[0] +
 420                                  delta[1] * delta[1] +
 421                                  delta[2] * delta[2]);
 422
 423   // Note: w is first in the constructor.
 424   Eigen::Quaterniond q_delta(cos(delta_norm),
 425                              sin(delta_norm) / delta_norm * delta[0],
 426                              sin(delta_norm) / delta_norm * delta[1],
 427                              sin(delta_norm) / delta_norm * delta[2]);
 428
 429   Eigen::Quaterniond x_plus_delta = q_delta * x;
 430   QuaternionParameterizationTestHelper<EigenQuaternionParameterization,
 431                                        EigenQuaternionPlus>(
 432       x.coeffs().data(), delta, x_plus_delta.coeffs().data());
 433 }
 434
 435 // Functor needed to implement automatically differentiated Plus for
 436 // homogeneous vectors. Note this explicitly defined for vectors of size 4.
 437 struct HomogeneousVectorParameterizationPlus {
 438   template<typename Scalar>
 439   bool operator()(const Scalar* p_x, const Scalar* p_delta,
 440                   Scalar* p_x_plus_delta) const {
 441     Eigen::Map<const Eigen::Matrix<Scalar, 4, 1> > x(p_x);
 442     Eigen::Map<const Eigen::Matrix<Scalar, 3, 1> > delta(p_delta);
 443     Eigen::Map<Eigen::Matrix<Scalar, 4, 1> > x_plus_delta(p_x_plus_delta);
 444
 445     const Scalar squared_norm_delta =
 446         delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
 447
 448     Eigen::Matrix<Scalar, 4, 1> y;
 449     Scalar one_half(0.5);
 450     if (squared_norm_delta > Scalar(0.0)) {
 451       Scalar norm_delta = sqrt(squared_norm_delta);
 452       Scalar norm_delta_div_2 = 0.5 * norm_delta;
 453       const Scalar sin_delta_by_delta = sin(norm_delta_div_2) /
 454           norm_delta_div_2;
 455       y[0] = sin_delta_by_delta * delta[0] * one_half;
 456       y[1] = sin_delta_by_delta * delta[1] * one_half;
 457       y[2] = sin_delta_by_delta * delta[2] * one_half;
 458       y[3] = cos(norm_delta_div_2);
 459
 460     } else {
 461       // We do not just use y = [0,0,0,1] here because that is a
 462       // constant and when used for automatic differentiation will
 463       // lead to a zero derivative. Instead we take a first order
 464       // approximation and evaluate it at zero.
 465       y[0] = delta[0] * one_half;
 466       y[1] = delta[1] * one_half;
 467       y[2] = delta[2] * one_half;
 468       y[3] = Scalar(1.0);
 469     }
 470
 471     Eigen::Matrix<Scalar, Eigen::Dynamic, 1> v(4);
 472     Scalar beta;
 473     internal::ComputeHouseholderVector<Scalar>(x, &v, &beta);
 474
 475     x_plus_delta = x.norm() * (y - v * (beta * v.dot(y)));
 476
 477     return true;
 478   }
 479 };
 480
 481 void HomogeneousVectorParameterizationHelper(const double* x,
 482                                              const double* delta) {
 483   const double kTolerance = 1e-14;
 484
 485   HomogeneousVectorParameterization homogeneous_vector_parameterization(4);
 486
 487   // Ensure the update maintains the norm.
 488   double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
 489   homogeneous_vector_parameterization.Plus(x, delta, x_plus_delta);
 490
 491   const double x_plus_delta_norm =
 492       sqrt(x_plus_delta[0] * x_plus_delta[0] +
 493            x_plus_delta[1] * x_plus_delta[1] +
 494            x_plus_delta[2] * x_plus_delta[2] +
 495            x_plus_delta[3] * x_plus_delta[3]);
 496
 497   const double x_norm = sqrt(x[0] * x[0] + x[1] * x[1] +
 498                              x[2] * x[2] + x[3] * x[3]);
 499
 500   EXPECT_NEAR(x_plus_delta_norm, x_norm, kTolerance);
 501
 502   // Autodiff jacobian at delta_x = 0.
 503   AutoDiffLocalParameterization<HomogeneousVectorParameterizationPlus, 4, 3>
 504       autodiff_jacobian;
 505
 506   double jacobian_autodiff[12];
 507   double jacobian_analytic[12];
 508
 509   homogeneous_vector_parameterization.ComputeJacobian(x, jacobian_analytic);
 510   autodiff_jacobian.ComputeJacobian(x, jacobian_autodiff);
 511
 512   for (int i = 0; i < 12; ++i) {
 513     EXPECT_TRUE(ceres::IsFinite(jacobian_analytic[i]));
 514     EXPECT_NEAR(jacobian_analytic[i], jacobian_autodiff[i], kTolerance)
 515         << "Jacobian mismatch: i = " << i << ", " << jacobian_analytic[i] << " "
 516         << jacobian_autodiff[i];
 517   }
 518 }
 519
 520 TEST(HomogeneousVectorParameterization, ZeroTest) {
 521   double x[4] = {0.0, 0.0, 0.0, 1.0};
 522   Normalize<4>(x);
 523   double delta[3] = {0.0, 0.0, 0.0};
 524
 525   HomogeneousVectorParameterizationHelper(x, delta);
 526 }
 527
 528 TEST(HomogeneousVectorParameterization, NearZeroTest1) {
 529   double x[4] = {1e-5, 1e-5, 1e-5, 1.0};
 530   Normalize<4>(x);
 531   double delta[3] = {0.0, 1.0, 0.0};
 532
 533   HomogeneousVectorParameterizationHelper(x, delta);
 534 }
 535
 536 TEST(HomogeneousVectorParameterization, NearZeroTest2) {
 537   double x[4] = {0.001, 0.0, 0.0, 0.0};
 538   double delta[3] = {0.0, 1.0, 0.0};
 539
 540   HomogeneousVectorParameterizationHelper(x, delta);
 541 }
 542
 543 TEST(HomogeneousVectorParameterization, AwayFromZeroTest1) {
 544   double x[4] = {0.52, 0.25, 0.15, 0.45};
 545   Normalize<4>(x);
 546   double delta[3] = {0.0, 1.0, -0.5};
 547
 548   HomogeneousVectorParameterizationHelper(x, delta);
 549 }
 550
 551 TEST(HomogeneousVectorParameterization, AwayFromZeroTest2) {
 552   double x[4] = {0.87, -0.25, -0.34, 0.45};
 553   Normalize<4>(x);
 554   double delta[3] = {0.0, 0.0, -0.5};
 555
 556   HomogeneousVectorParameterizationHelper(x, delta);
 557 }
 558
 559 TEST(HomogeneousVectorParameterization, AwayFromZeroTest3) {
 560   double x[4] = {0.0, 0.0, 0.0, 2.0};
 561   double delta[3] = {0.0, 0.0, 0};
 562
 563   HomogeneousVectorParameterizationHelper(x, delta);
 564 }
 565
 566 TEST(HomogeneousVectorParameterization, AwayFromZeroTest4) {
 567   double x[4] = {0.2, -1.0, 0.0, 2.0};
 568   double delta[3] = {1.4, 0.0, -0.5};
 569
 570   HomogeneousVectorParameterizationHelper(x, delta);
 571 }
 572
 573 TEST(HomogeneousVectorParameterization, AwayFromZeroTest5) {
 574   double x[4] = {2.0, 0.0, 0.0, 0.0};
 575   double delta[3] = {1.4, 0.0, -0.5};
 576
 577   HomogeneousVectorParameterizationHelper(x, delta);
 578 }
 579
 580 TEST(HomogeneousVectorParameterization, DeathTests) {
 581   EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size");
 582 }
 583
 584
 585 class ProductParameterizationTest : public ::testing::Test {
 586  protected :
 587   virtual void SetUp() {
 588     const int global_size1 = 5;
 589     std::vector<int> constant_parameters1;
 590     constant_parameters1.push_back(2);
 591     param1_.reset(new SubsetParameterization(global_size1,
 592                                              constant_parameters1));
 593
 594     const int global_size2 = 3;
 595     std::vector<int> constant_parameters2;
 596     constant_parameters2.push_back(0);
 597     constant_parameters2.push_back(1);
 598     param2_.reset(new SubsetParameterization(global_size2,
 599                                              constant_parameters2));
 600
 601     const int global_size3 = 4;
 602     std::vector<int> constant_parameters3;
 603     constant_parameters3.push_back(1);
 604     param3_.reset(new SubsetParameterization(global_size3,
 605                                              constant_parameters3));
 606
 607     const int global_size4 = 2;
 608     std::vector<int> constant_parameters4;
 609     constant_parameters4.push_back(1);
 610     param4_.reset(new SubsetParameterization(global_size4,
 611                                              constant_parameters4));
 612   }
 613
 614   scoped_ptr<LocalParameterization> param1_;
 615   scoped_ptr<LocalParameterization> param2_;
 616   scoped_ptr<LocalParameterization> param3_;
 617   scoped_ptr<LocalParameterization> param4_;
 618 };
 619
 620 TEST_F(ProductParameterizationTest, LocalAndGlobalSize2) {
 621   LocalParameterization* param1 = param1_.release();
 622   LocalParameterization* param2 = param2_.release();
 623
 624   ProductParameterization product_param(param1, param2);
 625   EXPECT_EQ(product_param.LocalSize(),
 626             param1->LocalSize() + param2->LocalSize());
 627   EXPECT_EQ(product_param.GlobalSize(),
 628             param1->GlobalSize() + param2->GlobalSize());
 629 }
 630
 631
 632 TEST_F(ProductParameterizationTest, LocalAndGlobalSize3) {
 633   LocalParameterization* param1 = param1_.release();
 634   LocalParameterization* param2 = param2_.release();
 635   LocalParameterization* param3 = param3_.release();
 636
 637   ProductParameterization product_param(param1, param2, param3);
 638   EXPECT_EQ(product_param.LocalSize(),
 639             param1->LocalSize() + param2->LocalSize() + param3->LocalSize());
 640   EXPECT_EQ(product_param.GlobalSize(),
 641             param1->GlobalSize() + param2->GlobalSize() + param3->GlobalSize());
 642 }
 643
 644 TEST_F(ProductParameterizationTest, LocalAndGlobalSize4) {
 645   LocalParameterization* param1 = param1_.release();
 646   LocalParameterization* param2 = param2_.release();
 647   LocalParameterization* param3 = param3_.release();
 648   LocalParameterization* param4 = param4_.release();
 649
 650   ProductParameterization product_param(param1, param2, param3, param4);
 651   EXPECT_EQ(product_param.LocalSize(),
 652             param1->LocalSize() +
 653             param2->LocalSize() +
 654             param3->LocalSize() +
 655             param4->LocalSize());
 656   EXPECT_EQ(product_param.GlobalSize(),
 657             param1->GlobalSize() +
 658             param2->GlobalSize() +
 659             param3->GlobalSize() +
 660             param4->GlobalSize());
 661 }
 662
 663 TEST_F(ProductParameterizationTest, Plus) {
 664   LocalParameterization* param1 = param1_.release();
 665   LocalParameterization* param2 = param2_.release();
 666   LocalParameterization* param3 = param3_.release();
 667   LocalParameterization* param4 = param4_.release();
 668
 669   ProductParameterization product_param(param1, param2, param3, param4);
 670   std::vector<double> x(product_param.GlobalSize(), 0.0);
 671   std::vector<double> delta(product_param.LocalSize(), 0.0);
 672   std::vector<double> x_plus_delta_expected(product_param.GlobalSize(), 0.0);
 673   std::vector<double> x_plus_delta(product_param.GlobalSize(), 0.0);
 674
 675   for (int i = 0; i < product_param.GlobalSize(); ++i) {
 676     x[i] = RandNormal();
 677   }
 678
 679   for (int i = 0; i < product_param.LocalSize(); ++i) {
 680     delta[i] = RandNormal();
 681   }
 682
 683   EXPECT_TRUE(product_param.Plus(&x[0], &delta[0], &x_plus_delta_expected[0]));
 684   int x_cursor = 0;
 685   int delta_cursor = 0;
 686
 687   EXPECT_TRUE(param1->Plus(&x[x_cursor],
 688                            &delta[delta_cursor],
 689                            &x_plus_delta[x_cursor]));
 690   x_cursor += param1->GlobalSize();
 691   delta_cursor += param1->LocalSize();
 692
 693   EXPECT_TRUE(param2->Plus(&x[x_cursor],
 694                            &delta[delta_cursor],
 695                            &x_plus_delta[x_cursor]));
 696   x_cursor += param2->GlobalSize();
 697   delta_cursor += param2->LocalSize();
 698
 699   EXPECT_TRUE(param3->Plus(&x[x_cursor],
 700                            &delta[delta_cursor],
 701                            &x_plus_delta[x_cursor]));
 702   x_cursor += param3->GlobalSize();
 703   delta_cursor += param3->LocalSize();
 704
 705   EXPECT_TRUE(param4->Plus(&x[x_cursor],
 706                            &delta[delta_cursor],
 707                            &x_plus_delta[x_cursor]));
 708   x_cursor += param4->GlobalSize();
 709   delta_cursor += param4->LocalSize();
 710
 711   for (int i = 0; i < x.size(); ++i) {
 712     EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]);
 713   }
 714 }
 715
 716 TEST_F(ProductParameterizationTest, ComputeJacobian) {
 717   LocalParameterization* param1 = param1_.release();
 718   LocalParameterization* param2 = param2_.release();
 719   LocalParameterization* param3 = param3_.release();
 720   LocalParameterization* param4 = param4_.release();
 721
 722   ProductParameterization product_param(param1, param2, param3, param4);
 723   std::vector<double> x(product_param.GlobalSize(), 0.0);
 724
 725   for (int i = 0; i < product_param.GlobalSize(); ++i) {
 726     x[i] = RandNormal();
 727   }
 728
 729   Matrix jacobian = Matrix::Random(product_param.GlobalSize(),
 730                                    product_param.LocalSize());
 731   EXPECT_TRUE(product_param.ComputeJacobian(&x[0], jacobian.data()));
 732   int x_cursor = 0;
 733   int delta_cursor = 0;
 734
 735   Matrix jacobian1(param1->GlobalSize(), param1->LocalSize());
 736   EXPECT_TRUE(param1->ComputeJacobian(&x[x_cursor], jacobian1.data()));
 737   jacobian.block(x_cursor, delta_cursor,
 738                  param1->GlobalSize(),
 739                  param1->LocalSize())
 740       -= jacobian1;
 741   x_cursor += param1->GlobalSize();
 742   delta_cursor += param1->LocalSize();
 743
 744   Matrix jacobian2(param2->GlobalSize(), param2->LocalSize());
 745   EXPECT_TRUE(param2->ComputeJacobian(&x[x_cursor], jacobian2.data()));
 746   jacobian.block(x_cursor, delta_cursor,
 747                  param2->GlobalSize(),
 748                  param2->LocalSize())
 749       -= jacobian2;
 750   x_cursor += param2->GlobalSize();
 751   delta_cursor += param2->LocalSize();
 752
 753   Matrix jacobian3(param3->GlobalSize(), param3->LocalSize());
 754   EXPECT_TRUE(param3->ComputeJacobian(&x[x_cursor], jacobian3.data()));
 755   jacobian.block(x_cursor, delta_cursor,
 756                  param3->GlobalSize(),
 757                  param3->LocalSize())
 758       -= jacobian3;
 759   x_cursor += param3->GlobalSize();
 760   delta_cursor += param3->LocalSize();
 761
 762   Matrix jacobian4(param4->GlobalSize(), param4->LocalSize());
 763   EXPECT_TRUE(param4->ComputeJacobian(&x[x_cursor], jacobian4.data()));
 764   jacobian.block(x_cursor, delta_cursor,
 765                  param4->GlobalSize(),
 766                  param4->LocalSize())
 767       -= jacobian4;
 768   x_cursor += param4->GlobalSize();
 769   delta_cursor += param4->LocalSize();
 770
 771   EXPECT_NEAR(jacobian.norm(), 0.0, std::numeric_limits<double>::epsilon());
 772 }
 773
 774 }  // namespace internal
 775 }  // namespace ceres