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 // This include must come before any #ifndef check on Ceres compile options.
32 #include "ceres/internal/port.h"
33
34 #ifndef CERES_NO_SUITESPARSE
35 #include "ceres/suitesparse.h"
36
37 #include <vector>
38
39 #include "ceres/compressed_col_sparse_matrix_utils.h"
40 #include "ceres/compressed_row_sparse_matrix.h"
41 #include "ceres/linear_solver.h"
42 #include "ceres/triplet_sparse_matrix.h"
43 #include "cholmod.h"
44
45 namespace ceres {
46 namespace internal {
47
48 using std::string;
49 using std::vector;
50
SuiteSparse()51 SuiteSparse::SuiteSparse() { cholmod_start(&cc_); }
52
~SuiteSparse()53 SuiteSparse::~SuiteSparse() { cholmod_finish(&cc_); }
54
CreateSparseMatrix(TripletSparseMatrix * A)55 cholmod_sparse* SuiteSparse::CreateSparseMatrix(TripletSparseMatrix* A) {
56 cholmod_triplet triplet;
57
58 triplet.nrow = A->num_rows();
59 triplet.ncol = A->num_cols();
60 triplet.nzmax = A->max_num_nonzeros();
61 triplet.nnz = A->num_nonzeros();
62 triplet.i = reinterpret_cast<void*>(A->mutable_rows());
63 triplet.j = reinterpret_cast<void*>(A->mutable_cols());
64 triplet.x = reinterpret_cast<void*>(A->mutable_values());
65 triplet.stype = 0; // Matrix is not symmetric.
66 triplet.itype = CHOLMOD_INT;
67 triplet.xtype = CHOLMOD_REAL;
68 triplet.dtype = CHOLMOD_DOUBLE;
69
70 return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_);
71 }
72
CreateSparseMatrixTranspose(TripletSparseMatrix * A)73 cholmod_sparse* SuiteSparse::CreateSparseMatrixTranspose(
74 TripletSparseMatrix* A) {
75 cholmod_triplet triplet;
76
77 triplet.ncol = A->num_rows(); // swap row and columns
78 triplet.nrow = A->num_cols();
79 triplet.nzmax = A->max_num_nonzeros();
80 triplet.nnz = A->num_nonzeros();
81
82 // swap rows and columns
83 triplet.j = reinterpret_cast<void*>(A->mutable_rows());
84 triplet.i = reinterpret_cast<void*>(A->mutable_cols());
85 triplet.x = reinterpret_cast<void*>(A->mutable_values());
86 triplet.stype = 0; // Matrix is not symmetric.
87 triplet.itype = CHOLMOD_INT;
88 triplet.xtype = CHOLMOD_REAL;
89 triplet.dtype = CHOLMOD_DOUBLE;
90
91 return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_);
92 }
93
CreateSparseMatrixTransposeView(CompressedRowSparseMatrix * A)94 cholmod_sparse SuiteSparse::CreateSparseMatrixTransposeView(
95 CompressedRowSparseMatrix* A) {
96 cholmod_sparse m;
97 m.nrow = A->num_cols();
98 m.ncol = A->num_rows();
99 m.nzmax = A->num_nonzeros();
100 m.nz = NULL;
101 m.p = reinterpret_cast<void*>(A->mutable_rows());
102 m.i = reinterpret_cast<void*>(A->mutable_cols());
103 m.x = reinterpret_cast<void*>(A->mutable_values());
104 m.z = NULL;
105
106 if (A->storage_type() == CompressedRowSparseMatrix::LOWER_TRIANGULAR) {
107 m.stype = 1;
108 } else if (A->storage_type() == CompressedRowSparseMatrix::UPPER_TRIANGULAR) {
109 m.stype = -1;
110 } else {
111 m.stype = 0;
112 }
113
114 m.itype = CHOLMOD_INT;
115 m.xtype = CHOLMOD_REAL;
116 m.dtype = CHOLMOD_DOUBLE;
117 m.sorted = 1;
118 m.packed = 1;
119
120 return m;
121 }
122
CreateDenseVector(const double * x,int in_size,int out_size)123 cholmod_dense* SuiteSparse::CreateDenseVector(const double* x,
124 int in_size,
125 int out_size) {
126 CHECK_LE(in_size, out_size);
127 cholmod_dense* v = cholmod_zeros(out_size, 1, CHOLMOD_REAL, &cc_);
128 if (x != NULL) {
129 memcpy(v->x, x, in_size * sizeof(*x));
130 }
131 return v;
132 }
133
AnalyzeCholesky(cholmod_sparse * A,string * message)134 cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A,
135 string* message) {
136 // Cholmod can try multiple re-ordering strategies to find a fill
137 // reducing ordering. Here we just tell it use AMD with automatic
138 // matrix dependence choice of supernodal versus simplicial
139 // factorization.
140 cc_.nmethods = 1;
141 cc_.method[0].ordering = CHOLMOD_AMD;
142 cc_.supernodal = CHOLMOD_AUTO;
143
144 cholmod_factor* factor = cholmod_analyze(A, &cc_);
145 if (VLOG_IS_ON(2)) {
146 cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
147 }
148
149 if (cc_.status != CHOLMOD_OK) {
150 *message =
151 StringPrintf("cholmod_analyze failed. error code: %d", cc_.status);
152 return NULL;
153 }
154
155 return CHECK_NOTNULL(factor);
156 }
157
BlockAnalyzeCholesky(cholmod_sparse * A,const vector<int> & row_blocks,const vector<int> & col_blocks,string * message)158 cholmod_factor* SuiteSparse::BlockAnalyzeCholesky(cholmod_sparse* A,
159 const vector<int>& row_blocks,
160 const vector<int>& col_blocks,
161 string* message) {
162 vector<int> ordering;
163 if (!BlockAMDOrdering(A, row_blocks, col_blocks, &ordering)) {
164 return NULL;
165 }
166 return AnalyzeCholeskyWithUserOrdering(A, ordering, message);
167 }
168
AnalyzeCholeskyWithUserOrdering(cholmod_sparse * A,const vector<int> & ordering,string * message)169 cholmod_factor* SuiteSparse::AnalyzeCholeskyWithUserOrdering(
170 cholmod_sparse* A, const vector<int>& ordering, string* message) {
171 CHECK_EQ(ordering.size(), A->nrow);
172
173 cc_.nmethods = 1;
174 cc_.method[0].ordering = CHOLMOD_GIVEN;
175
176 cholmod_factor* factor =
177 cholmod_analyze_p(A, const_cast<int*>(&ordering[0]), NULL, 0, &cc_);
178 if (VLOG_IS_ON(2)) {
179 cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
180 }
181 if (cc_.status != CHOLMOD_OK) {
182 *message =
183 StringPrintf("cholmod_analyze failed. error code: %d", cc_.status);
184 return NULL;
185 }
186
187 return CHECK_NOTNULL(factor);
188 }
189
AnalyzeCholeskyWithNaturalOrdering(cholmod_sparse * A,string * message)190 cholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering(
191 cholmod_sparse* A, string* message) {
192 cc_.nmethods = 1;
193 cc_.method[0].ordering = CHOLMOD_NATURAL;
194 cc_.postorder = 0;
195
196 cholmod_factor* factor = cholmod_analyze(A, &cc_);
197 if (VLOG_IS_ON(2)) {
198 cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
199 }
200 if (cc_.status != CHOLMOD_OK) {
201 *message =
202 StringPrintf("cholmod_analyze failed. error code: %d", cc_.status);
203 return NULL;
204 }
205
206 return CHECK_NOTNULL(factor);
207 }
208
BlockAMDOrdering(const cholmod_sparse * A,const vector<int> & row_blocks,const vector<int> & col_blocks,vector<int> * ordering)209 bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A,
210 const vector<int>& row_blocks,
211 const vector<int>& col_blocks,
212 vector<int>* ordering) {
213 const int num_row_blocks = row_blocks.size();
214 const int num_col_blocks = col_blocks.size();
215
216 // Arrays storing the compressed column structure of the matrix
217 // incoding the block sparsity of A.
218 vector<int> block_cols;
219 vector<int> block_rows;
220
221 CompressedColumnScalarMatrixToBlockMatrix(reinterpret_cast<const int*>(A->i),
222 reinterpret_cast<const int*>(A->p),
223 row_blocks,
224 col_blocks,
225 &block_rows,
226 &block_cols);
227 cholmod_sparse_struct block_matrix;
228 block_matrix.nrow = num_row_blocks;
229 block_matrix.ncol = num_col_blocks;
230 block_matrix.nzmax = block_rows.size();
231 block_matrix.p = reinterpret_cast<void*>(&block_cols[0]);
232 block_matrix.i = reinterpret_cast<void*>(&block_rows[0]);
233 block_matrix.x = NULL;
234 block_matrix.stype = A->stype;
235 block_matrix.itype = CHOLMOD_INT;
236 block_matrix.xtype = CHOLMOD_PATTERN;
237 block_matrix.dtype = CHOLMOD_DOUBLE;
238 block_matrix.sorted = 1;
239 block_matrix.packed = 1;
240
241 vector<int> block_ordering(num_row_blocks);
242 if (!cholmod_amd(&block_matrix, NULL, 0, &block_ordering[0], &cc_)) {
243 return false;
244 }
245
246 BlockOrderingToScalarOrdering(row_blocks, block_ordering, ordering);
247 return true;
248 }
249
Cholesky(cholmod_sparse * A,cholmod_factor * L,string * message)250 LinearSolverTerminationType SuiteSparse::Cholesky(cholmod_sparse* A,
251 cholmod_factor* L,
252 string* message) {
253 CHECK_NOTNULL(A);
254 CHECK_NOTNULL(L);
255
256 // Save the current print level and silence CHOLMOD, otherwise
257 // CHOLMOD is prone to dumping stuff to stderr, which can be
258 // distracting when the error (matrix is indefinite) is not a fatal
259 // failure.
260 const int old_print_level = cc_.print;
261 cc_.print = 0;
262
263 cc_.quick_return_if_not_posdef = 1;
264 int cholmod_status = cholmod_factorize(A, L, &cc_);
265 cc_.print = old_print_level;
266
267 switch (cc_.status) {
268 case CHOLMOD_NOT_INSTALLED:
269 *message = "CHOLMOD failure: Method not installed.";
270 return LINEAR_SOLVER_FATAL_ERROR;
271 case CHOLMOD_OUT_OF_MEMORY:
272 *message = "CHOLMOD failure: Out of memory.";
273 return LINEAR_SOLVER_FATAL_ERROR;
274 case CHOLMOD_TOO_LARGE:
275 *message = "CHOLMOD failure: Integer overflow occurred.";
276 return LINEAR_SOLVER_FATAL_ERROR;
277 case CHOLMOD_INVALID:
278 *message = "CHOLMOD failure: Invalid input.";
279 return LINEAR_SOLVER_FATAL_ERROR;
280 case CHOLMOD_NOT_POSDEF:
281 *message = "CHOLMOD warning: Matrix not positive definite.";
282 return LINEAR_SOLVER_FAILURE;
283 case CHOLMOD_DSMALL:
284 *message =
285 "CHOLMOD warning: D for LDL' or diag(L) or "
286 "LL' has tiny absolute value.";
287 return LINEAR_SOLVER_FAILURE;
288 case CHOLMOD_OK:
289 if (cholmod_status != 0) {
290 return LINEAR_SOLVER_SUCCESS;
291 }
292
293 *message =
294 "CHOLMOD failure: cholmod_factorize returned false "
295 "but cholmod_common::status is CHOLMOD_OK."
296 "Please report this to ceres-solver@googlegroups.com.";
297 return LINEAR_SOLVER_FATAL_ERROR;
298 default:
299 *message = StringPrintf(
300 "Unknown cholmod return code: %d. "
301 "Please report this to ceres-solver@googlegroups.com.",
302 cc_.status);
303 return LINEAR_SOLVER_FATAL_ERROR;
304 }
305
306 return LINEAR_SOLVER_FATAL_ERROR;
307 }
308
Solve(cholmod_factor * L,cholmod_dense * b,string * message)309 cholmod_dense* SuiteSparse::Solve(cholmod_factor* L,
310 cholmod_dense* b,
311 string* message) {
312 if (cc_.status != CHOLMOD_OK) {
313 *message = "cholmod_solve failed. CHOLMOD status is not CHOLMOD_OK";
314 return NULL;
315 }
316
317 return cholmod_solve(CHOLMOD_A, L, b, &cc_);
318 }
319
ApproximateMinimumDegreeOrdering(cholmod_sparse * matrix,int * ordering)320 bool SuiteSparse::ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix,
321 int* ordering) {
322 return cholmod_amd(matrix, NULL, 0, ordering, &cc_);
323 }
324
ConstrainedApproximateMinimumDegreeOrdering(cholmod_sparse * matrix,int * constraints,int * ordering)325 bool SuiteSparse::ConstrainedApproximateMinimumDegreeOrdering(
326 cholmod_sparse* matrix, int* constraints, int* ordering) {
327 #ifndef CERES_NO_CAMD
328 return cholmod_camd(matrix, NULL, 0, constraints, ordering, &cc_);
329 #else
330 LOG(FATAL) << "Congratulations you have found a bug in Ceres."
331 << "Ceres Solver was compiled with SuiteSparse "
332 << "version 4.1.0 or less. Calling this function "
333 << "in that case is a bug. Please contact the"
334 << "the Ceres Solver developers.";
335 return false;
336 #endif
337 }
338
Create(const OrderingType ordering_type)339 SuiteSparseCholesky* SuiteSparseCholesky::Create(
340 const OrderingType ordering_type) {
341 return new SuiteSparseCholesky(ordering_type);
342 }
343
SuiteSparseCholesky(const OrderingType ordering_type)344 SuiteSparseCholesky::SuiteSparseCholesky(const OrderingType ordering_type)
345 : ordering_type_(ordering_type), factor_(NULL) {}
346
~SuiteSparseCholesky()347 SuiteSparseCholesky::~SuiteSparseCholesky() {
348 if (factor_ != NULL) {
349 ss_.Free(factor_);
350 }
351 }
352
Factorize(CompressedRowSparseMatrix * lhs,string * message)353 LinearSolverTerminationType SuiteSparseCholesky::Factorize(
354 CompressedRowSparseMatrix* lhs, string* message) {
355 if (lhs == NULL) {
356 *message = "Failure: Input lhs is NULL.";
357 return LINEAR_SOLVER_FATAL_ERROR;
358 }
359
360 cholmod_sparse cholmod_lhs = ss_.CreateSparseMatrixTransposeView(lhs);
361
362 if (factor_ == NULL) {
363 if (ordering_type_ == NATURAL) {
364 factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&cholmod_lhs, message);
365 } else {
366 if (!lhs->col_blocks().empty() && !(lhs->row_blocks().empty())) {
367 factor_ = ss_.BlockAnalyzeCholesky(
368 &cholmod_lhs, lhs->col_blocks(), lhs->row_blocks(), message);
369 } else {
370 factor_ = ss_.AnalyzeCholesky(&cholmod_lhs, message);
371 }
372 }
373
374 if (factor_ == NULL) {
375 return LINEAR_SOLVER_FATAL_ERROR;
376 }
377 }
378
379 return ss_.Cholesky(&cholmod_lhs, factor_, message);
380 }
381
StorageType() const382 CompressedRowSparseMatrix::StorageType SuiteSparseCholesky::StorageType()
383 const {
384 return ((ordering_type_ == NATURAL)
385 ? CompressedRowSparseMatrix::UPPER_TRIANGULAR
386 : CompressedRowSparseMatrix::LOWER_TRIANGULAR);
387 }
388
Solve(const double * rhs,double * solution,string * message)389 LinearSolverTerminationType SuiteSparseCholesky::Solve(const double* rhs,
390 double* solution,
391 string* message) {
392 // Error checking
393 if (factor_ == NULL) {
394 *message = "Solve called without a call to Factorize first.";
395 return LINEAR_SOLVER_FATAL_ERROR;
396 }
397
398 const int num_cols = factor_->n;
399 cholmod_dense* cholmod_dense_rhs =
400 ss_.CreateDenseVector(rhs, num_cols, num_cols);
401 cholmod_dense* cholmod_dense_solution =
402 ss_.Solve(factor_, cholmod_dense_rhs, message);
403 ss_.Free(cholmod_dense_rhs);
404 if (cholmod_dense_solution == NULL) {
405 return LINEAR_SOLVER_FAILURE;
406 }
407
408 memcpy(solution, cholmod_dense_solution->x, num_cols * sizeof(*solution));
409 ss_.Free(cholmod_dense_solution);
410 return LINEAR_SOLVER_SUCCESS;
411 }
412
413 } // namespace internal
414 } // namespace ceres
415
416 #endif // CERES_NO_SUITESPARSE
417