mirror of
https://github.com/ceres-solver/ceres-solver.git
synced 2026-08-29 16:40:38 +08:00
Sameer Agarwal
5742b7d0f1
The outer product computation logic in SparseNormalCholeskySolver does not work well with dynamic sparsity. The overhead of computing the sparsity pattern of the normal equations is only amortized if the sparsity is constant. If the sparsity can change from call to call SparseNormalCholeskySolver will actually be more expensive. For Eigen and for CXSparse we now explicitly compute the normal equations using their respective matrix-matrix product routines and solve. Change-Id: Ifbd8ed78987cdf71640e66ed69500442526a23d4
487 lines
16 KiB
C++
487 lines
16 KiB
C++
// 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)
|
|
|
|
#include "ceres/sparse_normal_cholesky_solver.h"
|
|
|
|
#include <algorithm>
|
|
#include <cstring>
|
|
#include <ctime>
|
|
|
|
#include "ceres/compressed_row_sparse_matrix.h"
|
|
#include "ceres/cxsparse.h"
|
|
#include "ceres/internal/eigen.h"
|
|
#include "ceres/internal/scoped_ptr.h"
|
|
#include "ceres/linear_solver.h"
|
|
#include "ceres/suitesparse.h"
|
|
#include "ceres/triplet_sparse_matrix.h"
|
|
#include "ceres/types.h"
|
|
#include "ceres/wall_time.h"
|
|
#include "Eigen/SparseCore"
|
|
|
|
#ifdef CERES_USE_EIGEN_SPARSE
|
|
#include "Eigen/SparseCholesky"
|
|
#endif
|
|
|
|
namespace ceres {
|
|
namespace internal {
|
|
namespace {
|
|
|
|
#ifdef CERES_USE_EIGEN_SPARSE
|
|
// A templated factorized and solve function, which allows us to use
|
|
// the same code independent of whether a AMD or a Natural ordering is
|
|
// used.
|
|
template <typename SimplicialCholeskySolver, typename SparseMatrixType>
|
|
LinearSolver::Summary SimplicialLDLTSolve(
|
|
const SparseMatrixType& lhs,
|
|
const bool do_symbolic_analysis,
|
|
SimplicialCholeskySolver* solver,
|
|
double* rhs_and_solution,
|
|
EventLogger* event_logger) {
|
|
LinearSolver::Summary summary;
|
|
summary.num_iterations = 1;
|
|
summary.termination_type = LINEAR_SOLVER_SUCCESS;
|
|
summary.message = "Success.";
|
|
|
|
if (do_symbolic_analysis) {
|
|
solver->analyzePattern(lhs);
|
|
event_logger->AddEvent("Analyze");
|
|
if (solver->info() != Eigen::Success) {
|
|
summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
|
|
summary.message =
|
|
"Eigen failure. Unable to find symbolic factorization.";
|
|
return summary;
|
|
}
|
|
}
|
|
|
|
solver->factorize(lhs);
|
|
event_logger->AddEvent("Factorize");
|
|
if (solver->info() != Eigen::Success) {
|
|
summary.termination_type = LINEAR_SOLVER_FAILURE;
|
|
summary.message = "Eigen failure. Unable to find numeric factorization.";
|
|
return summary;
|
|
}
|
|
|
|
const Vector rhs = VectorRef(rhs_and_solution, lhs.cols());
|
|
|
|
VectorRef(rhs_and_solution, lhs.cols()) = solver->solve(rhs);
|
|
event_logger->AddEvent("Solve");
|
|
if (solver->info() != Eigen::Success) {
|
|
summary.termination_type = LINEAR_SOLVER_FAILURE;
|
|
summary.message = "Eigen failure. Unable to do triangular solve.";
|
|
return summary;
|
|
}
|
|
|
|
return summary;
|
|
}
|
|
|
|
#endif // CERES_USE_EIGEN_SPARSE
|
|
|
|
#ifndef CERES_NO_CXSPARSE
|
|
LinearSolver::Summary ComputeNormalEquationsAndSolveUsingCXSparse(
|
|
CompressedRowSparseMatrix* A,
|
|
double * rhs_and_solution,
|
|
EventLogger* event_logger) {
|
|
LinearSolver::Summary summary;
|
|
summary.num_iterations = 1;
|
|
summary.termination_type = LINEAR_SOLVER_SUCCESS;
|
|
summary.message = "Success.";
|
|
|
|
CXSparse cxsparse;
|
|
|
|
// Wrap the augmented Jacobian in a compressed sparse column matrix.
|
|
cs_di a_transpose = cxsparse.CreateSparseMatrixTransposeView(A);
|
|
|
|
// Compute the normal equations. J'J delta = J'f and solve them
|
|
// using a sparse Cholesky factorization. Notice that when compared
|
|
// to SuiteSparse we have to explicitly compute the transpose of Jt,
|
|
// and then the normal equations before they can be
|
|
// factorized. CHOLMOD/SuiteSparse on the other hand can just work
|
|
// off of Jt to compute the Cholesky factorization of the normal
|
|
// equations.
|
|
cs_di* a = cxsparse.TransposeMatrix(&a_transpose);
|
|
cs_di* lhs = cxsparse.MatrixMatrixMultiply(&a_transpose, a);
|
|
cxsparse.Free(a);
|
|
event_logger->AddEvent("NormalEquations");
|
|
|
|
cs_dis* factor = cxsparse.AnalyzeCholesky(lhs);
|
|
event_logger->AddEvent("Analysis");
|
|
|
|
if (factor == NULL) {
|
|
summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
|
|
summary.message = "CXSparse::AnalyzeCholesky failed.";
|
|
} else if (!cxsparse.SolveCholesky(lhs, factor, rhs_and_solution)) {
|
|
summary.termination_type = LINEAR_SOLVER_FAILURE;
|
|
summary.message = "CXSparse::SolveCholesky failed.";
|
|
}
|
|
event_logger->AddEvent("Solve");
|
|
|
|
cxsparse.Free(lhs);
|
|
cxsparse.Free(factor);
|
|
event_logger->AddEvent("TearDown");
|
|
return summary;
|
|
}
|
|
|
|
#endif // CERES_NO_CXSPARSE
|
|
|
|
} // namespace
|
|
|
|
SparseNormalCholeskySolver::SparseNormalCholeskySolver(
|
|
const LinearSolver::Options& options)
|
|
: factor_(NULL),
|
|
cxsparse_factor_(NULL),
|
|
options_(options) {
|
|
}
|
|
|
|
void SparseNormalCholeskySolver::FreeFactorization() {
|
|
if (factor_ != NULL) {
|
|
ss_.Free(factor_);
|
|
factor_ = NULL;
|
|
}
|
|
|
|
if (cxsparse_factor_ != NULL) {
|
|
cxsparse_.Free(cxsparse_factor_);
|
|
cxsparse_factor_ = NULL;
|
|
}
|
|
}
|
|
|
|
SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {
|
|
FreeFactorization();
|
|
}
|
|
|
|
LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl(
|
|
CompressedRowSparseMatrix* A,
|
|
const double* b,
|
|
const LinearSolver::PerSolveOptions& per_solve_options,
|
|
double * x) {
|
|
|
|
const int num_cols = A->num_cols();
|
|
VectorRef(x, num_cols).setZero();
|
|
A->LeftMultiply(b, x);
|
|
|
|
if (per_solve_options.D != NULL) {
|
|
// Temporarily append a diagonal block to the A matrix, but undo
|
|
// it before returning the matrix to the user.
|
|
scoped_ptr<CompressedRowSparseMatrix> regularizer;
|
|
if (A->col_blocks().size() > 0) {
|
|
regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix(
|
|
per_solve_options.D, A->col_blocks()));
|
|
} else {
|
|
regularizer.reset(new CompressedRowSparseMatrix(
|
|
per_solve_options.D, num_cols));
|
|
}
|
|
A->AppendRows(*regularizer);
|
|
}
|
|
|
|
LinearSolver::Summary summary;
|
|
switch (options_.sparse_linear_algebra_library_type) {
|
|
case SUITE_SPARSE:
|
|
summary = SolveImplUsingSuiteSparse(A, x);
|
|
break;
|
|
case CX_SPARSE:
|
|
summary = SolveImplUsingCXSparse(A, x);
|
|
break;
|
|
case EIGEN_SPARSE:
|
|
summary = SolveImplUsingEigen(A, x);
|
|
break;
|
|
default:
|
|
LOG(FATAL) << "Unknown sparse linear algebra library : "
|
|
<< options_.sparse_linear_algebra_library_type;
|
|
}
|
|
|
|
if (per_solve_options.D != NULL) {
|
|
A->DeleteRows(num_cols);
|
|
}
|
|
|
|
return summary;
|
|
}
|
|
|
|
LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen(
|
|
CompressedRowSparseMatrix* A,
|
|
double * rhs_and_solution) {
|
|
#ifndef CERES_USE_EIGEN_SPARSE
|
|
|
|
LinearSolver::Summary summary;
|
|
summary.num_iterations = 0;
|
|
summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
|
|
summary.message =
|
|
"SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE "
|
|
"because Ceres was not built with support for "
|
|
"Eigen's SimplicialLDLT decomposition. "
|
|
"This requires enabling building with -DEIGENSPARSE=ON.";
|
|
return summary;
|
|
|
|
#else
|
|
|
|
EventLogger event_logger("SparseNormalCholeskySolver::Eigen::Solve");
|
|
// Compute the normal equations. J'J delta = J'f and solve them
|
|
// using a sparse Cholesky factorization. Notice that when compared
|
|
// to SuiteSparse we have to explicitly compute the normal equations
|
|
// before they can be factorized. CHOLMOD/SuiteSparse on the other
|
|
// hand can just work off of Jt to compute the Cholesky
|
|
// factorization of the normal equations.
|
|
|
|
if (options_.dynamic_sparsity) {
|
|
// In the case where the problem has dynamic sparsity, it is not
|
|
// worth using the ComputeOuterProduct routine, as the setup cost
|
|
// is not amortized over multiple calls to Solve.
|
|
Eigen::MappedSparseMatrix<double, Eigen::RowMajor> a(
|
|
A->num_rows(),
|
|
A->num_cols(),
|
|
A->num_nonzeros(),
|
|
A->mutable_rows(),
|
|
A->mutable_cols(),
|
|
A->mutable_values());
|
|
|
|
Eigen::SparseMatrix<double> lhs = a.transpose() * a;
|
|
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
|
|
return SimplicialLDLTSolve(lhs,
|
|
true,
|
|
&solver,
|
|
rhs_and_solution,
|
|
&event_logger);
|
|
}
|
|
|
|
if (outer_product_.get() == NULL) {
|
|
outer_product_.reset(
|
|
CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
|
|
*A, &pattern_));
|
|
}
|
|
|
|
CompressedRowSparseMatrix::ComputeOuterProduct(
|
|
*A, pattern_, outer_product_.get());
|
|
|
|
// Map to an upper triangular column major matrix.
|
|
//
|
|
// outer_product_ is a compressed row sparse matrix and in lower
|
|
// triangular form, when mapped to a compressed column sparse
|
|
// matrix, it becomes an upper triangular matrix.
|
|
Eigen::MappedSparseMatrix<double, Eigen::ColMajor> lhs(
|
|
outer_product_->num_rows(),
|
|
outer_product_->num_rows(),
|
|
outer_product_->num_nonzeros(),
|
|
outer_product_->mutable_rows(),
|
|
outer_product_->mutable_cols(),
|
|
outer_product_->mutable_values());
|
|
|
|
bool do_symbolic_analysis = false;
|
|
|
|
// If using post ordering or an old version of Eigen, we cannot
|
|
// depend on a preordered jacobian, so we work with a SimplicialLDLT
|
|
// decomposition with AMD ordering.
|
|
if (options_.use_postordering ||
|
|
!EIGEN_VERSION_AT_LEAST(3, 2, 2)) {
|
|
if (amd_ldlt_.get() == NULL) {
|
|
amd_ldlt_.reset(new SimplicialLDLTWithAMDOrdering);
|
|
do_symbolic_analysis = true;
|
|
}
|
|
|
|
return SimplicialLDLTSolve(lhs,
|
|
do_symbolic_analysis,
|
|
amd_ldlt_.get(),
|
|
rhs_and_solution,
|
|
&event_logger);
|
|
}
|
|
|
|
#if EIGEN_VERSION_AT_LEAST(3,2,2)
|
|
// The common case
|
|
if (natural_ldlt_.get() == NULL) {
|
|
natural_ldlt_.reset(new SimplicialLDLTWithNaturalOrdering);
|
|
do_symbolic_analysis = true;
|
|
}
|
|
|
|
return SimplicialLDLTSolve(lhs,
|
|
do_symbolic_analysis,
|
|
natural_ldlt_.get(),
|
|
rhs_and_solution,
|
|
&event_logger);
|
|
#endif
|
|
|
|
#endif // EIGEN_USE_EIGEN_SPARSE
|
|
}
|
|
|
|
LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse(
|
|
CompressedRowSparseMatrix* A,
|
|
double * rhs_and_solution) {
|
|
#ifdef CERES_NO_CXSPARSE
|
|
|
|
LinearSolver::Summary summary;
|
|
summary.num_iterations = 0;
|
|
summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
|
|
summary.message =
|
|
"SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE "
|
|
"because Ceres was not built with support for CXSparse. "
|
|
"This requires enabling building with -DCXSPARSE=ON.";
|
|
|
|
return summary;
|
|
|
|
#else
|
|
|
|
EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve");
|
|
if (options_.dynamic_sparsity) {
|
|
return ComputeNormalEquationsAndSolveUsingCXSparse(A,
|
|
rhs_and_solution,
|
|
&event_logger);
|
|
}
|
|
|
|
LinearSolver::Summary summary;
|
|
summary.num_iterations = 1;
|
|
summary.termination_type = LINEAR_SOLVER_SUCCESS;
|
|
summary.message = "Success.";
|
|
|
|
// Compute the normal equations. J'J delta = J'f and solve them
|
|
// using a sparse Cholesky factorization. Notice that when compared
|
|
// to SuiteSparse we have to explicitly compute the normal equations
|
|
// before they can be factorized. CHOLMOD/SuiteSparse on the other
|
|
// hand can just work off of Jt to compute the Cholesky
|
|
// factorization of the normal equations.
|
|
if (outer_product_.get() == NULL) {
|
|
outer_product_.reset(
|
|
CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
|
|
*A, &pattern_));
|
|
}
|
|
|
|
CompressedRowSparseMatrix::ComputeOuterProduct(
|
|
*A, pattern_, outer_product_.get());
|
|
cs_di lhs =
|
|
cxsparse_.CreateSparseMatrixTransposeView(outer_product_.get());
|
|
|
|
event_logger.AddEvent("Setup");
|
|
|
|
// Compute symbolic factorization if not available.
|
|
if (cxsparse_factor_ == NULL) {
|
|
if (options_.use_postordering) {
|
|
cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(&lhs,
|
|
A->col_blocks(),
|
|
A->col_blocks());
|
|
} else {
|
|
cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(&lhs);
|
|
}
|
|
}
|
|
event_logger.AddEvent("Analysis");
|
|
|
|
if (cxsparse_factor_ == NULL) {
|
|
summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
|
|
summary.message =
|
|
"CXSparse failure. Unable to find symbolic factorization.";
|
|
} else if (!cxsparse_.SolveCholesky(&lhs,
|
|
cxsparse_factor_,
|
|
rhs_and_solution)) {
|
|
summary.termination_type = LINEAR_SOLVER_FAILURE;
|
|
summary.message = "CXSparse::SolveCholesky failed.";
|
|
}
|
|
event_logger.AddEvent("Solve");
|
|
|
|
return summary;
|
|
#endif
|
|
}
|
|
|
|
LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
|
|
CompressedRowSparseMatrix* A,
|
|
double * rhs_and_solution) {
|
|
#ifdef CERES_NO_SUITESPARSE
|
|
|
|
LinearSolver::Summary summary;
|
|
summary.num_iterations = 0;
|
|
summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
|
|
summary.message =
|
|
"SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE "
|
|
"because Ceres was not built with support for SuiteSparse. "
|
|
"This requires enabling building with -DSUITESPARSE=ON.";
|
|
return summary;
|
|
|
|
#else
|
|
|
|
EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve");
|
|
LinearSolver::Summary summary;
|
|
summary.termination_type = LINEAR_SOLVER_SUCCESS;
|
|
summary.num_iterations = 1;
|
|
summary.message = "Success.";
|
|
|
|
const int num_cols = A->num_cols();
|
|
cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A);
|
|
event_logger.AddEvent("Setup");
|
|
|
|
if (options_.dynamic_sparsity) {
|
|
FreeFactorization();
|
|
}
|
|
|
|
if (factor_ == NULL) {
|
|
if (options_.use_postordering) {
|
|
factor_ = ss_.BlockAnalyzeCholesky(&lhs,
|
|
A->col_blocks(),
|
|
A->row_blocks(),
|
|
&summary.message);
|
|
} else {
|
|
if (options_.dynamic_sparsity) {
|
|
factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message);
|
|
} else {
|
|
factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs,
|
|
&summary.message);
|
|
}
|
|
}
|
|
}
|
|
event_logger.AddEvent("Analysis");
|
|
|
|
if (factor_ == NULL) {
|
|
summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
|
|
// No need to set message as it has already been set by the
|
|
// symbolic analysis routines above.
|
|
return summary;
|
|
}
|
|
|
|
summary.termination_type = ss_.Cholesky(&lhs, factor_, &summary.message);
|
|
if (summary.termination_type != LINEAR_SOLVER_SUCCESS) {
|
|
return summary;
|
|
}
|
|
|
|
cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution,
|
|
num_cols,
|
|
num_cols);
|
|
cholmod_dense* solution = ss_.Solve(factor_, rhs, &summary.message);
|
|
event_logger.AddEvent("Solve");
|
|
|
|
ss_.Free(rhs);
|
|
if (solution != NULL) {
|
|
memcpy(rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution));
|
|
ss_.Free(solution);
|
|
} else {
|
|
// No need to set message as it has already been set by the
|
|
// numeric factorization routine above.
|
|
summary.termination_type = LINEAR_SOLVER_FAILURE;
|
|
}
|
|
|
|
event_logger.AddEvent("Teardown");
|
|
return summary;
|
|
#endif
|
|
}
|
|
|
|
} // namespace internal
|
|
} // namespace ceres
|