Covariance estimation using SuiteSparseQR.
Change-Id: I70d1686e3288fdde5f9723e832e15ffb857d6d85
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index dfa567c..a7f1d97 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -152,6 +152,7 @@
ENDIF (GFLAGS)
IF (SUITESPARSE_FOUND)
+ LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${SUITESPARSEQR_LIB})
LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CHOLMOD_LIB})
LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CCOLAMD_LIB})
LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CAMD_LIB})
@@ -170,8 +171,14 @@
IF (EXISTS ${BLAS_LIB})
LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${BLAS_LIB})
ENDIF (EXISTS ${BLAS_LIB})
+
+ IF (TBB_FOUND)
+ LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${TBB_LIB})
+ LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${TBB_MALLOC_LIB})
+ ENDIF (TBB_FOUND)
ENDIF (SUITESPARSE_FOUND)
+
IF (CXSPARSE_FOUND)
LIST(APPEND CERES_LIBRARY_DEPENDENCIES ${CXSPARSE_LIB})
ENDIF (CXSPARSE_FOUND)
diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
index e3401ea..bb04cd0 100644
--- a/internal/ceres/covariance_impl.cc
+++ b/internal/ceres/covariance_impl.cc
@@ -48,6 +48,7 @@
#include "ceres/suitesparse.h"
#include "ceres/wall_time.h"
#include "glog/logging.h"
+#include "SuiteSparseQR.hpp"
namespace ceres {
namespace internal {
@@ -388,28 +389,34 @@
}
bool CovarianceImpl::ComputeCovarianceValues() {
- if (options_.use_dense_linear_algebra) {
- return ComputeCovarianceValuesUsingEigen();
- }
-
+ switch (options_.algorithm_type) {
+ case (DENSE_SVD):
+ return ComputeCovarianceValuesUsingDenseSVD();
#ifndef CERES_NO_SUITESPARSE
- return ComputeCovarianceValuesUsingSuiteSparse();
-#else
- LOG(ERROR) << "Ceres compiled without SuiteSparse. "
- << "Large scale covariance computation is not possible.";
- return false;
+ case (SPARSE_CHOLESKY):
+ return ComputeCovarianceValuesUsingSparseCholesky();
+ case (SPARSE_QR):
+ return ComputeCovarianceValuesUsingSparseQR();
#endif
+ default:
+ LOG(ERROR) << "Unsupported covariance estimation algorithm type: "
+ << CovarianceAlgorithmTypeToString(options_.algorithm_type);
+ return false;
+ }
+ return false;
}
-bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparse() {
+bool CovarianceImpl::ComputeCovarianceValuesUsingSparseCholesky() {
EventLogger event_logger(
- "CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparse");
+ "CovarianceImpl::ComputeCovarianceValuesUsingSparseCholesky");
#ifndef CERES_NO_SUITESPARSE
if (covariance_matrix_.get() == NULL) {
// Nothing to do, all zeros covariance matrix.
return true;
}
+ SuiteSparse ss;
+
CRSMatrix jacobian;
problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
@@ -431,12 +438,12 @@
cholmod_jacobian_view.sorted = 1;
cholmod_jacobian_view.packed = 1;
- cholmod_factor* factor = ss_.AnalyzeCholesky(&cholmod_jacobian_view);
+ cholmod_factor* factor = ss.AnalyzeCholesky(&cholmod_jacobian_view);
event_logger.AddEvent("Symbolic Factorization");
- bool factorization_succeeded = ss_.Cholesky(&cholmod_jacobian_view, factor);
+ bool factorization_succeeded = ss.Cholesky(&cholmod_jacobian_view, factor);
if (factorization_succeeded) {
const double reciprocal_condition_number =
- cholmod_rcond(factor, ss_.mutable_cc());
+ cholmod_rcond(factor, ss.mutable_cc());
if (reciprocal_condition_number <
options_.min_reciprocal_condition_number) {
LOG(WARNING) << "Cholesky factorization of J'J is not reliable. "
@@ -450,7 +457,7 @@
event_logger.AddEvent("Numeric Factorization");
if (!factorization_succeeded) {
- ss_.Free(factor);
+ ss.Free(factor);
LOG(WARNING) << "Cholesky factorization failed.";
return false;
}
@@ -474,7 +481,7 @@
// versions of SuiteSparse have the cholmod_solve2 function which
// re-uses memory across calls.
#if (SUITESPARSE_VERSION < 4002)
- cholmod_dense* rhs = ss_.CreateDenseVector(NULL, num_rows, num_rows);
+ cholmod_dense* rhs = ss.CreateDenseVector(NULL, num_rows, num_rows);
double* rhs_x = reinterpret_cast<double*>(rhs->x);
for (int r = 0; r < num_rows; ++r) {
@@ -485,17 +492,17 @@
}
rhs_x[r] = 1.0;
- cholmod_dense* solution = ss_.Solve(factor, rhs);
+ cholmod_dense* solution = ss.Solve(factor, rhs);
double* solution_x = reinterpret_cast<double*>(solution->x);
for (int idx = row_begin; idx < row_end; ++idx) {
const int c = cols[idx];
values[idx] = solution_x[c];
}
- ss_.Free(solution);
+ ss.Free(solution);
rhs_x[r] = 0.0;
}
- ss_.Free(rhs);
+ ss.Free(rhs);
#else // SUITESPARSE_VERSION < 4002
const int num_threads = options_.num_threads;
@@ -567,7 +574,7 @@
#endif // SUITESPARSE_VERSION < 4002
- ss_.Free(factor);
+ ss.Free(factor);
event_logger.AddEvent("Inversion");
return true;
@@ -578,9 +585,144 @@
#endif // CERES_NO_SUITESPARSE
};
-bool CovarianceImpl::ComputeCovarianceValuesUsingEigen() {
+bool CovarianceImpl::ComputeCovarianceValuesUsingSparseQR() {
EventLogger event_logger(
- "CovarianceImpl::ComputeCovarianceValuesUsingEigen");
+ "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR");
+
+#ifndef CERES_NO_SUITESPARSE
+ if (covariance_matrix_.get() == NULL) {
+ // Nothing to do, all zeros covariance matrix.
+ return true;
+ }
+
+ CRSMatrix jacobian;
+ problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
+ event_logger.AddEvent("Evaluate");
+
+ // Construct a compressed column form of the Jacobian.
+ const int num_rows = jacobian.num_rows;
+ const int num_cols = jacobian.num_cols;
+ const int num_nonzeros = jacobian.values.size();
+
+ vector<SuiteSparse_long> transpose_rows(num_cols + 1, 0);
+ vector<SuiteSparse_long> transpose_cols(num_nonzeros, 0);
+ vector<double> transpose_values(num_nonzeros, 0);
+
+ for (int idx = 0; idx < num_nonzeros; ++idx) {
+ transpose_rows[jacobian.cols[idx] + 1] += 1;
+ }
+
+ for (int i = 1; i < transpose_rows.size(); ++i) {
+ transpose_rows[i] += transpose_rows[i - 1];
+ }
+
+ for (int r = 0; r < num_rows; ++r) {
+ for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
+ const int c = jacobian.cols[idx];
+ const int transpose_idx = transpose_rows[c];
+ transpose_cols[transpose_idx] = r;
+ transpose_values[transpose_idx] = jacobian.values[idx];
+ ++transpose_rows[c];
+ }
+ }
+
+ for (int i = transpose_rows.size() - 1; i > 0 ; --i) {
+ transpose_rows[i] = transpose_rows[i - 1];
+ }
+ transpose_rows[0] = 0;
+
+ cholmod_sparse cholmod_jacobian;
+ cholmod_jacobian.nrow = num_rows;
+ cholmod_jacobian.ncol = num_cols;
+ cholmod_jacobian.nzmax = num_nonzeros;
+ cholmod_jacobian.nz = NULL;
+ cholmod_jacobian.p = reinterpret_cast<void*>(&transpose_rows[0]);
+ cholmod_jacobian.i = reinterpret_cast<void*>(&transpose_cols[0]);
+ cholmod_jacobian.x = reinterpret_cast<void*>(&transpose_values[0]);
+ cholmod_jacobian.z = NULL;
+ cholmod_jacobian.stype = 0; // Matrix is not symmetric.
+ cholmod_jacobian.itype = CHOLMOD_LONG;
+ cholmod_jacobian.xtype = CHOLMOD_REAL;
+ cholmod_jacobian.dtype = CHOLMOD_DOUBLE;
+ cholmod_jacobian.sorted = 1;
+ cholmod_jacobian.packed = 1;
+
+ cholmod_common cc;
+ cholmod_l_start(&cc);
+
+ SuiteSparseQR_factorization<double>* factor =
+ SuiteSparseQR_factorize<double>(SPQR_ORDERING_BESTAMD,
+ SPQR_DEFAULT_TOL,
+ &cholmod_jacobian,
+ &cc);
+ event_logger.AddEvent("Numeric Factorization");
+
+ const int rank = cc.SPQR_istat[4];
+ if (rank < cholmod_jacobian.ncol) {
+ LOG(WARNING) << "Jacobian matrix is rank deficient."
+ << "Number of columns: " << cholmod_jacobian.ncol
+ << " rank: " << rank;
+ SuiteSparseQR_free(&factor, &cc);
+ cholmod_l_finish(&cc);
+ return false;
+ }
+
+ const int* rows = covariance_matrix_->rows();
+ const int* cols = covariance_matrix_->cols();
+ double* values = covariance_matrix_->mutable_values();
+
+ // The following loop exploits the fact that the i^th column of A^{-1}
+ // is given by the solution to the linear system
+ //
+ // A x = e_i
+ //
+ // where e_i is a vector with e(i) = 1 and all other entries zero.
+ //
+ // Since the covariance matrix is symmetric, the i^th row and column
+ // are equal.
+
+ cholmod_dense* rhs = cholmod_l_zeros(num_cols, 1, CHOLMOD_REAL, &cc);
+ double* rhs_x = reinterpret_cast<double*>(rhs->x);
+
+ for (int r = 0; r < num_cols; ++r) {
+ int row_begin = rows[r];
+ int row_end = rows[r + 1];
+ if (row_end == row_begin) {
+ continue;
+ }
+
+ rhs_x[r] = 1.0;
+
+ cholmod_dense* y1 = SuiteSparseQR_solve<double>(SPQR_RTX_EQUALS_ETB, factor, rhs, &cc);
+ cholmod_dense* solution = SuiteSparseQR_solve<double>(SPQR_RETX_EQUALS_B, factor, y1, &cc);
+
+ double* solution_x = reinterpret_cast<double*>(solution->x);
+ for (int idx = row_begin; idx < row_end; ++idx) {
+ const int c = cols[idx];
+ values[idx] = solution_x[c];
+ }
+
+ cholmod_l_free_dense(&y1, &cc);
+ cholmod_l_free_dense(&solution, &cc);
+ rhs_x[r] = 0.0;
+ }
+
+ cholmod_l_free_dense(&rhs, &cc);
+ SuiteSparseQR_free(&factor, &cc);
+ cholmod_l_finish(&cc);
+ event_logger.AddEvent("Inversion");
+ return true;
+
+#else // CERES_NO_SUITESPARSE
+
+ return false;
+
+#endif // CERES_NO_SUITESPARSE
+};
+
+bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() {
+ EventLogger event_logger(
+ "CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD");
if (covariance_matrix_.get() == NULL) {
// Nothing to do, all zeros covariance matrix.
return true;
@@ -602,6 +744,7 @@
Eigen::JacobiSVD<Matrix> svd(dense_jacobian,
Eigen::ComputeThinU | Eigen::ComputeThinV);
+
event_logger.AddEvent("SingularValueDecomposition");
const Vector singular_values = svd.singularValues();
diff --git a/internal/ceres/covariance_impl.h b/internal/ceres/covariance_impl.h
index 0be53e7..0e7e217 100644
--- a/internal/ceres/covariance_impl.h
+++ b/internal/ceres/covariance_impl.h
@@ -64,8 +64,9 @@
ProblemImpl* problem);
bool ComputeCovarianceValues();
- bool ComputeCovarianceValuesUsingSuiteSparse();
- bool ComputeCovarianceValuesUsingEigen();
+ bool ComputeCovarianceValuesUsingSparseCholesky();
+ bool ComputeCovarianceValuesUsingSparseQR();
+ bool ComputeCovarianceValuesUsingDenseSVD();
const CompressedRowSparseMatrix* covariance_matrix() const {
return covariance_matrix_.get();
@@ -80,9 +81,6 @@
map<const double*, int> parameter_block_to_row_index_;
set<const double*> constant_parameter_blocks_;
scoped_ptr<CompressedRowSparseMatrix> covariance_matrix_;
-#ifndef CERES_NO_SUITESPARSE
- SuiteSparse ss_;
-#endif
};
} // namespace internal
diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc
index f9c5cd4..e7d25a1 100644
--- a/internal/ceres/covariance_test.cc
+++ b/internal/ceres/covariance_test.cc
@@ -400,11 +400,14 @@
Covariance::Options options;
#ifndef CERES_NO_SUITESPARSE
- options.use_dense_linear_algebra = false;
+ options.algorithm_type = SPARSE_CHOLESKY;
+ ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+
+ options.algorithm_type = SPARSE_QR;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
#endif
- options.use_dense_linear_algebra = true;
+ options.algorithm_type = DENSE_SVD;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
@@ -445,11 +448,14 @@
options.num_threads = 4;
#ifndef CERES_NO_SUITESPARSE
- options.use_dense_linear_algebra = false;
+ options.algorithm_type = SPARSE_CHOLESKY;
+ ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+
+ options.algorithm_type = SPARSE_QR;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
#endif
- options.use_dense_linear_algebra = true;
+ options.algorithm_type = DENSE_SVD;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
@@ -491,11 +497,11 @@
Covariance::Options options;
#ifndef CERES_NO_SUITESPARSE
- options.use_dense_linear_algebra = false;
+ options.algorithm_type = SPARSE_CHOLESKY;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
#endif
- options.use_dense_linear_algebra = true;
+ options.algorithm_type = DENSE_SVD;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
@@ -544,11 +550,11 @@
Covariance::Options options;
#ifndef CERES_NO_SUITESPARSE
- options.use_dense_linear_algebra = false;
+ options.algorithm_type = SPARSE_CHOLESKY;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
#endif
- options.use_dense_linear_algebra = true;
+ options.algorithm_type = DENSE_SVD;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
@@ -589,7 +595,7 @@
{
Covariance::Options options;
- options.use_dense_linear_algebra = true;
+ options.algorithm_type = DENSE_SVD;
// Force dropping of the smallest eigenvector.
options.null_space_rank = 1;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
@@ -597,7 +603,7 @@
{
Covariance::Options options;
- options.use_dense_linear_algebra = true;
+ options.algorithm_type = DENSE_SVD;
// Force dropping of the smallest eigenvector via the ratio but
// automatic truncation.
options.min_reciprocal_condition_number = 0.044494;
@@ -693,7 +699,7 @@
};
Covariance::Options options;
- options.use_dense_linear_algebra = true;
+ options.algorithm_type = DENSE_SVD;
options.null_space_rank = -1;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
@@ -723,9 +729,10 @@
}
}
- void ComputeAndCompare(int num_threads) {
+ void ComputeAndCompare(CovarianceAlgorithmType algorithm_type,
+ int num_threads) {
Covariance::Options options;
- options.use_dense_linear_algebra = false;
+ options.algorithm_type = algorithm_type;
options.num_threads = num_threads;
Covariance covariance(options);
EXPECT_TRUE(covariance.Compute(all_covariance_blocks_, &problem_));
@@ -768,7 +775,7 @@
#if !defined(CERES_NO_SUITESPARSE) && defined(CERES_USE_OPENMP)
TEST_F(LargeScaleCovarianceTest, Parallel) {
- ComputeAndCompare(4);
+ ComputeAndCompare(SPARSE_CHOLESKY, 4);
}
#endif // !defined(CERES_NO_SUITESPARSE) && defined(CERES_USE_OPENMP)
diff --git a/internal/ceres/types.cc b/internal/ceres/types.cc
index 42990e3..164185e 100644
--- a/internal/ceres/types.cc
+++ b/internal/ceres/types.cc
@@ -239,6 +239,27 @@
return false;
}
+const char* CovarianceAlgorithmTypeToString(
+ CovarianceAlgorithmType type) {
+ switch (type) {
+ CASESTR(DENSE_SVD);
+ CASESTR(SPARSE_CHOLESKY);
+ CASESTR(SPARSE_QR);
+ default:
+ return "UNKNOWN";
+ }
+}
+
+bool StringToCovarianceAlgorithmType(
+ string value,
+ CovarianceAlgorithmType* type) {
+ UpperCase(&value);
+ STRENUM(DENSE_SVD);
+ STRENUM(SPARSE_CHOLESKY);
+ STRENUM(SPARSE_QR);
+ return false;
+}
+
const char* SolverTerminationTypeToString(SolverTerminationType type) {
switch (type) {
CASESTR(NO_CONVERGENCE);
