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@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