Add covariance matrix for a vector of parameters

Computing the covariance matrix for a number of parameter blocks
previously required adding all parameter blocks to the computation and
subsequently assembling the matrix by concatenating all the blocks.

This patch adds the computation of the covariance matrix for a vector
of parameter blocks. All covariance block pairs are added automatically
and the resulting covariance matrix is assembled in the order the
parameter blocks appear.

Change-Id: I3b70c63f16862adc23a1d7fb7a21dde4e68abe9a

include/ceres/covariance.h

@@ -357,6 +357,28 @@ class CERES_EXPORT Covariance {
                                   const double*> >& covariance_blocks,
       Problem* problem);
 
+  // Compute a part of the covariance matrix.
+  //
+  // The vector parameter_blocks contains the parameter blocks that
+  // are used for computing the covariance matrix. From this vector
+  // all covariance pairs are generated. This allows the covariance
+  // estimation algorithm to only compute and store these blocks.
+  //
+  // parameter_blocks cannot contain duplicates. Bad things will
+  // happen if they do.
+  //
+  // Note that the list of covariance_blocks is only used to determine
+  // what parts of the covariance matrix are computed. The full
+  // Jacobian is used to do the computation, i.e. they do not have an
+  // impact on what part of the Jacobian is used for computation.
+  //
+  // The return value indicates the success or failure of the
+  // covariance computation. Please see the documentation for
+  // Covariance::Options for more on the conditions under which this
+  // function returns false.
+  bool Compute(const std::vector<const double*>& parameter_blocks,
+               Problem* problem);
+
   // Return the block of the cross-covariance matrix corresponding to
   // parameter_block1 and parameter_block2.
   //
@@ -394,6 +416,40 @@ class CERES_EXPORT Covariance {
                                         const double* parameter_block2,
                                         double* covariance_block) const;
 
+  // Return the covariance matrix corresponding to all parameter_blocks.
+  //
+  // Compute must be called before calling GetCovarianceMatrix and all
+  // parameter_blocks must have been present in the vector
+  // parameters_blocks when Compute was called. Otherwise
+  // GetCovarianceMatrix returns false.
+  //
+  // covariance_matrix must point to a memory location that can store
+  // the size of the covariance matrix. The size of the covariance
+  // matrix corresponds the sum of the sizes of the individual
+  // parameter blocks. The covariance matrix will be a row-major
+  // matrix.
+  bool GetCovarianceMatrix(const std::vector<const double*>& parameter_blocks,
+                           double* covariance_matrix);
+
+  // Return the covariance matrix corresponding to parameter_blocks
+  // in the tangent space if a local parameterization is associated
+  // with one of the parameter blocks else returns the covariance
+  // matrix in the ambient space.
+  //
+  // Compute must be called before calling GetCovarianceMatrix and all
+  // parameter_blocks must have been present in the vector
+  // parameters_blocks when Compute was called. Otherwise
+  // GetCovarianceMatrix returns false.
+  //
+  // covariance_matrix must point to a memory location that can store
+  // the size of the covariance matrix. The size of the covariance
+  // matrix corresponds the sum of the sizes of the individual
+  // parameter blocks. The covariance matrix will be a row-major
+  // matrix.
+  bool GetCovarianceMatrixInTangentSpace(
+      const std::vector<const double*>& parameter_blocks,
+      double* covariance_matrix);
+
  private:
   internal::scoped_ptr<internal::CovarianceImpl> impl_;
 };

internal/ceres/covariance.cc

