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