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