Sparse covariance estimation.
Add a Covariance object to the API.
Given a Problem object and a set of parameter block pairs the
Covariance object computes a sparse covariance matrix corresponding
to those block pairs and provides random access to them.
Constant parameter blocks and parameter blocks with local parameterizations
are correctly handled.
Sparse and dense implementations are provided. With the dense implementation
rank deficient Jacobians can also be handled.
Parts of the code are threaded using OpenMP if available.
Change-Id: I5b49583b3d79579df3e0f334c22567acb23ed4ad
diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
new file mode 100644
index 0000000..1f73ffd
--- /dev/null
+++ b/internal/ceres/covariance_impl.cc
@@ -0,0 +1,535 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// 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/covariance_impl.h"
+
+#include <algorithm>
+#include <utility>
+#include <vector>
+#include "Eigen/SVD"
+#include "ceres/compressed_row_sparse_matrix.h"
+#include "ceres/covariance.h"
+#include "ceres/crs_matrix.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/map_util.h"
+#include "ceres/parameter_block.h"
+#include "ceres/problem_impl.h"
+#include "ceres/suitesparse.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+typedef vector<pair<const double*, const double*> > CovarianceBlocks;
+
+CovarianceImpl::CovarianceImpl(const Covariance::Options& options)
+ : options_(options) {
+ evaluate_options_.num_threads = options.num_threads;
+ evaluate_options_.apply_loss_function = options.apply_loss_function;
+}
+
+CovarianceImpl::~CovarianceImpl() {
+}
+
+bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks,
+ ProblemImpl* problem) {
+ problem_ = problem;
+
+ parameter_block_to_row_index_.clear();
+ covariance_matrix_.reset(NULL);
+
+ return (ComputeCovarianceSparsity(covariance_blocks, problem) &&
+ ComputeCovarianceValues());
+}
+
+bool CovarianceImpl::GetCovarianceBlock(const double* original_parameter_block1,
+ const double* original_parameter_block2,
+ double* covariance_block) const {
+ // If either of the two parameter blocks is constant, then the
+ // covariance block is also zero.
+ if (constant_parameter_blocks_.count(original_parameter_block1) > 0 ||
+ constant_parameter_blocks_.count(original_parameter_block2) > 0) {
+ const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map();
+ ParameterBlock* block1 =
+ FindOrDie(parameter_map,
+ const_cast<double*>(original_parameter_block1));
+
+ ParameterBlock* block2 =
+ FindOrDie(parameter_map,
+ const_cast<double*>(original_parameter_block2));
+ const int block1_size = block1->Size();
+ const int block2_size = block2->Size();
+ MatrixRef(covariance_block, block1_size, block2_size).setZero();
+ return true;
+ }
+
+ const double* parameter_block1 = original_parameter_block1;
+ const double* parameter_block2 = original_parameter_block2;
+ const bool transpose = parameter_block1 > parameter_block2;
+ if (transpose) {
+ std::swap(parameter_block1, parameter_block2);
+ }
+
+ // Find where in the covariance matrix the block is located.
+ const int row_begin =
+ FindOrDie(parameter_block_to_row_index_, parameter_block1);
+ const int col_begin =
+ FindOrDie(parameter_block_to_row_index_, parameter_block2);
+ const int* rows = covariance_matrix_->rows();
+ const int* cols = covariance_matrix_->cols();
+ const int row_size = rows[row_begin + 1] - rows[row_begin];
+ const int* cols_begin = cols + rows[row_begin];
+
+ // The only part that requires work is walking the compressed column
+ // vector to determine where the set of columns correspnding to the
+ // covariance block begin.
+ int offset = 0;
+ while (cols_begin[offset] != col_begin && offset < row_size) {
+ ++offset;
+ }
+
+ if (offset == row_size) {
+ LOG(WARNING) << "Unable to find covariance block for "
+ << original_parameter_block1 << " "
+ << original_parameter_block2;
+ return false;
+ }
+
+ const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map();
+ ParameterBlock* block1 =
+ FindOrDie(parameter_map, const_cast<double*>(parameter_block1));
+ ParameterBlock* block2 =
+ FindOrDie(parameter_map, const_cast<double*>(parameter_block2));
+ const LocalParameterization* local_param1 = block1->local_parameterization();
+ const LocalParameterization* local_param2 = block2->local_parameterization();
+ const int block1_size = block1->Size();
+ const int block1_local_size = block1->LocalSize();
+ const int block2_size = block2->Size();
+ const int block2_local_size = block2->LocalSize();
+
+ ConstMatrixRef cov(covariance_matrix_->values() + rows[row_begin],
+ block1_size,
+ row_size);
+
+ // Fast path when there are no local parameterizations.
+ if (local_param1 == NULL && local_param2 == NULL) {
+ if (transpose) {
+ MatrixRef(covariance_block, block2_size, block1_size) =
+ cov.block(0, offset, block1_size, block2_size).transpose();
+ } else {
+ MatrixRef(covariance_block, block1_size, block2_size) =
+ cov.block(0, offset, block1_size, block2_size);
+ }
+ return true;
+ }
+
+ // If local parameterizations are used then the covariance that has
+ // been computed is in the tangent space and it needs to be lifted
+ // back to the ambient space.
+ //
+ // This is given by the formula
+ //
+ // C'_12 = J_1 C_12 J_2'
+ //
+ // Where C_12 is the local tangent space covariance for parameter
+ // blocks 1 and 2. J_1 and J_2 are respectively the local to global
+ // jacobians for parameter blocks 1 and 2.
+ //
+ // See Result 5.11 on page 142 of Hartley & Zisserman (2nd Edition)
+ // for a proof.
+ //
+ // TODO(sameeragarwal): Add caching of local parameterization, so
+ // that they are computed just once per parameter block.
+ Matrix block1_jacobian(block1_size, block1_local_size);
+ if (local_param1 == NULL) {
+ block1_jacobian.setIdentity();
+ } else {
+ local_param1->ComputeJacobian(parameter_block1, block1_jacobian.data());
+ }
+
+ Matrix block2_jacobian(block2_size, block2_local_size);
+ // Fast path if the user is requesting a diagonal block.
+ if (parameter_block1 == parameter_block2) {
+ block2_jacobian = block1_jacobian;
+ } else {
+ if (local_param2 == NULL) {
+ block2_jacobian.setIdentity();
+ } else {
+ local_param2->ComputeJacobian(parameter_block2, block2_jacobian.data());
+ }
+ }
+
+ if (transpose) {
+ MatrixRef(covariance_block, block2_size, block1_size) =
+ block2_jacobian *
+ cov.block(0, offset, block1_local_size, block2_local_size).transpose() *
+ block1_jacobian.transpose();
+ } else {
+ MatrixRef(covariance_block, block1_size, block2_size) =
+ block1_jacobian *
+ cov.block(0, offset, block1_local_size, block2_local_size) *
+ block2_jacobian.transpose();
+ }
+
+ return true;
+}
+
+// Determine the sparsity pattern of the covariance matrix based on
+// the block pairs requested by the user.
+bool CovarianceImpl::ComputeCovarianceSparsity(
+ const CovarianceBlocks& original_covariance_blocks,
+ ProblemImpl* problem) {
+
+ // Determine an ordering for the parameter block, by sorting the
+ // parameter blocks by their pointers.
+ vector<double*> all_parameter_blocks;
+ problem->GetParameterBlocks(&all_parameter_blocks);
+ const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map();
+ constant_parameter_blocks_.clear();
+ vector<double*>& active_parameter_blocks = evaluate_options_.parameter_blocks;
+ active_parameter_blocks.clear();
+ for (int i = 0; i < all_parameter_blocks.size(); ++i) {
+ double* parameter_block = all_parameter_blocks[i];
+
+ ParameterBlock* block = FindOrDie(parameter_map, parameter_block);
+ if (block->IsConstant()) {
+ constant_parameter_blocks_.insert(parameter_block);
+ } else {
+ active_parameter_blocks.push_back(parameter_block);
+ }
+ }
+
+ sort(active_parameter_blocks.begin(), active_parameter_blocks.end());
+
+ // Compute the number of rows. Map each parameter block to the
+ // first row corresponding to it in the covariance matrix using the
+ // ordering of parameter blocks just constructed.
+ int num_rows = 0;
+ parameter_block_to_row_index_.clear();
+ for (int i = 0; i < active_parameter_blocks.size(); ++i) {
+ double* parameter_block = active_parameter_blocks[i];
+ const int parameter_block_size =
+ problem->ParameterBlockLocalSize(parameter_block);
+ parameter_block_to_row_index_[parameter_block] = num_rows;
+ num_rows += parameter_block_size;
+ }
+
+ // Compute the number of non-zeros in the covariance matrix. Along
+ // the way flip any covariance blocks which are in the lower
+ // triangular part of the matrix.
+ int num_nonzeros = 0;
+ CovarianceBlocks covariance_blocks;
+ for (int i = 0; i < original_covariance_blocks.size(); ++i) {
+ const pair<const double*, const double*>& block_pair =
+ original_covariance_blocks[i];
+ if (constant_parameter_blocks_.count(block_pair.first) > 0 ||
+ constant_parameter_blocks_.count(block_pair.second) > 0) {
+ continue;
+ }
+
+ int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first);
+ int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second);
+ const int size1 = problem->ParameterBlockLocalSize(block_pair.first);
+ const int size2 = problem->ParameterBlockLocalSize(block_pair.second);
+ num_nonzeros += size1 * size2;
+
+ // Make sure we are constructing a block upper triangular matrix.
+ if (index1 > index2) {
+ covariance_blocks.push_back(make_pair(block_pair.second,
+ block_pair.first));
+ } else {
+ covariance_blocks.push_back(block_pair);
+ }
+ }
+
+ if (covariance_blocks.size() == 0) {
+ VLOG(2) << "No non-zero covariance blocks found";
+ covariance_matrix_.reset(NULL);
+ return true;
+ }
+
+ // Sort the block pairs. As a consequence we get the covariance
+ // blocks as they will occur in the CompressedRowSparseMatrix that
+ // will store the covariance.
+ sort(covariance_blocks.begin(), covariance_blocks.end());
+
+ // Fill the sparsity pattern of the covariance matrix.
+ covariance_matrix_.reset(
+ new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros));
+
+ int* rows = covariance_matrix_->mutable_rows();
+ int* cols = covariance_matrix_->mutable_cols();
+
+ // Iterate over parameter blocks and in turn over the rows of the
+ // covariance matrix. For each parameter block, look in the upper
+ // triangular part of the covariance matrix to see if there are any
+ // blocks requested by the user. If this is the case then fill out a
+ // set of compressed rows corresponding to this parameter block.
+ //
+ // The key thing that makes this loop work is the fact that the
+ // row/columns of the covariance matrix are ordered by the pointer
+ // values of the parameter blocks. Thus iterating over the keys of
+ // parameter_block_to_row_index_ corresponds to iterating over the
+ // rows of the covariance matrix in order.
+ int i = 0; // index into covariance_blocks.
+ int cursor = 0; // index into the covariance matrix.
+ for (map<const double*, int>::const_iterator it =
+ parameter_block_to_row_index_.begin();
+ it != parameter_block_to_row_index_.end();
+ ++it) {
+ const double* row_block = it->first;
+ const int row_block_size = problem->ParameterBlockLocalSize(row_block);
+ int row_begin = it->second;
+
+ // Iterate over the covariance blocks contained in this row block
+ // and count the number of columns in this row block.
+ int num_col_blocks = 0;
+ int num_columns = 0;
+ for (int j = i; j < covariance_blocks.size(); ++j, ++num_col_blocks) {
+ const pair<const double*, const double*>& block_pair =
+ covariance_blocks[j];
+ if (block_pair.first != row_block) {
+ break;
+ }
+ num_columns += problem->ParameterBlockLocalSize(block_pair.second);
+ }
+
+ // Fill out all the compressed rows for this parameter block.
+ for (int r = 0; r < row_block_size; ++r) {
+ rows[row_begin + r] = cursor;
+ for (int c = 0; c < num_col_blocks; ++c) {
+ const double* col_block = covariance_blocks[i + c].second;
+ const int col_block_size = problem->ParameterBlockLocalSize(col_block);
+ int col_begin = FindOrDie(parameter_block_to_row_index_, col_block);
+ for (int k = 0; k < col_block_size; ++k) {
+ cols[cursor++] = col_begin++;
+ }
+ }
+ }
+
+ i+= num_col_blocks;
+ }
+
+ rows[num_rows] = cursor;
+ return true;
+}
+
+bool CovarianceImpl::ComputeCovarianceValues() {
+ if (options_.use_dense_linear_algebra) {
+ return ComputeCovarianceValuesUsingEigen();
+ }
+
+#ifndef CERES_NO_SUITESPARSE
+ return ComputeCovarianceValuesUsingSuiteSparse();
+#else
+ LOG(ERROR) << "Ceres compiled without SuiteSparse. "
+ << "Large scale covariance computation is not possible.";
+ return false;
+#endif
+}
+
+bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparse() {
+#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);
+
+ // m is a transposed view of the Jacobian.
+ cholmod_sparse cholmod_jacobian_view;
+ cholmod_jacobian_view.nrow = jacobian.num_cols;
+ cholmod_jacobian_view.ncol = jacobian.num_rows;
+ cholmod_jacobian_view.nzmax = jacobian.values.size();
+ cholmod_jacobian_view.nz = NULL;
+ cholmod_jacobian_view.p = reinterpret_cast<void*>(&jacobian.rows[0]);
+ cholmod_jacobian_view.i = reinterpret_cast<void*>(&jacobian.cols[0]);
+ cholmod_jacobian_view.x = reinterpret_cast<void*>(&jacobian.values[0]);
+ cholmod_jacobian_view.z = NULL;
+ cholmod_jacobian_view.stype = 0; // Matrix is not symmetric.
+ cholmod_jacobian_view.itype = CHOLMOD_INT;
+ cholmod_jacobian_view.xtype = CHOLMOD_REAL;
+ cholmod_jacobian_view.dtype = CHOLMOD_DOUBLE;
+ cholmod_jacobian_view.sorted = 1;
+ cholmod_jacobian_view.packed = 1;
+
+ cholmod_factor* factor = ss_.AnalyzeCholesky(&cholmod_jacobian_view);
+ if (!ss_.Cholesky(&cholmod_jacobian_view, factor)) {
+ ss_.Free(factor);
+ LOG(WARNING) << "Cholesky factorization failed.";
+ return false;
+ }
+
+ const int num_rows = covariance_matrix_->num_rows();
+ const int* rows = covariance_matrix_->rows();
+ const int* cols = covariance_matrix_->cols();
+ double* values = covariance_matrix_->mutable_values();
+
+ cholmod_dense* rhs = ss_.CreateDenseVector(NULL, num_rows, num_rows);
+ double* rhs_x = reinterpret_cast<double*>(rhs->x);
+
+ // 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.
+ //
+ // The ifdef separates two different version of SuiteSparse. Newer
+ // versions of SuiteSparse have the cholmod_solve2 function which
+ // re-uses memory across calls.
+#if (SUITESPARSE_VERSION < 4002)
+ for (int r = 0; r < num_rows; ++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* 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);
+ rhs_x[r] = 0.0;
+ }
+#else
+ // TODO(sameeragarwal) There should be a more efficient way
+ // involving the use of Bset but I am unable to make it work right
+ // now.
+ cholmod_dense* solution = NULL;
+ cholmod_sparse* solution_set = NULL;
+ cholmod_dense* y_workspace = NULL;
+ cholmod_dense* e_workspace = NULL;
+
+ for (int r = 0; r < num_rows; ++r) {
+ int row_begin = rows[r];
+ int row_end = rows[r + 1];
+ if (row_end == row_begin) {
+ continue;
+ }
+
+ rhs_x[r] = 1.0;
+
+ cholmod_solve2(CHOLMOD_A,
+ factor,
+ rhs,
+ NULL,
+ &solution,
+ &solution_set,
+ &y_workspace,
+ &e_workspace,
+ ss_.mutable_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];
+ }
+ rhs_x[r] = 0.0;
+ }
+
+ ss_.Free(solution);
+ ss_.Free(solution_set);
+ ss_.Free(y_workspace);
+ ss_.Free(e_workspace);
+
+#endif
+
+ ss_.Free(rhs);
+ ss_.Free(factor);
+ return true;
+#else
+ return false;
+#endif
+};
+
+bool CovarianceImpl::ComputeCovarianceValuesUsingEigen() {
+ if (covariance_matrix_.get() == NULL) {
+ // Nothing to do, all zeros covariance matrix.
+ return true;
+ }
+
+ CRSMatrix jacobian;
+ problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
+ Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols);
+ dense_jacobian.setZero();
+ for (int r = 0; r < jacobian.num_rows; ++r) {
+ for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
+ const int c = jacobian.cols[idx];
+ dense_jacobian(r, c) = jacobian.values[idx];
+ }
+ }
+
+ Eigen::JacobiSVD<Matrix> svd(dense_jacobian,
+ Eigen::ComputeThinU | Eigen::ComputeThinV);
+ Vector inverse_singular_values = svd.singularValues();
+
+ for (int i = 0; i < inverse_singular_values.rows(); ++i) {
+ if (inverse_singular_values[i] > options_.min_singular_value_threshold &&
+ i < (inverse_singular_values.rows() - options_.null_space_rank)) {
+ inverse_singular_values[i] =
+ 1.0 / (inverse_singular_values[i] * inverse_singular_values[i]);
+ } else {
+ inverse_singular_values[i] = 0.0;
+ }
+ }
+
+ Matrix dense_covariance =
+ svd.matrixV() *
+ inverse_singular_values.asDiagonal() *
+ svd.matrixV().transpose();
+
+ const int num_rows = covariance_matrix_->num_rows();
+ const int* rows = covariance_matrix_->rows();
+ const int* cols = covariance_matrix_->cols();
+ double* values = covariance_matrix_->mutable_values();
+
+ for (int r = 0; r < num_rows; ++r) {
+ for (int idx = rows[r]; idx < rows[r + 1]; ++idx) {
+ const int c = cols[idx];
+ values[idx] = dense_covariance(r, c);
+ }
+ }
+ return true;
+};
+
+
+
+} // namespace internal
+} // namespace ceres
