internal/ceres/schur_complement_solver.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/schur_complement_solver.h"
  32
  33 #include <algorithm>
  34 #include <ctime>
  35 #include <set>
  36 #include <vector>
  37
  38 #include "Eigen/Dense"
  39 #include "Eigen/SparseCore"
  40 #include "ceres/block_random_access_dense_matrix.h"
  41 #include "ceres/block_random_access_matrix.h"
  42 #include "ceres/block_random_access_sparse_matrix.h"
  43 #include "ceres/block_sparse_matrix.h"
  44 #include "ceres/block_structure.h"
  45 #include "ceres/conjugate_gradients_solver.h"
  46 #include "ceres/detect_structure.h"
  47 #include "ceres/internal/eigen.h"
  48 #include "ceres/internal/scoped_ptr.h"
  49 #include "ceres/lapack.h"
  50 #include "ceres/linear_solver.h"
  51 #include "ceres/sparse_cholesky.h"
  52 #include "ceres/triplet_sparse_matrix.h"
  53 #include "ceres/types.h"
  54 #include "ceres/wall_time.h"
  55
  56 namespace ceres {
  57 namespace internal {
  58
  59 using std::make_pair;
  60 using std::pair;
  61 using std::set;
  62 using std::vector;
  63
  64 namespace {
  65
  66 class BlockRandomAccessSparseMatrixAdapter : public LinearOperator {
  67  public:
  68   explicit BlockRandomAccessSparseMatrixAdapter(
  69       const BlockRandomAccessSparseMatrix& m)
  70       : m_(m) {
  71   }
  72
  73   virtual ~BlockRandomAccessSparseMatrixAdapter() {}
  74
  75   // y = y + Ax;
  76   virtual void RightMultiply(const double* x, double* y) const {
  77     m_.SymmetricRightMultiply(x, y);
  78   }
  79
  80   // y = y + A'x;
  81   virtual void LeftMultiply(const double* x, double* y) const {
  82     m_.SymmetricRightMultiply(x, y);
  83   }
  84
  85   virtual int num_rows() const { return m_.num_rows(); }
  86   virtual int num_cols() const { return m_.num_rows(); }
  87
  88  private:
  89   const BlockRandomAccessSparseMatrix& m_;
  90 };
  91
  92 class BlockRandomAccessDiagonalMatrixAdapter : public LinearOperator {
  93  public:
  94   explicit BlockRandomAccessDiagonalMatrixAdapter(
  95       const BlockRandomAccessDiagonalMatrix& m)
  96       : m_(m) {
  97   }
  98
  99   virtual ~BlockRandomAccessDiagonalMatrixAdapter() {}
 100
 101   // y = y + Ax;
 102   virtual void RightMultiply(const double* x, double* y) const {
 103     m_.RightMultiply(x, y);
 104   }
 105
 106   // y = y + A'x;
 107   virtual void LeftMultiply(const double* x, double* y) const {
 108     m_.RightMultiply(x, y);
 109   }
 110
 111   virtual int num_rows() const { return m_.num_rows(); }
 112   virtual int num_cols() const { return m_.num_rows(); }
 113
 114  private:
 115   const BlockRandomAccessDiagonalMatrix& m_;
 116 };
 117
 118 } // namespace
 119
 120 LinearSolver::Summary SchurComplementSolver::SolveImpl(
 121     BlockSparseMatrix* A,
 122     const double* b,
 123     const LinearSolver::PerSolveOptions& per_solve_options,
 124     double* x) {
 125   EventLogger event_logger("SchurComplementSolver::Solve");
 126
 127   if (eliminator_.get() == NULL) {
 128     InitStorage(A->block_structure());
 129     DetectStructure(*A->block_structure(),
 130                     options_.elimination_groups[0],
 131                     &options_.row_block_size,
 132                     &options_.e_block_size,
 133                     &options_.f_block_size);
 134     eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_)));
 135     const bool kFullRankETE = true;
 136     eliminator_->Init(
 137         options_.elimination_groups[0], kFullRankETE, A->block_structure());
 138   };
 139
 140   std::fill(x, x + A->num_cols(), 0.0);
 141   event_logger.AddEvent("Setup");
 142
 143   eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get());
 144   event_logger.AddEvent("Eliminate");
 145
 146   double* reduced_solution = x + A->num_cols() - lhs_->num_cols();
 147   const LinearSolver::Summary summary =
 148       SolveReducedLinearSystem(per_solve_options, reduced_solution);
 149   event_logger.AddEvent("ReducedSolve");
 150
 151   if (summary.termination_type == LINEAR_SOLVER_SUCCESS) {
 152     eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x);
 153     event_logger.AddEvent("BackSubstitute");
 154   }
 155
 156   return summary;
 157 }
 158
 159 // Initialize a BlockRandomAccessDenseMatrix to store the Schur
 160 // complement.
 161 void DenseSchurComplementSolver::InitStorage(
 162     const CompressedRowBlockStructure* bs) {
 163   const int num_eliminate_blocks = options().elimination_groups[0];
 164   const int num_col_blocks = bs->cols.size();
 165
 166   vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0);
 167   for (int i = num_eliminate_blocks, j = 0;
 168        i < num_col_blocks;
 169        ++i, ++j) {
 170     blocks[j] = bs->cols[i].size;
 171   }
 172
 173   set_lhs(new BlockRandomAccessDenseMatrix(blocks));
 174   set_rhs(new double[lhs()->num_rows()]);
 175 }
 176
 177 // Solve the system Sx = r, assuming that the matrix S is stored in a
 178 // BlockRandomAccessDenseMatrix. The linear system is solved using
 179 // Eigen's Cholesky factorization.
 180 LinearSolver::Summary
 181 DenseSchurComplementSolver::SolveReducedLinearSystem(
 182     const LinearSolver::PerSolveOptions& per_solve_options,
 183     double* solution) {
 184   LinearSolver::Summary summary;
 185   summary.num_iterations = 0;
 186   summary.termination_type = LINEAR_SOLVER_SUCCESS;
 187   summary.message = "Success.";
 188
 189   const BlockRandomAccessDenseMatrix* m =
 190       down_cast<const BlockRandomAccessDenseMatrix*>(lhs());
 191   const int num_rows = m->num_rows();
 192
 193   // The case where there are no f blocks, and the system is block
 194   // diagonal.
 195   if (num_rows == 0) {
 196     return summary;
 197   }
 198
 199   summary.num_iterations = 1;
 200
 201   if (options().dense_linear_algebra_library_type == EIGEN) {
 202     Eigen::LLT<Matrix, Eigen::Upper> llt =
 203         ConstMatrixRef(m->values(), num_rows, num_rows)
 204         .selfadjointView<Eigen::Upper>()
 205         .llt();
 206     if (llt.info() != Eigen::Success) {
 207       summary.termination_type = LINEAR_SOLVER_FAILURE;
 208       summary.message =
 209           "Eigen failure. Unable to perform dense Cholesky factorization.";
 210       return summary;
 211     }
 212
 213     VectorRef(solution, num_rows) = llt.solve(ConstVectorRef(rhs(), num_rows));
 214   } else {
 215     VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows);
 216     summary.termination_type =
 217         LAPACK::SolveInPlaceUsingCholesky(num_rows,
 218                                           m->values(),
 219                                           solution,
 220                                           &summary.message);
 221   }
 222
 223   return summary;
 224 }
 225
 226 SparseSchurComplementSolver::SparseSchurComplementSolver(
 227     const LinearSolver::Options& options)
 228     : SchurComplementSolver(options) {
 229   if (options.type != ITERATIVE_SCHUR) {
 230     sparse_cholesky_.reset(
 231         SparseCholesky::Create(options.sparse_linear_algebra_library_type,
 232                                options.use_postordering ? AMD : NATURAL));
 233   }
 234 }
 235
 236 SparseSchurComplementSolver::~SparseSchurComplementSolver() {
 237 }
 238
 239 // Determine the non-zero blocks in the Schur Complement matrix, and
 240 // initialize a BlockRandomAccessSparseMatrix object.
 241 void SparseSchurComplementSolver::InitStorage(
 242     const CompressedRowBlockStructure* bs) {
 243   const int num_eliminate_blocks = options().elimination_groups[0];
 244   const int num_col_blocks = bs->cols.size();
 245   const int num_row_blocks = bs->rows.size();
 246
 247   blocks_.resize(num_col_blocks - num_eliminate_blocks, 0);
 248   for (int i = num_eliminate_blocks; i < num_col_blocks; ++i) {
 249     blocks_[i - num_eliminate_blocks] = bs->cols[i].size;
 250   }
 251
 252   set<pair<int, int> > block_pairs;
 253   for (int i = 0; i < blocks_.size(); ++i) {
 254     block_pairs.insert(make_pair(i, i));
 255   }
 256
 257   int r = 0;
 258   while (r < num_row_blocks) {
 259     int e_block_id = bs->rows[r].cells.front().block_id;
 260     if (e_block_id >= num_eliminate_blocks) {
 261       break;
 262     }
 263     vector<int> f_blocks;
 264
 265     // Add to the chunk until the first block in the row is
 266     // different than the one in the first row for the chunk.
 267     for (; r < num_row_blocks; ++r) {
 268       const CompressedRow& row = bs->rows[r];
 269       if (row.cells.front().block_id != e_block_id) {
 270         break;
 271       }
 272
 273       // Iterate over the blocks in the row, ignoring the first
 274       // block since it is the one to be eliminated.
 275       for (int c = 1; c < row.cells.size(); ++c) {
 276         const Cell& cell = row.cells[c];
 277         f_blocks.push_back(cell.block_id - num_eliminate_blocks);
 278       }
 279     }
 280
 281     sort(f_blocks.begin(), f_blocks.end());
 282     f_blocks.erase(unique(f_blocks.begin(), f_blocks.end()), f_blocks.end());
 283     for (int i = 0; i < f_blocks.size(); ++i) {
 284       for (int j = i + 1; j < f_blocks.size(); ++j) {
 285         block_pairs.insert(make_pair(f_blocks[i], f_blocks[j]));
 286       }
 287     }
 288   }
 289
 290   // Remaing rows do not contribute to the chunks and directly go
 291   // into the schur complement via an outer product.
 292   for (; r < num_row_blocks; ++r) {
 293     const CompressedRow& row = bs->rows[r];
 294     CHECK_GE(row.cells.front().block_id, num_eliminate_blocks);
 295     for (int i = 0; i < row.cells.size(); ++i) {
 296       int r_block1_id = row.cells[i].block_id - num_eliminate_blocks;
 297       for (int j = 0; j < row.cells.size(); ++j) {
 298         int r_block2_id = row.cells[j].block_id - num_eliminate_blocks;
 299         if (r_block1_id <= r_block2_id) {
 300           block_pairs.insert(make_pair(r_block1_id, r_block2_id));
 301         }
 302       }
 303     }
 304   }
 305
 306   set_lhs(new BlockRandomAccessSparseMatrix(blocks_, block_pairs));
 307   set_rhs(new double[lhs()->num_rows()]);
 308 }
 309
 310 LinearSolver::Summary SparseSchurComplementSolver::SolveReducedLinearSystem(
 311     const LinearSolver::PerSolveOptions& per_solve_options, double* solution) {
 312   if (options().type == ITERATIVE_SCHUR) {
 313     return SolveReducedLinearSystemUsingConjugateGradients(per_solve_options,
 314                                                            solution);
 315   }
 316
 317   LinearSolver::Summary summary;
 318   summary.num_iterations = 0;
 319   summary.termination_type = LINEAR_SOLVER_SUCCESS;
 320   summary.message = "Success.";
 321
 322   const TripletSparseMatrix* tsm =
 323       down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix();
 324   if (tsm->num_rows() == 0) {
 325     return summary;
 326   }
 327
 328   scoped_ptr<CompressedRowSparseMatrix> lhs;
 329   const CompressedRowSparseMatrix::StorageType storage_type =
 330       sparse_cholesky_->StorageType();
 331   if (storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR) {
 332     lhs.reset(CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm));
 333     lhs->set_storage_type(CompressedRowSparseMatrix::UPPER_TRIANGULAR);
 334   } else {
 335     lhs.reset(
 336         CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm));
 337     lhs->set_storage_type(CompressedRowSparseMatrix::LOWER_TRIANGULAR);
 338   }
 339
 340   *lhs->mutable_col_blocks() = blocks_;
 341   *lhs->mutable_row_blocks() = blocks_;
 342
 343   summary.num_iterations = 1;
 344   summary.termination_type = sparse_cholesky_->FactorAndSolve(
 345       lhs.get(), rhs(), solution, &summary.message);
 346   return summary;
 347 }
 348
 349 LinearSolver::Summary
 350 SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients(
 351     const LinearSolver::PerSolveOptions& per_solve_options,
 352     double* solution) {
 353   CHECK(options().use_explicit_schur_complement);
 354   const int num_rows = lhs()->num_rows();
 355   // The case where there are no f blocks, and the system is block
 356   // diagonal.
 357   if (num_rows == 0) {
 358     LinearSolver::Summary summary;
 359     summary.num_iterations = 0;
 360     summary.termination_type = LINEAR_SOLVER_SUCCESS;
 361     summary.message = "Success.";
 362     return summary;
 363   }
 364
 365   // Only SCHUR_JACOBI is supported over here right now.
 366   CHECK_EQ(options().preconditioner_type, SCHUR_JACOBI);
 367
 368   if (preconditioner_.get() == NULL) {
 369     preconditioner_.reset(new BlockRandomAccessDiagonalMatrix(blocks_));
 370   }
 371
 372   BlockRandomAccessSparseMatrix* sc =
 373       down_cast<BlockRandomAccessSparseMatrix*>(
 374           const_cast<BlockRandomAccessMatrix*>(lhs()));
 375
 376   // Extract block diagonal from the Schur complement to construct the
 377   // schur_jacobi preconditioner.
 378   for (int i = 0; i  < blocks_.size(); ++i) {
 379     const int block_size = blocks_[i];
 380
 381     int sc_r, sc_c, sc_row_stride, sc_col_stride;
 382     CellInfo* sc_cell_info =
 383         CHECK_NOTNULL(sc->GetCell(i, i,
 384                                   &sc_r, &sc_c,
 385                                   &sc_row_stride, &sc_col_stride));
 386     MatrixRef sc_m(sc_cell_info->values, sc_row_stride, sc_col_stride);
 387
 388     int pre_r, pre_c, pre_row_stride, pre_col_stride;
 389     CellInfo* pre_cell_info = CHECK_NOTNULL(
 390         preconditioner_->GetCell(i, i,
 391                                  &pre_r, &pre_c,
 392                                  &pre_row_stride, &pre_col_stride));
 393     MatrixRef pre_m(pre_cell_info->values, pre_row_stride, pre_col_stride);
 394
 395     pre_m.block(pre_r, pre_c, block_size, block_size) =
 396         sc_m.block(sc_r, sc_c, block_size, block_size);
 397   }
 398   preconditioner_->Invert();
 399
 400   VectorRef(solution, num_rows).setZero();
 401
 402   scoped_ptr<LinearOperator> lhs_adapter(
 403       new BlockRandomAccessSparseMatrixAdapter(*sc));
 404   scoped_ptr<LinearOperator> preconditioner_adapter(
 405       new BlockRandomAccessDiagonalMatrixAdapter(*preconditioner_));
 406
 407
 408   LinearSolver::Options cg_options;
 409   cg_options.min_num_iterations = options().min_num_iterations;
 410   cg_options.max_num_iterations = options().max_num_iterations;
 411   ConjugateGradientsSolver cg_solver(cg_options);
 412
 413   LinearSolver::PerSolveOptions cg_per_solve_options;
 414   cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance;
 415   cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance;
 416   cg_per_solve_options.preconditioner = preconditioner_adapter.get();
 417
 418   return cg_solver.Solve(lhs_adapter.get(),
 419                          rhs(),
 420                          cg_per_solve_options,
 421                          solution);
 422 }
 423
 424 }  // namespace internal
 425 }  // namespace ceres