179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Ceres Solver - A fast non-linear least squares minimizer 279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Copyright 2014 Google Inc. All rights reserved. 379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// http://code.google.com/p/ceres-solver/ 479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Redistribution and use in source and binary forms, with or without 679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// modification, are permitted provided that the following conditions are met: 779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Redistributions of source code must retain the above copyright notice, 979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// this list of conditions and the following disclaimer. 1079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Redistributions in binary form must reproduce the above copyright notice, 1179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// this list of conditions and the following disclaimer in the documentation 1279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// and/or other materials provided with the distribution. 1379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Neither the name of Google Inc. nor the names of its contributors may be 1479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// used to endorse or promote products derived from this software without 1579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// specific prior written permission. 1679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 1779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 1879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 1979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 2079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 2179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 2279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 2379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 2479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 2579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 2679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 2779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// POSSIBILITY OF SUCH DAMAGE. 2879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 2979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Author: sameeragarwal@google.com (Sameer Agarwal) 3079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/reorder_program.h" 3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/parameter_block.h" 3479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/problem_impl.h" 3579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/program.h" 3679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/sized_cost_function.h" 3779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/solver.h" 3879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "gtest/gtest.h" 4079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 4179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeznamespace ceres { 4279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeznamespace internal { 4379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 4479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Templated base class for the CostFunction signatures. 4579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztemplate <int kNumResiduals, int N0, int N1, int N2> 4679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezclass MockCostFunctionBase : public 4779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezSizedCostFunction<kNumResiduals, N0, N1, N2> { 4879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez public: 4979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez virtual bool Evaluate(double const* const* parameters, 5079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* residuals, 5179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double** jacobians) const { 5279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Do nothing. This is never called. 5379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 5479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 5579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 5679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 5779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezclass UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {}; 5879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezclass BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {}; 5979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezclass TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {}; 6079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 6179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTEST(_, ReorderResidualBlockNormalFunction) { 6279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ProblemImpl problem; 6379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double x; 6479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double y; 6579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double z; 6679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 6779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&x, 1); 6879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&y, 1); 6979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&z, 1); 7079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); 7279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); 7379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); 7479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z); 7579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); 7679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); 7779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering; 7979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering->AddElementToGroup(&x, 0); 8079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering->AddElementToGroup(&y, 0); 8179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering->AddElementToGroup(&z, 1); 8279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 8379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Solver::Options options; 8479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options.linear_solver_type = DENSE_SCHUR; 8579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options.linear_solver_ordering.reset(linear_solver_ordering); 8679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 8779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<ResidualBlock*>& residual_blocks = 8879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.program().residual_blocks(); 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<ResidualBlock*> expected_residual_blocks; 9179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // This is a bit fragile, but it serves the purpose. We know the 9379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // bucketing algorithm that the reordering function uses, so we 9479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // expect the order for residual blocks for each e_block to be 9579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // filled in reverse. 9679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez expected_residual_blocks.push_back(residual_blocks[4]); 9779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez expected_residual_blocks.push_back(residual_blocks[1]); 9879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez expected_residual_blocks.push_back(residual_blocks[0]); 9979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez expected_residual_blocks.push_back(residual_blocks[5]); 10079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez expected_residual_blocks.push_back(residual_blocks[2]); 10179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez expected_residual_blocks.push_back(residual_blocks[3]); 10279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 10379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Program* program = problem.mutable_program(); 10479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program->SetParameterOffsetsAndIndex(); 10579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 10679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string message; 10779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_TRUE(LexicographicallyOrderResidualBlocks( 10879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 2, 10979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.mutable_program(), 11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &message)); 11179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size()); 11279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < expected_residual_blocks.size(); ++i) { 11379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]); 11479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 11579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 11679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 11779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTEST(_, ApplyOrderingOrderingTooSmall) { 11879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ProblemImpl problem; 11979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double x; 12079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double y; 12179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double z; 12279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 12379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&x, 1); 12479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&y, 1); 12579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&z, 1); 12679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 12779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ParameterBlockOrdering linear_solver_ordering; 12879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering.AddElementToGroup(&x, 0); 12979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering.AddElementToGroup(&y, 1); 13079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 13179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Program program(problem.program()); 13279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string message; 13379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_FALSE(ApplyOrdering(problem.parameter_map(), 13479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering, 13579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &program, 13679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &message)); 13779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 13879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 13979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTEST(_, ApplyOrderingNormal) { 14079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ProblemImpl problem; 14179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double x; 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double y; 14379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double z; 14479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&x, 1); 14679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&y, 1); 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddParameterBlock(&z, 1); 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 14979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ParameterBlockOrdering linear_solver_ordering; 15079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering.AddElementToGroup(&x, 0); 15179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering.AddElementToGroup(&y, 2); 15279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering.AddElementToGroup(&z, 1); 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Program* program = problem.mutable_program(); 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string message; 15679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 15779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_TRUE(ApplyOrdering(problem.parameter_map(), 15879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez linear_solver_ordering, 15979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program, 16079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &message)); 16179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); 16279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 16379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(parameter_blocks.size(), 3); 16479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(parameter_blocks[0]->user_state(), &x); 16579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(parameter_blocks[1]->user_state(), &z); 16679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(parameter_blocks[2]->user_state(), &y); 16779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 16879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 16979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} // namespace internal 17079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} // namespace ceres 171