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 #ifndef CERES_NO_SUITESPARSE
     32 
     33 #include "ceres/visibility_based_preconditioner.h"
     34 
     35 #include "Eigen/Dense"
     36 #include "ceres/block_random_access_dense_matrix.h"
     37 #include "ceres/block_random_access_sparse_matrix.h"
     38 #include "ceres/block_sparse_matrix.h"
     39 #include "ceres/casts.h"
     40 #include "ceres/collections_port.h"
     41 #include "ceres/file.h"
     42 #include "ceres/internal/eigen.h"
     43 #include "ceres/internal/scoped_ptr.h"
     44 #include "ceres/linear_least_squares_problems.h"
     45 #include "ceres/schur_eliminator.h"
     46 #include "ceres/stringprintf.h"
     47 #include "ceres/types.h"
     48 #include "ceres/test_util.h"
     49 #include "glog/logging.h"
     50 #include "gtest/gtest.h"
     51 
     52 namespace ceres {
     53 namespace internal {
     54 
     55 using testing::AssertionResult;
     56 using testing::AssertionSuccess;
     57 using testing::AssertionFailure;
     58 
     59 static const double kTolerance = 1e-12;
     60 
     61 class VisibilityBasedPreconditionerTest : public ::testing::Test {
     62  public:
     63   static const int kCameraSize = 9;
     64 
     65  protected:
     66   void SetUp() {
     67     string input_file = TestFileAbsolutePath("problem-6-1384-000.lsqp");
     68 
     69     scoped_ptr<LinearLeastSquaresProblem> problem(
     70         CHECK_NOTNULL(CreateLinearLeastSquaresProblemFromFile(input_file)));
     71     A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
     72     b_.reset(problem->b.release());
     73     D_.reset(problem->D.release());
     74 
     75     const CompressedRowBlockStructure* bs =
     76         CHECK_NOTNULL(A_->block_structure());
     77     const int num_col_blocks = bs->cols.size();
     78 
     79     num_cols_ = A_->num_cols();
     80     num_rows_ = A_->num_rows();
     81     num_eliminate_blocks_ = problem->num_eliminate_blocks;
     82     num_camera_blocks_ = num_col_blocks - num_eliminate_blocks_;
     83     options_.elimination_groups.push_back(num_eliminate_blocks_);
     84     options_.elimination_groups.push_back(
     85         A_->block_structure()->cols.size() - num_eliminate_blocks_);
     86 
     87     vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0);
     88     for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
     89       blocks[i - num_eliminate_blocks_] = bs->cols[i].size;
     90     }
     91 
     92     // The input matrix is a real jacobian and fairly poorly
     93     // conditioned. Setting D to a large constant makes the normal
     94     // equations better conditioned and makes the tests below better
     95     // conditioned.
     96     VectorRef(D_.get(), num_cols_).setConstant(10.0);
     97 
     98     schur_complement_.reset(new BlockRandomAccessDenseMatrix(blocks));
     99     Vector rhs(schur_complement_->num_rows());
    100 
    101     scoped_ptr<SchurEliminatorBase> eliminator;
    102     eliminator.reset(SchurEliminatorBase::Create(options_));
    103     eliminator->Init(num_eliminate_blocks_, bs);
    104     eliminator->Eliminate(A_.get(), b_.get(), D_.get(),
    105                           schur_complement_.get(), rhs.data());
    106   }
    107 
    108 
    109   AssertionResult IsSparsityStructureValid() {
    110     preconditioner_->InitStorage(*A_->block_structure());
    111     const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
    112     const vector<int>& cluster_membership = get_cluster_membership();
    113 
    114     for (int i = 0; i < num_camera_blocks_; ++i) {
    115       for (int j = i; j < num_camera_blocks_; ++j) {
    116         if (cluster_pairs.count(make_pair(cluster_membership[i],
    117                                           cluster_membership[j]))) {
    118           if (!IsBlockPairInPreconditioner(i, j)) {
    119             return AssertionFailure()
    120                 << "block pair (" << i << "," << j << "missing";
    121           }
    122         } else {
    123           if (IsBlockPairInPreconditioner(i, j)) {
    124             return AssertionFailure()
    125                 << "block pair (" << i << "," << j << "should not be present";
    126           }
    127         }
    128       }
    129     }
    130     return AssertionSuccess();
    131   }
    132 
    133   AssertionResult PreconditionerValuesMatch() {
    134     preconditioner_->Update(*A_, D_.get());
    135     const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
    136     const BlockRandomAccessSparseMatrix* m = get_m();
    137     Matrix preconditioner_matrix;
    138     m->matrix()->ToDenseMatrix(&preconditioner_matrix);
    139     ConstMatrixRef full_schur_complement(schur_complement_->values(),
    140                                          m->num_rows(),
    141                                          m->num_rows());
    142     const int num_clusters = get_num_clusters();
    143     const int kDiagonalBlockSize =
    144         kCameraSize * num_camera_blocks_ / num_clusters;
    145 
    146     for (int i = 0; i < num_clusters; ++i) {
    147       for (int j = i; j < num_clusters; ++j) {
    148         double diff = 0.0;
    149         if (cluster_pairs.count(make_pair(i, j))) {
    150           diff =
    151               (preconditioner_matrix.block(kDiagonalBlockSize * i,
    152                                            kDiagonalBlockSize * j,
    153                                            kDiagonalBlockSize,
    154                                            kDiagonalBlockSize) -
    155                full_schur_complement.block(kDiagonalBlockSize * i,
    156                                            kDiagonalBlockSize * j,
    157                                            kDiagonalBlockSize,
    158                                            kDiagonalBlockSize)).norm();
    159         } else {
    160           diff = preconditioner_matrix.block(kDiagonalBlockSize * i,
    161                                              kDiagonalBlockSize * j,
    162                                              kDiagonalBlockSize,
    163                                              kDiagonalBlockSize).norm();
    164         }
    165         if (diff > kTolerance) {
    166           return AssertionFailure()
    167               << "Preconditioner block " << i << " " << j << " differs "
    168               << "from expected value by " << diff;
    169         }
    170       }
    171     }
    172     return AssertionSuccess();
    173   }
    174 
    175   // Accessors
    176   int get_num_blocks() { return preconditioner_->num_blocks_; }
    177 
    178   int get_num_clusters() { return preconditioner_->num_clusters_; }
    179   int* get_mutable_num_clusters() { return &preconditioner_->num_clusters_; }
    180 
    181   const vector<int>& get_block_size() {
    182     return preconditioner_->block_size_; }
    183 
    184   vector<int>* get_mutable_block_size() {
    185     return &preconditioner_->block_size_; }
    186 
    187   const vector<int>& get_cluster_membership() {
    188     return preconditioner_->cluster_membership_;
    189   }
    190 
    191   vector<int>* get_mutable_cluster_membership() {
    192     return &preconditioner_->cluster_membership_;
    193   }
    194 
    195   const set<pair<int, int> >& get_block_pairs() {
    196     return preconditioner_->block_pairs_;
    197   }
    198 
    199   set<pair<int, int> >* get_mutable_block_pairs() {
    200     return &preconditioner_->block_pairs_;
    201   }
    202 
    203   const HashSet<pair<int, int> >& get_cluster_pairs() {
    204     return preconditioner_->cluster_pairs_;
    205   }
    206 
    207   HashSet<pair<int, int> >* get_mutable_cluster_pairs() {
    208     return &preconditioner_->cluster_pairs_;
    209   }
    210 
    211   bool IsBlockPairInPreconditioner(const int block1, const int block2) {
    212     return preconditioner_->IsBlockPairInPreconditioner(block1, block2);
    213   }
    214 
    215   bool IsBlockPairOffDiagonal(const int block1, const int block2) {
    216     return preconditioner_->IsBlockPairOffDiagonal(block1, block2);
    217   }
    218 
    219   const BlockRandomAccessSparseMatrix* get_m() {
    220     return preconditioner_->m_.get();
    221   }
    222 
    223   int num_rows_;
    224   int num_cols_;
    225   int num_eliminate_blocks_;
    226   int num_camera_blocks_;
    227 
    228   scoped_ptr<BlockSparseMatrix> A_;
    229   scoped_array<double> b_;
    230   scoped_array<double> D_;
    231 
    232   LinearSolver::Options options_;
    233   scoped_ptr<VisibilityBasedPreconditioner> preconditioner_;
    234   scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_;
    235 };
    236 
    237 #ifndef CERES_NO_PROTOCOL_BUFFERS
    238 TEST_F(VisibilityBasedPreconditionerTest, SchurJacobiStructure) {
    239   options_.preconditioner_type = SCHUR_JACOBI;
    240   preconditioner_.reset(
    241       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
    242   EXPECT_EQ(get_num_blocks(), num_camera_blocks_);
    243   EXPECT_EQ(get_num_clusters(), num_camera_blocks_);
    244   for (int i = 0; i < num_camera_blocks_; ++i) {
    245     for (int j = 0; j < num_camera_blocks_; ++j) {
    246       const string msg = StringPrintf("Camera pair: %d %d", i, j);
    247       SCOPED_TRACE(msg);
    248       if (i == j) {
    249         EXPECT_TRUE(IsBlockPairInPreconditioner(i, j));
    250         EXPECT_FALSE(IsBlockPairOffDiagonal(i, j));
    251       } else {
    252         EXPECT_FALSE(IsBlockPairInPreconditioner(i, j));
    253         EXPECT_TRUE(IsBlockPairOffDiagonal(i, j));
    254       }
    255     }
    256   }
    257 }
    258 
    259 TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) {
    260   options_.preconditioner_type = CLUSTER_JACOBI;
    261   preconditioner_.reset(
    262       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
    263 
    264   // Override the clustering to be a single clustering containing all
    265   // the cameras.
    266   vector<int>& cluster_membership = *get_mutable_cluster_membership();
    267   for (int i = 0; i < num_camera_blocks_; ++i) {
    268     cluster_membership[i] = 0;
    269   }
    270 
    271   *get_mutable_num_clusters() = 1;
    272 
    273   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
    274   cluster_pairs.clear();
    275   cluster_pairs.insert(make_pair(0, 0));
    276 
    277   EXPECT_TRUE(IsSparsityStructureValid());
    278   EXPECT_TRUE(PreconditionerValuesMatch());
    279 
    280   // Multiplication by the inverse of the preconditioner.
    281   const int num_rows = schur_complement_->num_rows();
    282   ConstMatrixRef full_schur_complement(schur_complement_->values(),
    283                                        num_rows,
    284                                        num_rows);
    285   Vector x(num_rows);
    286   Vector y(num_rows);
    287   Vector z(num_rows);
    288 
    289   for (int i = 0; i < num_rows; ++i) {
    290     x.setZero();
    291     y.setZero();
    292     z.setZero();
    293     x[i] = 1.0;
    294     preconditioner_->RightMultiply(x.data(), y.data());
    295     z = full_schur_complement
    296         .selfadjointView<Eigen::Upper>()
    297         .ldlt().solve(x);
    298     double max_relative_difference =
    299         ((y - z).array() / z.array()).matrix().lpNorm<Eigen::Infinity>();
    300     EXPECT_NEAR(max_relative_difference, 0.0, kTolerance);
    301   }
    302 }
    303 
    304 
    305 
    306 TEST_F(VisibilityBasedPreconditionerTest, ClusterJacobi) {
    307   options_.preconditioner_type = CLUSTER_JACOBI;
    308   preconditioner_.reset(
    309       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
    310 
    311   // Override the clustering to be equal number of cameras.
    312   vector<int>& cluster_membership = *get_mutable_cluster_membership();
    313   cluster_membership.resize(num_camera_blocks_);
    314   static const int kNumClusters = 3;
    315 
    316   for (int i = 0; i < num_camera_blocks_; ++i) {
    317     cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
    318   }
    319   *get_mutable_num_clusters() = kNumClusters;
    320 
    321   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
    322   cluster_pairs.clear();
    323   for (int i = 0; i < kNumClusters; ++i) {
    324     cluster_pairs.insert(make_pair(i, i));
    325   }
    326 
    327   EXPECT_TRUE(IsSparsityStructureValid());
    328   EXPECT_TRUE(PreconditionerValuesMatch());
    329 }
    330 
    331 
    332 TEST_F(VisibilityBasedPreconditionerTest, ClusterTridiagonal) {
    333   options_.preconditioner_type = CLUSTER_TRIDIAGONAL;
    334   preconditioner_.reset(
    335       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
    336   static const int kNumClusters = 3;
    337 
    338   // Override the clustering to be 3 clusters.
    339   vector<int>& cluster_membership = *get_mutable_cluster_membership();
    340   cluster_membership.resize(num_camera_blocks_);
    341   for (int i = 0; i < num_camera_blocks_; ++i) {
    342     cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
    343   }
    344   *get_mutable_num_clusters() = kNumClusters;
    345 
    346   // Spanning forest has structure 0-1 2
    347   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
    348   cluster_pairs.clear();
    349   for (int i = 0; i < kNumClusters; ++i) {
    350     cluster_pairs.insert(make_pair(i, i));
    351   }
    352   cluster_pairs.insert(make_pair(0, 1));
    353 
    354   EXPECT_TRUE(IsSparsityStructureValid());
    355   EXPECT_TRUE(PreconditionerValuesMatch());
    356 }
    357 #endif  // CERES_NO_PROTOCOL_BUFFERS
    358 
    359 }  // namespace internal
    360 }  // namespace ceres
    361 
    362 #endif  // CERES_NO_SUITESPARSE
    363