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