visibility_based_preconditioner_test.cc revision 399f7d09e0c45af54b77b4ab9508d6f23759b927
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#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 52namespace ceres { 53namespace 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