1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3// http://code.google.com/p/ceres-solver/
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//         keir@google.com (Keir Mierle)
31
32#include "ceres/problem.h"
33#include "ceres/problem_impl.h"
34
35#include "ceres/casts.h"
36#include "ceres/cost_function.h"
37#include "ceres/crs_matrix.h"
38#include "ceres/evaluator_test_utils.cc"
39#include "ceres/internal/eigen.h"
40#include "ceres/internal/scoped_ptr.h"
41#include "ceres/local_parameterization.h"
42#include "ceres/map_util.h"
43#include "ceres/parameter_block.h"
44#include "ceres/program.h"
45#include "ceres/sized_cost_function.h"
46#include "ceres/sparse_matrix.h"
47#include "ceres/types.h"
48#include "gtest/gtest.h"
49
50namespace ceres {
51namespace internal {
52
53// The following three classes are for the purposes of defining
54// function signatures. They have dummy Evaluate functions.
55
56// Trivial cost function that accepts a single argument.
57class UnaryCostFunction : public CostFunction {
58 public:
59  UnaryCostFunction(int num_residuals, int16 parameter_block_size) {
60    set_num_residuals(num_residuals);
61    mutable_parameter_block_sizes()->push_back(parameter_block_size);
62  }
63  virtual ~UnaryCostFunction() {}
64
65  virtual bool Evaluate(double const* const* parameters,
66                        double* residuals,
67                        double** jacobians) const {
68    for (int i = 0; i < num_residuals(); ++i) {
69      residuals[i] = 1;
70    }
71    return true;
72  }
73};
74
75// Trivial cost function that accepts two arguments.
76class BinaryCostFunction: public CostFunction {
77 public:
78  BinaryCostFunction(int num_residuals,
79                     int16 parameter_block1_size,
80                     int16 parameter_block2_size) {
81    set_num_residuals(num_residuals);
82    mutable_parameter_block_sizes()->push_back(parameter_block1_size);
83    mutable_parameter_block_sizes()->push_back(parameter_block2_size);
84  }
85
86  virtual bool Evaluate(double const* const* parameters,
87                        double* residuals,
88                        double** jacobians) const {
89    for (int i = 0; i < num_residuals(); ++i) {
90      residuals[i] = 2;
91    }
92    return true;
93  }
94};
95
96// Trivial cost function that accepts three arguments.
97class TernaryCostFunction: public CostFunction {
98 public:
99  TernaryCostFunction(int num_residuals,
100                      int16 parameter_block1_size,
101                      int16 parameter_block2_size,
102                      int16 parameter_block3_size) {
103    set_num_residuals(num_residuals);
104    mutable_parameter_block_sizes()->push_back(parameter_block1_size);
105    mutable_parameter_block_sizes()->push_back(parameter_block2_size);
106    mutable_parameter_block_sizes()->push_back(parameter_block3_size);
107  }
108
109  virtual bool Evaluate(double const* const* parameters,
110                        double* residuals,
111                        double** jacobians) const {
112    for (int i = 0; i < num_residuals(); ++i) {
113      residuals[i] = 3;
114    }
115    return true;
116  }
117};
118
119TEST(Problem, AddResidualWithNullCostFunctionDies) {
120  double x[3], y[4], z[5];
121
122  Problem problem;
123  problem.AddParameterBlock(x, 3);
124  problem.AddParameterBlock(y, 4);
125  problem.AddParameterBlock(z, 5);
126
127  EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(NULL, NULL, x),
128                            "'cost_function' Must be non NULL");
129}
130
131TEST(Problem, AddResidualWithIncorrectNumberOfParameterBlocksDies) {
132  double x[3], y[4], z[5];
133
134  Problem problem;
135  problem.AddParameterBlock(x, 3);
136  problem.AddParameterBlock(y, 4);
137  problem.AddParameterBlock(z, 5);
138
139  // UnaryCostFunction takes only one parameter, but two are passed.
140  EXPECT_DEATH_IF_SUPPORTED(
141      problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x, y),
142      "parameter_blocks.size()");
143}
144
145TEST(Problem, AddResidualWithDifferentSizesOnTheSameVariableDies) {
146  double x[3];
147
148  Problem problem;
149  problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
150  EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(
151                                new UnaryCostFunction(
152                                    2, 4 /* 4 != 3 */), NULL, x),
153                            "different block sizes");
154}
155
156TEST(Problem, AddResidualWithDuplicateParametersDies) {
157  double x[3], z[5];
158
159  Problem problem;
160  EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(
161                                new BinaryCostFunction(2, 3, 3), NULL, x, x),
162                            "Duplicate parameter blocks");
163  EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(
164                                new TernaryCostFunction(1, 5, 3, 5),
165                                NULL, z, x, z),
166                            "Duplicate parameter blocks");
167}
168
169TEST(Problem, AddResidualWithIncorrectSizesOfParameterBlockDies) {
170  double x[3], y[4], z[5];
171
172  Problem problem;
173  problem.AddParameterBlock(x, 3);
174  problem.AddParameterBlock(y, 4);
175  problem.AddParameterBlock(z, 5);
176
177  // The cost function expects the size of the second parameter, z, to be 4
178  // instead of 5 as declared above. This is fatal.
179  EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(
180      new BinaryCostFunction(2, 3, 4), NULL, x, z),
181               "different block sizes");
182}
183
184TEST(Problem, AddResidualAddsDuplicatedParametersOnlyOnce) {
185  double x[3], y[4], z[5];
186
187  Problem problem;
188  problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
189  problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
190  problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y);
191  problem.AddResidualBlock(new UnaryCostFunction(2, 5), NULL, z);
192
193  EXPECT_EQ(3, problem.NumParameterBlocks());
194  EXPECT_EQ(12, problem.NumParameters());
195}
196
197TEST(Problem, AddParameterWithDifferentSizesOnTheSameVariableDies) {
198  double x[3], y[4];
199
200  Problem problem;
201  problem.AddParameterBlock(x, 3);
202  problem.AddParameterBlock(y, 4);
203
204  EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(x, 4),
205                            "different block sizes");
206}
207
208static double *IntToPtr(int i) {
209  return reinterpret_cast<double*>(sizeof(double) * i);  // NOLINT
210}
211
212TEST(Problem, AddParameterWithAliasedParametersDies) {
213  // Layout is
214  //
215  //   0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 16 17
216  //                 [x] x  x  x  x          [y] y  y
217  //         o==o==o                 o==o==o           o==o
218  //               o--o--o     o--o--o     o--o  o--o--o
219  //
220  // Parameter block additions are tested as listed above; expected successful
221  // ones marked with o==o and aliasing ones marked with o--o.
222
223  Problem problem;
224  problem.AddParameterBlock(IntToPtr(5),  5);  // x
225  problem.AddParameterBlock(IntToPtr(13), 3);  // y
226
227  EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 2),
228                            "Aliasing detected");
229  EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 3),
230                            "Aliasing detected");
231  EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 9),
232                            "Aliasing detected");
233  EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 8), 3),
234                            "Aliasing detected");
235  EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(12), 2),
236                            "Aliasing detected");
237  EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(14), 3),
238                            "Aliasing detected");
239
240  // These ones should work.
241  problem.AddParameterBlock(IntToPtr( 2), 3);
242  problem.AddParameterBlock(IntToPtr(10), 3);
243  problem.AddParameterBlock(IntToPtr(16), 2);
244
245  ASSERT_EQ(5, problem.NumParameterBlocks());
246}
247
248TEST(Problem, AddParameterIgnoresDuplicateCalls) {
249  double x[3], y[4];
250
251  Problem problem;
252  problem.AddParameterBlock(x, 3);
253  problem.AddParameterBlock(y, 4);
254
255  // Creating parameter blocks multiple times is ignored.
256  problem.AddParameterBlock(x, 3);
257  problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
258
259  // ... even repeatedly.
260  problem.AddParameterBlock(x, 3);
261  problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
262
263  // More parameters are fine.
264  problem.AddParameterBlock(y, 4);
265  problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y);
266
267  EXPECT_EQ(2, problem.NumParameterBlocks());
268  EXPECT_EQ(7, problem.NumParameters());
269}
270
271TEST(Problem, AddingParametersAndResidualsResultsInExpectedProblem) {
272  double x[3], y[4], z[5], w[4];
273
274  Problem problem;
275  problem.AddParameterBlock(x, 3);
276  EXPECT_EQ(1, problem.NumParameterBlocks());
277  EXPECT_EQ(3, problem.NumParameters());
278
279  problem.AddParameterBlock(y, 4);
280  EXPECT_EQ(2, problem.NumParameterBlocks());
281  EXPECT_EQ(7, problem.NumParameters());
282
283  problem.AddParameterBlock(z, 5);
284  EXPECT_EQ(3,  problem.NumParameterBlocks());
285  EXPECT_EQ(12, problem.NumParameters());
286
287  // Add a parameter that has a local parameterization.
288  w[0] = 1.0; w[1] = 0.0; w[2] = 0.0; w[3] = 0.0;
289  problem.AddParameterBlock(w, 4, new QuaternionParameterization);
290  EXPECT_EQ(4,  problem.NumParameterBlocks());
291  EXPECT_EQ(16, problem.NumParameters());
292
293  problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
294  problem.AddResidualBlock(new BinaryCostFunction(6, 5, 4) , NULL, z, y);
295  problem.AddResidualBlock(new BinaryCostFunction(3, 3, 5), NULL, x, z);
296  problem.AddResidualBlock(new BinaryCostFunction(7, 5, 3), NULL, z, x);
297  problem.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), NULL, z, x, y);
298
299  const int total_residuals = 2 + 6 + 3 + 7 + 1;
300  EXPECT_EQ(problem.NumResidualBlocks(), 5);
301  EXPECT_EQ(problem.NumResiduals(), total_residuals);
302}
303
304class DestructorCountingCostFunction : public SizedCostFunction<3, 4, 5> {
305 public:
306  explicit DestructorCountingCostFunction(int *num_destructions)
307      : num_destructions_(num_destructions) {}
308
309  virtual ~DestructorCountingCostFunction() {
310    *num_destructions_ += 1;
311  }
312
313  virtual bool Evaluate(double const* const* parameters,
314                        double* residuals,
315                        double** jacobians) const {
316    return true;
317  }
318
319 private:
320  int* num_destructions_;
321};
322
323TEST(Problem, ReusedCostFunctionsAreOnlyDeletedOnce) {
324  double y[4], z[5];
325  int num_destructions = 0;
326
327  // Add a cost function multiple times and check to make sure that
328  // the destructor on the cost function is only called once.
329  {
330    Problem problem;
331    problem.AddParameterBlock(y, 4);
332    problem.AddParameterBlock(z, 5);
333
334    CostFunction* cost = new DestructorCountingCostFunction(&num_destructions);
335    problem.AddResidualBlock(cost, NULL, y, z);
336    problem.AddResidualBlock(cost, NULL, y, z);
337    problem.AddResidualBlock(cost, NULL, y, z);
338    EXPECT_EQ(3, problem.NumResidualBlocks());
339  }
340
341  // Check that the destructor was called only once.
342  CHECK_EQ(num_destructions, 1);
343}
344
345TEST(Problem, CostFunctionsAreDeletedEvenWithRemovals) {
346  double y[4], z[5], w[4];
347  int num_destructions = 0;
348  {
349    Problem problem;
350    problem.AddParameterBlock(y, 4);
351    problem.AddParameterBlock(z, 5);
352
353    CostFunction* cost_yz =
354        new DestructorCountingCostFunction(&num_destructions);
355    CostFunction* cost_wz =
356        new DestructorCountingCostFunction(&num_destructions);
357    ResidualBlock* r_yz = problem.AddResidualBlock(cost_yz, NULL, y, z);
358    ResidualBlock* r_wz = problem.AddResidualBlock(cost_wz, NULL, w, z);
359    EXPECT_EQ(2, problem.NumResidualBlocks());
360
361    // In the current implementation, the destructor shouldn't get run yet.
362    problem.RemoveResidualBlock(r_yz);
363    CHECK_EQ(num_destructions, 0);
364    problem.RemoveResidualBlock(r_wz);
365    CHECK_EQ(num_destructions, 0);
366
367    EXPECT_EQ(0, problem.NumResidualBlocks());
368  }
369  CHECK_EQ(num_destructions, 2);
370}
371
372// Make the dynamic problem tests (e.g. for removing residual blocks)
373// parameterized on whether the low-latency mode is enabled or not.
374//
375// This tests against ProblemImpl instead of Problem in order to inspect the
376// state of the resulting Program; this is difficult with only the thin Problem
377// interface.
378struct DynamicProblem : public ::testing::TestWithParam<bool> {
379  DynamicProblem() {
380    Problem::Options options;
381    options.enable_fast_parameter_block_removal = GetParam();
382    problem.reset(new ProblemImpl(options));
383  }
384
385  ParameterBlock* GetParameterBlock(int block) {
386    return problem->program().parameter_blocks()[block];
387  }
388  ResidualBlock* GetResidualBlock(int block) {
389    return problem->program().residual_blocks()[block];
390  }
391
392  bool HasResidualBlock(ResidualBlock* residual_block) {
393    return find(problem->program().residual_blocks().begin(),
394                problem->program().residual_blocks().end(),
395                residual_block) != problem->program().residual_blocks().end();
396  }
397
398  // The next block of functions until the end are only for testing the
399  // residual block removals.
400  void ExpectParameterBlockContainsResidualBlock(
401      double* values,
402      ResidualBlock* residual_block) {
403    ParameterBlock* parameter_block =
404        FindOrDie(problem->parameter_map(), values);
405    EXPECT_TRUE(ContainsKey(*(parameter_block->mutable_residual_blocks()),
406                            residual_block));
407  }
408
409  void ExpectSize(double* values, int size) {
410    ParameterBlock* parameter_block =
411        FindOrDie(problem->parameter_map(), values);
412    EXPECT_EQ(size, parameter_block->mutable_residual_blocks()->size());
413  }
414
415  // Degenerate case.
416  void ExpectParameterBlockContains(double* values) {
417    ExpectSize(values, 0);
418  }
419
420  void ExpectParameterBlockContains(double* values,
421                                    ResidualBlock* r1) {
422    ExpectSize(values, 1);
423    ExpectParameterBlockContainsResidualBlock(values, r1);
424  }
425
426  void ExpectParameterBlockContains(double* values,
427                                    ResidualBlock* r1,
428                                    ResidualBlock* r2) {
429    ExpectSize(values, 2);
430    ExpectParameterBlockContainsResidualBlock(values, r1);
431    ExpectParameterBlockContainsResidualBlock(values, r2);
432  }
433
434  void ExpectParameterBlockContains(double* values,
435                                    ResidualBlock* r1,
436                                    ResidualBlock* r2,
437                                    ResidualBlock* r3) {
438    ExpectSize(values, 3);
439    ExpectParameterBlockContainsResidualBlock(values, r1);
440    ExpectParameterBlockContainsResidualBlock(values, r2);
441    ExpectParameterBlockContainsResidualBlock(values, r3);
442  }
443
444  void ExpectParameterBlockContains(double* values,
445                                    ResidualBlock* r1,
446                                    ResidualBlock* r2,
447                                    ResidualBlock* r3,
448                                    ResidualBlock* r4) {
449    ExpectSize(values, 4);
450    ExpectParameterBlockContainsResidualBlock(values, r1);
451    ExpectParameterBlockContainsResidualBlock(values, r2);
452    ExpectParameterBlockContainsResidualBlock(values, r3);
453    ExpectParameterBlockContainsResidualBlock(values, r4);
454  }
455
456  scoped_ptr<ProblemImpl> problem;
457  double y[4], z[5], w[3];
458};
459
460TEST(Problem, SetParameterBlockConstantWithUnknownPtrDies) {
461  double x[3];
462  double y[2];
463
464  Problem problem;
465  problem.AddParameterBlock(x, 3);
466
467  EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockConstant(y),
468                            "Parameter block not found:");
469}
470
471TEST(Problem, SetParameterBlockVariableWithUnknownPtrDies) {
472  double x[3];
473  double y[2];
474
475  Problem problem;
476  problem.AddParameterBlock(x, 3);
477
478  EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockVariable(y),
479                            "Parameter block not found:");
480}
481
482TEST(Problem, SetLocalParameterizationWithUnknownPtrDies) {
483  double x[3];
484  double y[2];
485
486  Problem problem;
487  problem.AddParameterBlock(x, 3);
488
489  EXPECT_DEATH_IF_SUPPORTED(
490      problem.SetParameterization(y, new IdentityParameterization(3)),
491      "Parameter block not found:");
492}
493
494TEST(Problem, RemoveParameterBlockWithUnknownPtrDies) {
495  double x[3];
496  double y[2];
497
498  Problem problem;
499  problem.AddParameterBlock(x, 3);
500
501  EXPECT_DEATH_IF_SUPPORTED(
502      problem.RemoveParameterBlock(y), "Parameter block not found:");
503}
504
505TEST(Problem, ParameterBlockQueryTest) {
506  double x[3];
507  double y[4];
508  Problem problem;
509  problem.AddParameterBlock(x, 3);
510  problem.AddParameterBlock(y, 4);
511
512  vector<int> constant_parameters;
513  constant_parameters.push_back(0);
514  problem.SetParameterization(
515      x,
516      new SubsetParameterization(3, constant_parameters));
517  EXPECT_EQ(problem.ParameterBlockSize(x), 3);
518  EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2);
519  EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4);
520
521  vector<double*> parameter_blocks;
522  problem.GetParameterBlocks(&parameter_blocks);
523  EXPECT_EQ(parameter_blocks.size(), 2);
524  EXPECT_NE(parameter_blocks[0], parameter_blocks[1]);
525  EXPECT_TRUE(parameter_blocks[0] == x || parameter_blocks[0] == y);
526  EXPECT_TRUE(parameter_blocks[1] == x || parameter_blocks[1] == y);
527
528  problem.RemoveParameterBlock(x);
529  problem.GetParameterBlocks(&parameter_blocks);
530  EXPECT_EQ(parameter_blocks.size(), 1);
531  EXPECT_TRUE(parameter_blocks[0] == y);
532}
533
534TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) {
535  problem->AddParameterBlock(y, 4);
536  problem->AddParameterBlock(z, 5);
537  problem->AddParameterBlock(w, 3);
538  ASSERT_EQ(3, problem->NumParameterBlocks());
539  ASSERT_EQ(0, problem->NumResidualBlocks());
540  EXPECT_EQ(y, GetParameterBlock(0)->user_state());
541  EXPECT_EQ(z, GetParameterBlock(1)->user_state());
542  EXPECT_EQ(w, GetParameterBlock(2)->user_state());
543
544  // w is at the end, which might break the swapping logic so try adding and
545  // removing it.
546  problem->RemoveParameterBlock(w);
547  ASSERT_EQ(2, problem->NumParameterBlocks());
548  ASSERT_EQ(0, problem->NumResidualBlocks());
549  EXPECT_EQ(y, GetParameterBlock(0)->user_state());
550  EXPECT_EQ(z, GetParameterBlock(1)->user_state());
551  problem->AddParameterBlock(w, 3);
552  ASSERT_EQ(3, problem->NumParameterBlocks());
553  ASSERT_EQ(0, problem->NumResidualBlocks());
554  EXPECT_EQ(y, GetParameterBlock(0)->user_state());
555  EXPECT_EQ(z, GetParameterBlock(1)->user_state());
556  EXPECT_EQ(w, GetParameterBlock(2)->user_state());
557
558  // Now remove z, which is in the middle, and add it back.
559  problem->RemoveParameterBlock(z);
560  ASSERT_EQ(2, problem->NumParameterBlocks());
561  ASSERT_EQ(0, problem->NumResidualBlocks());
562  EXPECT_EQ(y, GetParameterBlock(0)->user_state());
563  EXPECT_EQ(w, GetParameterBlock(1)->user_state());
564  problem->AddParameterBlock(z, 5);
565  ASSERT_EQ(3, problem->NumParameterBlocks());
566  ASSERT_EQ(0, problem->NumResidualBlocks());
567  EXPECT_EQ(y, GetParameterBlock(0)->user_state());
568  EXPECT_EQ(w, GetParameterBlock(1)->user_state());
569  EXPECT_EQ(z, GetParameterBlock(2)->user_state());
570
571  // Now remove everything.
572  // y
573  problem->RemoveParameterBlock(y);
574  ASSERT_EQ(2, problem->NumParameterBlocks());
575  ASSERT_EQ(0, problem->NumResidualBlocks());
576  EXPECT_EQ(z, GetParameterBlock(0)->user_state());
577  EXPECT_EQ(w, GetParameterBlock(1)->user_state());
578
579  // z
580  problem->RemoveParameterBlock(z);
581  ASSERT_EQ(1, problem->NumParameterBlocks());
582  ASSERT_EQ(0, problem->NumResidualBlocks());
583  EXPECT_EQ(w, GetParameterBlock(0)->user_state());
584
585  // w
586  problem->RemoveParameterBlock(w);
587  EXPECT_EQ(0, problem->NumParameterBlocks());
588  EXPECT_EQ(0, problem->NumResidualBlocks());
589}
590
591TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) {
592  problem->AddParameterBlock(y, 4);
593  problem->AddParameterBlock(z, 5);
594  problem->AddParameterBlock(w, 3);
595  ASSERT_EQ(3, problem->NumParameterBlocks());
596  ASSERT_EQ(0, problem->NumResidualBlocks());
597  EXPECT_EQ(y, GetParameterBlock(0)->user_state());
598  EXPECT_EQ(z, GetParameterBlock(1)->user_state());
599  EXPECT_EQ(w, GetParameterBlock(2)->user_state());
600
601  // Add all combinations of cost functions.
602  CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3);
603  CostFunction* cost_yz  = new BinaryCostFunction (1, 4, 5);
604  CostFunction* cost_yw  = new BinaryCostFunction (1, 4, 3);
605  CostFunction* cost_zw  = new BinaryCostFunction (1, 5, 3);
606  CostFunction* cost_y   = new UnaryCostFunction  (1, 4);
607  CostFunction* cost_z   = new UnaryCostFunction  (1, 5);
608  CostFunction* cost_w   = new UnaryCostFunction  (1, 3);
609
610  ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w);
611  ResidualBlock* r_yz  = problem->AddResidualBlock(cost_yz,  NULL, y, z);
612  ResidualBlock* r_yw  = problem->AddResidualBlock(cost_yw,  NULL, y, w);
613  ResidualBlock* r_zw  = problem->AddResidualBlock(cost_zw,  NULL, z, w);
614  ResidualBlock* r_y   = problem->AddResidualBlock(cost_y,   NULL, y);
615  ResidualBlock* r_z   = problem->AddResidualBlock(cost_z,   NULL, z);
616  ResidualBlock* r_w   = problem->AddResidualBlock(cost_w,   NULL, w);
617
618  EXPECT_EQ(3, problem->NumParameterBlocks());
619  EXPECT_EQ(7, problem->NumResidualBlocks());
620
621  // Remove w, which should remove r_yzw, r_yw, r_zw, r_w.
622  problem->RemoveParameterBlock(w);
623  ASSERT_EQ(2, problem->NumParameterBlocks());
624  ASSERT_EQ(3, problem->NumResidualBlocks());
625
626  ASSERT_FALSE(HasResidualBlock(r_yzw));
627  ASSERT_TRUE (HasResidualBlock(r_yz ));
628  ASSERT_FALSE(HasResidualBlock(r_yw ));
629  ASSERT_FALSE(HasResidualBlock(r_zw ));
630  ASSERT_TRUE (HasResidualBlock(r_y  ));
631  ASSERT_TRUE (HasResidualBlock(r_z  ));
632  ASSERT_FALSE(HasResidualBlock(r_w  ));
633
634  // Remove z, which will remove almost everything else.
635  problem->RemoveParameterBlock(z);
636  ASSERT_EQ(1, problem->NumParameterBlocks());
637  ASSERT_EQ(1, problem->NumResidualBlocks());
638
639  ASSERT_FALSE(HasResidualBlock(r_yzw));
640  ASSERT_FALSE(HasResidualBlock(r_yz ));
641  ASSERT_FALSE(HasResidualBlock(r_yw ));
642  ASSERT_FALSE(HasResidualBlock(r_zw ));
643  ASSERT_TRUE (HasResidualBlock(r_y  ));
644  ASSERT_FALSE(HasResidualBlock(r_z  ));
645  ASSERT_FALSE(HasResidualBlock(r_w  ));
646
647  // Remove y; all gone.
648  problem->RemoveParameterBlock(y);
649  EXPECT_EQ(0, problem->NumParameterBlocks());
650  EXPECT_EQ(0, problem->NumResidualBlocks());
651}
652
653TEST_P(DynamicProblem, RemoveResidualBlock) {
654  problem->AddParameterBlock(y, 4);
655  problem->AddParameterBlock(z, 5);
656  problem->AddParameterBlock(w, 3);
657
658  // Add all combinations of cost functions.
659  CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3);
660  CostFunction* cost_yz  = new BinaryCostFunction (1, 4, 5);
661  CostFunction* cost_yw  = new BinaryCostFunction (1, 4, 3);
662  CostFunction* cost_zw  = new BinaryCostFunction (1, 5, 3);
663  CostFunction* cost_y   = new UnaryCostFunction  (1, 4);
664  CostFunction* cost_z   = new UnaryCostFunction  (1, 5);
665  CostFunction* cost_w   = new UnaryCostFunction  (1, 3);
666
667  ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w);
668  ResidualBlock* r_yz  = problem->AddResidualBlock(cost_yz,  NULL, y, z);
669  ResidualBlock* r_yw  = problem->AddResidualBlock(cost_yw,  NULL, y, w);
670  ResidualBlock* r_zw  = problem->AddResidualBlock(cost_zw,  NULL, z, w);
671  ResidualBlock* r_y   = problem->AddResidualBlock(cost_y,   NULL, y);
672  ResidualBlock* r_z   = problem->AddResidualBlock(cost_z,   NULL, z);
673  ResidualBlock* r_w   = problem->AddResidualBlock(cost_w,   NULL, w);
674
675  if (GetParam()) {
676    // In this test parameterization, there should be back-pointers from the
677    // parameter blocks to the residual blocks.
678    ExpectParameterBlockContains(y, r_yzw, r_yz, r_yw, r_y);
679    ExpectParameterBlockContains(z, r_yzw, r_yz, r_zw, r_z);
680    ExpectParameterBlockContains(w, r_yzw, r_yw, r_zw, r_w);
681  } else {
682    // Otherwise, nothing.
683    EXPECT_TRUE(GetParameterBlock(0)->mutable_residual_blocks() == NULL);
684    EXPECT_TRUE(GetParameterBlock(1)->mutable_residual_blocks() == NULL);
685    EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL);
686  }
687  EXPECT_EQ(3, problem->NumParameterBlocks());
688  EXPECT_EQ(7, problem->NumResidualBlocks());
689
690  // Remove each residual and check the state after each removal.
691
692  // Remove r_yzw.
693  problem->RemoveResidualBlock(r_yzw);
694  ASSERT_EQ(3, problem->NumParameterBlocks());
695  ASSERT_EQ(6, problem->NumResidualBlocks());
696  if (GetParam()) {
697    ExpectParameterBlockContains(y, r_yz, r_yw, r_y);
698    ExpectParameterBlockContains(z, r_yz, r_zw, r_z);
699    ExpectParameterBlockContains(w, r_yw, r_zw, r_w);
700  }
701  ASSERT_TRUE (HasResidualBlock(r_yz ));
702  ASSERT_TRUE (HasResidualBlock(r_yw ));
703  ASSERT_TRUE (HasResidualBlock(r_zw ));
704  ASSERT_TRUE (HasResidualBlock(r_y  ));
705  ASSERT_TRUE (HasResidualBlock(r_z  ));
706  ASSERT_TRUE (HasResidualBlock(r_w  ));
707
708  // Remove r_yw.
709  problem->RemoveResidualBlock(r_yw);
710  ASSERT_EQ(3, problem->NumParameterBlocks());
711  ASSERT_EQ(5, problem->NumResidualBlocks());
712  if (GetParam()) {
713    ExpectParameterBlockContains(y, r_yz, r_y);
714    ExpectParameterBlockContains(z, r_yz, r_zw, r_z);
715    ExpectParameterBlockContains(w, r_zw, r_w);
716  }
717  ASSERT_TRUE (HasResidualBlock(r_yz ));
718  ASSERT_TRUE (HasResidualBlock(r_zw ));
719  ASSERT_TRUE (HasResidualBlock(r_y  ));
720  ASSERT_TRUE (HasResidualBlock(r_z  ));
721  ASSERT_TRUE (HasResidualBlock(r_w  ));
722
723  // Remove r_zw.
724  problem->RemoveResidualBlock(r_zw);
725  ASSERT_EQ(3, problem->NumParameterBlocks());
726  ASSERT_EQ(4, problem->NumResidualBlocks());
727  if (GetParam()) {
728    ExpectParameterBlockContains(y, r_yz, r_y);
729    ExpectParameterBlockContains(z, r_yz, r_z);
730    ExpectParameterBlockContains(w, r_w);
731  }
732  ASSERT_TRUE (HasResidualBlock(r_yz ));
733  ASSERT_TRUE (HasResidualBlock(r_y  ));
734  ASSERT_TRUE (HasResidualBlock(r_z  ));
735  ASSERT_TRUE (HasResidualBlock(r_w  ));
736
737  // Remove r_w.
738  problem->RemoveResidualBlock(r_w);
739  ASSERT_EQ(3, problem->NumParameterBlocks());
740  ASSERT_EQ(3, problem->NumResidualBlocks());
741  if (GetParam()) {
742    ExpectParameterBlockContains(y, r_yz, r_y);
743    ExpectParameterBlockContains(z, r_yz, r_z);
744    ExpectParameterBlockContains(w);
745  }
746  ASSERT_TRUE (HasResidualBlock(r_yz ));
747  ASSERT_TRUE (HasResidualBlock(r_y  ));
748  ASSERT_TRUE (HasResidualBlock(r_z  ));
749
750  // Remove r_yz.
751  problem->RemoveResidualBlock(r_yz);
752  ASSERT_EQ(3, problem->NumParameterBlocks());
753  ASSERT_EQ(2, problem->NumResidualBlocks());
754  if (GetParam()) {
755    ExpectParameterBlockContains(y, r_y);
756    ExpectParameterBlockContains(z, r_z);
757    ExpectParameterBlockContains(w);
758  }
759  ASSERT_TRUE (HasResidualBlock(r_y  ));
760  ASSERT_TRUE (HasResidualBlock(r_z  ));
761
762  // Remove the last two.
763  problem->RemoveResidualBlock(r_z);
764  problem->RemoveResidualBlock(r_y);
765  ASSERT_EQ(3, problem->NumParameterBlocks());
766  ASSERT_EQ(0, problem->NumResidualBlocks());
767  if (GetParam()) {
768    ExpectParameterBlockContains(y);
769    ExpectParameterBlockContains(z);
770    ExpectParameterBlockContains(w);
771  }
772}
773
774INSTANTIATE_TEST_CASE_P(OptionsInstantiation,
775                        DynamicProblem,
776                        ::testing::Values(true, false));
777
778// Test for Problem::Evaluate
779
780// r_i = i - (j + 1) * x_ij^2
781template <int kNumResiduals, int kNumParameterBlocks>
782class QuadraticCostFunction : public CostFunction {
783 public:
784  QuadraticCostFunction() {
785    CHECK_GT(kNumResiduals, 0);
786    CHECK_GT(kNumParameterBlocks, 0);
787    set_num_residuals(kNumResiduals);
788    for (int i = 0; i < kNumParameterBlocks; ++i) {
789      mutable_parameter_block_sizes()->push_back(kNumResiduals);
790    }
791  }
792
793  virtual bool Evaluate(double const* const* parameters,
794                        double* residuals,
795                        double** jacobians) const {
796    for (int i = 0; i < kNumResiduals; ++i) {
797      residuals[i] = i;
798      for (int j = 0; j < kNumParameterBlocks; ++j) {
799        residuals[i] -= (j + 1.0) * parameters[j][i] * parameters[j][i];
800      }
801    }
802
803    if (jacobians == NULL) {
804      return true;
805    }
806
807    for (int j = 0; j < kNumParameterBlocks; ++j) {
808      if (jacobians[j] != NULL) {
809        MatrixRef(jacobians[j], kNumResiduals, kNumResiduals) =
810            (-2.0 * (j + 1.0) *
811             ConstVectorRef(parameters[j], kNumResiduals)).asDiagonal();
812      }
813    }
814
815    return true;
816  }
817};
818
819// Convert a CRSMatrix to a dense Eigen matrix.
820void CRSToDenseMatrix(const CRSMatrix& input, Matrix* output) {
821  Matrix& m = *CHECK_NOTNULL(output);
822  m.resize(input.num_rows, input.num_cols);
823  m.setZero();
824  for (int row = 0; row < input.num_rows; ++row) {
825    for (int j = input.rows[row]; j < input.rows[row + 1]; ++j) {
826      const int col = input.cols[j];
827      m(row, col) = input.values[j];
828    }
829  }
830}
831
832class ProblemEvaluateTest : public ::testing::Test {
833 protected:
834  void SetUp() {
835    for (int i = 0; i < 6; ++i) {
836      parameters_[i] = static_cast<double>(i + 1);
837    }
838
839    parameter_blocks_.push_back(parameters_);
840    parameter_blocks_.push_back(parameters_ + 2);
841    parameter_blocks_.push_back(parameters_ + 4);
842
843
844    CostFunction* cost_function = new QuadraticCostFunction<2, 2>;
845
846    // f(x, y)
847    residual_blocks_.push_back(
848        problem_.AddResidualBlock(cost_function,
849                                  NULL,
850                                  parameters_,
851                                  parameters_ + 2));
852    // g(y, z)
853    residual_blocks_.push_back(
854        problem_.AddResidualBlock(cost_function,
855                                  NULL, parameters_ + 2,
856                                  parameters_ + 4));
857    // h(z, x)
858    residual_blocks_.push_back(
859        problem_.AddResidualBlock(cost_function,
860                                  NULL,
861                                  parameters_ + 4,
862                                  parameters_));
863  }
864
865
866
867  void EvaluateAndCompare(const Problem::EvaluateOptions& options,
868                          const int expected_num_rows,
869                          const int expected_num_cols,
870                          const double expected_cost,
871                          const double* expected_residuals,
872                          const double* expected_gradient,
873                          const double* expected_jacobian) {
874    double cost;
875    vector<double> residuals;
876    vector<double> gradient;
877    CRSMatrix jacobian;
878
879    EXPECT_TRUE(
880        problem_.Evaluate(options,
881                          &cost,
882                          expected_residuals != NULL ? &residuals : NULL,
883                          expected_gradient != NULL ? &gradient : NULL,
884                          expected_jacobian != NULL ? &jacobian : NULL));
885
886    if (expected_residuals != NULL) {
887      EXPECT_EQ(residuals.size(), expected_num_rows);
888    }
889
890    if (expected_gradient != NULL) {
891      EXPECT_EQ(gradient.size(), expected_num_cols);
892    }
893
894    if (expected_jacobian != NULL) {
895      EXPECT_EQ(jacobian.num_rows, expected_num_rows);
896      EXPECT_EQ(jacobian.num_cols, expected_num_cols);
897    }
898
899    Matrix dense_jacobian;
900    if (expected_jacobian != NULL) {
901      CRSToDenseMatrix(jacobian, &dense_jacobian);
902    }
903
904    CompareEvaluations(expected_num_rows,
905                       expected_num_cols,
906                       expected_cost,
907                       expected_residuals,
908                       expected_gradient,
909                       expected_jacobian,
910                       cost,
911                       residuals.size() > 0 ? &residuals[0] : NULL,
912                       gradient.size() > 0 ? &gradient[0] : NULL,
913                       dense_jacobian.data());
914  }
915
916  void CheckAllEvaluationCombinations(const Problem::EvaluateOptions& options,
917                                      const ExpectedEvaluation& expected) {
918    for (int i = 0; i < 8; ++i) {
919      EvaluateAndCompare(options,
920                         expected.num_rows,
921                         expected.num_cols,
922                         expected.cost,
923                         (i & 1) ? expected.residuals : NULL,
924                         (i & 2) ? expected.gradient  : NULL,
925                         (i & 4) ? expected.jacobian  : NULL);
926    }
927  }
928
929  ProblemImpl problem_;
930  double parameters_[6];
931  vector<double*> parameter_blocks_;
932  vector<ResidualBlockId> residual_blocks_;
933};
934
935
936TEST_F(ProblemEvaluateTest, MultipleParameterAndResidualBlocks) {
937  ExpectedEvaluation expected = {
938    // Rows/columns
939    6, 6,
940    // Cost
941    7607.0,
942    // Residuals
943    { -19.0, -35.0,  // f
944      -59.0, -87.0,  // g
945      -27.0, -43.0   // h
946    },
947    // Gradient
948    {  146.0,  484.0,   // x
949       582.0, 1256.0,   // y
950      1450.0, 2604.0,   // z
951    },
952    // Jacobian
953    //                       x             y             z
954    { /* f(x, y) */ -2.0,  0.0, -12.0,   0.0,   0.0,   0.0,
955                     0.0, -4.0,   0.0, -16.0,   0.0,   0.0,
956      /* g(y, z) */  0.0,  0.0,  -6.0,   0.0, -20.0,   0.0,
957                     0.0,  0.0,   0.0,  -8.0,   0.0, -24.0,
958      /* h(z, x) */ -4.0,  0.0,   0.0,   0.0, -10.0,   0.0,
959                     0.0, -8.0,   0.0,   0.0,   0.0, -12.0
960    }
961  };
962
963  CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
964}
965
966TEST_F(ProblemEvaluateTest, ParameterAndResidualBlocksPassedInOptions) {
967  ExpectedEvaluation expected = {
968    // Rows/columns
969    6, 6,
970    // Cost
971    7607.0,
972    // Residuals
973    { -19.0, -35.0,  // f
974      -59.0, -87.0,  // g
975      -27.0, -43.0   // h
976    },
977    // Gradient
978    {  146.0,  484.0,   // x
979       582.0, 1256.0,   // y
980      1450.0, 2604.0,   // z
981    },
982    // Jacobian
983    //                       x             y             z
984    { /* f(x, y) */ -2.0,  0.0, -12.0,   0.0,   0.0,   0.0,
985                     0.0, -4.0,   0.0, -16.0,   0.0,   0.0,
986      /* g(y, z) */  0.0,  0.0,  -6.0,   0.0, -20.0,   0.0,
987                     0.0,  0.0,   0.0,  -8.0,   0.0, -24.0,
988      /* h(z, x) */ -4.0,  0.0,   0.0,   0.0, -10.0,   0.0,
989                     0.0, -8.0,   0.0,   0.0,   0.0, -12.0
990    }
991  };
992
993  Problem::EvaluateOptions evaluate_options;
994  evaluate_options.parameter_blocks = parameter_blocks_;
995  evaluate_options.residual_blocks = residual_blocks_;
996  CheckAllEvaluationCombinations(evaluate_options, expected);
997}
998
999TEST_F(ProblemEvaluateTest, ReorderedResidualBlocks) {
1000  ExpectedEvaluation expected = {
1001    // Rows/columns
1002    6, 6,
1003    // Cost
1004    7607.0,
1005    // Residuals
1006    { -19.0, -35.0,  // f
1007      -27.0, -43.0,  // h
1008      -59.0, -87.0   // g
1009    },
1010    // Gradient
1011    {  146.0,  484.0,   // x
1012       582.0, 1256.0,   // y
1013      1450.0, 2604.0,   // z
1014    },
1015    // Jacobian
1016    //                       x             y             z
1017    { /* f(x, y) */ -2.0,  0.0, -12.0,   0.0,   0.0,   0.0,
1018                     0.0, -4.0,   0.0, -16.0,   0.0,   0.0,
1019      /* h(z, x) */ -4.0,  0.0,   0.0,   0.0, -10.0,   0.0,
1020                     0.0, -8.0,   0.0,   0.0,   0.0, -12.0,
1021      /* g(y, z) */  0.0,  0.0,  -6.0,   0.0, -20.0,   0.0,
1022                     0.0,  0.0,   0.0,  -8.0,   0.0, -24.0
1023    }
1024  };
1025
1026  Problem::EvaluateOptions evaluate_options;
1027  evaluate_options.parameter_blocks = parameter_blocks_;
1028
1029  // f, h, g
1030  evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
1031  evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
1032  evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
1033
1034  CheckAllEvaluationCombinations(evaluate_options, expected);
1035}
1036
1037TEST_F(ProblemEvaluateTest, ReorderedResidualBlocksAndReorderedParameterBlocks) {
1038  ExpectedEvaluation expected = {
1039    // Rows/columns
1040    6, 6,
1041    // Cost
1042    7607.0,
1043    // Residuals
1044    { -19.0, -35.0,  // f
1045      -27.0, -43.0,  // h
1046      -59.0, -87.0   // g
1047    },
1048    // Gradient
1049    {  1450.0, 2604.0,   // z
1050        582.0, 1256.0,   // y
1051        146.0,  484.0,   // x
1052    },
1053     // Jacobian
1054    //                       z             y             x
1055    { /* f(x, y) */   0.0,   0.0, -12.0,   0.0,  -2.0,   0.0,
1056                      0.0,   0.0,   0.0, -16.0,   0.0,  -4.0,
1057      /* h(z, x) */ -10.0,   0.0,   0.0,   0.0,  -4.0,   0.0,
1058                      0.0, -12.0,   0.0,   0.0,   0.0,  -8.0,
1059      /* g(y, z) */ -20.0,   0.0,  -6.0,   0.0,   0.0,   0.0,
1060                      0.0, -24.0,   0.0,  -8.0,   0.0,   0.0
1061    }
1062  };
1063
1064  Problem::EvaluateOptions evaluate_options;
1065  // z, y, x
1066  evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
1067  evaluate_options.parameter_blocks.push_back(parameter_blocks_[1]);
1068  evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
1069
1070  // f, h, g
1071  evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
1072  evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
1073  evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
1074
1075  CheckAllEvaluationCombinations(evaluate_options, expected);
1076}
1077
1078TEST_F(ProblemEvaluateTest, ConstantParameterBlock) {
1079  ExpectedEvaluation expected = {
1080    // Rows/columns
1081    6, 6,
1082    // Cost
1083    7607.0,
1084    // Residuals
1085    { -19.0, -35.0,  // f
1086      -59.0, -87.0,  // g
1087      -27.0, -43.0   // h
1088    },
1089
1090    // Gradient
1091    {  146.0,  484.0,  // x
1092         0.0,    0.0,  // y
1093      1450.0, 2604.0,  // z
1094    },
1095
1096    // Jacobian
1097    //                       x             y             z
1098    { /* f(x, y) */ -2.0,  0.0,   0.0,   0.0,   0.0,   0.0,
1099                     0.0, -4.0,   0.0,   0.0,   0.0,   0.0,
1100      /* g(y, z) */  0.0,  0.0,   0.0,   0.0, -20.0,   0.0,
1101                     0.0,  0.0,   0.0,   0.0,   0.0, -24.0,
1102      /* h(z, x) */ -4.0,  0.0,   0.0,   0.0, -10.0,   0.0,
1103                     0.0, -8.0,   0.0,   0.0,   0.0, -12.0
1104    }
1105  };
1106
1107  problem_.SetParameterBlockConstant(parameters_ + 2);
1108  CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
1109}
1110
1111TEST_F(ProblemEvaluateTest, ExcludedAResidualBlock) {
1112  ExpectedEvaluation expected = {
1113    // Rows/columns
1114    4, 6,
1115    // Cost
1116    2082.0,
1117    // Residuals
1118    { -19.0, -35.0,  // f
1119      -27.0, -43.0   // h
1120    },
1121    // Gradient
1122    {  146.0,  484.0,   // x
1123       228.0,  560.0,   // y
1124       270.0,  516.0,   // z
1125    },
1126    // Jacobian
1127    //                       x             y             z
1128    { /* f(x, y) */ -2.0,  0.0, -12.0,   0.0,   0.0,   0.0,
1129                     0.0, -4.0,   0.0, -16.0,   0.0,   0.0,
1130      /* h(z, x) */ -4.0,  0.0,   0.0,   0.0, -10.0,   0.0,
1131                     0.0, -8.0,   0.0,   0.0,   0.0, -12.0
1132    }
1133  };
1134
1135  Problem::EvaluateOptions evaluate_options;
1136  evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
1137  evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
1138
1139  CheckAllEvaluationCombinations(evaluate_options, expected);
1140}
1141
1142TEST_F(ProblemEvaluateTest, ExcludedParameterBlock) {
1143  ExpectedEvaluation expected = {
1144    // Rows/columns
1145    6, 4,
1146    // Cost
1147    7607.0,
1148    // Residuals
1149    { -19.0, -35.0,  // f
1150      -59.0, -87.0,  // g
1151      -27.0, -43.0   // h
1152    },
1153
1154    // Gradient
1155    {  146.0,  484.0,  // x
1156      1450.0, 2604.0,  // z
1157    },
1158
1159    // Jacobian
1160    //                       x             z
1161    { /* f(x, y) */ -2.0,  0.0,   0.0,   0.0,
1162                     0.0, -4.0,   0.0,   0.0,
1163      /* g(y, z) */  0.0,  0.0, -20.0,   0.0,
1164                     0.0,  0.0,   0.0, -24.0,
1165      /* h(z, x) */ -4.0,  0.0, -10.0,   0.0,
1166                     0.0, -8.0,   0.0, -12.0
1167    }
1168  };
1169
1170  Problem::EvaluateOptions evaluate_options;
1171  // x, z
1172  evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
1173  evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
1174  evaluate_options.residual_blocks = residual_blocks_;
1175  CheckAllEvaluationCombinations(evaluate_options, expected);
1176}
1177
1178TEST_F(ProblemEvaluateTest, ExcludedParameterBlockAndExcludedResidualBlock) {
1179  ExpectedEvaluation expected = {
1180    // Rows/columns
1181    4, 4,
1182    // Cost
1183    6318.0,
1184    // Residuals
1185    { -19.0, -35.0,  // f
1186      -59.0, -87.0,  // g
1187    },
1188
1189    // Gradient
1190    {   38.0,  140.0,  // x
1191      1180.0, 2088.0,  // z
1192    },
1193
1194    // Jacobian
1195    //                       x             z
1196    { /* f(x, y) */ -2.0,  0.0,   0.0,   0.0,
1197                     0.0, -4.0,   0.0,   0.0,
1198      /* g(y, z) */  0.0,  0.0, -20.0,   0.0,
1199                     0.0,  0.0,   0.0, -24.0,
1200    }
1201  };
1202
1203  Problem::EvaluateOptions evaluate_options;
1204  // x, z
1205  evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
1206  evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
1207  evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
1208  evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
1209
1210  CheckAllEvaluationCombinations(evaluate_options, expected);
1211}
1212
1213TEST_F(ProblemEvaluateTest, LocalParameterization) {
1214  ExpectedEvaluation expected = {
1215    // Rows/columns
1216    6, 5,
1217    // Cost
1218    7607.0,
1219    // Residuals
1220    { -19.0, -35.0,  // f
1221      -59.0, -87.0,  // g
1222      -27.0, -43.0   // h
1223    },
1224    // Gradient
1225    {  146.0,  484.0,  // x
1226      1256.0,          // y with SubsetParameterization
1227      1450.0, 2604.0,  // z
1228    },
1229    // Jacobian
1230    //                       x      y             z
1231    { /* f(x, y) */ -2.0,  0.0,   0.0,   0.0,   0.0,
1232                     0.0, -4.0, -16.0,   0.0,   0.0,
1233      /* g(y, z) */  0.0,  0.0,   0.0, -20.0,   0.0,
1234                     0.0,  0.0,  -8.0,   0.0, -24.0,
1235      /* h(z, x) */ -4.0,  0.0,   0.0, -10.0,   0.0,
1236                     0.0, -8.0,   0.0,   0.0, -12.0
1237    }
1238  };
1239
1240  vector<int> constant_parameters;
1241  constant_parameters.push_back(0);
1242  problem_.SetParameterization(parameters_ + 2,
1243                               new SubsetParameterization(2,
1244                                                          constant_parameters));
1245
1246  CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
1247}
1248
1249}  // namespace internal
1250}  // namespace ceres
1251