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