internal/ceres/block_jacobi_preconditioner.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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@google.com (Keir Mierle)
  30
  31 #include "ceres/block_jacobi_preconditioner.h"
  32
  33 #include "ceres/block_sparse_matrix.h"
  34 #include "ceres/block_structure.h"
  35 #include "ceres/block_random_access_diagonal_matrix.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(
  44     const BlockSparseMatrix& A) {
  45   const CompressedRowBlockStructure* bs = A.block_structure();
  46   std::vector<int> blocks(bs->cols.size());
  47   for (int i = 0; i < blocks.size(); ++i) {
  48     blocks[i] = bs->cols[i].size;
  49   }
  50
  51   m_.reset(new BlockRandomAccessDiagonalMatrix(blocks));
  52 }
  53
  54 BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {}
  55
  56 bool BlockJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A,
  57                                            const double* D) {
  58   const CompressedRowBlockStructure* bs = A.block_structure();
  59   const double* values = A.values();
  60   m_->SetZero();
  61   for (int i = 0; i < bs->rows.size(); ++i) {
  62     const int row_block_size = bs->rows[i].block.size;
  63     const std::vector<Cell>& cells = bs->rows[i].cells;
  64     for (int j = 0; j < cells.size(); ++j) {
  65       const int block_id = cells[j].block_id;
  66       const int col_block_size = bs->cols[block_id].size;
  67
  68       int r, c, row_stride, col_stride;
  69       CellInfo* cell_info = m_->GetCell(block_id, block_id,
  70                                         &r, &c,
  71                                         &row_stride, &col_stride);
  72       MatrixRef m(cell_info->values, row_stride, col_stride);
  73       ConstMatrixRef b(values + cells[j].position,
  74                        row_block_size,
  75                        col_block_size);
  76       m.block(r, c, col_block_size, col_block_size) += b.transpose() * b;
  77     }
  78   }
  79
  80   if (D != NULL) {
  81     // Add the diagonal.
  82     int position = 0;
  83     for (int i = 0; i < bs->cols.size(); ++i) {
  84       const int block_size = bs->cols[i].size;
  85       int r, c, row_stride, col_stride;
  86       CellInfo* cell_info = m_->GetCell(i, i,
  87                                         &r, &c,
  88                                         &row_stride, &col_stride);
  89       MatrixRef m(cell_info->values, row_stride, col_stride);
  90       m.block(r, c, block_size, block_size).diagonal() +=
  91           ConstVectorRef(D + position, block_size).array().square().matrix();
  92       position += block_size;
  93     }
  94   }
  95
  96   m_->Invert();
  97   return true;
  98 }
  99
 100 void BlockJacobiPreconditioner::RightMultiply(const double* x,
 101                                               double* y) const {
 102   m_->RightMultiply(x, y);
 103 }
 104
 105 }  // namespace internal
 106 }  // namespace ceres