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