Home | History | Annotate | Download | only in ceres
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2010, 2011, 2012 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/implicit_schur_complement.h"
     32 
     33 #include <cstddef>
     34 #include "Eigen/Dense"
     35 #include "ceres/block_random_access_dense_matrix.h"
     36 #include "ceres/block_sparse_matrix.h"
     37 #include "ceres/casts.h"
     38 #include "ceres/internal/eigen.h"
     39 #include "ceres/internal/scoped_ptr.h"
     40 #include "ceres/linear_least_squares_problems.h"
     41 #include "ceres/linear_solver.h"
     42 #include "ceres/schur_eliminator.h"
     43 #include "ceres/triplet_sparse_matrix.h"
     44 #include "ceres/types.h"
     45 #include "glog/logging.h"
     46 #include "gtest/gtest.h"
     47 
     48 namespace ceres {
     49 namespace internal {
     50 
     51 using testing::AssertionResult;
     52 
     53 const double kEpsilon = 1e-14;
     54 
     55 class ImplicitSchurComplementTest : public ::testing::Test {
     56  protected :
     57   virtual void SetUp() {
     58     scoped_ptr<LinearLeastSquaresProblem> problem(
     59         CreateLinearLeastSquaresProblemFromId(2));
     60 
     61     CHECK_NOTNULL(problem.get());
     62     A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
     63     b_.reset(problem->b.release());
     64     D_.reset(problem->D.release());
     65 
     66     num_cols_ = A_->num_cols();
     67     num_rows_ = A_->num_rows();
     68     num_eliminate_blocks_ = problem->num_eliminate_blocks;
     69   }
     70 
     71   void ReducedLinearSystemAndSolution(double* D,
     72                                       Matrix* lhs,
     73                                       Vector* rhs,
     74                                       Vector* solution) {
     75     const CompressedRowBlockStructure* bs = A_->block_structure();
     76     const int num_col_blocks = bs->cols.size();
     77     vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0);
     78     for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
     79       blocks[i - num_eliminate_blocks_] = bs->cols[i].size;
     80     }
     81 
     82     BlockRandomAccessDenseMatrix blhs(blocks);
     83     const int num_schur_rows = blhs.num_rows();
     84 
     85     LinearSolver::Options options;
     86     options.elimination_groups.push_back(num_eliminate_blocks_);
     87     options.type = DENSE_SCHUR;
     88 
     89     scoped_ptr<SchurEliminatorBase> eliminator(
     90         SchurEliminatorBase::Create(options));
     91     CHECK_NOTNULL(eliminator.get());
     92     eliminator->Init(num_eliminate_blocks_, bs);
     93 
     94     lhs->resize(num_schur_rows, num_schur_rows);
     95     rhs->resize(num_schur_rows);
     96 
     97     eliminator->Eliminate(A_.get(), b_.get(), D, &blhs, rhs->data());
     98 
     99     MatrixRef lhs_ref(blhs.mutable_values(), num_schur_rows, num_schur_rows);
    100 
    101     // lhs_ref is an upper triangular matrix. Construct a full version
    102     // of lhs_ref in lhs by transposing lhs_ref, choosing the strictly
    103     // lower triangular part of the matrix and adding it to lhs_ref.
    104     *lhs = lhs_ref;
    105     lhs->triangularView<Eigen::StrictlyLower>() =
    106         lhs_ref.triangularView<Eigen::StrictlyUpper>().transpose();
    107 
    108     solution->resize(num_cols_);
    109     solution->setZero();
    110     VectorRef schur_solution(solution->data() + num_cols_ - num_schur_rows,
    111                              num_schur_rows);
    112     schur_solution = lhs->selfadjointView<Eigen::Upper>().llt().solve(*rhs);
    113     eliminator->BackSubstitute(A_.get(), b_.get(), D,
    114                                schur_solution.data(), solution->data());
    115   }
    116 
    117   AssertionResult TestImplicitSchurComplement(double* D) {
    118     Matrix lhs;
    119     Vector rhs;
    120     Vector reference_solution;
    121     ReducedLinearSystemAndSolution(D, &lhs, &rhs, &reference_solution);
    122 
    123     LinearSolver::Options options;
    124     options.elimination_groups.push_back(num_eliminate_blocks_);
    125     options.preconditioner_type = JACOBI;
    126     ImplicitSchurComplement isc(options);
    127     isc.Init(*A_, D, b_.get());
    128 
    129     int num_sc_cols = lhs.cols();
    130 
    131     for (int i = 0; i < num_sc_cols; ++i) {
    132       Vector x(num_sc_cols);
    133       x.setZero();
    134       x(i) = 1.0;
    135 
    136       Vector y(num_sc_cols);
    137       y = lhs * x;
    138 
    139       Vector z(num_sc_cols);
    140       isc.RightMultiply(x.data(), z.data());
    141 
    142       // The i^th column of the implicit schur complement is the same as
    143       // the explicit schur complement.
    144       if ((y - z).norm() > kEpsilon) {
    145         return testing::AssertionFailure()
    146             << "Explicit and Implicit SchurComplements differ in "
    147             << "column " << i << ". explicit: " << y.transpose()
    148             << " implicit: " << z.transpose();
    149       }
    150     }
    151 
    152     // Compare the rhs of the reduced linear system
    153     if ((isc.rhs() - rhs).norm() > kEpsilon) {
    154       return testing::AssertionFailure()
    155             << "Explicit and Implicit SchurComplements differ in "
    156             << "rhs. explicit: " << rhs.transpose()
    157             << " implicit: " << isc.rhs().transpose();
    158     }
    159 
    160     // Reference solution to the f_block.
    161     const Vector reference_f_sol =
    162         lhs.selfadjointView<Eigen::Upper>().llt().solve(rhs);
    163 
    164     // Backsubstituted solution from the implicit schur solver using the
    165     // reference solution to the f_block.
    166     Vector sol(num_cols_);
    167     isc.BackSubstitute(reference_f_sol.data(), sol.data());
    168     if ((sol - reference_solution).norm() > kEpsilon) {
    169       return testing::AssertionFailure()
    170           << "Explicit and Implicit SchurComplements solutions differ. "
    171           << "explicit: " << reference_solution.transpose()
    172           << " implicit: " << sol.transpose();
    173     }
    174 
    175     return testing::AssertionSuccess();
    176   }
    177 
    178   int num_rows_;
    179   int num_cols_;
    180   int num_eliminate_blocks_;
    181 
    182   scoped_ptr<BlockSparseMatrix> A_;
    183   scoped_array<double> b_;
    184   scoped_array<double> D_;
    185 };
    186 
    187 // Verify that the Schur Complement matrix implied by the
    188 // ImplicitSchurComplement class matches the one explicitly computed
    189 // by the SchurComplement solver.
    190 //
    191 // We do this with and without regularization to check that the
    192 // support for the LM diagonal is correct.
    193 TEST_F(ImplicitSchurComplementTest, SchurMatrixValuesTest) {
    194   EXPECT_TRUE(TestImplicitSchurComplement(NULL));
    195   EXPECT_TRUE(TestImplicitSchurComplement(D_.get()));
    196 }
    197 
    198 }  // namespace internal
    199 }  // namespace ceres
    200