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 41namespace ceres { 42namespace internal { 43 44// Templated base class for the CostFunction signatures. 45template <int kNumResiduals, int N0, int N1, int N2> 46class MockCostFunctionBase : public 47SizedCostFunction<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 57class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {}; 58class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {}; 59class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {}; 60 61TEST(_, 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 117TEST(_, 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 139TEST(_, 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