internal/ceres/dynamic_numeric_diff_cost_function_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 //         mierle@gmail.com (Keir Mierle)
  31
  32 #include <cstddef>
  33
  34 #include "ceres/dynamic_numeric_diff_cost_function.h"
  35 #include "ceres/internal/scoped_ptr.h"
  36 #include "gtest/gtest.h"
  37
  38 namespace ceres {
  39 namespace internal {
  40
  41 using std::vector;
  42
  43 const double kTolerance = 1e-6;
  44
  45 // Takes 2 parameter blocks:
  46 //     parameters[0] is size 10.
  47 //     parameters[1] is size 5.
  48 // Emits 21 residuals:
  49 //     A: i - parameters[0][i], for i in [0,10)  -- this is 10 residuals
  50 //     B: parameters[0][i] - i, for i in [0,10)  -- this is another 10.
  51 //     C: sum(parameters[0][i]^2 - 8*parameters[0][i]) + sum(parameters[1][i])
  52 class MyCostFunctor {
  53  public:
  54   bool operator()(double const* const* parameters, double* residuals) const {
  55     const double* params0 = parameters[0];
  56     int r = 0;
  57     for (int i = 0; i < 10; ++i) {
  58       residuals[r++] = i - params0[i];
  59       residuals[r++] = params0[i] - i;
  60     }
  61
  62     double c_residual = 0.0;
  63     for (int i = 0; i < 10; ++i) {
  64       c_residual += pow(params0[i], 2) - 8.0 * params0[i];
  65     }
  66
  67     const double* params1 = parameters[1];
  68     for (int i = 0; i < 5; ++i) {
  69       c_residual += params1[i];
  70     }
  71     residuals[r++] = c_residual;
  72     return true;
  73   }
  74 };
  75
  76 TEST(DynamicNumericdiffCostFunctionTest, TestResiduals) {
  77   vector<double> param_block_0(10, 0.0);
  78   vector<double> param_block_1(5, 0.0);
  79   DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
  80       new MyCostFunctor());
  81   cost_function.AddParameterBlock(param_block_0.size());
  82   cost_function.AddParameterBlock(param_block_1.size());
  83   cost_function.SetNumResiduals(21);
  84
  85   // Test residual computation.
  86   vector<double> residuals(21, -100000);
  87   vector<double*> parameter_blocks(2);
  88   parameter_blocks[0] = &param_block_0[0];
  89   parameter_blocks[1] = &param_block_1[0];
  90   EXPECT_TRUE(cost_function.Evaluate(&parameter_blocks[0],
  91                                      residuals.data(),
  92                                      NULL));
  93   for (int r = 0; r < 10; ++r) {
  94     EXPECT_EQ(1.0 * r, residuals.at(r * 2));
  95     EXPECT_EQ(-1.0 * r, residuals.at(r * 2 + 1));
  96   }
  97   EXPECT_EQ(0, residuals.at(20));
  98 }
  99
 100
 101 TEST(DynamicNumericdiffCostFunctionTest, TestJacobian) {
 102   // Test the residual counting.
 103   vector<double> param_block_0(10, 0.0);
 104   for (int i = 0; i < 10; ++i) {
 105     param_block_0[i] = 2 * i;
 106   }
 107   vector<double> param_block_1(5, 0.0);
 108   DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
 109       new MyCostFunctor());
 110   cost_function.AddParameterBlock(param_block_0.size());
 111   cost_function.AddParameterBlock(param_block_1.size());
 112   cost_function.SetNumResiduals(21);
 113
 114   // Prepare the residuals.
 115   vector<double> residuals(21, -100000);
 116
 117   // Prepare the parameters.
 118   vector<double*> parameter_blocks(2);
 119   parameter_blocks[0] = &param_block_0[0];
 120   parameter_blocks[1] = &param_block_1[0];
 121
 122   // Prepare the jacobian.
 123   vector<vector<double> > jacobian_vect(2);
 124   jacobian_vect[0].resize(21 * 10, -100000);
 125   jacobian_vect[1].resize(21 * 5, -100000);
 126   vector<double*> jacobian;
 127   jacobian.push_back(jacobian_vect[0].data());
 128   jacobian.push_back(jacobian_vect[1].data());
 129
 130   // Test jacobian computation.
 131   EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
 132                                      residuals.data(),
 133                                      jacobian.data()));
 134
 135   for (int r = 0; r < 10; ++r) {
 136     EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
 137     EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
 138   }
 139   EXPECT_EQ(420, residuals.at(20));
 140   for (int p = 0; p < 10; ++p) {
 141     // Check "A" Jacobian.
 142     EXPECT_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance);
 143     // Check "B" Jacobian.
 144     EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance);
 145     jacobian_vect[0][2*p * 10 + p] = 0.0;
 146     jacobian_vect[0][(2*p+1) * 10 + p] = 0.0;
 147   }
 148
 149   // Check "C" Jacobian for first parameter block.
 150   for (int p = 0; p < 10; ++p) {
 151     EXPECT_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance);
 152     jacobian_vect[0][20 * 10 + p] = 0.0;
 153   }
 154   for (int i = 0; i < jacobian_vect[0].size(); ++i) {
 155     EXPECT_NEAR(0.0, jacobian_vect[0][i], kTolerance);
 156   }
 157
 158   // Check "C" Jacobian for second parameter block.
 159   for (int p = 0; p < 5; ++p) {
 160     EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance);
 161     jacobian_vect[1][20 * 5 + p] = 0.0;
 162   }
 163   for (int i = 0; i < jacobian_vect[1].size(); ++i) {
 164     EXPECT_NEAR(0.0, jacobian_vect[1][i], kTolerance);
 165   }
 166 }
 167
 168 TEST(DynamicNumericdiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) {  // NOLINT
 169   // Test the residual counting.
 170   vector<double> param_block_0(10, 0.0);
 171   for (int i = 0; i < 10; ++i) {
 172     param_block_0[i] = 2 * i;
 173   }
 174   vector<double> param_block_1(5, 0.0);
 175   DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
 176       new MyCostFunctor());
 177   cost_function.AddParameterBlock(param_block_0.size());
 178   cost_function.AddParameterBlock(param_block_1.size());
 179   cost_function.SetNumResiduals(21);
 180
 181   // Prepare the residuals.
 182   vector<double> residuals(21, -100000);
 183
 184   // Prepare the parameters.
 185   vector<double*> parameter_blocks(2);
 186   parameter_blocks[0] = &param_block_0[0];
 187   parameter_blocks[1] = &param_block_1[0];
 188
 189   // Prepare the jacobian.
 190   vector<vector<double> > jacobian_vect(2);
 191   jacobian_vect[0].resize(21 * 10, -100000);
 192   jacobian_vect[1].resize(21 * 5, -100000);
 193   vector<double*> jacobian;
 194   jacobian.push_back(NULL);
 195   jacobian.push_back(jacobian_vect[1].data());
 196
 197   // Test jacobian computation.
 198   EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
 199                                      residuals.data(),
 200                                      jacobian.data()));
 201
 202   for (int r = 0; r < 10; ++r) {
 203     EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
 204     EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
 205   }
 206   EXPECT_EQ(420, residuals.at(20));
 207
 208   // Check "C" Jacobian for second parameter block.
 209   for (int p = 0; p < 5; ++p) {
 210     EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance);
 211     jacobian_vect[1][20 * 5 + p] = 0.0;
 212   }
 213   for (int i = 0; i < jacobian_vect[1].size(); ++i) {
 214     EXPECT_EQ(0.0, jacobian_vect[1][i]);
 215   }
 216 }
 217
 218 TEST(DynamicNumericdiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) {  // NOLINT
 219   // Test the residual counting.
 220   vector<double> param_block_0(10, 0.0);
 221   for (int i = 0; i < 10; ++i) {
 222     param_block_0[i] = 2 * i;
 223   }
 224   vector<double> param_block_1(5, 0.0);
 225   DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
 226       new MyCostFunctor());
 227   cost_function.AddParameterBlock(param_block_0.size());
 228   cost_function.AddParameterBlock(param_block_1.size());
 229   cost_function.SetNumResiduals(21);
 230
 231   // Prepare the residuals.
 232   vector<double> residuals(21, -100000);
 233
 234   // Prepare the parameters.
 235   vector<double*> parameter_blocks(2);
 236   parameter_blocks[0] = &param_block_0[0];
 237   parameter_blocks[1] = &param_block_1[0];
 238
 239   // Prepare the jacobian.
 240   vector<vector<double> > jacobian_vect(2);
 241   jacobian_vect[0].resize(21 * 10, -100000);
 242   jacobian_vect[1].resize(21 * 5, -100000);
 243   vector<double*> jacobian;
 244   jacobian.push_back(jacobian_vect[0].data());
 245   jacobian.push_back(NULL);
 246
 247   // Test jacobian computation.
 248   EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
 249                                      residuals.data(),
 250                                      jacobian.data()));
 251
 252   for (int r = 0; r < 10; ++r) {
 253     EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
 254     EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
 255   }
 256   EXPECT_EQ(420, residuals.at(20));
 257   for (int p = 0; p < 10; ++p) {
 258     // Check "A" Jacobian.
 259     EXPECT_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance);
 260     // Check "B" Jacobian.
 261     EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance);
 262     jacobian_vect[0][2*p * 10 + p] = 0.0;
 263     jacobian_vect[0][(2*p+1) * 10 + p] = 0.0;
 264   }
 265
 266   // Check "C" Jacobian for first parameter block.
 267   for (int p = 0; p < 10; ++p) {
 268     EXPECT_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance);
 269     jacobian_vect[0][20 * 10 + p] = 0.0;
 270   }
 271   for (int i = 0; i < jacobian_vect[0].size(); ++i) {
 272     EXPECT_EQ(0.0, jacobian_vect[0][i]);
 273   }
 274 }
 275
 276 // Takes 3 parameter blocks:
 277 //     parameters[0] (x) is size 1.
 278 //     parameters[1] (y) is size 2.
 279 //     parameters[2] (z) is size 3.
 280 // Emits 7 residuals:
 281 //     A: x[0] (= sum_x)
 282 //     B: y[0] + 2.0 * y[1] (= sum_y)
 283 //     C: z[0] + 3.0 * z[1] + 6.0 * z[2] (= sum_z)
 284 //     D: sum_x * sum_y
 285 //     E: sum_y * sum_z
 286 //     F: sum_x * sum_z
 287 //     G: sum_x * sum_y * sum_z
 288 class MyThreeParameterCostFunctor {
 289  public:
 290   template <typename T>
 291   bool operator()(T const* const* parameters, T* residuals) const {
 292     const T* x = parameters[0];
 293     const T* y = parameters[1];
 294     const T* z = parameters[2];
 295
 296     T sum_x = x[0];
 297     T sum_y = y[0] + 2.0 * y[1];
 298     T sum_z = z[0] + 3.0 * z[1] + 6.0 * z[2];
 299
 300     residuals[0] = sum_x;
 301     residuals[1] = sum_y;
 302     residuals[2] = sum_z;
 303     residuals[3] = sum_x * sum_y;
 304     residuals[4] = sum_y * sum_z;
 305     residuals[5] = sum_x * sum_z;
 306     residuals[6] = sum_x * sum_y * sum_z;
 307     return true;
 308   }
 309 };
 310
 311 class ThreeParameterCostFunctorTest : public ::testing::Test {
 312  protected:
 313   virtual void SetUp() {
 314     // Prepare the parameters.
 315     x_.resize(1);
 316     x_[0] = 0.0;
 317
 318     y_.resize(2);
 319     y_[0] = 1.0;
 320     y_[1] = 3.0;
 321
 322     z_.resize(3);
 323     z_[0] = 2.0;
 324     z_[1] = 4.0;
 325     z_[2] = 6.0;
 326
 327     parameter_blocks_.resize(3);
 328     parameter_blocks_[0] = &x_[0];
 329     parameter_blocks_[1] = &y_[0];
 330     parameter_blocks_[2] = &z_[0];
 331
 332     // Prepare the cost function.
 333     typedef DynamicNumericDiffCostFunction<MyThreeParameterCostFunctor>
 334       DynamicMyThreeParameterCostFunction;
 335     DynamicMyThreeParameterCostFunction * cost_function =
 336       new DynamicMyThreeParameterCostFunction(
 337         new MyThreeParameterCostFunctor());
 338     cost_function->AddParameterBlock(1);
 339     cost_function->AddParameterBlock(2);
 340     cost_function->AddParameterBlock(3);
 341     cost_function->SetNumResiduals(7);
 342
 343     cost_function_.reset(cost_function);
 344
 345     // Setup jacobian data.
 346     jacobian_vect_.resize(3);
 347     jacobian_vect_[0].resize(7 * x_.size(), -100000);
 348     jacobian_vect_[1].resize(7 * y_.size(), -100000);
 349     jacobian_vect_[2].resize(7 * z_.size(), -100000);
 350
 351     // Prepare the expected residuals.
 352     const double sum_x = x_[0];
 353     const double sum_y = y_[0] + 2.0 * y_[1];
 354     const double sum_z = z_[0] + 3.0 * z_[1] + 6.0 * z_[2];
 355
 356     expected_residuals_.resize(7);
 357     expected_residuals_[0] = sum_x;
 358     expected_residuals_[1] = sum_y;
 359     expected_residuals_[2] = sum_z;
 360     expected_residuals_[3] = sum_x * sum_y;
 361     expected_residuals_[4] = sum_y * sum_z;
 362     expected_residuals_[5] = sum_x * sum_z;
 363     expected_residuals_[6] = sum_x * sum_y * sum_z;
 364
 365     // Prepare the expected jacobian entries.
 366     expected_jacobian_x_.resize(7);
 367     expected_jacobian_x_[0] = 1.0;
 368     expected_jacobian_x_[1] = 0.0;
 369     expected_jacobian_x_[2] = 0.0;
 370     expected_jacobian_x_[3] = sum_y;
 371     expected_jacobian_x_[4] = 0.0;
 372     expected_jacobian_x_[5] = sum_z;
 373     expected_jacobian_x_[6] = sum_y * sum_z;
 374
 375     expected_jacobian_y_.resize(14);
 376     expected_jacobian_y_[0] = 0.0;
 377     expected_jacobian_y_[1] = 0.0;
 378     expected_jacobian_y_[2] = 1.0;
 379     expected_jacobian_y_[3] = 2.0;
 380     expected_jacobian_y_[4] = 0.0;
 381     expected_jacobian_y_[5] = 0.0;
 382     expected_jacobian_y_[6] = sum_x;
 383     expected_jacobian_y_[7] = 2.0 * sum_x;
 384     expected_jacobian_y_[8] = sum_z;
 385     expected_jacobian_y_[9] = 2.0 * sum_z;
 386     expected_jacobian_y_[10] = 0.0;
 387     expected_jacobian_y_[11] = 0.0;
 388     expected_jacobian_y_[12] = sum_x * sum_z;
 389     expected_jacobian_y_[13] = 2.0 * sum_x * sum_z;
 390
 391     expected_jacobian_z_.resize(21);
 392     expected_jacobian_z_[0] = 0.0;
 393     expected_jacobian_z_[1] = 0.0;
 394     expected_jacobian_z_[2] = 0.0;
 395     expected_jacobian_z_[3] = 0.0;
 396     expected_jacobian_z_[4] = 0.0;
 397     expected_jacobian_z_[5] = 0.0;
 398     expected_jacobian_z_[6] = 1.0;
 399     expected_jacobian_z_[7] = 3.0;
 400     expected_jacobian_z_[8] = 6.0;
 401     expected_jacobian_z_[9] = 0.0;
 402     expected_jacobian_z_[10] = 0.0;
 403     expected_jacobian_z_[11] = 0.0;
 404     expected_jacobian_z_[12] = sum_y;
 405     expected_jacobian_z_[13] = 3.0 * sum_y;
 406     expected_jacobian_z_[14] = 6.0 * sum_y;
 407     expected_jacobian_z_[15] = sum_x;
 408     expected_jacobian_z_[16] = 3.0 * sum_x;
 409     expected_jacobian_z_[17] = 6.0 * sum_x;
 410     expected_jacobian_z_[18] = sum_x * sum_y;
 411     expected_jacobian_z_[19] = 3.0 * sum_x * sum_y;
 412     expected_jacobian_z_[20] = 6.0 * sum_x * sum_y;
 413   }
 414
 415  protected:
 416   vector<double> x_;
 417   vector<double> y_;
 418   vector<double> z_;
 419
 420   vector<double*> parameter_blocks_;
 421
 422   scoped_ptr<CostFunction> cost_function_;
 423
 424   vector<vector<double> > jacobian_vect_;
 425
 426   vector<double> expected_residuals_;
 427
 428   vector<double> expected_jacobian_x_;
 429   vector<double> expected_jacobian_y_;
 430   vector<double> expected_jacobian_z_;
 431 };
 432
 433 TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterResiduals) {
 434   vector<double> residuals(7, -100000);
 435   EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
 436                                        residuals.data(),
 437                                        NULL));
 438   for (int i = 0; i < 7; ++i) {
 439     EXPECT_EQ(expected_residuals_[i], residuals[i]);
 440   }
 441 }
 442
 443 TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterJacobian) {
 444   vector<double> residuals(7, -100000);
 445
 446   vector<double*> jacobian;
 447   jacobian.push_back(jacobian_vect_[0].data());
 448   jacobian.push_back(jacobian_vect_[1].data());
 449   jacobian.push_back(jacobian_vect_[2].data());
 450
 451   EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
 452                                        residuals.data(),
 453                                        jacobian.data()));
 454
 455   for (int i = 0; i < 7; ++i) {
 456     EXPECT_EQ(expected_residuals_[i], residuals[i]);
 457   }
 458
 459   for (int i = 0; i < 7; ++i) {
 460     EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance);
 461   }
 462
 463   for (int i = 0; i < 14; ++i) {
 464     EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance);
 465   }
 466
 467   for (int i = 0; i < 21; ++i) {
 468     EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance);
 469   }
 470 }
 471
 472 TEST_F(ThreeParameterCostFunctorTest,
 473        ThreeParameterJacobianWithFirstAndLastParameterBlockConstant) {
 474   vector<double> residuals(7, -100000);
 475
 476   vector<double*> jacobian;
 477   jacobian.push_back(NULL);
 478   jacobian.push_back(jacobian_vect_[1].data());
 479   jacobian.push_back(NULL);
 480
 481   EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
 482                                        residuals.data(),
 483                                        jacobian.data()));
 484
 485   for (int i = 0; i < 7; ++i) {
 486     EXPECT_EQ(expected_residuals_[i], residuals[i]);
 487   }
 488
 489   for (int i = 0; i < 14; ++i) {
 490     EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance);
 491   }
 492 }
 493
 494 TEST_F(ThreeParameterCostFunctorTest,
 495        ThreeParameterJacobianWithSecondParameterBlockConstant) {
 496   vector<double> residuals(7, -100000);
 497
 498   vector<double*> jacobian;
 499   jacobian.push_back(jacobian_vect_[0].data());
 500   jacobian.push_back(NULL);
 501   jacobian.push_back(jacobian_vect_[2].data());
 502
 503   EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
 504                                        residuals.data(),
 505                                        jacobian.data()));
 506
 507   for (int i = 0; i < 7; ++i) {
 508     EXPECT_EQ(expected_residuals_[i], residuals[i]);
 509   }
 510
 511   for (int i = 0; i < 7; ++i) {
 512     EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance);
 513   }
 514
 515   for (int i = 0; i < 21; ++i) {
 516     EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance);
 517   }
 518 }
 519
 520 }  // namespace internal
 521 }  // namespace ceres