10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/implicit_schur_complement.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstddef> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Dense" 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_dense_matrix.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_sparse_matrix.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/casts.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_least_squares_problems.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_solver.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/schur_eliminator.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/triplet_sparse_matrix.h" 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gtest/gtest.h" 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::AssertionResult; 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kEpsilon = 1e-14; 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass ImplicitSchurComplementTest : public ::testing::Test { 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong protected : 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void SetUp() { 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearLeastSquaresProblem> problem( 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CreateLinearLeastSquaresProblemFromId(2)); 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(problem.get()); 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release())); 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong b_.reset(problem->b.release()); 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong D_.reset(problem->D.release()); 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = A_->num_cols(); 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = A_->num_rows(); 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_eliminate_blocks_ = problem->num_eliminate_blocks; 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void ReducedLinearSystemAndSolution(double* D, 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix* lhs, 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector* rhs, 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector* solution) { 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = A_->block_structure(); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_col_blocks = bs->cols.size(); 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0); 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong blocks[i - num_eliminate_blocks_] = bs->cols[i].size; 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockRandomAccessDenseMatrix blhs(blocks); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_schur_rows = blhs.num_rows(); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearSolver::Options options; 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options.elimination_groups.push_back(num_eliminate_blocks_); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options.type = DENSE_SCHUR; 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<SchurEliminatorBase> eliminator( 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong SchurEliminatorBase::Create(options)); 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(eliminator.get()); 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator->Init(num_eliminate_blocks_, bs); 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lhs->resize(num_schur_rows, num_schur_rows); 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rhs->resize(num_schur_rows); 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator->Eliminate(A_.get(), b_.get(), D, &blhs, rhs->data()); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MatrixRef lhs_ref(blhs.mutable_values(), num_schur_rows, num_schur_rows); 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // lhs_ref is an upper triangular matrix. Construct a full version 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // of lhs_ref in lhs by transposing lhs_ref, choosing the strictly 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // lower triangular part of the matrix and adding it to lhs_ref. 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *lhs = lhs_ref; 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lhs->triangularView<Eigen::StrictlyLower>() = 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lhs_ref.triangularView<Eigen::StrictlyUpper>().transpose(); 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong solution->resize(num_cols_); 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong solution->setZero(); 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef schur_solution(solution->data() + num_cols_ - num_schur_rows, 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_schur_rows); 112399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger schur_solution = lhs->selfadjointView<Eigen::Upper>().llt().solve(*rhs); 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator->BackSubstitute(A_.get(), b_.get(), D, 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong schur_solution.data(), solution->data()); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AssertionResult TestImplicitSchurComplement(double* D) { 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix lhs; 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector rhs; 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector reference_solution; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ReducedLinearSystemAndSolution(D, &lhs, &rhs, &reference_solution); 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ImplicitSchurComplement isc(num_eliminate_blocks_, true); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong isc.Init(*A_, D, b_.get()); 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_sc_cols = lhs.cols(); 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_sc_cols; ++i) { 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector x(num_sc_cols); 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x.setZero(); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x(i) = 1.0; 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector y(num_sc_cols); 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong y = lhs * x; 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector z(num_sc_cols); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong isc.RightMultiply(x.data(), z.data()); 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The i^th column of the implicit schur complement is the same as 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the explicit schur complement. 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((y - z).norm() > kEpsilon) { 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return testing::AssertionFailure() 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Explicit and Implicit SchurComplements differ in " 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "column " << i << ". explicit: " << y.transpose() 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " implicit: " << z.transpose(); 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compare the rhs of the reduced linear system 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((isc.rhs() - rhs).norm() > kEpsilon) { 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return testing::AssertionFailure() 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Explicit and Implicit SchurComplements differ in " 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "rhs. explicit: " << rhs.transpose() 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " implicit: " << isc.rhs().transpose(); 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Reference solution to the f_block. 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vector reference_f_sol = 159399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger lhs.selfadjointView<Eigen::Upper>().llt().solve(rhs); 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Backsubstituted solution from the implicit schur solver using the 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // reference solution to the f_block. 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector sol(num_cols_); 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong isc.BackSubstitute(reference_f_sol.data(), sol.data()); 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((sol - reference_solution).norm() > kEpsilon) { 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return testing::AssertionFailure() 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Explicit and Implicit SchurComplements solutions differ. " 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "explicit: " << reference_solution.transpose() 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " implicit: " << sol.transpose(); 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return testing::AssertionSuccess(); 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows_; 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols_; 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_eliminate_blocks_; 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<BlockSparseMatrix> A_; 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_array<double> b_; 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_array<double> D_; 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Verify that the Schur Complement matrix implied by the 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ImplicitSchurComplement class matches the one explicitly computed 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// by the SchurComplement solver. 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// We do this with and without regularization to check that the 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// support for the LM diagonal is correct. 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(ImplicitSchurComplementTest, SchurMatrixValuesTest) { 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(TestImplicitSchurComplement(NULL)); 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(TestImplicitSchurComplement(D_.get())); 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 197