1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 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: keir (at) google.com (Keir Mierle) 30 31 #include "ceres/block_jacobi_preconditioner.h" 32 33 #include "Eigen/Cholesky" 34 #include "ceres/block_sparse_matrix.h" 35 #include "ceres/block_structure.h" 36 #include "ceres/casts.h" 37 #include "ceres/integral_types.h" 38 #include "ceres/internal/eigen.h" 39 40 namespace ceres { 41 namespace internal { 42 43 BlockJacobiPreconditioner::BlockJacobiPreconditioner(const LinearOperator& A) 44 : num_rows_(A.num_rows()), 45 block_structure_( 46 *(down_cast<const BlockSparseMatrix*>(&A)->block_structure())) { 47 // Calculate the amount of storage needed. 48 int storage_needed = 0; 49 for (int c = 0; c < block_structure_.cols.size(); ++c) { 50 int size = block_structure_.cols[c].size; 51 storage_needed += size * size; 52 } 53 54 // Size the offsets and storage. 55 blocks_.resize(block_structure_.cols.size()); 56 block_storage_.resize(storage_needed); 57 58 // Put pointers to the storage in the offsets. 59 double* block_cursor = &block_storage_[0]; 60 for (int c = 0; c < block_structure_.cols.size(); ++c) { 61 int size = block_structure_.cols[c].size; 62 blocks_[c] = block_cursor; 63 block_cursor += size * size; 64 } 65 } 66 67 BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { 68 } 69 70 void BlockJacobiPreconditioner::Update(const LinearOperator& matrix, const double* D) { 71 const BlockSparseMatrix& A = *(down_cast<const BlockSparseMatrix*>(&matrix)); 72 const CompressedRowBlockStructure* bs = A.block_structure(); 73 74 // Compute the diagonal blocks by block inner products. 75 std::fill(block_storage_.begin(), block_storage_.end(), 0.0); 76 for (int r = 0; r < bs->rows.size(); ++r) { 77 const int row_block_size = bs->rows[r].block.size; 78 const vector<Cell>& cells = bs->rows[r].cells; 79 const double* row_values = A.RowBlockValues(r); 80 for (int c = 0; c < cells.size(); ++c) { 81 const int col_block_size = bs->cols[cells[c].block_id].size; 82 ConstMatrixRef m(row_values + cells[c].position, 83 row_block_size, 84 col_block_size); 85 86 MatrixRef(blocks_[cells[c].block_id], 87 col_block_size, 88 col_block_size).noalias() += m.transpose() * m; 89 90 // TODO(keir): Figure out when the below expression is actually faster 91 // than doing the full rank update. The issue is that for smaller sizes, 92 // the rankUpdate() function is slower than the full product done above. 93 // 94 // On the typical bundling problems, the above product is ~5% faster. 95 // 96 // MatrixRef(blocks_[cells[c].block_id], 97 // col_block_size, 98 // col_block_size).selfadjointView<Eigen::Upper>().rankUpdate(m); 99 // 100 } 101 } 102 103 // Add the diagonal and invert each block. 104 for (int c = 0; c < bs->cols.size(); ++c) { 105 const int size = block_structure_.cols[c].size; 106 const int position = block_structure_.cols[c].position; 107 MatrixRef block(blocks_[c], size, size); 108 109 if (D != NULL) { 110 block.diagonal() += ConstVectorRef(D + position, size).array().square().matrix(); 111 } 112 113 block = block.selfadjointView<Eigen::Upper>() 114 .ldlt() 115 .solve(Matrix::Identity(size, size)); 116 } 117 } 118 119 void BlockJacobiPreconditioner::RightMultiply(const double* x, double* y) const { 120 for (int c = 0; c < block_structure_.cols.size(); ++c) { 121 const int size = block_structure_.cols[c].size; 122 const int position = block_structure_.cols[c].position; 123 ConstMatrixRef D(blocks_[c], size, size); 124 ConstVectorRef x_block(x + position, size); 125 VectorRef y_block(y + position, size); 126 y_block += D * x_block; 127 } 128 } 129 130 void BlockJacobiPreconditioner::LeftMultiply(const double* x, double* y) const { 131 RightMultiply(x, y); 132 } 133 134 } // namespace internal 135 } // namespace ceres 136
