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// This include must come before any #ifndef check on Ceres compile options.
32#include "ceres/internal/port.h"
33
34#include "ceres/sparse_normal_cholesky_solver.h"
35
36#include <algorithm>
37#include <cstring>
38#include <ctime>
39
40#include "ceres/compressed_row_sparse_matrix.h"
41#include "ceres/cxsparse.h"
42#include "ceres/internal/eigen.h"
43#include "ceres/internal/scoped_ptr.h"
44#include "ceres/linear_solver.h"
45#include "ceres/suitesparse.h"
46#include "ceres/triplet_sparse_matrix.h"
47#include "ceres/types.h"
48#include "ceres/wall_time.h"
49#include "Eigen/SparseCore"
50
51
52namespace ceres {
53namespace internal {
54
55SparseNormalCholeskySolver::SparseNormalCholeskySolver(
56    const LinearSolver::Options& options)
57    : factor_(NULL),
58      cxsparse_factor_(NULL),
59      options_(options){
60}
61
62void SparseNormalCholeskySolver::FreeFactorization() {
63  if (factor_ != NULL) {
64    ss_.Free(factor_);
65    factor_ = NULL;
66  }
67
68  if (cxsparse_factor_ != NULL) {
69    cxsparse_.Free(cxsparse_factor_);
70    cxsparse_factor_ = NULL;
71  }
72}
73
74SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {
75  FreeFactorization();
76}
77
78LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl(
79    CompressedRowSparseMatrix* A,
80    const double* b,
81    const LinearSolver::PerSolveOptions& per_solve_options,
82    double * x) {
83
84  const int num_cols = A->num_cols();
85  VectorRef(x, num_cols).setZero();
86  A->LeftMultiply(b, x);
87
88  if (per_solve_options.D != NULL) {
89    // Temporarily append a diagonal block to the A matrix, but undo
90    // it before returning the matrix to the user.
91    scoped_ptr<CompressedRowSparseMatrix> regularizer;
92    if (A->col_blocks().size() > 0) {
93      regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix(
94                            per_solve_options.D, A->col_blocks()));
95    } else {
96      regularizer.reset(new CompressedRowSparseMatrix(
97                            per_solve_options.D, num_cols));
98    }
99    A->AppendRows(*regularizer);
100  }
101
102  LinearSolver::Summary summary;
103  switch (options_.sparse_linear_algebra_library_type) {
104    case SUITE_SPARSE:
105      summary = SolveImplUsingSuiteSparse(A, per_solve_options, x);
106      break;
107    case CX_SPARSE:
108      summary = SolveImplUsingCXSparse(A, per_solve_options, x);
109      break;
110    case EIGEN_SPARSE:
111      summary = SolveImplUsingEigen(A, per_solve_options, x);
112      break;
113    default:
114      LOG(FATAL) << "Unknown sparse linear algebra library : "
115                 << options_.sparse_linear_algebra_library_type;
116  }
117
118  if (per_solve_options.D != NULL) {
119    A->DeleteRows(num_cols);
120  }
121
122  return summary;
123}
124
125LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen(
126    CompressedRowSparseMatrix* A,
127    const LinearSolver::PerSolveOptions& per_solve_options,
128    double * rhs_and_solution) {
129#ifndef CERES_USE_EIGEN_SPARSE
130
131  LinearSolver::Summary summary;
132  summary.num_iterations = 0;
133  summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
134  summary.message =
135      "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE "
136      "because Ceres was not built with support for "
137      "Eigen's SimplicialLDLT decomposition. "
138      "This requires enabling building with -DEIGENSPARSE=ON.";
139  return summary;
140
141#else
142
143  EventLogger event_logger("SparseNormalCholeskySolver::Eigen::Solve");
144
145  LinearSolver::Summary summary;
146  summary.num_iterations = 1;
147  summary.termination_type = LINEAR_SOLVER_SUCCESS;
148  summary.message = "Success.";
149
150  // Compute the normal equations. J'J delta = J'f and solve them
151  // using a sparse Cholesky factorization. Notice that when compared
152  // to SuiteSparse we have to explicitly compute the normal equations
153  // before they can be factorized. CHOLMOD/SuiteSparse on the other
154  // hand can just work off of Jt to compute the Cholesky
155  // factorization of the normal equations.
156  //
157  // TODO(sameeragarwal): See note about how this maybe a bad idea for
158  // dynamic sparsity.
159  if (outer_product_.get() == NULL) {
160    outer_product_.reset(
161        CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
162            *A, &pattern_));
163  }
164
165  CompressedRowSparseMatrix::ComputeOuterProduct(
166      *A, pattern_, outer_product_.get());
167
168  // Map to an upper triangular column major matrix.
169  //
170  // outer_product_ is a compressed row sparse matrix and in lower
171  // triangular form, when mapped to a compressed column sparse
172  // matrix, it becomes an upper triangular matrix.
173  Eigen::MappedSparseMatrix<double, Eigen::ColMajor> AtA(
174      outer_product_->num_rows(),
175      outer_product_->num_rows(),
176      outer_product_->num_nonzeros(),
177      outer_product_->mutable_rows(),
178      outer_product_->mutable_cols(),
179      outer_product_->mutable_values());
180
181  const Vector b = VectorRef(rhs_and_solution, outer_product_->num_rows());
182  if (simplicial_ldlt_.get() == NULL || options_.dynamic_sparsity) {
183    simplicial_ldlt_.reset(new SimplicialLDLT);
184    // This is a crappy way to be doing this. But right now Eigen does
185    // not expose a way to do symbolic analysis with a given
186    // permutation pattern, so we cannot use a block analysis of the
187    // Jacobian.
188    simplicial_ldlt_->analyzePattern(AtA);
189    if (simplicial_ldlt_->info() != Eigen::Success) {
190      summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
191      summary.message =
192          "Eigen failure. Unable to find symbolic factorization.";
193      return summary;
194    }
195  }
196  event_logger.AddEvent("Analysis");
197
198  simplicial_ldlt_->factorize(AtA);
199  if(simplicial_ldlt_->info() != Eigen::Success) {
200    summary.termination_type = LINEAR_SOLVER_FAILURE;
201    summary.message =
202        "Eigen failure. Unable to find numeric factorization.";
203    return summary;
204  }
205
206  VectorRef(rhs_and_solution, outer_product_->num_rows()) =
207      simplicial_ldlt_->solve(b);
208  if(simplicial_ldlt_->info() != Eigen::Success) {
209    summary.termination_type = LINEAR_SOLVER_FAILURE;
210    summary.message =
211        "Eigen failure. Unable to do triangular solve.";
212    return summary;
213  }
214
215  event_logger.AddEvent("Solve");
216  return summary;
217#endif  // EIGEN_USE_EIGEN_SPARSE
218}
219
220
221
222LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse(
223    CompressedRowSparseMatrix* A,
224    const LinearSolver::PerSolveOptions& per_solve_options,
225    double * rhs_and_solution) {
226#ifdef CERES_NO_CXSPARSE
227
228  LinearSolver::Summary summary;
229  summary.num_iterations = 0;
230  summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
231  summary.message =
232      "SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE "
233      "because Ceres was not built with support for CXSparse. "
234      "This requires enabling building with -DCXSPARSE=ON.";
235
236  return summary;
237
238#else
239
240  EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve");
241
242  LinearSolver::Summary summary;
243  summary.num_iterations = 1;
244  summary.termination_type = LINEAR_SOLVER_SUCCESS;
245  summary.message = "Success.";
246
247  // Compute the normal equations. J'J delta = J'f and solve them
248  // using a sparse Cholesky factorization. Notice that when compared
249  // to SuiteSparse we have to explicitly compute the normal equations
250  // before they can be factorized. CHOLMOD/SuiteSparse on the other
251  // hand can just work off of Jt to compute the Cholesky
252  // factorization of the normal equations.
253  //
254  // TODO(sameeragarwal): If dynamic sparsity is enabled, then this is
255  // not a good idea performance wise, since the jacobian has far too
256  // many entries and the program will go crazy with memory.
257  if (outer_product_.get() == NULL) {
258    outer_product_.reset(
259        CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
260            *A, &pattern_));
261  }
262
263  CompressedRowSparseMatrix::ComputeOuterProduct(
264      *A, pattern_, outer_product_.get());
265  cs_di AtA_view =
266      cxsparse_.CreateSparseMatrixTransposeView(outer_product_.get());
267  cs_di* AtA = &AtA_view;
268
269  event_logger.AddEvent("Setup");
270
271  // Compute symbolic factorization if not available.
272  if (options_.dynamic_sparsity) {
273    FreeFactorization();
274  }
275  if (cxsparse_factor_ == NULL) {
276    if (options_.use_postordering) {
277      cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(AtA,
278                                                        A->col_blocks(),
279                                                        A->col_blocks());
280    } else {
281      if (options_.dynamic_sparsity) {
282        cxsparse_factor_ = cxsparse_.AnalyzeCholesky(AtA);
283      } else {
284        cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA);
285      }
286    }
287  }
288  event_logger.AddEvent("Analysis");
289
290  if (cxsparse_factor_ == NULL) {
291    summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
292    summary.message =
293        "CXSparse failure. Unable to find symbolic factorization.";
294  } else if (!cxsparse_.SolveCholesky(AtA, cxsparse_factor_, rhs_and_solution)) {
295    summary.termination_type = LINEAR_SOLVER_FAILURE;
296    summary.message = "CXSparse::SolveCholesky failed.";
297  }
298  event_logger.AddEvent("Solve");
299
300  return summary;
301#endif
302}
303
304LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
305    CompressedRowSparseMatrix* A,
306    const LinearSolver::PerSolveOptions& per_solve_options,
307    double * rhs_and_solution) {
308#ifdef CERES_NO_SUITESPARSE
309
310  LinearSolver::Summary summary;
311  summary.num_iterations = 0;
312  summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
313  summary.message =
314      "SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE "
315      "because Ceres was not built with support for SuiteSparse. "
316      "This requires enabling building with -DSUITESPARSE=ON.";
317  return summary;
318
319#else
320
321  EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve");
322  LinearSolver::Summary summary;
323  summary.termination_type = LINEAR_SOLVER_SUCCESS;
324  summary.num_iterations = 1;
325  summary.message = "Success.";
326
327  const int num_cols = A->num_cols();
328  cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A);
329  event_logger.AddEvent("Setup");
330
331  if (options_.dynamic_sparsity) {
332    FreeFactorization();
333  }
334  if (factor_ == NULL) {
335    if (options_.use_postordering) {
336      factor_ = ss_.BlockAnalyzeCholesky(&lhs,
337                                         A->col_blocks(),
338                                         A->row_blocks(),
339                                         &summary.message);
340    } else {
341      if (options_.dynamic_sparsity) {
342        factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message);
343      } else {
344        factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs, &summary.message);
345      }
346    }
347  }
348  event_logger.AddEvent("Analysis");
349
350  if (factor_ == NULL) {
351    summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
352    // No need to set message as it has already been set by the
353    // symbolic analysis routines above.
354    return summary;
355  }
356
357  summary.termination_type = ss_.Cholesky(&lhs, factor_, &summary.message);
358  if (summary.termination_type != LINEAR_SOLVER_SUCCESS) {
359    return summary;
360  }
361
362  cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution, num_cols, num_cols);
363  cholmod_dense* solution = ss_.Solve(factor_, rhs, &summary.message);
364  event_logger.AddEvent("Solve");
365
366  ss_.Free(rhs);
367  if (solution != NULL) {
368    memcpy(rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution));
369    ss_.Free(solution);
370  } else {
371    // No need to set message as it has already been set by the
372    // numeric factorization routine above.
373    summary.termination_type = LINEAR_SOLVER_FAILURE;
374  }
375
376  event_logger.AddEvent("Teardown");
377  return summary;
378#endif
379}
380
381}   // namespace internal
382}   // namespace ceres
383