@@ -38,6 +38,7 @@
 
 namespace ceres {
 
+using std::make_pair;
 using std::pair;
 using std::vector;
 
@@ -54,6 +55,12 @@ bool Covariance::Compute(
   return impl_->Compute(covariance_blocks, problem->problem_impl_.get());
 }
 
+bool Covariance::Compute(
+    const vector<const double*>& parameter_blocks,
+    Problem* problem) {
+  return impl_->Compute(parameter_blocks, problem->problem_impl_.get());
+}
+
 bool Covariance::GetCovarianceBlock(const double* parameter_block1,
                                     const double* parameter_block2,
                                     double* covariance_block) const {
@@ -73,4 +80,20 @@ bool Covariance::GetCovarianceBlockInTangentSpace(
                                                           covariance_block);
 }
 
+bool Covariance::GetCovarianceMatrix(
+    const vector<const double*>& parameter_blocks,
+    double* covariance_matrix) {
+  return impl_->GetCovarianceMatrixInTangentOrAmbientSpace(parameter_blocks,
+                                                           true,  // ambient
+                                                           covariance_matrix);
+}
+
+bool Covariance::GetCovarianceMatrixInTangentSpace(
+    const std::vector<const double *>& parameter_blocks,
+    double *covariance_matrix) {
+  return impl_->GetCovarianceMatrixInTangentOrAmbientSpace(parameter_blocks,
+                                                           false,  // tangent
+                                                           covariance_matrix);
+}
+
 }  // namespace ceres

internal/ceres/covariance_impl.cc

@@ -36,6 +36,8 @@
 
 #include <algorithm>
 #include <cstdlib>
+#include <numeric>
+#include <sstream>
 #include <utility>
 #include <vector>
 
@@ -63,6 +65,7 @@ namespace internal {
 using std::make_pair;
 using std::map;
 using std::pair;
+using std::sort;
 using std::swap;
 using std::vector;
 
@@ -88,8 +91,37 @@ CovarianceImpl::CovarianceImpl(const Covariance::Options& options)
 CovarianceImpl::~CovarianceImpl() {
 }
 
+template <typename T> void CheckForDuplicates(vector<T> blocks) {
+  sort(blocks.begin(), blocks.end());
+  typename vector<T>::iterator it = std::adjacent_find(blocks.begin(), blocks
+      .end());
+  if (it != blocks.end()) {
+    // In case there are duplicates, we search for their location.
+    map<T, vector<int> > blocks_map;
+    for (int i=0; i < blocks.size(); ++i) {
+      blocks_map[blocks[i]].push_back(i);
+    }
+    std::ostringstream duplicates;
+    while (it != blocks.end()) {
+      duplicates << "(";
+      for (int i=0; i < blocks_map[*it].size()-1; ++i) {
+        duplicates << blocks_map[*it][i] << ", ";
+      }
+      duplicates << blocks_map[*it].back() << ")";
+      it = std::adjacent_find(it + 1, blocks.end());
+      if (it < blocks.end()) {
+        duplicates << " and ";
+      }
+    }
+    CHECK(false) << "Covariance::Compute called with duplicate blocks at "
+                 << "indices "
+                 << duplicates.str();
+  }
+}
+
 bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks,
                              ProblemImpl* problem) {
+  CheckForDuplicates<pair<const double*, const double*> >(covariance_blocks);
   problem_ = problem;
   parameter_block_to_row_index_.clear();
   covariance_matrix_.reset(NULL);
@@ -99,6 +131,25 @@ bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks,
   return is_valid_;
 }
 
