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 "ceres/parameter_block_ordering.h"
32 
33 #include <cstddef>
34 #include <vector>
35 #include "gtest/gtest.h"
36 #include "ceres/collections_port.h"
37 #include "ceres/graph.h"
38 #include "ceres/problem_impl.h"
39 #include "ceres/program.h"
40 #include "ceres/stl_util.h"
41 #include "ceres/cost_function.h"
42 #include "ceres/internal/scoped_ptr.h"
43 #include "ceres/sized_cost_function.h"
44 
45 namespace ceres {
46 namespace internal {
47 
48 typedef Graph<ParameterBlock*> HessianGraph;
49 typedef HashSet<ParameterBlock*> VertexSet;
50 
51 template <int M, int N1 = 0, int N2 = 0, int N3 = 0>
52 class DummyCostFunction: public SizedCostFunction<M, N1, N2, N3> {
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const53   virtual bool Evaluate(double const* const* parameters,
54                         double* residuals,
55                         double** jacobians) const {
56     return true;
57   }
58 };
59 
60 class SchurOrderingTest : public ::testing::Test {
61  protected :
SetUp()62   virtual void SetUp() {
63     // The explicit calls to AddParameterBlock are necessary because
64     // the below tests depend on the specific numbering of the
65     // parameter blocks.
66     problem_.AddParameterBlock(x_, 3);
67     problem_.AddParameterBlock(y_, 4);
68     problem_.AddParameterBlock(z_, 5);
69     problem_.AddParameterBlock(w_, 6);
70 
71     problem_.AddResidualBlock(new DummyCostFunction<2, 3>, NULL, x_);
72     problem_.AddResidualBlock(new DummyCostFunction<6, 5, 4>, NULL, z_, y_);
73     problem_.AddResidualBlock(new DummyCostFunction<3, 3, 5>, NULL, x_, z_);
74     problem_.AddResidualBlock(new DummyCostFunction<7, 5, 3>, NULL, z_, x_);
75     problem_.AddResidualBlock(new DummyCostFunction<1, 5, 3, 6>, NULL,
76                               z_, x_, w_);
77   }
78 
79   ProblemImpl problem_;
80   double x_[3], y_[4], z_[5], w_[6];
81 };
82 
TEST_F(SchurOrderingTest,NoFixed)83 TEST_F(SchurOrderingTest, NoFixed) {
84   const Program& program = problem_.program();
85   const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
86   scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
87 
88   const VertexSet& vertices = graph->vertices();
89   EXPECT_EQ(vertices.size(), 4);
90 
91   for (int i = 0; i < 4; ++i) {
92     EXPECT_TRUE(vertices.find(parameter_blocks[i]) != vertices.end());
93   }
94 
95   {
96     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[0]);
97     EXPECT_EQ(neighbors.size(), 2);
98     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
99     EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
100   }
101 
102   {
103     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[1]);
104     EXPECT_EQ(neighbors.size(), 1);
105     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
106   }
107 
108   {
109     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[2]);
110     EXPECT_EQ(neighbors.size(), 3);
111     EXPECT_TRUE(neighbors.find(parameter_blocks[0]) != neighbors.end());
112     EXPECT_TRUE(neighbors.find(parameter_blocks[1]) != neighbors.end());
113     EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
114   }
115 
116   {
117     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[3]);
118     EXPECT_EQ(neighbors.size(), 2);
119     EXPECT_TRUE(neighbors.find(parameter_blocks[0]) != neighbors.end());
120     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
121   }
122 }
123 
TEST_F(SchurOrderingTest,AllFixed)124 TEST_F(SchurOrderingTest, AllFixed) {
125   problem_.SetParameterBlockConstant(x_);
126   problem_.SetParameterBlockConstant(y_);
127   problem_.SetParameterBlockConstant(z_);
128   problem_.SetParameterBlockConstant(w_);
129 
130   const Program& program = problem_.program();
131   scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
132   EXPECT_EQ(graph->vertices().size(), 0);
133 }
134 
TEST_F(SchurOrderingTest,OneFixed)135 TEST_F(SchurOrderingTest, OneFixed) {
136   problem_.SetParameterBlockConstant(x_);
137 
138   const Program& program = problem_.program();
139   const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
140   scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
141 
142   const VertexSet& vertices = graph->vertices();
143 
144   EXPECT_EQ(vertices.size(), 3);
145   EXPECT_TRUE(vertices.find(parameter_blocks[0]) == vertices.end());
146 
147   for (int i = 1; i < 3; ++i) {
148     EXPECT_TRUE(vertices.find(parameter_blocks[i]) != vertices.end());
149   }
150 
151   {
152     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[1]);
153     EXPECT_EQ(neighbors.size(), 1);
154     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
155   }
156 
157   {
158     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[2]);
159     EXPECT_EQ(neighbors.size(), 2);
160     EXPECT_TRUE(neighbors.find(parameter_blocks[1]) != neighbors.end());
161     EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
162   }
163 
164   {
165     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[3]);
166     EXPECT_EQ(neighbors.size(), 1);
167     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
168   }
169 
170   // The constant parameter block is at the end.
171   vector<ParameterBlock*> ordering;
172   ComputeSchurOrdering(program, &ordering);
173   EXPECT_EQ(ordering.back(), parameter_blocks[0]);
174 }
175 
176 }  // namespace internal
177 }  // namespace ceres
178