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#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
49namespace ceres {
50namespace internal {
51
52SparseNormalCholeskySolver::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
64SparseNormalCholeskySolver::~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
80LinearSolver::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
101LinearSolver::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
154LinearSolver::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
167LinearSolver::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
240LinearSolver::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