internal/ceres/dense_normal_cholesky_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/dense_normal_cholesky_solver.h"
  32
  33 #include <cstddef>
  34
  35 #include "Eigen/Dense"
  36 #include "ceres/blas.h"
  37 #include "ceres/dense_sparse_matrix.h"
  38 #include "ceres/internal/eigen.h"
  39 #include "ceres/internal/scoped_ptr.h"
  40 #include "ceres/lapack.h"
  41 #include "ceres/linear_solver.h"
  42 #include "ceres/types.h"
  43 #include "ceres/wall_time.h"
  44
  45 namespace ceres {
  46 namespace internal {
  47
  48 DenseNormalCholeskySolver::DenseNormalCholeskySolver(
  49     const LinearSolver::Options& options)
  50     : options_(options) {}
  51
  52 LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl(
  53     DenseSparseMatrix* A,
  54     const double* b,
  55     const LinearSolver::PerSolveOptions& per_solve_options,
  56     double* x) {
  57   if (options_.dense_linear_algebra_library_type == EIGEN) {
  58     return SolveUsingEigen(A, b, per_solve_options, x);
  59   } else {
  60     return SolveUsingLAPACK(A, b, per_solve_options, x);
  61   }
  62 }
  63
  64 LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingEigen(
  65     DenseSparseMatrix* A,
  66     const double* b,
  67     const LinearSolver::PerSolveOptions& per_solve_options,
  68     double* x) {
  69   EventLogger event_logger("DenseNormalCholeskySolver::Solve");
  70
  71   const int num_rows = A->num_rows();
  72   const int num_cols = A->num_cols();
  73
  74   ConstColMajorMatrixRef Aref = A->matrix();
  75   Matrix lhs(num_cols, num_cols);
  76   lhs.setZero();
  77
  78   event_logger.AddEvent("Setup");
  79
  80   //   lhs += A'A
  81   //
  82   // Using rankUpdate instead of GEMM, exposes the fact that its the
  83   // same matrix being multiplied with itself and that the product is
  84   // symmetric.
  85   lhs.selfadjointView<Eigen::Upper>().rankUpdate(Aref.transpose());
  86
  87   //   rhs = A'b
  88   Vector rhs = Aref.transpose() * ConstVectorRef(b, num_rows);
  89
  90   if (per_solve_options.D != NULL) {
  91     ConstVectorRef D(per_solve_options.D, num_cols);
  92     lhs += D.array().square().matrix().asDiagonal();
  93   }
  94   event_logger.AddEvent("Product");
  95
  96   LinearSolver::Summary summary;
  97   summary.num_iterations = 1;
  98   summary.termination_type = LINEAR_SOLVER_SUCCESS;
  99   Eigen::LLT<Matrix, Eigen::Upper> llt =
 100       lhs.selfadjointView<Eigen::Upper>().llt();
 101
 102   if (llt.info() != Eigen::Success) {
 103     summary.termination_type = LINEAR_SOLVER_FAILURE;
 104     summary.message = "Eigen LLT decomposition failed.";
 105   } else {
 106     summary.termination_type = LINEAR_SOLVER_SUCCESS;
 107     summary.message = "Success.";
 108   }
 109
 110   VectorRef(x, num_cols) = llt.solve(rhs);
 111   event_logger.AddEvent("Solve");
 112   return summary;
 113 }
 114
 115 LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingLAPACK(
 116     DenseSparseMatrix* A,
 117     const double* b,
 118     const LinearSolver::PerSolveOptions& per_solve_options,
 119     double* x) {
 120   EventLogger event_logger("DenseNormalCholeskySolver::Solve");
 121
 122   if (per_solve_options.D != NULL) {
 123     // Temporarily append a diagonal block to the A matrix, but undo
 124     // it before returning the matrix to the user.
 125     A->AppendDiagonal(per_solve_options.D);
 126   }
 127
 128   const int num_cols = A->num_cols();
 129   Matrix lhs(num_cols, num_cols);
 130   event_logger.AddEvent("Setup");
 131
 132   // lhs = A'A
 133   //
 134   // Note: This is a bit delicate, it assumes that the stride on this
 135   // matrix is the same as the number of rows.
 136   BLAS::SymmetricRankKUpdate(A->num_rows(),
 137                              num_cols,
 138                              A->values(),
 139                              true,
 140                              1.0,
 141                              0.0,
 142                              lhs.data());
 143
 144   if (per_solve_options.D != NULL) {
 145     // Undo the modifications to the matrix A.
 146     A->RemoveDiagonal();
 147   }
 148
 149   // TODO(sameeragarwal): Replace this with a gemv call for true blasness.
 150   //   rhs = A'b
 151   VectorRef(x, num_cols) =
 152       A->matrix().transpose() * ConstVectorRef(b, A->num_rows());
 153   event_logger.AddEvent("Product");
 154
 155   LinearSolver::Summary summary;
 156   summary.num_iterations = 1;
 157   summary.termination_type =
 158       LAPACK::SolveInPlaceUsingCholesky(num_cols,
 159                                         lhs.data(),
 160                                         x,
 161                                         &summary.message);
 162   event_logger.AddEvent("Solve");
 163   return summary;
 164 }
 165 }   // namespace internal
 166 }   // namespace ceres