internal/ceres/block_random_access_diagonal_matrix.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: sameeragarwal@google.com (Sameer Agarwal)
  30
  31 #include "ceres/block_random_access_diagonal_matrix.h"
  32
  33 #include <algorithm>
  34 #include <set>
  35 #include <utility>
  36 #include <vector>
  37 #include "Eigen/Dense"
  38 #include "ceres/internal/port.h"
  39 #include "ceres/internal/scoped_ptr.h"
  40 #include "ceres/stl_util.h"
  41 #include "ceres/triplet_sparse_matrix.h"
  42 #include "ceres/types.h"
  43 #include "glog/logging.h"
  44
  45 namespace ceres {
  46 namespace internal {
  47
  48 using std::vector;
  49
  50 // TODO(sameeragarwal): Drop the dependence on TripletSparseMatrix.
  51
  52 BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix(
  53     const vector<int>& blocks)
  54     : blocks_(blocks) {
  55   // Build the row/column layout vector and count the number of scalar
  56   // rows/columns.
  57   int num_cols = 0;
  58   int num_nonzeros = 0;
  59   vector<int> block_positions;
  60   for (int i = 0; i < blocks_.size(); ++i) {
  61     block_positions.push_back(num_cols);
  62     num_cols += blocks_[i];
  63     num_nonzeros += blocks_[i] * blocks_[i];
  64   }
  65
  66   VLOG(1) << "Matrix Size [" << num_cols
  67           << "," << num_cols
  68           << "] " << num_nonzeros;
  69
  70   tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros));
  71   tsm_->set_num_nonzeros(num_nonzeros);
  72   int* rows = tsm_->mutable_rows();
  73   int* cols = tsm_->mutable_cols();
  74   double* values = tsm_->mutable_values();
  75
  76   int pos = 0;
  77   for (int i = 0; i < blocks_.size(); ++i) {
  78     const int block_size = blocks_[i];
  79     layout_.push_back(new CellInfo(values + pos));
  80     const int block_begin = block_positions[i];
  81     for (int r = 0; r < block_size; ++r) {
  82       for (int c = 0; c < block_size; ++c, ++pos) {
  83         rows[pos] = block_begin + r;
  84         cols[pos] = block_begin + c;
  85       }
  86     }
  87   }
  88 }
  89
  90 // Assume that the user does not hold any locks on any cell blocks
  91 // when they are calling SetZero.
  92 BlockRandomAccessDiagonalMatrix::~BlockRandomAccessDiagonalMatrix() {
  93   STLDeleteContainerPointers(layout_.begin(), layout_.end());
  94 }
  95
  96 CellInfo* BlockRandomAccessDiagonalMatrix::GetCell(int row_block_id,
  97                                                    int col_block_id,
  98                                                    int* row,
  99                                                    int* col,
 100                                                    int* row_stride,
 101                                                    int* col_stride) {
 102   if (row_block_id != col_block_id) {
 103     return NULL;
 104   }
 105   const int stride = blocks_[row_block_id];
 106
 107   // Each cell is stored contiguously as its own little dense matrix.
 108   *row = 0;
 109   *col = 0;
 110   *row_stride = stride;
 111   *col_stride = stride;
 112   return layout_[row_block_id];
 113 }
 114
 115 // Assume that the user does not hold any locks on any cell blocks
 116 // when they are calling SetZero.
 117 void BlockRandomAccessDiagonalMatrix::SetZero() {
 118   if (tsm_->num_nonzeros()) {
 119     VectorRef(tsm_->mutable_values(),
 120               tsm_->num_nonzeros()).setZero();
 121   }
 122 }
 123
 124 void BlockRandomAccessDiagonalMatrix::Invert() {
 125   double* values = tsm_->mutable_values();
 126   for (int i = 0; i < blocks_.size(); ++i) {
 127     const int block_size = blocks_[i];
 128     MatrixRef block(values, block_size, block_size);
 129     block =
 130         block
 131         .selfadjointView<Eigen::Upper>()
 132         .llt()
 133         .solve(Matrix::Identity(block_size, block_size));
 134     values += block_size * block_size;
 135   }
 136 }
 137
 138 void BlockRandomAccessDiagonalMatrix::RightMultiply(const double* x,
 139                                                     double* y) const {
 140   CHECK_NOTNULL(x);
 141   CHECK_NOTNULL(y);
 142   const double* values = tsm_->values();
 143   for (int i = 0; i < blocks_.size(); ++i) {
 144     const int block_size = blocks_[i];
 145     ConstMatrixRef block(values, block_size, block_size);
 146     VectorRef(y, block_size).noalias() += block * ConstVectorRef(x, block_size);
 147     x += block_size;
 148     y += block_size;
 149     values += block_size * block_size;
 150   }
 151 }
 152
 153 }  // namespace internal
 154 }  // namespace ceres