Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/internal/ceres/suitesparse.cc b/internal/ceres/suitesparse.cc
new file mode 100644
index 0000000..190d175
--- /dev/null
+++ b/internal/ceres/suitesparse.cc
@@ -0,0 +1,430 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+// This include must come before any #ifndef check on Ceres compile options.
+#include "ceres/internal/port.h"
+
+#ifndef CERES_NO_SUITESPARSE
+#include "ceres/suitesparse.h"
+
+#include <vector>
+
+#include "ceres/compressed_col_sparse_matrix_utils.h"
+#include "ceres/compressed_row_sparse_matrix.h"
+#include "ceres/linear_solver.h"
+#include "ceres/triplet_sparse_matrix.h"
+#include "cholmod.h"
+
+namespace ceres {
+namespace internal {
+
+using std::string;
+using std::vector;
+
+SuiteSparse::SuiteSparse() { cholmod_start(&cc_); }
+
+SuiteSparse::~SuiteSparse() { cholmod_finish(&cc_); }
+
+cholmod_sparse* SuiteSparse::CreateSparseMatrix(TripletSparseMatrix* A) {
+ cholmod_triplet triplet;
+
+ triplet.nrow = A->num_rows();
+ triplet.ncol = A->num_cols();
+ triplet.nzmax = A->max_num_nonzeros();
+ triplet.nnz = A->num_nonzeros();
+ triplet.i = reinterpret_cast<void*>(A->mutable_rows());
+ triplet.j = reinterpret_cast<void*>(A->mutable_cols());
+ triplet.x = reinterpret_cast<void*>(A->mutable_values());
+ triplet.stype = 0; // Matrix is not symmetric.
+ triplet.itype = CHOLMOD_INT;
+ triplet.xtype = CHOLMOD_REAL;
+ triplet.dtype = CHOLMOD_DOUBLE;
+
+ return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_);
+}
+
+cholmod_sparse* SuiteSparse::CreateSparseMatrixTranspose(
+ TripletSparseMatrix* A) {
+ cholmod_triplet triplet;
+
+ triplet.ncol = A->num_rows(); // swap row and columns
+ triplet.nrow = A->num_cols();
+ triplet.nzmax = A->max_num_nonzeros();
+ triplet.nnz = A->num_nonzeros();
+
+ // swap rows and columns
+ triplet.j = reinterpret_cast<void*>(A->mutable_rows());
+ triplet.i = reinterpret_cast<void*>(A->mutable_cols());
+ triplet.x = reinterpret_cast<void*>(A->mutable_values());
+ triplet.stype = 0; // Matrix is not symmetric.
+ triplet.itype = CHOLMOD_INT;
+ triplet.xtype = CHOLMOD_REAL;
+ triplet.dtype = CHOLMOD_DOUBLE;
+
+ return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_);
+}
+
+cholmod_sparse SuiteSparse::CreateSparseMatrixTransposeView(
+ CompressedRowSparseMatrix* A) {
+ cholmod_sparse m;
+ m.nrow = A->num_cols();
+ m.ncol = A->num_rows();
+ m.nzmax = A->num_nonzeros();
+ m.nz = nullptr;
+ m.p = reinterpret_cast<void*>(A->mutable_rows());
+ m.i = reinterpret_cast<void*>(A->mutable_cols());
+ m.x = reinterpret_cast<void*>(A->mutable_values());
+ m.z = nullptr;
+
+ if (A->storage_type() == CompressedRowSparseMatrix::LOWER_TRIANGULAR) {
+ m.stype = 1;
+ } else if (A->storage_type() == CompressedRowSparseMatrix::UPPER_TRIANGULAR) {
+ m.stype = -1;
+ } else {
+ m.stype = 0;
+ }
+
+ m.itype = CHOLMOD_INT;
+ m.xtype = CHOLMOD_REAL;
+ m.dtype = CHOLMOD_DOUBLE;
+ m.sorted = 1;
+ m.packed = 1;
+
+ return m;
+}
+
+cholmod_dense SuiteSparse::CreateDenseVectorView(const double* x, int size) {
+ cholmod_dense v;
+ v.nrow = size;
+ v.ncol = 1;
+ v.nzmax = size;
+ v.d = size;
+ v.x = const_cast<void*>(reinterpret_cast<const void*>(x));
+ v.xtype = CHOLMOD_REAL;
+ v.dtype = CHOLMOD_DOUBLE;
+ return v;
+}
+
+cholmod_dense* SuiteSparse::CreateDenseVector(const double* x,
+ int in_size,
+ int out_size) {
+ CHECK_LE(in_size, out_size);
+ cholmod_dense* v = cholmod_zeros(out_size, 1, CHOLMOD_REAL, &cc_);
+ if (x != nullptr) {
+ memcpy(v->x, x, in_size * sizeof(*x));
+ }
+ return v;
+}
+
+cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A,
+ string* message) {
+ // Cholmod can try multiple re-ordering strategies to find a fill
+ // reducing ordering. Here we just tell it use AMD with automatic
+ // matrix dependence choice of supernodal versus simplicial
+ // factorization.
+ cc_.nmethods = 1;
+ cc_.method[0].ordering = CHOLMOD_AMD;
+ cc_.supernodal = CHOLMOD_AUTO;
+
+ cholmod_factor* factor = cholmod_analyze(A, &cc_);
+ if (VLOG_IS_ON(2)) {
+ cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
+ }
+
+ if (cc_.status != CHOLMOD_OK) {
+ *message =
+ StringPrintf("cholmod_analyze failed. error code: %d", cc_.status);
+ return nullptr;
+ }
+
+ CHECK(factor != nullptr);
+ return factor;
+}
+
+cholmod_factor* SuiteSparse::BlockAnalyzeCholesky(cholmod_sparse* A,
+ const vector<int>& row_blocks,
+ const vector<int>& col_blocks,
+ string* message) {
+ vector<int> ordering;
+ if (!BlockAMDOrdering(A, row_blocks, col_blocks, &ordering)) {
+ return nullptr;
+ }
+ return AnalyzeCholeskyWithUserOrdering(A, ordering, message);
+}
+
+cholmod_factor* SuiteSparse::AnalyzeCholeskyWithUserOrdering(
+ cholmod_sparse* A, const vector<int>& ordering, string* message) {
+ CHECK_EQ(ordering.size(), A->nrow);
+
+ cc_.nmethods = 1;
+ cc_.method[0].ordering = CHOLMOD_GIVEN;
+
+ cholmod_factor* factor =
+ cholmod_analyze_p(A, const_cast<int*>(&ordering[0]), nullptr, 0, &cc_);
+ if (VLOG_IS_ON(2)) {
+ cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
+ }
+ if (cc_.status != CHOLMOD_OK) {
+ *message =
+ StringPrintf("cholmod_analyze failed. error code: %d", cc_.status);
+ return nullptr;
+ }
+
+ CHECK(factor != nullptr);
+ return factor;
+}
+
+cholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering(
+ cholmod_sparse* A, string* message) {
+ cc_.nmethods = 1;
+ cc_.method[0].ordering = CHOLMOD_NATURAL;
+ cc_.postorder = 0;
+
+ cholmod_factor* factor = cholmod_analyze(A, &cc_);
+ if (VLOG_IS_ON(2)) {
+ cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
+ }
+ if (cc_.status != CHOLMOD_OK) {
+ *message =
+ StringPrintf("cholmod_analyze failed. error code: %d", cc_.status);
+ return nullptr;
+ }
+
+ CHECK(factor != nullptr);
+ return factor;
+}
+
+bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A,
+ const vector<int>& row_blocks,
+ const vector<int>& col_blocks,
+ vector<int>* ordering) {
+ const int num_row_blocks = row_blocks.size();
+ const int num_col_blocks = col_blocks.size();
+
+ // Arrays storing the compressed column structure of the matrix
+ // incoding the block sparsity of A.
+ vector<int> block_cols;
+ vector<int> block_rows;
+
+ CompressedColumnScalarMatrixToBlockMatrix(reinterpret_cast<const int*>(A->i),
+ reinterpret_cast<const int*>(A->p),
+ row_blocks,
+ col_blocks,
+ &block_rows,
+ &block_cols);
+ cholmod_sparse_struct block_matrix;
+ block_matrix.nrow = num_row_blocks;
+ block_matrix.ncol = num_col_blocks;
+ block_matrix.nzmax = block_rows.size();
+ block_matrix.p = reinterpret_cast<void*>(&block_cols[0]);
+ block_matrix.i = reinterpret_cast<void*>(&block_rows[0]);
+ block_matrix.x = nullptr;
+ block_matrix.stype = A->stype;
+ block_matrix.itype = CHOLMOD_INT;
+ block_matrix.xtype = CHOLMOD_PATTERN;
+ block_matrix.dtype = CHOLMOD_DOUBLE;
+ block_matrix.sorted = 1;
+ block_matrix.packed = 1;
+
+ vector<int> block_ordering(num_row_blocks);
+ if (!cholmod_amd(&block_matrix, nullptr, 0, &block_ordering[0], &cc_)) {
+ return false;
+ }
+
+ BlockOrderingToScalarOrdering(row_blocks, block_ordering, ordering);
+ return true;
+}
+
+LinearSolverTerminationType SuiteSparse::Cholesky(cholmod_sparse* A,
+ cholmod_factor* L,
+ string* message) {
+ CHECK(A != nullptr);
+ CHECK(L != nullptr);
+
+ // Save the current print level and silence CHOLMOD, otherwise
+ // CHOLMOD is prone to dumping stuff to stderr, which can be
+ // distracting when the error (matrix is indefinite) is not a fatal
+ // failure.
+ const int old_print_level = cc_.print;
+ cc_.print = 0;
+
+ cc_.quick_return_if_not_posdef = 1;
+ int cholmod_status = cholmod_factorize(A, L, &cc_);
+ cc_.print = old_print_level;
+
+ switch (cc_.status) {
+ case CHOLMOD_NOT_INSTALLED:
+ *message = "CHOLMOD failure: Method not installed.";
+ return LINEAR_SOLVER_FATAL_ERROR;
+ case CHOLMOD_OUT_OF_MEMORY:
+ *message = "CHOLMOD failure: Out of memory.";
+ return LINEAR_SOLVER_FATAL_ERROR;
+ case CHOLMOD_TOO_LARGE:
+ *message = "CHOLMOD failure: Integer overflow occurred.";
+ return LINEAR_SOLVER_FATAL_ERROR;
+ case CHOLMOD_INVALID:
+ *message = "CHOLMOD failure: Invalid input.";
+ return LINEAR_SOLVER_FATAL_ERROR;
+ case CHOLMOD_NOT_POSDEF:
+ *message = "CHOLMOD warning: Matrix not positive definite.";
+ return LINEAR_SOLVER_FAILURE;
+ case CHOLMOD_DSMALL:
+ *message =
+ "CHOLMOD warning: D for LDL' or diag(L) or "
+ "LL' has tiny absolute value.";
+ return LINEAR_SOLVER_FAILURE;
+ case CHOLMOD_OK:
+ if (cholmod_status != 0) {
+ return LINEAR_SOLVER_SUCCESS;
+ }
+
+ *message =
+ "CHOLMOD failure: cholmod_factorize returned false "
+ "but cholmod_common::status is CHOLMOD_OK."
+ "Please report this to ceres-solver@googlegroups.com.";
+ return LINEAR_SOLVER_FATAL_ERROR;
+ default:
+ *message = StringPrintf(
+ "Unknown cholmod return code: %d. "
+ "Please report this to ceres-solver@googlegroups.com.",
+ cc_.status);
+ return LINEAR_SOLVER_FATAL_ERROR;
+ }
+
+ return LINEAR_SOLVER_FATAL_ERROR;
+}
+
+cholmod_dense* SuiteSparse::Solve(cholmod_factor* L,
+ cholmod_dense* b,
+ string* message) {
+ if (cc_.status != CHOLMOD_OK) {
+ *message = "cholmod_solve failed. CHOLMOD status is not CHOLMOD_OK";
+ return nullptr;
+ }
+
+ return cholmod_solve(CHOLMOD_A, L, b, &cc_);
+}
+
+bool SuiteSparse::ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix,
+ int* ordering) {
+ return cholmod_amd(matrix, nullptr, 0, ordering, &cc_);
+}
+
+bool SuiteSparse::ConstrainedApproximateMinimumDegreeOrdering(
+ cholmod_sparse* matrix, int* constraints, int* ordering) {
+#ifndef CERES_NO_CAMD
+ return cholmod_camd(matrix, nullptr, 0, constraints, ordering, &cc_);
+#else
+ LOG(FATAL) << "Congratulations you have found a bug in Ceres."
+ << "Ceres Solver was compiled with SuiteSparse "
+ << "version 4.1.0 or less. Calling this function "
+ << "in that case is a bug. Please contact the"
+ << "the Ceres Solver developers.";
+ return false;
+#endif
+}
+
+std::unique_ptr<SparseCholesky> SuiteSparseCholesky::Create(
+ const OrderingType ordering_type) {
+ return std::unique_ptr<SparseCholesky>(new SuiteSparseCholesky(ordering_type));
+}
+
+SuiteSparseCholesky::SuiteSparseCholesky(const OrderingType ordering_type)
+ : ordering_type_(ordering_type), factor_(nullptr) {}
+
+SuiteSparseCholesky::~SuiteSparseCholesky() {
+ if (factor_ != nullptr) {
+ ss_.Free(factor_);
+ }
+}
+
+LinearSolverTerminationType SuiteSparseCholesky::Factorize(
+ CompressedRowSparseMatrix* lhs, string* message) {
+ if (lhs == nullptr) {
+ *message = "Failure: Input lhs is NULL.";
+ return LINEAR_SOLVER_FATAL_ERROR;
+ }
+
+ cholmod_sparse cholmod_lhs = ss_.CreateSparseMatrixTransposeView(lhs);
+
+ if (factor_ == nullptr) {
+ if (ordering_type_ == NATURAL) {
+ factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&cholmod_lhs, message);
+ } else {
+ if (!lhs->col_blocks().empty() && !(lhs->row_blocks().empty())) {
+ factor_ = ss_.BlockAnalyzeCholesky(
+ &cholmod_lhs, lhs->col_blocks(), lhs->row_blocks(), message);
+ } else {
+ factor_ = ss_.AnalyzeCholesky(&cholmod_lhs, message);
+ }
+ }
+
+ if (factor_ == nullptr) {
+ return LINEAR_SOLVER_FATAL_ERROR;
+ }
+ }
+
+ return ss_.Cholesky(&cholmod_lhs, factor_, message);
+}
+
+CompressedRowSparseMatrix::StorageType SuiteSparseCholesky::StorageType()
+ const {
+ return ((ordering_type_ == NATURAL)
+ ? CompressedRowSparseMatrix::UPPER_TRIANGULAR
+ : CompressedRowSparseMatrix::LOWER_TRIANGULAR);
+}
+
+LinearSolverTerminationType SuiteSparseCholesky::Solve(const double* rhs,
+ double* solution,
+ string* message) {
+ // Error checking
+ if (factor_ == nullptr) {
+ *message = "Solve called without a call to Factorize first.";
+ return LINEAR_SOLVER_FATAL_ERROR;
+ }
+
+ const int num_cols = factor_->n;
+ cholmod_dense cholmod_rhs = ss_.CreateDenseVectorView(rhs, num_cols);
+ cholmod_dense* cholmod_dense_solution =
+ ss_.Solve(factor_, &cholmod_rhs, message);
+
+ if (cholmod_dense_solution == nullptr) {
+ return LINEAR_SOLVER_FAILURE;
+ }
+
+ memcpy(solution, cholmod_dense_solution->x, num_cols * sizeof(*solution));
+ ss_.Free(cholmod_dense_solution);
+ return LINEAR_SOLVER_SUCCESS;
+}
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_NO_SUITESPARSE