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