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