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@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
SparseNormalCholeskySolver(const LinearSolver::Options & options)52 SparseNormalCholeskySolver::SparseNormalCholeskySolver(
53 const LinearSolver::Options& options)
54 : factor_(NULL),
55 cxsparse_factor_(NULL),
56 options_(options) {
57 }
58
~SparseNormalCholeskySolver()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
SolveImpl(CompressedRowSparseMatrix * A,const double * b,const LinearSolver::PerSolveOptions & per_solve_options,double * x)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
SolveImplUsingCXSparse(CompressedRowSparseMatrix * A,const double * b,const LinearSolver::PerSolveOptions & per_solve_options,double * x)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
SolveImplUsingCXSparse(CompressedRowSparseMatrix * A,const double * b,const LinearSolver::PerSolveOptions & per_solve_options,double * x)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
SolveImplUsingSuiteSparse(CompressedRowSparseMatrix * A,const double * b,const LinearSolver::PerSolveOptions & per_solve_options,double * x)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
SolveImplUsingSuiteSparse(CompressedRowSparseMatrix * A,const double * b,const LinearSolver::PerSolveOptions & per_solve_options,double * x)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