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
42namespace ceres {
43namespace internal {
44
45// A cost function that simply returns its argument.
46class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
47 public:
48  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.
60template <int kNumResiduals, int N0, int N1, int N2>
61class MockCostFunctionBase : public
62SizedCostFunction<kNumResiduals, N0, N1, N2> {
63 public:
64  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
74class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
75class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
76class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
77
78TEST(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
107TEST(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
132TEST(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
157TEST(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
184TEST(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
211TEST(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
251TEST(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
328template <int kNumResiduals, int kNumParameterBlocks>
329class NumParameterBlocksCostFunction : public CostFunction {
330 public:
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
338  virtual ~NumParameterBlocksCostFunction() {
339  }
340
341  virtual bool Evaluate(double const* const* parameters,
342                        double* residuals,
343                        double** jacobians) const {
344    return true;
345  }
346};
347
348TEST(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
395TEST(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
407TEST(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
418TEST(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