Home | History | Annotate | Download | only in ceres 
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2010, 2011, 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: sameeragarwal (at) google.com (Sameer Agarwal)
     30 
     31 #if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE)
     32 
     33 #include "ceres/sparse_normal_cholesky_solver.h"
     34 
     35 #include <algorithm>
     36 #include <cstring>
     37 #include <ctime>
     38 
     39 #include "ceres/compressed_row_sparse_matrix.h"
     40 #include "ceres/cxsparse.h"
     41 #include "ceres/internal/eigen.h"
     42 #include "ceres/internal/scoped_ptr.h"
     43 #include "ceres/linear_solver.h"
     44 #include "ceres/suitesparse.h"
     45 #include "ceres/triplet_sparse_matrix.h"
     46 #include "ceres/types.h"
     47 #include "ceres/wall_time.h"
     48 
     49 namespace ceres {
     50 namespace internal {
     51 
     52 SparseNormalCholeskySolver::SparseNormalCholeskySolver(
     53     const LinearSolver::Options& options)
     54     : factor_(NULL),
     55       cxsparse_factor_(NULL),
     56       options_(options) {
     57 }
     58 
     59 SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {
     60 #ifndef CERES_NO_SUITESPARSE
     61   if (factor_ != NULL) {
     62     ss_.Free(factor_);
     63     factor_ = NULL;
     64   }
     65 #endif
     66 
     67 #ifndef CERES_NO_CXSPARSE
     68   if (cxsparse_factor_ != NULL) {
     69     cxsparse_.Free(cxsparse_factor_);
     70     cxsparse_factor_ = NULL;
     71   }
     72 #endif  // CERES_NO_CXSPARSE
     73 }
     74 
     75 LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl(
     76     CompressedRowSparseMatrix* A,
     77     const double* b,
     78     const LinearSolver::PerSolveOptions& per_solve_options,
     79     double * x) {
     80   switch (options_.sparse_linear_algebra_library_type) {
     81     case SUITE_SPARSE:
     82       return SolveImplUsingSuiteSparse(A, b, per_solve_options, x);
     83     case CX_SPARSE:
     84       return SolveImplUsingCXSparse(A, b, per_solve_options, x);
     85     default:
     86       LOG(FATAL) << "Unknown sparse linear algebra library : "
     87                  << options_.sparse_linear_algebra_library_type;
     88   }
     89 
     90   LOG(FATAL) << "Unknown sparse linear algebra library : "
     91              << options_.sparse_linear_algebra_library_type;
     92   return LinearSolver::Summary();
     93 }
     94 
     95 #ifndef CERES_NO_CXSPARSE
     96 LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse(
     97     CompressedRowSparseMatrix* A,
     98     const double* b,
     99     const LinearSolver::PerSolveOptions& per_solve_options,
    100     double * x) {
    101   EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve");
    102 
    103   LinearSolver::Summary summary;
    104   summary.num_iterations = 1;
    105   const int num_cols = A->num_cols();
    106   Vector Atb = Vector::Zero(num_cols);
    107   A->LeftMultiply(b, Atb.data());
    108 
    109   if (per_solve_options.D != NULL) {
    110     // Temporarily append a diagonal block to the A matrix, but undo
    111     // it before returning the matrix to the user.
    112     CompressedRowSparseMatrix D(per_solve_options.D, num_cols);
    113     A->AppendRows(D);
    114   }
    115 
    116   VectorRef(x, num_cols).setZero();
    117 
    118   // Wrap the augmented Jacobian in a compressed sparse column matrix.
    119   cs_di At = cxsparse_.CreateSparseMatrixTransposeView(A);
    120 
    121   // Compute the normal equations. J'J delta = J'f and solve them
    122   // using a sparse Cholesky factorization. Notice that when compared
    123   // to SuiteSparse we have to explicitly compute the transpose of Jt,
    124   // and then the normal equations before they can be
    125   // factorized. CHOLMOD/SuiteSparse on the other hand can just work
    126   // off of Jt to compute the Cholesky factorization of the normal
    127   // equations.
    128   cs_di* A2 = cxsparse_.TransposeMatrix(&At);
    129   cs_di* AtA = cxsparse_.MatrixMatrixMultiply(&At, A2);
    130 
    131   cxsparse_.Free(A2);
    132   if (per_solve_options.D != NULL) {
    133     A->DeleteRows(num_cols);
    134   }
    135   event_logger.AddEvent("Setup");
    136 
    137   // Compute symbolic factorization if not available.
    138   if (cxsparse_factor_ == NULL) {
    139     if (options_.use_postordering) {
    140       cxsparse_factor_ =
    141           CHECK_NOTNULL(cxsparse_.BlockAnalyzeCholesky(AtA,
    142                                                        A->col_blocks(),
    143                                                        A->col_blocks()));
    144     } else {
    145       cxsparse_factor_ =
    146           CHECK_NOTNULL(cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA));
    147     }
    148   }
    149   event_logger.AddEvent("Analysis");
    150 
    151   // Solve the linear system.
    152   if (cxsparse_.SolveCholesky(AtA, cxsparse_factor_, Atb.data())) {
    153     VectorRef(x, Atb.rows()) = Atb;
    154     summary.termination_type = TOLERANCE;
    155   }
    156   event_logger.AddEvent("Solve");
    157 
    158   cxsparse_.Free(AtA);
    159   event_logger.AddEvent("Teardown");
    160   return summary;
    161 }
    162 #else
    163 LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse(
    164     CompressedRowSparseMatrix* A,
    165     const double* b,
    166     const LinearSolver::PerSolveOptions& per_solve_options,
    167     double * x) {
    168   LOG(FATAL) << "No CXSparse support in Ceres.";
    169 
    170   // Unreachable but MSVC does not know this.
    171   return LinearSolver::Summary();
    172 }
    173 #endif
    174 
    175 #ifndef CERES_NO_SUITESPARSE
    176 LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
    177     CompressedRowSparseMatrix* A,
    178     const double* b,
    179     const LinearSolver::PerSolveOptions& per_solve_options,
    180     double * x) {
    181   EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve");
    182 
    183   const int num_cols = A->num_cols();
    184   LinearSolver::Summary summary;
    185   Vector Atb = Vector::Zero(num_cols);
    186   A->LeftMultiply(b, Atb.data());
    187 
    188   if (per_solve_options.D != NULL) {
    189     // Temporarily append a diagonal block to the A matrix, but undo it before
    190     // returning the matrix to the user.
    191     CompressedRowSparseMatrix D(per_solve_options.D, num_cols);
    192     A->AppendRows(D);
    193   }
    194 
    195   VectorRef(x, num_cols).setZero();
    196 
    197   cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A);
    198   cholmod_dense* rhs = ss_.CreateDenseVector(Atb.data(), num_cols, num_cols);
    199   event_logger.AddEvent("Setup");
    200 
    201   if (factor_ == NULL) {
    202     if (options_.use_postordering) {
    203       factor_ =
    204           CHECK_NOTNULL(ss_.BlockAnalyzeCholesky(&lhs,
    205                                                  A->col_blocks(),
    206                                                  A->row_blocks()));
    207     } else {
    208       factor_ =
    209       CHECK_NOTNULL(ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs));
    210     }
    211   }
    212 
    213   event_logger.AddEvent("Analysis");
    214 
    215   cholmod_dense* sol = ss_.SolveCholesky(&lhs, factor_, rhs);
    216   event_logger.AddEvent("Solve");
    217 
    218   ss_.Free(rhs);
    219   rhs = NULL;
    220 
    221   if (per_solve_options.D != NULL) {
    222     A->DeleteRows(num_cols);
    223   }
    224 
    225   summary.num_iterations = 1;
    226   if (sol != NULL) {
    227     memcpy(x, sol->x, num_cols * sizeof(*x));
    228 
    229     ss_.Free(sol);
    230     sol = NULL;
    231     summary.termination_type = TOLERANCE;
    232   }
    233 
    234   event_logger.AddEvent("Teardown");
    235   return summary;
    236 }
    237 #else
    238 LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
    239     CompressedRowSparseMatrix* A,
    240     const double* b,
    241     const LinearSolver::PerSolveOptions& per_solve_options,
    242     double * x) {
    243   LOG(FATAL) << "No SuiteSparse support in Ceres.";
    244 
    245   // Unreachable but MSVC does not know this.
    246   return LinearSolver::Summary();
    247 }
    248 #endif
    249 
    250 }   // namespace internal
    251 }   // namespace ceres
    252 
    253 #endif  // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE)
    254