Fix covariance computation for constant blocks
This commit fixes a bug related to the computation of covariance blocks
in the tangent space for constant parameter blocks, causing out of
bounds memory access.
Change-Id: Iaeee7992405fcaaae6086612798e96f2e10ebc5c
diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
index 3e8302b..cd823b3 100644
--- a/internal/ceres/covariance_impl.cc
+++ b/internal/ceres/covariance_impl.cc
@@ -120,9 +120,17 @@
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();
+ const int block1_local_size = block1->LocalSize();
+ const int block2_local_size = block2->LocalSize();
+ if (!lift_covariance_to_ambient_space) {
+ MatrixRef(covariance_block, block1_local_size, block2_local_size)
+ .setZero();
+ } else {
+ MatrixRef(covariance_block, block1_size, block2_size).setZero();
+ }
return true;
}
diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc
index e47d991..c2b83e3 100644
--- a/internal/ceres/covariance_test.cc
+++ b/internal/ceres/covariance_test.cc
@@ -318,7 +318,7 @@
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) {
+ for (int i = 0; i <= 64; ++i) {
vector<pair<const double*, const double*> > covariance_blocks;
if (i & 1) {
covariance_blocks.push_back(all_covariance_blocks_[0]);
@@ -676,6 +676,65 @@
ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);
}
+TEST_F(CovarianceTest, LocalParameterizationInTangentSpaceWithConstantBlocks) {
+ double* x = parameters_;
+ double* y = x + 2;
+ double* z = y + 3;
+
+ problem_.SetParameterization(x, new PolynomialParameterization);
+ problem_.SetParameterBlockConstant(x);
+
+ vector<int> subset;
+ subset.push_back(2);
+ problem_.SetParameterization(y, new SubsetParameterization(3, subset));
+ problem_.SetParameterBlockConstant(y);
+
+ 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
+ //
+ // 0 0 0 0
+ // 0 0 0 0
+ // 0 0 0 0
+ // 0 0 0 0
+ // 0 0 0 0
+ // 0 0 0 1
+
+ // inv((J*A)'*(J*A))
+ // Computed using octave.
+ double expected_covariance[] = {
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.034482 // 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
