internal/ceres/visibility_based_preconditioner_test.cc

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