+bool CovarianceImpl::Compute(const vector<const double*>& parameter_blocks,
+                             ProblemImpl* problem) {
+  CheckForDuplicates<const double*>(parameter_blocks);
+  vector<pair<const double*, const double*> > covariance_blocks;
+  for (int i = 0; i < parameter_blocks.size(); ++i) {
+    for (int j = i; j < parameter_blocks.size(); ++j) {
+      covariance_blocks.push_back(make_pair(parameter_blocks[i],
+                                            parameter_blocks[j]));
+    }
+  }
+  problem_ = problem;
+  parameter_block_to_row_index_.clear();
+  covariance_matrix_.reset(NULL);
+  is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) &&
+      ComputeCovarianceValues());
+  is_computed_ = true;
+  return is_valid_;
+}
+
 bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace(
     const double* original_parameter_block1,
     const double* original_parameter_block2,
@@ -250,6 +301,83 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace(
   return true;
 }
 
+bool CovarianceImpl::GetCovarianceMatrixInTangentOrAmbientSpace(
+    const vector<const double*>& parameters,
+    bool lift_covariance_to_ambient_space,
+    double* covariance_matrix) const {
+  CHECK(is_computed_)
+      << "Covariance::GetCovarianceMatrix called before Covariance::Compute";
+  CHECK(is_valid_)
+      << "Covariance::GetCovarianceMatrix called when Covariance::Compute "
+      << "returned false.";
+
+  const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map();
+  // For OpenMP compatibility we need to define these vectors in advance
+  vector<int> parameter_sizes;
+  vector<int> cum_parameter_size;
+  parameter_sizes.reserve(parameters.size());
+  cum_parameter_size.resize(parameters.size() + 1);
+  cum_parameter_size[0] = 0;
+  for (int i = 0; i < parameters.size(); ++i) {
+    ParameterBlock* block = FindOrDie(parameter_map,
+                                      const_cast<double*>(parameters[i]));
+    if (lift_covariance_to_ambient_space) {
+      parameter_sizes.push_back(block->Size());
+    } else {
+      parameter_sizes.push_back(block->LocalSize());
+    }
+  }
+  std::partial_sum(parameter_sizes.begin(), parameter_sizes.end(),
+                   cum_parameter_size.begin() + 1);
+  const int max_covariance_block_size = *std::max_element(
+      parameter_sizes.begin(),
+      parameter_sizes.end());
+  const int covariance_size = cum_parameter_size.back();
+
+  // Assemble the blocks in the covariance matrix.
+  MatrixRef covariance(covariance_matrix, covariance_size, covariance_size);
+  const int num_threads = options_.num_threads;
+  scoped_array<double> workspace(new double[num_threads *
+      max_covariance_block_size * max_covariance_block_size]);
+
+  bool success = true;
+#pragma omp parallel for num_threads (num_threads) schedule(dynamic) collapse(2)
+  for (int i = 0; i < parameters.size(); ++i) {
+    for (int j = 0; j < parameters.size(); ++j) {
+      // The second loop can't start from j = i for compatibility with OpenMP
+      // collapse command. The conditional serves as a workaround
+      if (j >= i) {
+        int covariance_row_idx = cum_parameter_size[i];
+        int covariance_col_idx = cum_parameter_size[j];
+        int size_i = parameter_sizes[i];
+        int size_j = parameter_sizes[j];
+#ifdef CERES_USE_OPENMP
+        int thread_id = omp_get_thread_num();
+#else
+        int thread_id = 0;
+#endif
+        double* covariance_block = workspace.get() + thread_id *
+            max_covariance_block_size * max_covariance_block_size;
+        if (!GetCovarianceBlockInTangentOrAmbientSpace(
+            parameters[i],
+            parameters[j],
+            lift_covariance_to_ambient_space,
+            covariance_block)) {
+          success = false;
+        }
+        covariance.block(covariance_row_idx, covariance_col_idx, size_i,
+                         size_j) = MatrixRef(covariance_block, size_i, size_j);
+        if (i != j) {
+          covariance.block(covariance_col_idx, covariance_row_idx, size_j,
+                           size_i) = MatrixRef(covariance_block, size_i,
+                                               size_j).transpose();
+        }
+      }
+    }
+  }
+  return success;
+}
+
 // Determine the sparsity pattern of the covariance matrix based on
 // the block pairs requested by the user.
 bool CovarianceImpl::ComputeCovarianceSparsity(

internal/ceres/covariance_impl.h

@@ -55,12 +55,21 @@ class CovarianceImpl {
                                   const double*> >& covariance_blocks,
       ProblemImpl* problem);
 
+  bool Compute(
+      const std::vector<const double*>& parameter_blocks,
+      ProblemImpl* problem);
+
   bool GetCovarianceBlockInTangentOrAmbientSpace(
       const double* parameter_block1,
       const double* parameter_block2,
       bool lift_covariance_to_ambient_space,
       double* covariance_block) const;
 
+  bool GetCovarianceMatrixInTangentOrAmbientSpace(
+      const std::vector<const double*>& parameters,
+      bool lift_covariance_to_ambient_space,
+      double *covariance_matrix) const;
+
   bool ComputeCovarianceSparsity(
       const std::vector<std::pair<const double*,
                                   const double*> >& covariance_blocks,

internal/ceres/covariance_test.cc

@@ -963,6 +963,120 @@ TEST_F(CovarianceTest, TruncatedRank) {
   }
 }
 
+TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParameters) {
+  Covariance::Options options;
+  Covariance covariance(options);
+  double* x = parameters_;
+  double* y = x + 2;
+  double* z = y + 3;
+  vector<const double*> parameter_blocks;
+  parameter_blocks.push_back(x);
+  parameter_blocks.push_back(y);
+  parameter_blocks.push_back(z);
+  covariance.Compute(parameter_blocks, &problem_);
+  double expected_covariance[36];
+  covariance.GetCovarianceMatrix(parameter_blocks, expected_covariance);
+
+#ifndef CERES_NO_SUITESPARSE
+  options.algorithm_type = SUITE_SPARSE_QR;
+  ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+#endif
+
+  options.algorithm_type = DENSE_SVD;
+  ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+
+  options.algorithm_type = EIGEN_SPARSE_QR;
+  ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+}
+
+TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParametersThreaded) {
+  Covariance::Options options;
+  options.num_threads = 4;
+  Covariance covariance(options);
+  double* x = parameters_;
+  double* y = x + 2;
+  double* z = y + 3;
+  vector<const double*> parameter_blocks;
+  parameter_blocks.push_back(x);
+  parameter_blocks.push_back(y);
+  parameter_blocks.push_back(z);
+  covariance.Compute(parameter_blocks, &problem_);
+  double expected_covariance[36];
+  covariance.GetCovarianceMatrix(parameter_blocks, expected_covariance);
+
+#ifndef CERES_NO_SUITESPARSE
+  options.algorithm_type = SUITE_SPARSE_QR;
+  ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+#endif
+
+  options.algorithm_type = DENSE_SVD;
+  ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+
+  options.algorithm_type = EIGEN_SPARSE_QR;
+  ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+}
+
+TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParametersInTangentSpace) {
+  Covariance::Options options;
+  Covariance covariance(options);
+  double* x = parameters_;
+  double* y = x + 2;
+  double* z = y + 3;
+
+  problem_.SetParameterization(x, new PolynomialParameterization);
+
+  vector<int> subset;
+  subset.push_back(2);
+  problem_.SetParameterization(y, new SubsetParameterization(3, subset));
+
+  local_column_bounds_[x] = make_pair(0, 1);
+  local_column_bounds_[y] = make_pair(1, 3);
+  local_column_bounds_[z] = make_pair(3, 4);
+
+  vector<const double*> parameter_blocks;
+  parameter_blocks.push_back(x);
+  parameter_blocks.push_back(y);
+  parameter_blocks.push_back(z);
+  covariance.Compute(parameter_blocks, &problem_);
+  double expected_covariance[16];
+  covariance.GetCovarianceMatrixInTangentSpace(parameter_blocks,
+                                               expected_covariance);
+
+#ifndef CERES_NO_SUITESPARSE
+  options.algorithm_type = SUITE_SPARSE_QR;
+  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);
+#endif
+
+  options.algorithm_type = DENSE_SVD;
+  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);
+
+  options.algorithm_type = EIGEN_SPARSE_QR;
+  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);
+}
+
+TEST_F(CovarianceTest, ComputeCovarianceFailure) {
+  Covariance::Options options;
+  Covariance covariance(options);
+  double* x = parameters_;
+  double* y = x + 2;
+  vector<const double*> parameter_blocks;
+  parameter_blocks.push_back(x);
+  parameter_blocks.push_back(x);
+  parameter_blocks.push_back(y);
+  parameter_blocks.push_back(y);
+  EXPECT_DEATH_IF_SUPPORTED(covariance.Compute(parameter_blocks, &problem_),
+                            "Covariance::Compute called with duplicate blocks "
+                            "at indices \\(0, 1\\) and \\(2, 3\\)");
+  vector<pair<const double*, const double*> > covariance_blocks;
+  covariance_blocks.push_back(make_pair(x, x));
+  covariance_blocks.push_back(make_pair(x, x));
+  covariance_blocks.push_back(make_pair(y, y));
+  covariance_blocks.push_back(make_pair(y, y));
+  EXPECT_DEATH_IF_SUPPORTED(covariance.Compute(covariance_blocks, &problem_),
+                            "Covariance::Compute called with duplicate blocks "
+                            "at indices \\(0, 1\\) and \\(2, 3\\)");
+}
+
 class RankDeficientCovarianceTest : public CovarianceTest {
  protected:
   virtual void SetUp() {