Add method to return covariance in tangent space
This CL is required to build Tango.
Inspired by this commit in RedwoodInternal repository:
commit 09dde53c248e04f432b5eccceea5daeedb706aea
Author: Mike Vitus <mike@hidof.com>
Date: Wed Apr 23 11:05:17 2014 -0700
Change-Id: I328b6634969de4ccdd71947945aa67a49ee9073f
diff --git a/docs/source/nnls_solving.rst b/docs/source/nnls_solving.rst
index c2d2369..95f1a36 100644
--- a/docs/source/nnls_solving.rst
+++ b/docs/source/nnls_solving.rst
@@ -2519,7 +2519,7 @@
.. function:: bool GetCovarianceBlock(const double* parameter_block1, const double* parameter_block2, double* covariance_block) const
- Return the block of the covariance matrix corresponding to
+ Return the block of the cross-covariance matrix corresponding to
``parameter_block1`` and ``parameter_block2``.
Compute must be called before the first call to ``GetCovarianceBlock``
@@ -2532,6 +2532,24 @@
a ``parameter_block1_size x parameter_block2_size`` matrix. The
returned covariance will be a row-major matrix.
+.. function:: bool GetCovarianceBlockInTangentSpace(const double* parameter_block1, const double* parameter_block2, double* covariance_block) const
+
+ Return the block of the cross-covariance matrix corresponding to
+ ``parameter_block1`` and ``parameter_block2``.
+ Returns cross-covariance in the tangent space if a local
+ parameterization is associated with either parameter block;
+ else returns cross-covariance in the ambient space.
+
+ Compute must be called before the first call to ``GetCovarianceBlock``
+ and the pair ``<parameter_block1, parameter_block2>`` OR the pair
+ ``<parameter_block2, parameter_block1>`` must have been present in the
+ vector covariance_blocks when ``Compute`` was called. Otherwise
+ ``GetCovarianceBlock`` will return false.
+
+ ``covariance_block`` must point to a memory location that can store
+ a ``parameter_block1_local_size x parameter_block2_local_size`` matrix. The
+ returned covariance will be a row-major matrix.
+
Example Usage
-------------
diff --git a/include/ceres/covariance.h b/include/ceres/covariance.h
index 1966340..5d342e8 100644
--- a/include/ceres/covariance.h
+++ b/include/ceres/covariance.h
@@ -357,7 +357,7 @@
const double*> >& covariance_blocks,
Problem* problem);
- // Return the block of the covariance matrix corresponding to
+ // Return the block of the cross-covariance matrix corresponding to
// parameter_block1 and parameter_block2.
//
// Compute must be called before the first call to
@@ -374,6 +374,26 @@
const double* parameter_block2,
double* covariance_block) const;
+ // Return the block of the cross-covariance matrix corresponding to
+ // parameter_block1 and parameter_block2.
+ // Returns cross-covariance in the tangent space if a local
+ // parameterization is associated with either parameter block;
+ // else returns cross-covariance in the ambient space.
+ //
+ // Compute must be called before the first call to
+ // GetCovarianceBlock and the pair <parameter_block1,
+ // parameter_block2> OR the pair <parameter_block2,
+ // parameter_block1> must have been present in the vector
+ // covariance_blocks when Compute was called. Otherwise
+ // GetCovarianceBlock will return false.
+ //
+ // covariance_block must point to a memory location that can store a
+ // parameter_block1_local_size x parameter_block2_local_size matrix. The
+ // returned covariance will be a row-major matrix.
+ bool GetCovarianceBlockInTangentSpace(const double* parameter_block1,
+ const double* parameter_block2,
+ double* covariance_block) const;
+
private:
internal::scoped_ptr<internal::CovarianceImpl> impl_;
};
diff --git a/internal/ceres/covariance.cc b/internal/ceres/covariance.cc
index 4955f4a..b6d3201 100644
--- a/internal/ceres/covariance.cc
+++ b/internal/ceres/covariance.cc
@@ -57,9 +57,20 @@
bool Covariance::GetCovarianceBlock(const double* parameter_block1,
const double* parameter_block2,
double* covariance_block) const {
- return impl_->GetCovarianceBlock(parameter_block1,
- parameter_block2,
- covariance_block);
+ return impl_->GetCovarianceBlockInTangentOrAmbientSpace(parameter_block1,
+ parameter_block2,
+ true, // ambient
+ covariance_block);
+}
+
+bool Covariance::GetCovarianceBlockInTangentSpace(
+ const double* parameter_block1,
+ const double* parameter_block2,
+ double* covariance_block) const {
+ return impl_->GetCovarianceBlockInTangentOrAmbientSpace(parameter_block1,
+ parameter_block2,
+ false, // tangent
+ covariance_block);
}
} // namespace ceres
diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
index e5d0c44..7790374 100644
--- a/internal/ceres/covariance_impl.cc
+++ b/internal/ceres/covariance_impl.cc
@@ -97,9 +97,11 @@
return is_valid_;
}
-bool CovarianceImpl::GetCovarianceBlock(const double* original_parameter_block1,
- const double* original_parameter_block2,
- double* covariance_block) const {
+bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace(
+ const double* original_parameter_block1,
+ const double* original_parameter_block2,
+ bool lift_covariance_to_ambient_space,
+ double* covariance_block) const {
CHECK(is_computed_)
<< "Covariance::GetCovarianceBlock called before Covariance::Compute";
CHECK(is_valid_)
@@ -172,14 +174,17 @@
block1_size,
row_size);
- // Fast path when there are no local parameterizations.
- if (local_param1 == NULL && local_param2 == NULL) {
+ // Fast path when there are no local parameterizations or if the
+ // user does not want it lifted to the ambient space.
+ if ((local_param1 == NULL && local_param2 == NULL) ||
+ !lift_covariance_to_ambient_space) {
if (transpose) {
- MatrixRef(covariance_block, block2_size, block1_size) =
- cov.block(0, offset, block1_size, block2_size).transpose();
+ MatrixRef(covariance_block, block2_local_size, block1_local_size) =
+ cov.block(0, offset, block1_local_size,
+ block2_local_size).transpose();
} else {
- MatrixRef(covariance_block, block1_size, block2_size) =
- cov.block(0, offset, block1_size, block2_size);
+ MatrixRef(covariance_block, block1_local_size, block2_local_size) =
+ cov.block(0, offset, block1_local_size, block2_local_size);
}
return true;
}
diff --git a/internal/ceres/covariance_impl.h b/internal/ceres/covariance_impl.h
index f73025e..6898246 100644
--- a/internal/ceres/covariance_impl.h
+++ b/internal/ceres/covariance_impl.h
@@ -55,9 +55,11 @@
const double*> >& covariance_blocks,
ProblemImpl* problem);
- bool GetCovarianceBlock(const double* parameter_block1,
- const double* parameter_block2,
- double* covariance_block) const;
+ bool GetCovarianceBlockInTangentOrAmbientSpace(
+ const double* parameter_block1,
+ const double* parameter_block2,
+ bool lift_covariance_to_ambient_space,
+ double* covariance_block) const;
bool ComputeCovarianceSparsity(
const std::vector<std::pair<const double*,
diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc
index f4fae0e..2b73a74 100644
--- a/internal/ceres/covariance_test.cc
+++ b/internal/ceres/covariance_test.cc
@@ -32,6 +32,8 @@
#include <algorithm>
#include <cmath>
+#include <map>
+#include <utility>
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/cost_function.h"
#include "ceres/covariance_impl.h"
@@ -228,6 +230,8 @@
class CovarianceTest : public ::testing::Test {
protected:
+ typedef map<const double*, pair<int, int> > BoundsMap;
+
virtual void SetUp() {
double* x = parameters_;
double* y = x + 2;
@@ -289,8 +293,29 @@
column_bounds_[z] = make_pair(5, 6);
}
+ // Computes covariance in ambient space.
void ComputeAndCompareCovarianceBlocks(const Covariance::Options& options,
const double* expected_covariance) {
+ ComputeAndCompareCovarianceBlocksInTangentOrAmbientSpace(
+ options,
+ true, // ambient
+ expected_covariance);
+ }
+
+ // Computes covariance in tangent space.
+ void ComputeAndCompareCovarianceBlocksInTangentSpace(
+ const Covariance::Options& options,
+ const double* expected_covariance) {
+ ComputeAndCompareCovarianceBlocksInTangentOrAmbientSpace(
+ options,
+ false, // tangent
+ expected_covariance);
+ }
+
+ void ComputeAndCompareCovarianceBlocksInTangentOrAmbientSpace(
+ const Covariance::Options& options,
+ bool lift_covariance_to_ambient_space,
+ const double* expected_covariance) {
// Generate all possible combination of block pairs and check if the
// covariance computation is correct.
for (int i = 1; i <= 64; ++i) {
@@ -328,11 +353,13 @@
// block1, block2
GetCovarianceBlockAndCompare(block1,
block2,
+ lift_covariance_to_ambient_space,
covariance,
expected_covariance);
// block2, block1
GetCovarianceBlockAndCompare(block2,
block1,
+ lift_covariance_to_ambient_space,
covariance,
expected_covariance);
}
@@ -341,19 +368,33 @@
void GetCovarianceBlockAndCompare(const double* block1,
const double* block2,
+ bool lift_covariance_to_ambient_space,
const Covariance& covariance,
const double* expected_covariance) {
- const int row_begin = FindOrDie(column_bounds_, block1).first;
- const int row_end = FindOrDie(column_bounds_, block1).second;
- const int col_begin = FindOrDie(column_bounds_, block2).first;
- const int col_end = FindOrDie(column_bounds_, block2).second;
+ const BoundsMap& column_bounds = lift_covariance_to_ambient_space ?
+ column_bounds_ : local_column_bounds_;
+ const int row_begin = FindOrDie(column_bounds, block1).first;
+ const int row_end = FindOrDie(column_bounds, block1).second;
+ const int col_begin = FindOrDie(column_bounds, block2).first;
+ const int col_end = FindOrDie(column_bounds, block2).second;
Matrix actual(row_end - row_begin, col_end - col_begin);
- EXPECT_TRUE(covariance.GetCovarianceBlock(block1,
- block2,
- actual.data()));
+ if (lift_covariance_to_ambient_space) {
+ EXPECT_TRUE(covariance.GetCovarianceBlock(block1,
+ block2,
+ actual.data()));
+ } else {
+ EXPECT_TRUE(covariance.GetCovarianceBlockInTangentSpace(block1,
+ block2,
+ actual.data()));
+ }
- ConstMatrixRef expected(expected_covariance, 6, 6);
+ int dof = 0; // degrees of freedom = sum of LocalSize()s
+ for (BoundsMap::const_iterator iter = column_bounds.begin();
+ iter != column_bounds.end(); ++iter) {
+ dof = std::max(dof, iter->second.second);
+ }
+ ConstMatrixRef expected(expected_covariance, dof, dof);
double diff_norm = (expected.block(row_begin,
col_begin,
row_end - row_begin,
@@ -372,10 +413,11 @@
<< "\n\n full expected: \n" << expected;
}
- double parameters_[10];
+ double parameters_[6];
Problem problem_;
vector<pair<const double*, const double*> > all_covariance_blocks_;
- map<const double*, pair<int, int> > column_bounds_;
+ BoundsMap column_bounds_;
+ BoundsMap local_column_bounds_;
};
@@ -537,22 +579,21 @@
// 0 1 0 0 0 0
// 0 0 2 0 0 0
// 0 0 0 2 0 0
- // 0 0 0 0 0 0
+ // 0 0 0 0 2 0
// 0 0 0 0 0 5
- // -5 -6 1 2 0 0
+ // -5 -6 1 2 3 0
// 3 -2 0 0 0 2
- // Global to local jacobian: A
+ // Local to global jacobian: A
//
- //
- // 1 0 0 0 0
- // 1 0 0 0 0
- // 0 1 0 0 0
- // 0 0 1 0 0
- // 0 0 0 1 0
- // 0 0 0 0 1
+ // 1 0 0 0
+ // 1 0 0 0
+ // 0 1 0 0
+ // 0 0 1 0
+ // 0 0 0 0
+ // 0 0 0 1
- // A * pinv((J*A)'*(J*A)) * A'
+ // A * inv((J*A)'*(J*A)) * A'
// Computed using octave.
double expected_covariance[] = {
0.01766, 0.01766, 0.02158, 0.04316, 0.00000, -0.00122,
@@ -577,6 +618,64 @@
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
+TEST_F(CovarianceTest, LocalParameterizationInTangentSpace) {
+ 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);
+
+ // Raw Jacobian: J
+ //
+ // 1 0 0 0 0 0
+ // 0 1 0 0 0 0
+ // 0 0 2 0 0 0
+ // 0 0 0 2 0 0
+ // 0 0 0 0 2 0
+ // 0 0 0 0 0 5
+ // -5 -6 1 2 3 0
+ // 3 -2 0 0 0 2
+
+ // Local to global jacobian: A
+ //
+ // 1 0 0 0
+ // 1 0 0 0
+ // 0 1 0 0
+ // 0 0 1 0
+ // 0 0 0 0
+ // 0 0 0 1
+
+ // inv((J*A)'*(J*A))
+ // Computed using octave.
+ double expected_covariance[] = {
+ 0.01766, 0.02158, 0.04316, -0.00122,
+ 0.02158, 0.24860, -0.00281, -0.00149,
+ 0.04316, -0.00281, 0.24439, -0.00298,
+ -0.00122, -0.00149, -0.00298, 0.03457 // NOLINT
+ };
+
+ Covariance::Options options;
+
+#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, TruncatedRank) {
// J
