1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2013 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 #include "ceres/schur_jacobi_preconditioner.h" 32 33 #include <utility> 34 #include <vector> 35 #include "Eigen/Dense" 36 #include "ceres/block_random_access_sparse_matrix.h" 37 #include "ceres/block_sparse_matrix.h" 38 #include "ceres/collections_port.h" 39 #include "ceres/detect_structure.h" 40 #include "ceres/internal/scoped_ptr.h" 41 #include "ceres/linear_solver.h" 42 #include "ceres/schur_eliminator.h" 43 #include "glog/logging.h" 44 45 namespace ceres { 46 namespace internal { 47 48 SchurJacobiPreconditioner::SchurJacobiPreconditioner( 49 const CompressedRowBlockStructure& bs, 50 const Preconditioner::Options& options) 51 : options_(options) { 52 CHECK_GT(options_.elimination_groups.size(), 1); 53 CHECK_GT(options_.elimination_groups[0], 0); 54 const int num_blocks = bs.cols.size() - options_.elimination_groups[0]; 55 CHECK_GT(num_blocks, 0) 56 << "Jacobian should have atleast 1 f_block for " 57 << "SCHUR_JACOBI preconditioner."; 58 59 block_size_.resize(num_blocks); 60 set<pair<int, int> > block_pairs; 61 62 int num_block_diagonal_entries = 0; 63 for (int i = 0; i < num_blocks; ++i) { 64 block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size; 65 block_pairs.insert(make_pair(i, i)); 66 num_block_diagonal_entries += block_size_[i] * block_size_[i]; 67 } 68 69 m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs)); 70 InitEliminator(bs); 71 } 72 73 SchurJacobiPreconditioner::~SchurJacobiPreconditioner() { 74 } 75 76 // Initialize the SchurEliminator. 77 void SchurJacobiPreconditioner::InitEliminator( 78 const CompressedRowBlockStructure& bs) { 79 LinearSolver::Options eliminator_options; 80 81 eliminator_options.elimination_groups = options_.elimination_groups; 82 eliminator_options.num_threads = options_.num_threads; 83 84 DetectStructure(bs, options_.elimination_groups[0], 85 &eliminator_options.row_block_size, 86 &eliminator_options.e_block_size, 87 &eliminator_options.f_block_size); 88 89 eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); 90 eliminator_->Init(options_.elimination_groups[0], &bs); 91 } 92 93 // Update the values of the preconditioner matrix and factorize it. 94 bool SchurJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, 95 const double* D) { 96 const int num_rows = m_->num_rows(); 97 CHECK_GT(num_rows, 0); 98 99 // We need a dummy rhs vector and a dummy b vector since the Schur 100 // eliminator combines the computation of the reduced camera matrix 101 // with the computation of the right hand side of that linear 102 // system. 103 // 104 // TODO(sameeragarwal): Perhaps its worth refactoring the 105 // SchurEliminator::Eliminate function to allow NULL for the rhs. As 106 // of now it does not seem to be worth the effort. 107 Vector rhs = Vector::Zero(m_->num_rows()); 108 Vector b = Vector::Zero(A.num_rows()); 109 110 // Compute a subset of the entries of the Schur complement. 111 eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); 112 return true; 113 } 114 115 void SchurJacobiPreconditioner::RightMultiply(const double* x, 116 double* y) const { 117 CHECK_NOTNULL(x); 118 CHECK_NOTNULL(y); 119 120 const double* lhs_values = 121 down_cast<BlockRandomAccessSparseMatrix*>(m_.get())->matrix()->values(); 122 123 // This loop can be easily multi-threaded with OpenMP if need be. 124 for (int i = 0; i < block_size_.size(); ++i) { 125 const int block_size = block_size_[i]; 126 ConstMatrixRef block(lhs_values, block_size, block_size); 127 128 VectorRef(y, block_size) = 129 block 130 .selfadjointView<Eigen::Upper>() 131 .llt() 132 .solve(ConstVectorRef(x, block_size)); 133 134 x += block_size; 135 y += block_size; 136 lhs_values += block_size * block_size; 137 } 138 } 139 140 int SchurJacobiPreconditioner::num_rows() const { 141 return m_->num_rows(); 142 } 143 144 } // namespace internal 145 } // namespace ceres 146
