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