1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2014 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 
31 #include "ceres/program.h"
32 
33 #include <limits>
34 #include <cmath>
35 #include <vector>
36 #include "ceres/sized_cost_function.h"
37 #include "ceres/problem_impl.h"
38 #include "ceres/residual_block.h"
39 #include "ceres/triplet_sparse_matrix.h"
40 #include "gtest/gtest.h"
41 
42 namespace ceres {
43 namespace internal {
44 
45 // A cost function that simply returns its argument.
46 class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
47  public:
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const48   virtual bool Evaluate(double const* const* parameters,
49                         double* residuals,
50                         double** jacobians) const {
51     residuals[0] = parameters[0][0];
52     if (jacobians != NULL && jacobians[0] != NULL) {
53       jacobians[0][0] = 1.0;
54     }
55     return true;
56   }
57 };
58 
59 // Templated base class for the CostFunction signatures.
60 template <int kNumResiduals, int N0, int N1, int N2>
61 class MockCostFunctionBase : public
62 SizedCostFunction<kNumResiduals, N0, N1, N2> {
63  public:
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const64   virtual bool Evaluate(double const* const* parameters,
65                         double* residuals,
66                         double** jacobians) const {
67     for (int i = 0; i < kNumResiduals; ++i) {
68       residuals[i] = kNumResiduals +  N0 + N1 + N2;
69     }
70     return true;
71   }
72 };
73 
74 class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
75 class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
76 class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
77 
TEST(Program,RemoveFixedBlocksNothingConstant)78 TEST(Program, RemoveFixedBlocksNothingConstant) {
79   ProblemImpl problem;
80   double x;
81   double y;
82   double z;
83 
84   problem.AddParameterBlock(&x, 1);
85   problem.AddParameterBlock(&y, 1);
86   problem.AddParameterBlock(&z, 1);
87   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
88   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
89   problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
90 
91   vector<double*> removed_parameter_blocks;
92   double fixed_cost = 0.0;
93   string message;
94   scoped_ptr<Program> reduced_program(
95       CHECK_NOTNULL(problem
96                     .program()
97                     .CreateReducedProgram(&removed_parameter_blocks,
98                                           &fixed_cost,
99                                           &message)));
100 
101   EXPECT_EQ(reduced_program->NumParameterBlocks(), 3);
102   EXPECT_EQ(reduced_program->NumResidualBlocks(), 3);
103   EXPECT_EQ(removed_parameter_blocks.size(), 0);
104   EXPECT_EQ(fixed_cost, 0.0);
105 }
106 
TEST(Program,RemoveFixedBlocksAllParameterBlocksConstant)107 TEST(Program, RemoveFixedBlocksAllParameterBlocksConstant) {
108   ProblemImpl problem;
109   double x = 1.0;
110 
111   problem.AddParameterBlock(&x, 1);
112   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
113   problem.SetParameterBlockConstant(&x);
114 
115   vector<double*> removed_parameter_blocks;
116   double fixed_cost = 0.0;
117   string message;
118   scoped_ptr<Program> reduced_program(
119       CHECK_NOTNULL(problem
120                     .program()
121                     .CreateReducedProgram(&removed_parameter_blocks,
122                                           &fixed_cost,
123                                           &message)));
124   EXPECT_EQ(reduced_program->NumParameterBlocks(), 0);
125   EXPECT_EQ(reduced_program->NumResidualBlocks(), 0);
126   EXPECT_EQ(removed_parameter_blocks.size(), 1);
127   EXPECT_EQ(removed_parameter_blocks[0], &x);
128   EXPECT_EQ(fixed_cost, 9.0);
129 }
130 
131 
TEST(Program,RemoveFixedBlocksNoResidualBlocks)132 TEST(Program, RemoveFixedBlocksNoResidualBlocks) {
133   ProblemImpl problem;
134   double x;
135   double y;
136   double z;
137 
138   problem.AddParameterBlock(&x, 1);
139   problem.AddParameterBlock(&y, 1);
140   problem.AddParameterBlock(&z, 1);
141 
142   vector<double*> removed_parameter_blocks;
143   double fixed_cost = 0.0;
144   string message;
145   scoped_ptr<Program> reduced_program(
146       CHECK_NOTNULL(problem
147                     .program()
148                     .CreateReducedProgram(&removed_parameter_blocks,
149                                           &fixed_cost,
150                                           &message)));
151   EXPECT_EQ(reduced_program->NumParameterBlocks(), 0);
152   EXPECT_EQ(reduced_program->NumResidualBlocks(), 0);
153   EXPECT_EQ(removed_parameter_blocks.size(), 3);
154   EXPECT_EQ(fixed_cost, 0.0);
155 }
156 
TEST(Program,RemoveFixedBlocksOneParameterBlockConstant)157 TEST(Program, RemoveFixedBlocksOneParameterBlockConstant) {
158   ProblemImpl problem;
159   double x;
160   double y;
161   double z;
162 
163   problem.AddParameterBlock(&x, 1);
164   problem.AddParameterBlock(&y, 1);
165   problem.AddParameterBlock(&z, 1);
166 
167   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
168   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
169   problem.SetParameterBlockConstant(&x);
170 
171   vector<double*> removed_parameter_blocks;
172   double fixed_cost = 0.0;
173   string message;
174   scoped_ptr<Program> reduced_program(
175       CHECK_NOTNULL(problem
176                     .program()
177                     .CreateReducedProgram(&removed_parameter_blocks,
178                                           &fixed_cost,
179                                           &message)));
180   EXPECT_EQ(reduced_program->NumParameterBlocks(), 1);
181   EXPECT_EQ(reduced_program->NumResidualBlocks(), 1);
182 }
183 
TEST(Program,RemoveFixedBlocksNumEliminateBlocks)184 TEST(Program, RemoveFixedBlocksNumEliminateBlocks) {
185   ProblemImpl problem;
186   double x;
187   double y;
188   double z;
189 
190   problem.AddParameterBlock(&x, 1);
191   problem.AddParameterBlock(&y, 1);
192   problem.AddParameterBlock(&z, 1);
193   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
194   problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
195   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
196   problem.SetParameterBlockConstant(&x);
197 
198   vector<double*> removed_parameter_blocks;
199   double fixed_cost = 0.0;
200   string message;
201   scoped_ptr<Program> reduced_program(
202       CHECK_NOTNULL(problem
203                     .program()
204                     .CreateReducedProgram(&removed_parameter_blocks,
205                                           &fixed_cost,
206                                           &message)));
207   EXPECT_EQ(reduced_program->NumParameterBlocks(), 2);
208   EXPECT_EQ(reduced_program->NumResidualBlocks(), 2);
209 }
210 
TEST(Program,RemoveFixedBlocksFixedCost)211 TEST(Program, RemoveFixedBlocksFixedCost) {
212   ProblemImpl problem;
213   double x = 1.23;
214   double y = 4.56;
215   double z = 7.89;
216 
217   problem.AddParameterBlock(&x, 1);
218   problem.AddParameterBlock(&y, 1);
219   problem.AddParameterBlock(&z, 1);
220   problem.AddResidualBlock(new UnaryIdentityCostFunction(), NULL, &x);
221   problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
222   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
223   problem.SetParameterBlockConstant(&x);
224 
225   ResidualBlock *expected_removed_block = problem.program().residual_blocks()[0];
226   scoped_array<double> scratch(
227       new double[expected_removed_block->NumScratchDoublesForEvaluate()]);
228   double expected_fixed_cost;
229   expected_removed_block->Evaluate(true,
230                                    &expected_fixed_cost,
231                                    NULL,
232                                    NULL,
233                                    scratch.get());
234 
235 
236   vector<double*> removed_parameter_blocks;
237   double fixed_cost = 0.0;
238   string message;
239   scoped_ptr<Program> reduced_program(
240       CHECK_NOTNULL(problem
241                     .program()
242                     .CreateReducedProgram(&removed_parameter_blocks,
243                                           &fixed_cost,
244                                           &message)));
245 
246   EXPECT_EQ(reduced_program->NumParameterBlocks(), 2);
247   EXPECT_EQ(reduced_program->NumResidualBlocks(), 2);
248   EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost);
249 }
250 
TEST(Program,CreateJacobianBlockSparsityTranspose)251 TEST(Program, CreateJacobianBlockSparsityTranspose) {
252   ProblemImpl problem;
253   double x[2];
254   double y[3];
255   double z;
256 
257   problem.AddParameterBlock(x, 2);
258   problem.AddParameterBlock(y, 3);
259   problem.AddParameterBlock(&z, 1);
260 
261   problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 0, 0>(), NULL, x);
262   problem.AddResidualBlock(new MockCostFunctionBase<3, 1, 2, 0>(), NULL, &z, x);
263   problem.AddResidualBlock(new MockCostFunctionBase<4, 1, 3, 0>(), NULL, &z, y);
264   problem.AddResidualBlock(new MockCostFunctionBase<5, 1, 3, 0>(), NULL, &z, y);
265   problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 1, 0>(), NULL, x, &z);
266   problem.AddResidualBlock(new MockCostFunctionBase<2, 1, 3, 0>(), NULL, &z, y);
267   problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 1, 0>(), NULL, x, &z);
268   problem.AddResidualBlock(new MockCostFunctionBase<1, 3, 0, 0>(), NULL, y);
269 
270   TripletSparseMatrix expected_block_sparse_jacobian(3, 8, 14);
271   {
272     int* rows = expected_block_sparse_jacobian.mutable_rows();
273     int* cols = expected_block_sparse_jacobian.mutable_cols();
274     double* values = expected_block_sparse_jacobian.mutable_values();
275     rows[0] = 0;
276     cols[0] = 0;
277 
278     rows[1] = 2;
279     cols[1] = 1;
280     rows[2] = 0;
281     cols[2] = 1;
282 
283     rows[3] = 2;
284     cols[3] = 2;
285     rows[4] = 1;
286     cols[4] = 2;
287 
288     rows[5] = 2;
289     cols[5] = 3;
290     rows[6] = 1;
291     cols[6] = 3;
292 
293     rows[7] = 0;
294     cols[7] = 4;
295     rows[8] = 2;
296     cols[8] = 4;
297 
298     rows[9] = 2;
299     cols[9] = 5;
300     rows[10] = 1;
301     cols[10] = 5;
302 
303     rows[11] = 0;
304     cols[11] = 6;
305     rows[12] = 2;
306     cols[12] = 6;
307 
308     rows[13] = 1;
309     cols[13] = 7;
310     fill(values, values + 14, 1.0);
311     expected_block_sparse_jacobian.set_num_nonzeros(14);
312   }
313 
314   Program* program = problem.mutable_program();
315   program->SetParameterOffsetsAndIndex();
316 
317   scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
318       program->CreateJacobianBlockSparsityTranspose());
319 
320   Matrix expected_dense_jacobian;
321   expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
322 
323   Matrix actual_dense_jacobian;
324   actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
325   EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
326 }
327 
328 template <int kNumResiduals, int kNumParameterBlocks>
329 class NumParameterBlocksCostFunction : public CostFunction {
330  public:
NumParameterBlocksCostFunction()331   NumParameterBlocksCostFunction() {
332     set_num_residuals(kNumResiduals);
333     for (int i = 0; i < kNumParameterBlocks; ++i) {
334       mutable_parameter_block_sizes()->push_back(1);
335     }
336   }
337 
~NumParameterBlocksCostFunction()338   virtual ~NumParameterBlocksCostFunction() {
339   }
340 
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const341   virtual bool Evaluate(double const* const* parameters,
342                         double* residuals,
343                         double** jacobians) const {
344     return true;
345   }
346 };
347 
TEST(Program,ReallocationInCreateJacobianBlockSparsityTranspose)348 TEST(Program, ReallocationInCreateJacobianBlockSparsityTranspose) {
349   // CreateJacobianBlockSparsityTranspose starts with a conservative
350   // estimate of the size of the sparsity pattern. This test ensures
351   // that when those estimates are violated, the reallocation/resizing
352   // logic works correctly.
353 
354   ProblemImpl problem;
355   double x[20];
356 
357   vector<double*> parameter_blocks;
358   for (int i = 0; i < 20; ++i) {
359     problem.AddParameterBlock(x + i, 1);
360     parameter_blocks.push_back(x + i);
361   }
362 
363   problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
364                            NULL,
365                            parameter_blocks);
366 
367   TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
368   {
369     int* rows = expected_block_sparse_jacobian.mutable_rows();
370     int* cols = expected_block_sparse_jacobian.mutable_cols();
371     for (int i = 0; i < 20; ++i) {
372       rows[i] = i;
373       cols[i] = 0;
374     }
375 
376     double* values = expected_block_sparse_jacobian.mutable_values();
377     fill(values, values + 20, 1.0);
378     expected_block_sparse_jacobian.set_num_nonzeros(20);
379   }
380 
381   Program* program = problem.mutable_program();
382   program->SetParameterOffsetsAndIndex();
383 
384   scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
385       program->CreateJacobianBlockSparsityTranspose());
386 
387   Matrix expected_dense_jacobian;
388   expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
389 
390   Matrix actual_dense_jacobian;
391   actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
392   EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
393 }
394 
TEST(Program,ProblemHasNanParameterBlocks)395 TEST(Program, ProblemHasNanParameterBlocks) {
396   ProblemImpl problem;
397   double x[2];
398   x[0] = 1.0;
399   x[1] = std::numeric_limits<double>::quiet_NaN();
400   problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
401   string error;
402   EXPECT_FALSE(problem.program().ParameterBlocksAreFinite(&error));
403   EXPECT_NE(error.find("has at least one invalid value"),
404             string::npos) << error;
405 }
406 
TEST(Program,InfeasibleParameterBlock)407 TEST(Program, InfeasibleParameterBlock) {
408   ProblemImpl problem;
409   double x[] = {0.0, 0.0};
410   problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
411   problem.SetParameterLowerBound(x, 0, 2.0);
412   problem.SetParameterUpperBound(x, 0, 1.0);
413   string error;
414   EXPECT_FALSE(problem.program().IsFeasible(&error));
415   EXPECT_NE(error.find("infeasible bound"), string::npos) << error;
416 }
417 
TEST(Program,InfeasibleConstantParameterBlock)418 TEST(Program, InfeasibleConstantParameterBlock) {
419   ProblemImpl problem;
420   double x[] = {0.0, 0.0};
421   problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
422   problem.SetParameterLowerBound(x, 0, 1.0);
423   problem.SetParameterUpperBound(x, 0, 2.0);
424   problem.SetParameterBlockConstant(x);
425   string error;
426   EXPECT_FALSE(problem.program().IsFeasible(&error));
427   EXPECT_NE(error.find("infeasible value"), string::npos) << error;
428 }
429 
430 }  // namespace internal
431 }  // namespace ceres
432