solver_impl_test.cc revision 1d2624a10e2c559f8ba9ef89eaa30832c0a83a96 
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
31#include "gtest/gtest.h"
32#include "ceres/autodiff_cost_function.h"
33#include "ceres/linear_solver.h"
34#include "ceres/ordered_groups.h"
35#include "ceres/parameter_block.h"
36#include "ceres/problem_impl.h"
37#include "ceres/program.h"
38#include "ceres/residual_block.h"
39#include "ceres/solver_impl.h"
40#include "ceres/sized_cost_function.h"
41
42namespace ceres {
43namespace internal {
44
45// A cost function that sipmply 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    // Do nothing. This is never called.
68    return true;
69  }
70};
71
72class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
73class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
74class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
75
76TEST(SolverImpl, RemoveFixedBlocksNothingConstant) {
77  ProblemImpl problem;
78  double x;
79  double y;
80  double z;
81
82  problem.AddParameterBlock(&x, 1);
83  problem.AddParameterBlock(&y, 1);
84  problem.AddParameterBlock(&z, 1);
85  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
86  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
87  problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
88
89  string error;
90  {
91    ParameterBlockOrdering ordering;
92    ordering.AddElementToGroup(&x, 0);
93    ordering.AddElementToGroup(&y, 0);
94    ordering.AddElementToGroup(&z, 0);
95
96    Program program(*problem.mutable_program());
97    EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
98                                                         &ordering,
99                                                         NULL,
100                                                         &error));
101    EXPECT_EQ(program.NumParameterBlocks(), 3);
102    EXPECT_EQ(program.NumResidualBlocks(), 3);
103    EXPECT_EQ(ordering.NumElements(), 3);
104  }
105}
106
107TEST(SolverImpl, RemoveFixedBlocksAllParameterBlocksConstant) {
108  ProblemImpl problem;
109  double x;
110
111  problem.AddParameterBlock(&x, 1);
112  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
113  problem.SetParameterBlockConstant(&x);
114
115  ParameterBlockOrdering ordering;
116  ordering.AddElementToGroup(&x, 0);
117
118  Program program(problem.program());
119  string error;
120  EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
121                                                       &ordering,
122                                                       NULL,
123                                                       &error));
124  EXPECT_EQ(program.NumParameterBlocks(), 0);
125  EXPECT_EQ(program.NumResidualBlocks(), 0);
126  EXPECT_EQ(ordering.NumElements(), 0);
127}
128
129TEST(SolverImpl, RemoveFixedBlocksNoResidualBlocks) {
130  ProblemImpl problem;
131  double x;
132  double y;
133  double z;
134
135  problem.AddParameterBlock(&x, 1);
136  problem.AddParameterBlock(&y, 1);
137  problem.AddParameterBlock(&z, 1);
138
139  ParameterBlockOrdering ordering;
140  ordering.AddElementToGroup(&x, 0);
141  ordering.AddElementToGroup(&y, 0);
142  ordering.AddElementToGroup(&z, 0);
143
144
145  Program program(problem.program());
146  string error;
147  EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
148                                                       &ordering,
149                                                       NULL,
150                                                       &error));
151  EXPECT_EQ(program.NumParameterBlocks(), 0);
152  EXPECT_EQ(program.NumResidualBlocks(), 0);
153  EXPECT_EQ(ordering.NumElements(), 0);
154}
155
156TEST(SolverImpl, RemoveFixedBlocksOneParameterBlockConstant) {
157  ProblemImpl problem;
158  double x;
159  double y;
160  double z;
161
162  problem.AddParameterBlock(&x, 1);
163  problem.AddParameterBlock(&y, 1);
164  problem.AddParameterBlock(&z, 1);
165
166  ParameterBlockOrdering ordering;
167  ordering.AddElementToGroup(&x, 0);
168  ordering.AddElementToGroup(&y, 0);
169  ordering.AddElementToGroup(&z, 0);
170
171  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
172  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
173  problem.SetParameterBlockConstant(&x);
174
175
176  Program program(problem.program());
177  string error;
178  EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
179                                                       &ordering,
180                                                       NULL,
181                                                       &error));
182  EXPECT_EQ(program.NumParameterBlocks(), 1);
183  EXPECT_EQ(program.NumResidualBlocks(), 1);
184  EXPECT_EQ(ordering.NumElements(), 1);
185}
186
187TEST(SolverImpl, RemoveFixedBlocksNumEliminateBlocks) {
188  ProblemImpl problem;
189  double x;
190  double y;
191  double z;
192
193  problem.AddParameterBlock(&x, 1);
194  problem.AddParameterBlock(&y, 1);
195  problem.AddParameterBlock(&z, 1);
196  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
197  problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
198  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
199  problem.SetParameterBlockConstant(&x);
200
201  ParameterBlockOrdering ordering;
202  ordering.AddElementToGroup(&x, 0);
203  ordering.AddElementToGroup(&y, 0);
204  ordering.AddElementToGroup(&z, 1);
205
206  Program program(problem.program());
207  string error;
208  EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
209                                                       &ordering,
210                                                       NULL,
211                                                       &error));
212  EXPECT_EQ(program.NumParameterBlocks(), 2);
213  EXPECT_EQ(program.NumResidualBlocks(), 2);
214  EXPECT_EQ(ordering.NumElements(), 2);
215  EXPECT_EQ(ordering.GroupId(&y), 0);
216  EXPECT_EQ(ordering.GroupId(&z), 1);
217}
218
219TEST(SolverImpl, RemoveFixedBlocksFixedCost) {
220  ProblemImpl problem;
221  double x = 1.23;
222  double y = 4.56;
223  double z = 7.89;
224
225  problem.AddParameterBlock(&x, 1);
226  problem.AddParameterBlock(&y, 1);
227  problem.AddParameterBlock(&z, 1);
228  problem.AddResidualBlock(new UnaryIdentityCostFunction(), NULL, &x);
229  problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
230  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
231  problem.SetParameterBlockConstant(&x);
232
233  ParameterBlockOrdering ordering;
234  ordering.AddElementToGroup(&x, 0);
235  ordering.AddElementToGroup(&y, 0);
236  ordering.AddElementToGroup(&z, 1);
237
238  double fixed_cost = 0.0;
239  Program program(problem.program());
240
241  double expected_fixed_cost;
242  ResidualBlock *expected_removed_block = program.residual_blocks()[0];
243  scoped_array<double> scratch(
244      new double[expected_removed_block->NumScratchDoublesForEvaluate()]);
245  expected_removed_block->Evaluate(true,
246                                   &expected_fixed_cost,
247                                   NULL,
248                                   NULL,
249                                   scratch.get());
250
251  string error;
252  EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program,
253                                                       &ordering,
254                                                       &fixed_cost,
255                                                       &error));
256  EXPECT_EQ(program.NumParameterBlocks(), 2);
257  EXPECT_EQ(program.NumResidualBlocks(), 2);
258  EXPECT_EQ(ordering.NumElements(), 2);
259  EXPECT_EQ(ordering.GroupId(&y), 0);
260  EXPECT_EQ(ordering.GroupId(&z), 1);
261  EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost);
262}
263
264TEST(SolverImpl, ReorderResidualBlockNormalFunction) {
265  ProblemImpl problem;
266  double x;
267  double y;
268  double z;
269
270  problem.AddParameterBlock(&x, 1);
271  problem.AddParameterBlock(&y, 1);
272  problem.AddParameterBlock(&z, 1);
273
274  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
275  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);
276  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
277  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z);
278  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
279  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);
280
281  ParameterBlockOrdering* ordering = new ParameterBlockOrdering;
282  ordering->AddElementToGroup(&x, 0);
283  ordering->AddElementToGroup(&y, 0);
284  ordering->AddElementToGroup(&z, 1);
285
286  Solver::Options options;
287  options.linear_solver_type = DENSE_SCHUR;
288  options.linear_solver_ordering = ordering;
289
290  const vector<ResidualBlock*>& residual_blocks =
291      problem.program().residual_blocks();
292
293  vector<ResidualBlock*> expected_residual_blocks;
294
295  // This is a bit fragile, but it serves the purpose. We know the
296  // bucketing algorithm that the reordering function uses, so we
297  // expect the order for residual blocks for each e_block to be
298  // filled in reverse.
299  expected_residual_blocks.push_back(residual_blocks[4]);
300  expected_residual_blocks.push_back(residual_blocks[1]);
301  expected_residual_blocks.push_back(residual_blocks[0]);
302  expected_residual_blocks.push_back(residual_blocks[5]);
303  expected_residual_blocks.push_back(residual_blocks[2]);
304  expected_residual_blocks.push_back(residual_blocks[3]);
305
306  Program* program = problem.mutable_program();
307  program->SetParameterOffsetsAndIndex();
308
309  string error;
310  EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks(
311                  2,
312                  problem.mutable_program(),
313                  &error));
314  EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size());
315  for (int i = 0; i < expected_residual_blocks.size(); ++i) {
316    EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]);
317  }
318}
319
320TEST(SolverImpl, ReorderResidualBlockNormalFunctionWithFixedBlocks) {
321  ProblemImpl problem;
322  double x;
323  double y;
324  double z;
325
326  problem.AddParameterBlock(&x, 1);
327  problem.AddParameterBlock(&y, 1);
328  problem.AddParameterBlock(&z, 1);
329
330  // Set one parameter block constant.
331  problem.SetParameterBlockConstant(&z);
332
333  // Mark residuals for x's row block with "x" for readability.
334  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);       // 0 x
335  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);  // 1 x
336  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);  // 2
337  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);  // 3
338  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z);  // 4 x
339  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);  // 5
340  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z);  // 6 x
341  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);       // 7
342
343  ParameterBlockOrdering* ordering = new ParameterBlockOrdering;
344  ordering->AddElementToGroup(&x, 0);
345  ordering->AddElementToGroup(&z, 0);
346  ordering->AddElementToGroup(&y, 1);
347
348  Solver::Options options;
349  options.linear_solver_type = DENSE_SCHUR;
350  options.linear_solver_ordering = ordering;
351
352  // Create the reduced program. This should remove the fixed block "z",
353  // marking the index to -1 at the same time. x and y also get indices.
354  string error;
355  scoped_ptr<Program> reduced_program(
356      SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error));
357
358  const vector<ResidualBlock*>& residual_blocks =
359      problem.program().residual_blocks();
360
361  // This is a bit fragile, but it serves the purpose. We know the
362  // bucketing algorithm that the reordering function uses, so we
363  // expect the order for residual blocks for each e_block to be
364  // filled in reverse.
365
366  vector<ResidualBlock*> expected_residual_blocks;
367
368  // Row block for residuals involving "x". These are marked "x" in the block
369  // of code calling AddResidual() above.
370  expected_residual_blocks.push_back(residual_blocks[6]);
371  expected_residual_blocks.push_back(residual_blocks[4]);
372  expected_residual_blocks.push_back(residual_blocks[1]);
373  expected_residual_blocks.push_back(residual_blocks[0]);
374
375  // Row block for residuals involving "y".
376  expected_residual_blocks.push_back(residual_blocks[7]);
377  expected_residual_blocks.push_back(residual_blocks[5]);
378  expected_residual_blocks.push_back(residual_blocks[3]);
379  expected_residual_blocks.push_back(residual_blocks[2]);
380
381  EXPECT_EQ(reduced_program->residual_blocks().size(),
382            expected_residual_blocks.size());
383  for (int i = 0; i < expected_residual_blocks.size(); ++i) {
384    EXPECT_EQ(reduced_program->residual_blocks()[i],
385              expected_residual_blocks[i]);
386  }
387}
388
389TEST(SolverImpl, AutomaticSchurReorderingRespectsConstantBlocks) {
390  ProblemImpl problem;
391  double x;
392  double y;
393  double z;
394
395  problem.AddParameterBlock(&x, 1);
396  problem.AddParameterBlock(&y, 1);
397  problem.AddParameterBlock(&z, 1);
398
399  // Set one parameter block constant.
400  problem.SetParameterBlockConstant(&z);
401
402  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
403  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);
404  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
405  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
406  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z);
407  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
408  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z);
409  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);
410  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z);
411
412  ParameterBlockOrdering* ordering = new ParameterBlockOrdering;
413  ordering->AddElementToGroup(&x, 0);
414  ordering->AddElementToGroup(&z, 0);
415  ordering->AddElementToGroup(&y, 0);
416
417  Solver::Options options;
418  options.linear_solver_type = DENSE_SCHUR;
419  options.linear_solver_ordering = ordering;
420
421  string error;
422  scoped_ptr<Program> reduced_program(
423      SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error));
424
425  const vector<ResidualBlock*>& residual_blocks =
426      reduced_program->residual_blocks();
427  const vector<ParameterBlock*>& parameter_blocks =
428      reduced_program->parameter_blocks();
429
430  const vector<ResidualBlock*>& original_residual_blocks =
431      problem.program().residual_blocks();
432
433  EXPECT_EQ(residual_blocks.size(), 8);
434  EXPECT_EQ(reduced_program->parameter_blocks().size(), 2);
435
436  // Verify that right parmeter block and the residual blocks have
437  // been removed.
438  for (int i = 0; i < 8; ++i) {
439    EXPECT_NE(residual_blocks[i], original_residual_blocks.back());
440  }
441  for (int i = 0; i < 2; ++i) {
442    EXPECT_NE(parameter_blocks[i]->mutable_user_state(), &z);
443  }
444}
445
446TEST(SolverImpl, ApplyUserOrderingOrderingTooSmall) {
447  ProblemImpl problem;
448  double x;
449  double y;
450  double z;
451
452  problem.AddParameterBlock(&x, 1);
453  problem.AddParameterBlock(&y, 1);
454  problem.AddParameterBlock(&z, 1);
455
456  ParameterBlockOrdering ordering;
457  ordering.AddElementToGroup(&x, 0);
458  ordering.AddElementToGroup(&y, 1);
459
460  Program program(problem.program());
461  string error;
462  EXPECT_FALSE(SolverImpl::ApplyUserOrdering(problem.parameter_map(),
463                                             &ordering,
464                                             &program,
465                                             &error));
466}
467
468TEST(SolverImpl, ApplyUserOrderingNormal) {
469  ProblemImpl problem;
470  double x;
471  double y;
472  double z;
473
474  problem.AddParameterBlock(&x, 1);
475  problem.AddParameterBlock(&y, 1);
476  problem.AddParameterBlock(&z, 1);
477
478  ParameterBlockOrdering ordering;
479  ordering.AddElementToGroup(&x, 0);
480  ordering.AddElementToGroup(&y, 2);
481  ordering.AddElementToGroup(&z, 1);
482
483  Program* program = problem.mutable_program();
484  string error;
485
486  EXPECT_TRUE(SolverImpl::ApplyUserOrdering(problem.parameter_map(),
487                                            &ordering,
488                                            program,
489                                            &error));
490  const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
491
492  EXPECT_EQ(parameter_blocks.size(), 3);
493  EXPECT_EQ(parameter_blocks[0]->user_state(), &x);
494  EXPECT_EQ(parameter_blocks[1]->user_state(), &z);
495  EXPECT_EQ(parameter_blocks[2]->user_state(), &y);
496}
497
498#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE)
499TEST(SolverImpl, CreateLinearSolverNoSuiteSparse) {
500  Solver::Options options;
501  options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
502  // CreateLinearSolver assumes a non-empty ordering.
503  options.linear_solver_ordering = new ParameterBlockOrdering;
504  string error;
505  EXPECT_FALSE(SolverImpl::CreateLinearSolver(&options, &error));
506}
507#endif
508
509TEST(SolverImpl, CreateLinearSolverNegativeMaxNumIterations) {
510  Solver::Options options;
511  options.linear_solver_type = DENSE_QR;
512  options.max_linear_solver_iterations = -1;
513  // CreateLinearSolver assumes a non-empty ordering.
514  options.linear_solver_ordering = new ParameterBlockOrdering;
515  string error;
516  EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
517            static_cast<LinearSolver*>(NULL));
518}
519
520TEST(SolverImpl, CreateLinearSolverNegativeMinNumIterations) {
521  Solver::Options options;
522  options.linear_solver_type = DENSE_QR;
523  options.min_linear_solver_iterations = -1;
524  // CreateLinearSolver assumes a non-empty ordering.
525  options.linear_solver_ordering = new ParameterBlockOrdering;
526  string error;
527  EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
528            static_cast<LinearSolver*>(NULL));
529}
530
531TEST(SolverImpl, CreateLinearSolverMaxLessThanMinIterations) {
532  Solver::Options options;
533  options.linear_solver_type = DENSE_QR;
534  options.min_linear_solver_iterations = 10;
535  options.max_linear_solver_iterations = 5;
536  options.linear_solver_ordering = new ParameterBlockOrdering;
537  string error;
538  EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
539            static_cast<LinearSolver*>(NULL));
540}
541
542TEST(SolverImpl, CreateLinearSolverDenseSchurMultipleThreads) {
543  Solver::Options options;
544  options.linear_solver_type = DENSE_SCHUR;
545  options.num_linear_solver_threads = 2;
546  // The Schur type solvers can only be created with the Ordering
547  // contains at least one elimination group.
548  options.linear_solver_ordering = new ParameterBlockOrdering;
549  double x;
550  double y;
551  options.linear_solver_ordering->AddElementToGroup(&x, 0);
552  options.linear_solver_ordering->AddElementToGroup(&y, 0);
553
554  string error;
555  scoped_ptr<LinearSolver> solver(
556      SolverImpl::CreateLinearSolver(&options, &error));
557  EXPECT_TRUE(solver != NULL);
558  EXPECT_EQ(options.linear_solver_type, DENSE_SCHUR);
559  EXPECT_EQ(options.num_linear_solver_threads, 2);
560}
561
562TEST(SolverImpl, CreateIterativeLinearSolverForDogleg) {
563  Solver::Options options;
564  options.trust_region_strategy_type = DOGLEG;
565  // CreateLinearSolver assumes a non-empty ordering.
566  options.linear_solver_ordering = new ParameterBlockOrdering;
567  string error;
568  options.linear_solver_type = ITERATIVE_SCHUR;
569  EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
570            static_cast<LinearSolver*>(NULL));
571
572  options.linear_solver_type = CGNR;
573  EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error),
574            static_cast<LinearSolver*>(NULL));
575}
576
577TEST(SolverImpl, CreateLinearSolverNormalOperation) {
578  Solver::Options options;
579  scoped_ptr<LinearSolver> solver;
580  options.linear_solver_type = DENSE_QR;
581  // CreateLinearSolver assumes a non-empty ordering.
582  options.linear_solver_ordering = new ParameterBlockOrdering;
583  string error;
584  solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
585  EXPECT_EQ(options.linear_solver_type, DENSE_QR);
586  EXPECT_TRUE(solver.get() != NULL);
587
588  options.linear_solver_type = DENSE_NORMAL_CHOLESKY;
589  solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
590  EXPECT_EQ(options.linear_solver_type, DENSE_NORMAL_CHOLESKY);
591  EXPECT_TRUE(solver.get() != NULL);
592
593#ifndef CERES_NO_SUITESPARSE
594  options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
595  options.sparse_linear_algebra_library = SUITE_SPARSE;
596  solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
597  EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
598  EXPECT_TRUE(solver.get() != NULL);
599#endif
600
601#ifndef CERES_NO_CXSPARSE
602  options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
603  options.sparse_linear_algebra_library = CX_SPARSE;
604  solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
605  EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
606  EXPECT_TRUE(solver.get() != NULL);
607#endif
608
609  double x;
610  double y;
611  options.linear_solver_ordering->AddElementToGroup(&x, 0);
612  options.linear_solver_ordering->AddElementToGroup(&y, 0);
613
614  options.linear_solver_type = DENSE_SCHUR;
615  solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
616  EXPECT_EQ(options.linear_solver_type, DENSE_SCHUR);
617  EXPECT_TRUE(solver.get() != NULL);
618
619  options.linear_solver_type = SPARSE_SCHUR;
620  solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
621
622#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE)
623  EXPECT_TRUE(SolverImpl::CreateLinearSolver(&options, &error) == NULL);
624#else
625  EXPECT_TRUE(solver.get() != NULL);
626  EXPECT_EQ(options.linear_solver_type, SPARSE_SCHUR);
627#endif
628
629  options.linear_solver_type = ITERATIVE_SCHUR;
630  solver.reset(SolverImpl::CreateLinearSolver(&options, &error));
631  EXPECT_EQ(options.linear_solver_type, ITERATIVE_SCHUR);
632  EXPECT_TRUE(solver.get() != NULL);
633}
634
635struct QuadraticCostFunction {
636  template <typename T> bool operator()(const T* const x,
637                                        T* residual) const {
638    residual[0] = T(5.0) - *x;
639    return true;
640  }
641};
642
643struct RememberingCallback : public IterationCallback {
644  explicit RememberingCallback(double *x) : calls(0), x(x) {}
645  virtual ~RememberingCallback() {}
646  virtual CallbackReturnType operator()(const IterationSummary& summary) {
647    x_values.push_back(*x);
648    return SOLVER_CONTINUE;
649  }
650  int calls;
651  double *x;
652  vector<double> x_values;
653};
654
655TEST(SolverImpl, UpdateStateEveryIterationOption) {
656  double x = 50.0;
657  const double original_x = x;
658
659  scoped_ptr<CostFunction> cost_function(
660      new AutoDiffCostFunction<QuadraticCostFunction, 1, 1>(
661          new QuadraticCostFunction));
662
663  Problem::Options problem_options;
664  problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP;
665  ProblemImpl problem(problem_options);
666  problem.AddResidualBlock(cost_function.get(), NULL, &x);
667
668  Solver::Options options;
669  options.linear_solver_type = DENSE_QR;
670
671  RememberingCallback callback(&x);
672  options.callbacks.push_back(&callback);
673
674  Solver::Summary summary;
675
676  int num_iterations;
677
678  // First try: no updating.
679  SolverImpl::Solve(options, &problem, &summary);
680  num_iterations = summary.num_successful_steps +
681                   summary.num_unsuccessful_steps;
682  EXPECT_GT(num_iterations, 1);
683  for (int i = 0; i < callback.x_values.size(); ++i) {
684    EXPECT_EQ(50.0, callback.x_values[i]);
685  }
686
687  // Second try: with updating
688  x = 50.0;
689  options.update_state_every_iteration = true;
690  callback.x_values.clear();
691  SolverImpl::Solve(options, &problem, &summary);
692  num_iterations = summary.num_successful_steps +
693                   summary.num_unsuccessful_steps;
694  EXPECT_GT(num_iterations, 1);
695  EXPECT_EQ(original_x, callback.x_values[0]);
696  EXPECT_NE(original_x, callback.x_values[1]);
697}
698
699// The parameters must be in separate blocks so that they can be individually
700// set constant or not.
701struct Quadratic4DCostFunction {
702  template <typename T> bool operator()(const T* const x,
703                                        const T* const y,
704                                        const T* const z,
705                                        const T* const w,
706                                        T* residual) const {
707    // A 4-dimension axis-aligned quadratic.
708    residual[0] = T(10.0) - *x +
709                  T(20.0) - *y +
710                  T(30.0) - *z +
711                  T(40.0) - *w;
712    return true;
713  }
714};
715
716TEST(SolverImpl, ConstantParameterBlocksDoNotChangeAndStateInvariantKept) {
717  double x = 50.0;
718  double y = 50.0;
719  double z = 50.0;
720  double w = 50.0;
721  const double original_x = 50.0;
722  const double original_y = 50.0;
723  const double original_z = 50.0;
724  const double original_w = 50.0;
725
726  scoped_ptr<CostFunction> cost_function(
727      new AutoDiffCostFunction<Quadratic4DCostFunction, 1, 1, 1, 1, 1>(
728          new Quadratic4DCostFunction));
729
730  Problem::Options problem_options;
731  problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP;
732
733  ProblemImpl problem(problem_options);
734  problem.AddResidualBlock(cost_function.get(), NULL, &x, &y, &z, &w);
735  problem.SetParameterBlockConstant(&x);
736  problem.SetParameterBlockConstant(&w);
737
738  Solver::Options options;
739  options.linear_solver_type = DENSE_QR;
740
741  Solver::Summary summary;
742  SolverImpl::Solve(options, &problem, &summary);
743
744  // Verify only the non-constant parameters were mutated.
745  EXPECT_EQ(original_x, x);
746  EXPECT_NE(original_y, y);
747  EXPECT_NE(original_z, z);
748  EXPECT_EQ(original_w, w);
749
750  // Check that the parameter block state pointers are pointing back at the
751  // user state, instead of inside a random temporary vector made by Solve().
752  EXPECT_EQ(&x, problem.program().parameter_blocks()[0]->state());
753  EXPECT_EQ(&y, problem.program().parameter_blocks()[1]->state());
754  EXPECT_EQ(&z, problem.program().parameter_blocks()[2]->state());
755  EXPECT_EQ(&w, problem.program().parameter_blocks()[3]->state());
756}
757
758TEST(SolverImpl, NoParameterBlocks) {
759  ProblemImpl problem_impl;
760  Solver::Options options;
761  Solver::Summary summary;
762  SolverImpl::Solve(options, &problem_impl, &summary);
763  EXPECT_EQ(summary.termination_type, DID_NOT_RUN);
764  EXPECT_EQ(summary.error, "Problem contains no parameter blocks.");
765}
766
767TEST(SolverImpl, NoResiduals) {
768  ProblemImpl problem_impl;
769  Solver::Options options;
770  Solver::Summary summary;
771  double x = 1;
772  problem_impl.AddParameterBlock(&x, 1);
773  SolverImpl::Solve(options, &problem_impl, &summary);
774  EXPECT_EQ(summary.termination_type, DID_NOT_RUN);
775  EXPECT_EQ(summary.error, "Problem contains no residual blocks.");
776}
777
778
779TEST(SolverImpl, ProblemIsConstant) {
780  ProblemImpl problem_impl;
781  Solver::Options options;
782  Solver::Summary summary;
783  double x = 1;
784  problem_impl.AddResidualBlock(new UnaryIdentityCostFunction, NULL, &x);
785  problem_impl.SetParameterBlockConstant(&x);
786  SolverImpl::Solve(options, &problem_impl, &summary);
787  EXPECT_EQ(summary.termination_type, FUNCTION_TOLERANCE);
788  EXPECT_EQ(summary.initial_cost, 1.0 / 2.0);
789  EXPECT_EQ(summary.final_cost, 1.0 / 2.0);
790}
791
792TEST(SolverImpl, AlternateLinearSolverForSchurTypeLinearSolver) {
793  Solver::Options options;
794
795  options.linear_solver_type = DENSE_QR;
796  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
797  EXPECT_EQ(options.linear_solver_type, DENSE_QR);
798
799  options.linear_solver_type = DENSE_NORMAL_CHOLESKY;
800  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
801  EXPECT_EQ(options.linear_solver_type, DENSE_NORMAL_CHOLESKY);
802
803  options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
804  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
805  EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
806
807  options.linear_solver_type = CGNR;
808  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
809  EXPECT_EQ(options.linear_solver_type, CGNR);
810
811  options.linear_solver_type = DENSE_SCHUR;
812  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
813  EXPECT_EQ(options.linear_solver_type, DENSE_QR);
814
815  options.linear_solver_type = SPARSE_SCHUR;
816  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
817  EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY);
818
819  options.linear_solver_type = ITERATIVE_SCHUR;
820  options.preconditioner_type = IDENTITY;
821  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
822  EXPECT_EQ(options.linear_solver_type, CGNR);
823  EXPECT_EQ(options.preconditioner_type, IDENTITY);
824
825  options.linear_solver_type = ITERATIVE_SCHUR;
826  options.preconditioner_type = JACOBI;
827  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
828  EXPECT_EQ(options.linear_solver_type, CGNR);
829  EXPECT_EQ(options.preconditioner_type, JACOBI);
830
831  options.linear_solver_type = ITERATIVE_SCHUR;
832  options.preconditioner_type = SCHUR_JACOBI;
833  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
834  EXPECT_EQ(options.linear_solver_type, CGNR);
835  EXPECT_EQ(options.preconditioner_type, JACOBI);
836
837  options.linear_solver_type = ITERATIVE_SCHUR;
838  options.preconditioner_type = CLUSTER_JACOBI;
839  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
840  EXPECT_EQ(options.linear_solver_type, CGNR);
841  EXPECT_EQ(options.preconditioner_type, JACOBI);
842
843  options.linear_solver_type = ITERATIVE_SCHUR;
844  options.preconditioner_type = CLUSTER_TRIDIAGONAL;
845  SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver(&options);
846  EXPECT_EQ(options.linear_solver_type, CGNR);
847  EXPECT_EQ(options.preconditioner_type, JACOBI);
848}
849
850TEST(SolverImpl, CreateJacobianBlockSparsityTranspose) {
851  ProblemImpl problem;
852  double x[2];
853  double y[3];
854  double z;
855
856  problem.AddParameterBlock(x, 2);
857  problem.AddParameterBlock(y, 3);
858  problem.AddParameterBlock(&z, 1);
859
860  problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 0, 0>(), NULL, x);
861  problem.AddResidualBlock(new MockCostFunctionBase<3, 1, 2, 0>(), NULL, &z, x);
862  problem.AddResidualBlock(new MockCostFunctionBase<4, 1, 3, 0>(), NULL, &z, y);
863  problem.AddResidualBlock(new MockCostFunctionBase<5, 1, 3, 0>(), NULL, &z, y);
864  problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 1, 0>(), NULL, x, &z);
865  problem.AddResidualBlock(new MockCostFunctionBase<2, 1, 3, 0>(), NULL, &z, y);
866  problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 1, 0>(), NULL, x, &z);
867  problem.AddResidualBlock(new MockCostFunctionBase<1, 3, 0, 0>(), NULL, y);
868
869  TripletSparseMatrix expected_block_sparse_jacobian(3, 8, 14);
870  {
871    int* rows = expected_block_sparse_jacobian.mutable_rows();
872    int* cols = expected_block_sparse_jacobian.mutable_cols();
873    double* values = expected_block_sparse_jacobian.mutable_values();
874    rows[0] = 0;
875    cols[0] = 0;
876
877    rows[1] = 2;
878    cols[1] = 1;
879    rows[2] = 0;
880    cols[2] = 1;
881
882    rows[3] = 2;
883    cols[3] = 2;
884    rows[4] = 1;
885    cols[4] = 2;
886
887    rows[5] = 2;
888    cols[5] = 3;
889    rows[6] = 1;
890    cols[6] = 3;
891
892    rows[7] = 0;
893    cols[7] = 4;
894    rows[8] = 2;
895    cols[8] = 4;
896
897    rows[9] = 2;
898    cols[9] = 5;
899    rows[10] = 1;
900    cols[10] = 5;
901
902    rows[11] = 0;
903    cols[11] = 6;
904    rows[12] = 2;
905    cols[12] = 6;
906
907    rows[13] = 1;
908    cols[13] = 7;
909    fill(values, values + 14, 1.0);
910    expected_block_sparse_jacobian.set_num_nonzeros(14);
911  }
912
913  Program* program = problem.mutable_program();
914  program->SetParameterOffsetsAndIndex();
915
916  scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
917      SolverImpl::CreateJacobianBlockSparsityTranspose(program));
918
919  Matrix expected_dense_jacobian;
920  expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
921
922  Matrix actual_dense_jacobian;
923  actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
924  EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
925}
926
927template <int kNumResiduals, int kNumParameterBlocks>
928class NumParameterBlocksCostFunction : public CostFunction {
929 public:
930  NumParameterBlocksCostFunction() {
931    set_num_residuals(kNumResiduals);
932    for (int i = 0; i < kNumParameterBlocks; ++i) {
933      mutable_parameter_block_sizes()->push_back(1);
934    }
935  }
936
937  virtual ~NumParameterBlocksCostFunction() {
938  }
939
940  virtual bool Evaluate(double const* const* parameters,
941                        double* residuals,
942                        double** jacobians) const {
943    return true;
944  }
945};
946
947TEST(SolverImpl, ReallocationInCreateJacobianBlockSparsityTranspose) {
948  // CreateJacobianBlockSparsityTranspose starts with a conservative
949  // estimate of the size of the sparsity pattern. This test ensures
950  // that when those estimates are violated, the reallocation/resizing
951  // logic works correctly.
952
953  ProblemImpl problem;
954  double x[20];
955
956  vector<double*> parameter_blocks;
957  for (int i = 0; i < 20; ++i) {
958    problem.AddParameterBlock(x + i, 1);
959    parameter_blocks.push_back(x + i);
960  }
961
962  problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
963                           NULL,
964                           parameter_blocks);
965
966  TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
967  {
968    int* rows = expected_block_sparse_jacobian.mutable_rows();
969    int* cols = expected_block_sparse_jacobian.mutable_cols();
970    for (int i = 0; i < 20; ++i) {
971      rows[i] = i;
972      cols[i] = 0;
973    }
974
975    double* values = expected_block_sparse_jacobian.mutable_values();
976    fill(values, values + 20, 1.0);
977    expected_block_sparse_jacobian.set_num_nonzeros(20);
978  }
979
980  Program* program = problem.mutable_program();
981  program->SetParameterOffsetsAndIndex();
982
983  scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
984      SolverImpl::CreateJacobianBlockSparsityTranspose(program));
985
986  Matrix expected_dense_jacobian;
987  expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
988
989  Matrix actual_dense_jacobian;
990  actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
991  EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
992}
993
994}  // namespace internal
995}  // namespace ceres
996