11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://code.google.com/p/ceres-solver/ 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Redistribution and use in source and binary forms, with or without 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// modification, are permitted provided that the following conditions are met: 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions of source code must retain the above copyright notice, 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer. 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions in binary form must reproduce the above copyright notice, 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer in the documentation 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and/or other materials provided with the distribution. 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Neither the name of Google Inc. nor the names of its contributors may be 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// used to endorse or promote products derived from this software without 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// specific prior written permission. 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// POSSIBILITY OF SUCH DAMAGE. 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: sameeragarwal@google.com (Sameer Agarwal) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/schur_ordering.h" 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cstddef> 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gtest/gtest.h" 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/collections_port.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/graph.h" 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/problem_impl.h" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/program.h" 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/stl_util.h" 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/cost_function.h" 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/scoped_ptr.h" 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/sized_cost_function.h" 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtypedef Graph<ParameterBlock*> HessianGraph; 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtypedef HashSet<ParameterBlock*> VertexSet; 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <int M, int N1 = 0, int N2 = 0, int N3 = 0> 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass DummyCostFunction: public SizedCostFunction<M, N1, N2, N3> { 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual bool Evaluate(double const* const* parameters, 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* residuals, 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double** jacobians) const { 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass SchurOrderingTest : public ::testing::Test { 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling protected : 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual void SetUp() { 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The explicit calls to AddParameterBlock are necessary because 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the below tests depend on the specific numbering of the 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // parameter blocks. 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddParameterBlock(x_, 3); 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddParameterBlock(y_, 4); 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddParameterBlock(z_, 5); 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddParameterBlock(w_, 6); 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddResidualBlock(new DummyCostFunction<2, 3>, NULL, x_); 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddResidualBlock(new DummyCostFunction<6, 5, 4>, NULL, z_, y_); 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddResidualBlock(new DummyCostFunction<3, 3, 5>, NULL, x_, z_); 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddResidualBlock(new DummyCostFunction<7, 5, 3>, NULL, z_, x_); 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.AddResidualBlock(new DummyCostFunction<1, 5, 3, 6>, NULL, 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling z_, x_, w_); 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ProblemImpl problem_; 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x_[3], y_[4], z_[5], w_[6]; 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(SchurOrderingTest, NoFixed) { 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const Program& program = problem_.program(); 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<HessianGraph> graph(CreateHessianGraph(program)); 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& vertices = graph->vertices(); 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(vertices.size(), 4); 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 4; ++i) { 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(vertices.find(parameter_blocks[i]) != vertices.end()); 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& neighbors = graph->Neighbors(parameter_blocks[0]); 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(neighbors.size(), 2); 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end()); 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end()); 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& neighbors = graph->Neighbors(parameter_blocks[1]); 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(neighbors.size(), 1); 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end()); 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& neighbors = graph->Neighbors(parameter_blocks[2]); 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(neighbors.size(), 3); 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[0]) != neighbors.end()); 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[1]) != neighbors.end()); 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end()); 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& neighbors = graph->Neighbors(parameter_blocks[3]); 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(neighbors.size(), 2); 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[0]) != neighbors.end()); 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end()); 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(SchurOrderingTest, AllFixed) { 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.SetParameterBlockConstant(x_); 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.SetParameterBlockConstant(y_); 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.SetParameterBlockConstant(z_); 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.SetParameterBlockConstant(w_); 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const Program& program = problem_.program(); 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<HessianGraph> graph(CreateHessianGraph(program)); 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(graph->vertices().size(), 0); 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(SchurOrderingTest, OneFixed) { 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem_.SetParameterBlockConstant(x_); 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const Program& program = problem_.program(); 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<HessianGraph> graph(CreateHessianGraph(program)); 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& vertices = graph->vertices(); 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(vertices.size(), 3); 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(vertices.find(parameter_blocks[0]) == vertices.end()); 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 1; i < 3; ++i) { 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(vertices.find(parameter_blocks[i]) != vertices.end()); 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& neighbors = graph->Neighbors(parameter_blocks[1]); 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(neighbors.size(), 1); 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end()); 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& neighbors = graph->Neighbors(parameter_blocks[2]); 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(neighbors.size(), 2); 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[1]) != neighbors.end()); 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end()); 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling { 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const VertexSet& neighbors = graph->Neighbors(parameter_blocks[3]); 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(neighbors.size(), 1); 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end()); 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The constant parameter block is at the end. 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<ParameterBlock*> ordering; 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ComputeSchurOrdering(program, &ordering); 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(ordering.back(), parameter_blocks[0]); 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 178