internal/ceres/evaluator_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: keir@google.com (Keir Mierle)
  30 //
  31 // Tests shared across evaluators. The tests try all combinations of linear
  32 // solver and num_eliminate_blocks (for schur-based solvers).
  33
  34 #include "ceres/evaluator.h"
  35
  36 #include "ceres/casts.h"
  37 #include "ceres/cost_function.h"
  38 #include "ceres/crs_matrix.h"
  39 #include "ceres/evaluator_test_utils.h"
  40 #include "ceres/internal/eigen.h"
  41 #include "ceres/internal/scoped_ptr.h"
  42 #include "ceres/local_parameterization.h"
  43 #include "ceres/problem_impl.h"
  44 #include "ceres/program.h"
  45 #include "ceres/sized_cost_function.h"
  46 #include "ceres/sparse_matrix.h"
  47 #include "ceres/stringprintf.h"
  48 #include "ceres/types.h"
  49 #include "gtest/gtest.h"
  50
  51 namespace ceres {
  52 namespace internal {
  53
  54 using std::string;
  55 using std::vector;
  56
  57 // TODO(keir): Consider pushing this into a common test utils file.
  58 template<int kFactor, int kNumResiduals,
  59          int N0 = 0, int N1 = 0, int N2 = 0, bool kSucceeds = true>
  60 class ParameterIgnoringCostFunction
  61     : public SizedCostFunction<kNumResiduals, N0, N1, N2> {
  62   typedef SizedCostFunction<kNumResiduals, N0, N1, N2> Base;
  63  public:
  64   virtual bool Evaluate(double const* const* parameters,
  65                         double* residuals,
  66                         double** jacobians) const {
  67     for (int i = 0; i < Base::num_residuals(); ++i) {
  68       residuals[i] = i + 1;
  69     }
  70     if (jacobians) {
  71       for (int k = 0; k < Base::parameter_block_sizes().size(); ++k) {
  72         // The jacobians here are full sized, but they are transformed in the
  73         // evaluator into the "local" jacobian. In the tests, the "subset
  74         // constant" parameterization is used, which should pick out columns
  75         // from these jacobians. Put values in the jacobian that make this
  76         // obvious; in particular, make the jacobians like this:
  77         //
  78         //   1 2 3 4 ...
  79         //   1 2 3 4 ...   .*  kFactor
  80         //   1 2 3 4 ...
  81         //
  82         // where the multiplication by kFactor makes it easier to distinguish
  83         // between Jacobians of different residuals for the same parameter.
  84         if (jacobians[k] != NULL) {
  85           MatrixRef jacobian(jacobians[k],
  86                              Base::num_residuals(),
  87                              Base::parameter_block_sizes()[k]);
  88           for (int j = 0; j < Base::parameter_block_sizes()[k]; ++j) {
  89             jacobian.col(j).setConstant(kFactor * (j + 1));
  90           }
  91         }
  92       }
  93     }
  94     return kSucceeds;
  95   }
  96 };
  97
  98 struct EvaluatorTestOptions {
  99   EvaluatorTestOptions(LinearSolverType linear_solver_type,
 100                        int num_eliminate_blocks,
 101                        bool dynamic_sparsity = false)
 102     : linear_solver_type(linear_solver_type),
 103       num_eliminate_blocks(num_eliminate_blocks),
 104       dynamic_sparsity(dynamic_sparsity) {}
 105
 106   LinearSolverType linear_solver_type;
 107   int num_eliminate_blocks;
 108   bool dynamic_sparsity;
 109 };
 110
 111 struct EvaluatorTest
 112     : public ::testing::TestWithParam<EvaluatorTestOptions> {
 113   Evaluator* CreateEvaluator(Program* program) {
 114     // This program is straight from the ProblemImpl, and so has no index/offset
 115     // yet; compute it here as required by the evalutor implementations.
 116     program->SetParameterOffsetsAndIndex();
 117
 118     if (VLOG_IS_ON(1)) {
 119       string report;
 120       StringAppendF(&report, "Creating evaluator with type: %d",
 121                     GetParam().linear_solver_type);
 122       if (GetParam().linear_solver_type == SPARSE_NORMAL_CHOLESKY) {
 123         StringAppendF(&report, ", dynamic_sparsity: %d",
 124                       GetParam().dynamic_sparsity);
 125       }
 126       StringAppendF(&report, " and num_eliminate_blocks: %d",
 127                     GetParam().num_eliminate_blocks);
 128       VLOG(1) << report;
 129     }
 130     Evaluator::Options options;
 131     options.linear_solver_type = GetParam().linear_solver_type;
 132     options.num_eliminate_blocks = GetParam().num_eliminate_blocks;
 133     options.dynamic_sparsity = GetParam().dynamic_sparsity;
 134     string error;
 135     return Evaluator::Create(options, program, &error);
 136   }
 137
 138   void EvaluateAndCompare(ProblemImpl *problem,
 139                           int expected_num_rows,
 140                           int expected_num_cols,
 141                           double expected_cost,
 142                           const double* expected_residuals,
 143                           const double* expected_gradient,
 144                           const double* expected_jacobian) {
 145     scoped_ptr<Evaluator> evaluator(
 146         CreateEvaluator(problem->mutable_program()));
 147     int num_residuals = expected_num_rows;
 148     int num_parameters = expected_num_cols;
 149
 150     double cost = -1;
 151
 152     Vector residuals(num_residuals);
 153     residuals.setConstant(-2000);
 154
 155     Vector gradient(num_parameters);
 156     gradient.setConstant(-3000);
 157
 158     scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
 159
 160     ASSERT_EQ(expected_num_rows, evaluator->NumResiduals());
 161     ASSERT_EQ(expected_num_cols, evaluator->NumEffectiveParameters());
 162     ASSERT_EQ(expected_num_rows, jacobian->num_rows());
 163     ASSERT_EQ(expected_num_cols, jacobian->num_cols());
 164
 165     vector<double> state(evaluator->NumParameters());
 166
 167     ASSERT_TRUE(evaluator->Evaluate(
 168           &state[0],
 169           &cost,
 170           expected_residuals != NULL ? &residuals[0]  : NULL,
 171           expected_gradient  != NULL ? &gradient[0]   : NULL,
 172           expected_jacobian  != NULL ? jacobian.get() : NULL));
 173
 174     Matrix actual_jacobian;
 175     if (expected_jacobian != NULL) {
 176       jacobian->ToDenseMatrix(&actual_jacobian);
 177     }
 178
 179     CompareEvaluations(expected_num_rows,
 180                        expected_num_cols,
 181                        expected_cost,
 182                        expected_residuals,
 183                        expected_gradient,
 184                        expected_jacobian,
 185                        cost,
 186                        &residuals[0],
 187                        &gradient[0],
 188                        actual_jacobian.data());
 189   }
 190
 191   // Try all combinations of parameters for the evaluator.
 192   void CheckAllEvaluationCombinations(const ExpectedEvaluation &expected) {
 193     for (int i = 0; i < 8; ++i) {
 194       EvaluateAndCompare(&problem,
 195                          expected.num_rows,
 196                          expected.num_cols,
 197                          expected.cost,
 198                          (i & 1) ? expected.residuals : NULL,
 199                          (i & 2) ? expected.gradient  : NULL,
 200                          (i & 4) ? expected.jacobian  : NULL);
 201     }
 202   }
 203
 204   // The values are ignored completely by the cost function.
 205   double x[2];
 206   double y[3];
 207   double z[4];
 208
 209   ProblemImpl problem;
 210 };
 211
 212 void SetSparseMatrixConstant(SparseMatrix* sparse_matrix, double value) {
 213   VectorRef(sparse_matrix->mutable_values(),
 214             sparse_matrix->num_nonzeros()).setConstant(value);
 215 }
 216
 217 TEST_P(EvaluatorTest, SingleResidualProblem) {
 218   problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>,
 219                            NULL,
 220                            x, y, z);
 221
 222   ExpectedEvaluation expected = {
 223     // Rows/columns
 224     3, 9,
 225     // Cost
 226     7.0,
 227     // Residuals
 228     { 1.0, 2.0, 3.0 },
 229     // Gradient
 230     { 6.0, 12.0,              // x
 231       6.0, 12.0, 18.0,        // y
 232       6.0, 12.0, 18.0, 24.0,  // z
 233     },
 234     // Jacobian
 235     //   x          y             z
 236     { 1, 2,   1, 2, 3,   1, 2, 3, 4,
 237       1, 2,   1, 2, 3,   1, 2, 3, 4,
 238       1, 2,   1, 2, 3,   1, 2, 3, 4
 239     }
 240   };
 241   CheckAllEvaluationCombinations(expected);
 242 }
 243
 244 TEST_P(EvaluatorTest, SingleResidualProblemWithPermutedParameters) {
 245   // Add the parameters in explicit order to force the ordering in the program.
 246   problem.AddParameterBlock(x,  2);
 247   problem.AddParameterBlock(y,  3);
 248   problem.AddParameterBlock(z,  4);
 249
 250   // Then use a cost function which is similar to the others, but swap around
 251   // the ordering of the parameters to the cost function. This shouldn't affect
 252   // the jacobian evaluation, but requires explicit handling in the evaluators.
 253   // At one point the compressed row evaluator had a bug that went undetected
 254   // for a long time, since by chance most users added parameters to the problem
 255   // in the same order that they occurred as parameters to a cost function.
 256   problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 4, 3, 2>,
 257                            NULL,
 258                            z, y, x);
 259
 260   ExpectedEvaluation expected = {
 261     // Rows/columns
 262     3, 9,
 263     // Cost
 264     7.0,
 265     // Residuals
 266     { 1.0, 2.0, 3.0 },
 267     // Gradient
 268     { 6.0, 12.0,              // x
 269       6.0, 12.0, 18.0,        // y
 270       6.0, 12.0, 18.0, 24.0,  // z
 271     },
 272     // Jacobian
 273     //   x          y             z
 274     { 1, 2,   1, 2, 3,   1, 2, 3, 4,
 275       1, 2,   1, 2, 3,   1, 2, 3, 4,
 276       1, 2,   1, 2, 3,   1, 2, 3, 4
 277     }
 278   };
 279   CheckAllEvaluationCombinations(expected);
 280 }
 281
 282 TEST_P(EvaluatorTest, SingleResidualProblemWithNuisanceParameters) {
 283   // These parameters are not used.
 284   double a[2];
 285   double b[1];
 286   double c[1];
 287   double d[3];
 288
 289   // Add the parameters in a mixed order so the Jacobian is "checkered" with the
 290   // values from the other parameters.
 291   problem.AddParameterBlock(a, 2);
 292   problem.AddParameterBlock(x, 2);
 293   problem.AddParameterBlock(b, 1);
 294   problem.AddParameterBlock(y, 3);
 295   problem.AddParameterBlock(c, 1);
 296   problem.AddParameterBlock(z, 4);
 297   problem.AddParameterBlock(d, 3);
 298
 299   problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>,
 300                            NULL,
 301                            x, y, z);
 302
 303   ExpectedEvaluation expected = {
 304     // Rows/columns
 305     3, 16,
 306     // Cost
 307     7.0,
 308     // Residuals
 309     { 1.0, 2.0, 3.0 },
 310     // Gradient
 311     { 0.0, 0.0,               // a
 312       6.0, 12.0,              // x
 313       0.0,                    // b
 314       6.0, 12.0, 18.0,        // y
 315       0.0,                    // c
 316       6.0, 12.0, 18.0, 24.0,  // z
 317       0.0, 0.0, 0.0,          // d
 318     },
 319     // Jacobian
 320     //   a        x     b           y     c              z           d
 321     { 0, 0,    1, 2,    0,    1, 2, 3,    0,    1, 2, 3, 4,    0, 0, 0,
 322       0, 0,    1, 2,    0,    1, 2, 3,    0,    1, 2, 3, 4,    0, 0, 0,
 323       0, 0,    1, 2,    0,    1, 2, 3,    0,    1, 2, 3, 4,    0, 0, 0
 324     }
 325   };
 326   CheckAllEvaluationCombinations(expected);
 327 }
 328
 329 TEST_P(EvaluatorTest, MultipleResidualProblem) {
 330   // Add the parameters in explicit order to force the ordering in the program.
 331   problem.AddParameterBlock(x,  2);
 332   problem.AddParameterBlock(y,  3);
 333   problem.AddParameterBlock(z,  4);
 334
 335   // f(x, y) in R^2
 336   problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
 337                            NULL,
 338                            x, y);
 339
 340   // g(x, z) in R^3
 341   problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
 342                            NULL,
 343                            x, z);
 344
 345   // h(y, z) in R^4
 346   problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
 347                            NULL,
 348                            y, z);
 349
 350   ExpectedEvaluation expected = {
 351     // Rows/columns
 352     9, 9,
 353     // Cost
 354     // f       g           h
 355     (  1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
 356     // Residuals
 357     { 1.0, 2.0,           // f
 358       1.0, 2.0, 3.0,      // g
 359       1.0, 2.0, 3.0, 4.0  // h
 360     },
 361     // Gradient
 362     { 15.0, 30.0,               // x
 363       33.0, 66.0, 99.0,         // y
 364       42.0, 84.0, 126.0, 168.0  // z
 365     },
 366     // Jacobian
 367     //                x        y           z
 368     {   /* f(x, y) */ 1, 2,    1, 2, 3,    0, 0, 0, 0,
 369                       1, 2,    1, 2, 3,    0, 0, 0, 0,
 370
 371         /* g(x, z) */ 2, 4,    0, 0, 0,    2, 4, 6, 8,
 372                       2, 4,    0, 0, 0,    2, 4, 6, 8,
 373                       2, 4,    0, 0, 0,    2, 4, 6, 8,
 374
 375         /* h(y, z) */ 0, 0,    3, 6, 9,    3, 6, 9, 12,
 376                       0, 0,    3, 6, 9,    3, 6, 9, 12,
 377                       0, 0,    3, 6, 9,    3, 6, 9, 12,
 378                       0, 0,    3, 6, 9,    3, 6, 9, 12
 379     }
 380   };
 381   CheckAllEvaluationCombinations(expected);
 382 }
 383
 384 TEST_P(EvaluatorTest, MultipleResidualsWithLocalParameterizations) {
 385   // Add the parameters in explicit order to force the ordering in the program.
 386   problem.AddParameterBlock(x,  2);
 387
 388   // Fix y's first dimension.
 389   vector<int> y_fixed;
 390   y_fixed.push_back(0);
 391   problem.AddParameterBlock(y, 3, new SubsetParameterization(3, y_fixed));
 392
 393   // Fix z's second dimension.
 394   vector<int> z_fixed;
 395   z_fixed.push_back(1);
 396   problem.AddParameterBlock(z, 4, new SubsetParameterization(4, z_fixed));
 397
 398   // f(x, y) in R^2
 399   problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
 400                            NULL,
 401                            x, y);
 402
 403   // g(x, z) in R^3
 404   problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
 405                            NULL,
 406                            x, z);
 407
 408   // h(y, z) in R^4
 409   problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
 410                            NULL,
 411                            y, z);
 412
 413   ExpectedEvaluation expected = {
 414     // Rows/columns
 415     9, 7,
 416     // Cost
 417     // f       g           h
 418     (  1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
 419     // Residuals
 420     { 1.0, 2.0,           // f
 421       1.0, 2.0, 3.0,      // g
 422       1.0, 2.0, 3.0, 4.0  // h
 423     },
 424     // Gradient
 425     { 15.0, 30.0,         // x
 426       66.0, 99.0,         // y
 427       42.0, 126.0, 168.0  // z
 428     },
 429     // Jacobian
 430     //                x        y           z
 431     {   /* f(x, y) */ 1, 2,    2, 3,    0, 0, 0,
 432                       1, 2,    2, 3,    0, 0, 0,
 433
 434         /* g(x, z) */ 2, 4,    0, 0,    2, 6, 8,
 435                       2, 4,    0, 0,    2, 6, 8,
 436                       2, 4,    0, 0,    2, 6, 8,
 437
 438         /* h(y, z) */ 0, 0,    6, 9,    3, 9, 12,
 439                       0, 0,    6, 9,    3, 9, 12,
 440                       0, 0,    6, 9,    3, 9, 12,
 441                       0, 0,    6, 9,    3, 9, 12
 442     }
 443   };
 444   CheckAllEvaluationCombinations(expected);
 445 }
 446
 447 TEST_P(EvaluatorTest, MultipleResidualProblemWithSomeConstantParameters) {
 448   // The values are ignored completely by the cost function.
 449   double x[2];
 450   double y[3];
 451   double z[4];
 452
 453   // Add the parameters in explicit order to force the ordering in the program.
 454   problem.AddParameterBlock(x,  2);
 455   problem.AddParameterBlock(y,  3);
 456   problem.AddParameterBlock(z,  4);
 457
 458   // f(x, y) in R^2
 459   problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
 460                            NULL,
 461                            x, y);
 462
 463   // g(x, z) in R^3
 464   problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
 465                            NULL,
 466                            x, z);
 467
 468   // h(y, z) in R^4
 469   problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
 470                            NULL,
 471                            y, z);
 472
 473   // For this test, "z" is constant.
 474   problem.SetParameterBlockConstant(z);
 475
 476   // Create the reduced program which is missing the fixed "z" variable.
 477   // Normally, the preprocessing of the program that happens in solver_impl
 478   // takes care of this, but we don't want to invoke the solver here.
 479   Program reduced_program;
 480   vector<ParameterBlock*>* parameter_blocks =
 481       problem.mutable_program()->mutable_parameter_blocks();
 482
 483   // "z" is the last parameter; save it for later and pop it off temporarily.
 484   // Note that "z" will still get read during evaluation, so it cannot be
 485   // deleted at this point.
 486   ParameterBlock* parameter_block_z = parameter_blocks->back();
 487   parameter_blocks->pop_back();
 488
 489   ExpectedEvaluation expected = {
 490     // Rows/columns
 491     9, 5,
 492     // Cost
 493     // f       g           h
 494     (  1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
 495     // Residuals
 496     { 1.0, 2.0,           // f
 497       1.0, 2.0, 3.0,      // g
 498       1.0, 2.0, 3.0, 4.0  // h
 499     },
 500     // Gradient
 501     { 15.0, 30.0,        // x
 502       33.0, 66.0, 99.0,  // y
 503     },
 504     // Jacobian
 505     //                x        y
 506     {   /* f(x, y) */ 1, 2,    1, 2, 3,
 507                       1, 2,    1, 2, 3,
 508
 509         /* g(x, z) */ 2, 4,    0, 0, 0,
 510                       2, 4,    0, 0, 0,
 511                       2, 4,    0, 0, 0,
 512
 513         /* h(y, z) */ 0, 0,    3, 6, 9,
 514                       0, 0,    3, 6, 9,
 515                       0, 0,    3, 6, 9,
 516                       0, 0,    3, 6, 9
 517     }
 518   };
 519   CheckAllEvaluationCombinations(expected);
 520
 521   // Restore parameter block z, so it will get freed in a consistent way.
 522   parameter_blocks->push_back(parameter_block_z);
 523 }
 524
 525 TEST_P(EvaluatorTest, EvaluatorAbortsForResidualsThatFailToEvaluate) {
 526   // Switch the return value to failure.
 527   problem.AddResidualBlock(
 528       new ParameterIgnoringCostFunction<20, 3, 2, 3, 4, false>, NULL, x, y, z);
 529
 530   // The values are ignored.
 531   double state[9];
 532
 533   scoped_ptr<Evaluator> evaluator(CreateEvaluator(problem.mutable_program()));
 534   scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
 535   double cost;
 536   EXPECT_FALSE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL));
 537 }
 538
 539 // In the pairs, the first argument is the linear solver type, and the second
 540 // argument is num_eliminate_blocks. Changing the num_eliminate_blocks only
 541 // makes sense for the schur-based solvers.
 542 //
 543 // Try all values of num_eliminate_blocks that make sense given that in the
 544 // tests a maximum of 4 parameter blocks are present.
 545 INSTANTIATE_TEST_CASE_P(
 546     LinearSolvers,
 547     EvaluatorTest,
 548     ::testing::Values(
 549       EvaluatorTestOptions(DENSE_QR, 0),
 550       EvaluatorTestOptions(DENSE_SCHUR, 0),
 551       EvaluatorTestOptions(DENSE_SCHUR, 1),
 552       EvaluatorTestOptions(DENSE_SCHUR, 2),
 553       EvaluatorTestOptions(DENSE_SCHUR, 3),
 554       EvaluatorTestOptions(DENSE_SCHUR, 4),
 555       EvaluatorTestOptions(SPARSE_SCHUR, 0),
 556       EvaluatorTestOptions(SPARSE_SCHUR, 1),
 557       EvaluatorTestOptions(SPARSE_SCHUR, 2),
 558       EvaluatorTestOptions(SPARSE_SCHUR, 3),
 559       EvaluatorTestOptions(SPARSE_SCHUR, 4),
 560       EvaluatorTestOptions(ITERATIVE_SCHUR, 0),
 561       EvaluatorTestOptions(ITERATIVE_SCHUR, 1),
 562       EvaluatorTestOptions(ITERATIVE_SCHUR, 2),
 563       EvaluatorTestOptions(ITERATIVE_SCHUR, 3),
 564       EvaluatorTestOptions(ITERATIVE_SCHUR, 4),
 565       EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, false),
 566       EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, true)));
 567
 568 // Simple cost function used to check if the evaluator is sensitive to
 569 // state changes.
 570 class ParameterSensitiveCostFunction : public SizedCostFunction<2, 2> {
 571  public:
 572   virtual bool Evaluate(double const* const* parameters,
 573                         double* residuals,
 574                         double** jacobians) const {
 575     double x1 = parameters[0][0];
 576     double x2 = parameters[0][1];
 577     residuals[0] = x1 * x1;
 578     residuals[1] = x2 * x2;
 579
 580     if (jacobians != NULL) {
 581       double* jacobian = jacobians[0];
 582       if (jacobian != NULL) {
 583         jacobian[0] = 2.0 * x1;
 584         jacobian[1] = 0.0;
 585         jacobian[2] = 0.0;
 586         jacobian[3] = 2.0 * x2;
 587       }
 588     }
 589     return true;
 590   }
 591 };
 592
 593 TEST(Evaluator, EvaluatorRespectsParameterChanges) {
 594   ProblemImpl problem;
 595
 596   double x[2];
 597   x[0] = 1.0;
 598   x[1] = 1.0;
 599
 600   problem.AddResidualBlock(new ParameterSensitiveCostFunction(), NULL, x);
 601   Program* program = problem.mutable_program();
 602   program->SetParameterOffsetsAndIndex();
 603
 604   Evaluator::Options options;
 605   options.linear_solver_type = DENSE_QR;
 606   options.num_eliminate_blocks = 0;
 607   string error;
 608   scoped_ptr<Evaluator> evaluator(Evaluator::Create(options, program, &error));
 609   scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
 610
 611   ASSERT_EQ(2, jacobian->num_rows());
 612   ASSERT_EQ(2, jacobian->num_cols());
 613
 614   double state[2];
 615   state[0] = 2.0;
 616   state[1] = 3.0;
 617
 618   // The original state of a residual block comes from the user's
 619   // state. So the original state is 1.0, 1.0, and the only way we get
 620   // the 2.0, 3.0 results in the following tests is if it respects the
 621   // values in the state vector.
 622
 623   // Cost only; no residuals and no jacobian.
 624   {
 625     double cost = -1;
 626     ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL));
 627     EXPECT_EQ(48.5, cost);
 628   }
 629
 630   // Cost and residuals, no jacobian.
 631   {
 632     double cost = -1;
 633     double residuals[2] = { -2, -2 };
 634     ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL, NULL));
 635     EXPECT_EQ(48.5, cost);
 636     EXPECT_EQ(4, residuals[0]);
 637     EXPECT_EQ(9, residuals[1]);
 638   }
 639
 640   // Cost, residuals, and jacobian.
 641   {
 642     double cost = -1;
 643     double residuals[2] = { -2, -2};
 644     SetSparseMatrixConstant(jacobian.get(), -1);
 645     ASSERT_TRUE(evaluator->Evaluate(state,
 646                                     &cost,
 647                                     residuals,
 648                                     NULL,
 649                                     jacobian.get()));
 650     EXPECT_EQ(48.5, cost);
 651     EXPECT_EQ(4, residuals[0]);
 652     EXPECT_EQ(9, residuals[1]);
 653     Matrix actual_jacobian;
 654     jacobian->ToDenseMatrix(&actual_jacobian);
 655
 656     Matrix expected_jacobian(2, 2);
 657     expected_jacobian
 658         << 2 * state[0], 0,
 659            0, 2 * state[1];
 660
 661     EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all())
 662         << "Actual:\n" << actual_jacobian
 663         << "\nExpected:\n" << expected_jacobian;
 664   }
 665 }
 666
 667 }  // namespace internal
 668 }  // namespace ceres