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/reorder_program.h"
32 
33 #include "ceres/parameter_block.h"
34 #include "ceres/problem_impl.h"
35 #include "ceres/program.h"
36 #include "ceres/sized_cost_function.h"
37 #include "ceres/solver.h"
38 
39 #include "gtest/gtest.h"
40 
41 namespace ceres {
42 namespace internal {
43 
44 // Templated base class for the CostFunction signatures.
45 template <int kNumResiduals, int N0, int N1, int N2>
46 class MockCostFunctionBase : public
47 SizedCostFunction<kNumResiduals, N0, N1, N2> {
48  public:
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const49   virtual bool Evaluate(double const* const* parameters,
50                         double* residuals,
51                         double** jacobians) const {
52     // Do nothing. This is never called.
53     return true;
54   }
55 };
56 
57 class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
58 class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
59 class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
60 
TEST(_,ReorderResidualBlockNormalFunction)61 TEST(_, ReorderResidualBlockNormalFunction) {
62   ProblemImpl problem;
63   double x;
64   double y;
65   double z;
66 
67   problem.AddParameterBlock(&x, 1);
68   problem.AddParameterBlock(&y, 1);
69   problem.AddParameterBlock(&z, 1);
70 
71   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
72   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);
73   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
74   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z);
75   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
76   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);
77 
78   ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering;
79   linear_solver_ordering->AddElementToGroup(&x, 0);
80   linear_solver_ordering->AddElementToGroup(&y, 0);
81   linear_solver_ordering->AddElementToGroup(&z, 1);
82 
83   Solver::Options options;
84   options.linear_solver_type = DENSE_SCHUR;
85   options.linear_solver_ordering.reset(linear_solver_ordering);
86 
87   const vector<ResidualBlock*>& residual_blocks =
88       problem.program().residual_blocks();
89 
90   vector<ResidualBlock*> expected_residual_blocks;
91 
92   // This is a bit fragile, but it serves the purpose. We know the
93   // bucketing algorithm that the reordering function uses, so we
94   // expect the order for residual blocks for each e_block to be
95   // filled in reverse.
96   expected_residual_blocks.push_back(residual_blocks[4]);
97   expected_residual_blocks.push_back(residual_blocks[1]);
98   expected_residual_blocks.push_back(residual_blocks[0]);
99   expected_residual_blocks.push_back(residual_blocks[5]);
100   expected_residual_blocks.push_back(residual_blocks[2]);
101   expected_residual_blocks.push_back(residual_blocks[3]);
102 
103   Program* program = problem.mutable_program();
104   program->SetParameterOffsetsAndIndex();
105 
106   string message;
107   EXPECT_TRUE(LexicographicallyOrderResidualBlocks(
108                   2,
109                   problem.mutable_program(),
110                   &message));
111   EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size());
112   for (int i = 0; i < expected_residual_blocks.size(); ++i) {
113     EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]);
114   }
115 }
116 
TEST(_,ApplyOrderingOrderingTooSmall)117 TEST(_, ApplyOrderingOrderingTooSmall) {
118   ProblemImpl problem;
119   double x;
120   double y;
121   double z;
122 
123   problem.AddParameterBlock(&x, 1);
124   problem.AddParameterBlock(&y, 1);
125   problem.AddParameterBlock(&z, 1);
126 
127   ParameterBlockOrdering linear_solver_ordering;
128   linear_solver_ordering.AddElementToGroup(&x, 0);
129   linear_solver_ordering.AddElementToGroup(&y, 1);
130 
131   Program program(problem.program());
132   string message;
133   EXPECT_FALSE(ApplyOrdering(problem.parameter_map(),
134                              linear_solver_ordering,
135                              &program,
136                              &message));
137 }
138 
TEST(_,ApplyOrderingNormal)139 TEST(_, ApplyOrderingNormal) {
140   ProblemImpl problem;
141   double x;
142   double y;
143   double z;
144 
145   problem.AddParameterBlock(&x, 1);
146   problem.AddParameterBlock(&y, 1);
147   problem.AddParameterBlock(&z, 1);
148 
149   ParameterBlockOrdering linear_solver_ordering;
150   linear_solver_ordering.AddElementToGroup(&x, 0);
151   linear_solver_ordering.AddElementToGroup(&y, 2);
152   linear_solver_ordering.AddElementToGroup(&z, 1);
153 
154   Program* program = problem.mutable_program();
155   string message;
156 
157   EXPECT_TRUE(ApplyOrdering(problem.parameter_map(),
158                             linear_solver_ordering,
159                             program,
160                             &message));
161   const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
162 
163   EXPECT_EQ(parameter_blocks.size(), 3);
164   EXPECT_EQ(parameter_blocks[0]->user_state(), &x);
165   EXPECT_EQ(parameter_blocks[1]->user_state(), &z);
166   EXPECT_EQ(parameter_blocks[2]->user_state(), &y);
167 }
168 
169 }  // namespace internal
170 }  // namespace ceres
171