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 (at) 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> { 53 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 : 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 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 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 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
