Fix free parameter block handling in covariance computation
Parameter blocks that are not associated with any residual block
lead to structurally zero columns in the Jacobian. The covariance
computation algorithm was only paying attention to structural
sparsity caused by constant parameter blocks but not free parameter
blocks.
This patch fixes this, by iterating over the residual blocks in
the problem and collecting all the parameter blocks in use.
The tests for ComputeCovarianceSparsity are also extended to include
the case where there are constant and free parameter blocks.
Thanks to Wannes Van Loock for reporting this.
Change-Id: Ic298a6e93c53f2f95fb69105397a87200738a2b0
diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
index 907f8ad..ff65c77 100644
--- a/internal/ceres/covariance_impl.cc
+++ b/internal/ceres/covariance_impl.cc
@@ -43,6 +43,7 @@
#include "Eigen/SparseQR"
#include "Eigen/SVD"
+#include "ceres/collections_port.h"
#include "ceres/compressed_col_sparse_matrix_utils.h"
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/covariance.h"
@@ -51,6 +52,7 @@
#include "ceres/map_util.h"
#include "ceres/parameter_block.h"
#include "ceres/problem_impl.h"
+#include "ceres/residual_block.h"
#include "ceres/suitesparse.h"
#include "ceres/wall_time.h"
#include "glog/logging.h"
@@ -260,18 +262,28 @@
vector<double*> all_parameter_blocks;
problem->GetParameterBlocks(&all_parameter_blocks);
const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map();
+ HashSet<ParameterBlock*> parameter_blocks_in_use;
+ vector<ResidualBlock*> residual_blocks;
+ problem->GetResidualBlocks(&residual_blocks);
+
+ for (int i = 0; i < residual_blocks.size(); ++i) {
+ ResidualBlock* residual_block = residual_blocks[i];
+ parameter_blocks_in_use.insert(residual_block->parameter_blocks(),
+ residual_block->parameter_blocks() +
+ residual_block->NumParameterBlocks());
+ }
+
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 {
+ if (!block->IsConstant() && (parameter_blocks_in_use.count(block) > 0)) {
active_parameter_blocks.push_back(parameter_block);
+ } else {
+ constant_parameter_blocks_.insert(parameter_block);
}
}
@@ -632,7 +644,10 @@
if (automatic_truncation) {
break;
} else {
- LOG(ERROR) << "Cholesky factorization of J'J is not reliable. "
+ LOG(ERROR) << "Error: Covariance matrix is near rank deficient "
+ << "and the user did not specify a non-zero"
+ << "Covariance::Options::null_space_rank "
+ << "to enable the computation of a Pseudo-Inverse. "
<< "Reciprocal condition number: "
<< singular_value_ratio * singular_value_ratio << " "
<< "min_reciprocal_condition_number: "
diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc
index f50cc57..6b90d6e 100644
--- a/internal/ceres/covariance_test.cc
+++ b/internal/ceres/covariance_test.cc
@@ -50,85 +50,6 @@
using std::pair;
using std::vector;
-TEST(CovarianceImpl, ComputeCovarianceSparsity) {
- double parameters[10];
-
- double* block1 = parameters;
- double* block2 = block1 + 1;
- double* block3 = block2 + 2;
- double* block4 = block3 + 3;
-
- ProblemImpl problem;
-
- // Add in random order
- problem.AddParameterBlock(block1, 1);
- problem.AddParameterBlock(block4, 4);
- problem.AddParameterBlock(block3, 3);
- problem.AddParameterBlock(block2, 2);
-
- // Sparsity pattern
- //
- // x 0 0 0 0 0 x x x x
- // 0 x x x x x 0 0 0 0
- // 0 x x x x x 0 0 0 0
- // 0 0 0 x x x 0 0 0 0
- // 0 0 0 x x x 0 0 0 0
- // 0 0 0 x x x 0 0 0 0
- // 0 0 0 0 0 0 x x x x
- // 0 0 0 0 0 0 x x x x
- // 0 0 0 0 0 0 x x x x
- // 0 0 0 0 0 0 x x x x
-
- int expected_rows[] = {0, 5, 10, 15, 18, 21, 24, 28, 32, 36, 40};
- int expected_cols[] = {0, 6, 7, 8, 9,
- 1, 2, 3, 4, 5,
- 1, 2, 3, 4, 5,
- 3, 4, 5,
- 3, 4, 5,
- 3, 4, 5,
- 6, 7, 8, 9,
- 6, 7, 8, 9,
- 6, 7, 8, 9,
- 6, 7, 8, 9};
-
-
- vector<pair<const double*, const double*> > covariance_blocks;
- covariance_blocks.push_back(make_pair(block1, block1));
- covariance_blocks.push_back(make_pair(block4, block4));
- covariance_blocks.push_back(make_pair(block2, block2));
- covariance_blocks.push_back(make_pair(block3, block3));
- covariance_blocks.push_back(make_pair(block2, block3));
- covariance_blocks.push_back(make_pair(block4, block1)); // reversed
-
- Covariance::Options options;
- CovarianceImpl covariance_impl(options);
- EXPECT_TRUE(covariance_impl
- .ComputeCovarianceSparsity(covariance_blocks, &problem));
-
- const CompressedRowSparseMatrix* crsm = covariance_impl.covariance_matrix();
-
- EXPECT_EQ(crsm->num_rows(), 10);
- EXPECT_EQ(crsm->num_cols(), 10);
- EXPECT_EQ(crsm->num_nonzeros(), 40);
-
- const int* rows = crsm->rows();
- for (int r = 0; r < crsm->num_rows() + 1; ++r) {
- EXPECT_EQ(rows[r], expected_rows[r])
- << r << " "
- << rows[r] << " "
- << expected_rows[r];
- }
-
- const int* cols = crsm->cols();
- for (int c = 0; c < crsm->num_nonzeros(); ++c) {
- EXPECT_EQ(cols[c], expected_cols[c])
- << c << " "
- << cols[c] << " "
- << expected_cols[c];
- }
-}
-
-
class UnaryCostFunction: public CostFunction {
public:
UnaryCostFunction(const int num_residuals,
@@ -228,6 +149,259 @@
virtual int LocalSize() const { return 1; }
};
+TEST(CovarianceImpl, ComputeCovarianceSparsity) {
+ double parameters[10];
+
+ double* block1 = parameters;
+ double* block2 = block1 + 1;
+ double* block3 = block2 + 2;
+ double* block4 = block3 + 3;
+
+ ProblemImpl problem;
+
+ // Add in random order
+ Vector junk_jacobian = Vector::Zero(10);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 1, junk_jacobian.data()), NULL, block1);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 4, junk_jacobian.data()), NULL, block4);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 3, junk_jacobian.data()), NULL, block3);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 2, junk_jacobian.data()), NULL, block2);
+
+ // Sparsity pattern
+ //
+ // Note that the problem structure does not imply this sparsity
+ // pattern since all the residual blocks are unary. But the
+ // ComputeCovarianceSparsity function in its current incarnation
+ // does not pay attention to this fact and only looks at the
+ // parameter block pairs that the user provides.
+ //
+ // X . . . . . X X X X
+ // . X X X X X . . . .
+ // . X X X X X . . . .
+ // . . . X X X . . . .
+ // . . . X X X . . . .
+ // . . . X X X . . . .
+ // . . . . . . X X X X
+ // . . . . . . X X X X
+ // . . . . . . X X X X
+ // . . . . . . X X X X
+
+ int expected_rows[] = {0, 5, 10, 15, 18, 21, 24, 28, 32, 36, 40};
+ int expected_cols[] = {0, 6, 7, 8, 9,
+ 1, 2, 3, 4, 5,
+ 1, 2, 3, 4, 5,
+ 3, 4, 5,
+ 3, 4, 5,
+ 3, 4, 5,
+ 6, 7, 8, 9,
+ 6, 7, 8, 9,
+ 6, 7, 8, 9,
+ 6, 7, 8, 9};
+
+
+ vector<pair<const double*, const double*> > covariance_blocks;
+ covariance_blocks.push_back(make_pair(block1, block1));
+ covariance_blocks.push_back(make_pair(block4, block4));
+ covariance_blocks.push_back(make_pair(block2, block2));
+ covariance_blocks.push_back(make_pair(block3, block3));
+ covariance_blocks.push_back(make_pair(block2, block3));
+ covariance_blocks.push_back(make_pair(block4, block1)); // reversed
+
+ Covariance::Options options;
+ CovarianceImpl covariance_impl(options);
+ EXPECT_TRUE(covariance_impl
+ .ComputeCovarianceSparsity(covariance_blocks, &problem));
+
+ const CompressedRowSparseMatrix* crsm = covariance_impl.covariance_matrix();
+
+ EXPECT_EQ(crsm->num_rows(), 10);
+ EXPECT_EQ(crsm->num_cols(), 10);
+ EXPECT_EQ(crsm->num_nonzeros(), 40);
+
+ const int* rows = crsm->rows();
+ for (int r = 0; r < crsm->num_rows() + 1; ++r) {
+ EXPECT_EQ(rows[r], expected_rows[r])
+ << r << " "
+ << rows[r] << " "
+ << expected_rows[r];
+ }
+
+ const int* cols = crsm->cols();
+ for (int c = 0; c < crsm->num_nonzeros(); ++c) {
+ EXPECT_EQ(cols[c], expected_cols[c])
+ << c << " "
+ << cols[c] << " "
+ << expected_cols[c];
+ }
+}
+
+TEST(CovarianceImpl, ComputeCovarianceSparsityWithConstantParameterBlock) {
+ double parameters[10];
+
+ double* block1 = parameters;
+ double* block2 = block1 + 1;
+ double* block3 = block2 + 2;
+ double* block4 = block3 + 3;
+
+ ProblemImpl problem;
+
+ // Add in random order
+ Vector junk_jacobian = Vector::Zero(10);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 1, junk_jacobian.data()), NULL, block1);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 4, junk_jacobian.data()), NULL, block4);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 3, junk_jacobian.data()), NULL, block3);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 2, junk_jacobian.data()), NULL, block2);
+ problem.SetParameterBlockConstant(block3);
+
+ // Sparsity pattern
+ //
+ // Note that the problem structure does not imply this sparsity
+ // pattern since all the residual blocks are unary. But the
+ // ComputeCovarianceSparsity function in its current incarnation
+ // does not pay attention to this fact and only looks at the
+ // parameter block pairs that the user provides.
+ //
+ // X . . X X X X
+ // . X X . . . .
+ // . X X . . . .
+ // . . . X X X X
+ // . . . X X X X
+ // . . . X X X X
+ // . . . X X X X
+
+ int expected_rows[] = {0, 5, 7, 9, 13, 17, 21, 25};
+ int expected_cols[] = {0, 3, 4, 5, 6,
+ 1, 2,
+ 1, 2,
+ 3, 4, 5, 6,
+ 3, 4, 5, 6,
+ 3, 4, 5, 6,
+ 3, 4, 5, 6};
+
+ vector<pair<const double*, const double*> > covariance_blocks;
+ covariance_blocks.push_back(make_pair(block1, block1));
+ covariance_blocks.push_back(make_pair(block4, block4));
+ covariance_blocks.push_back(make_pair(block2, block2));
+ covariance_blocks.push_back(make_pair(block3, block3));
+ covariance_blocks.push_back(make_pair(block2, block3));
+ covariance_blocks.push_back(make_pair(block4, block1)); // reversed
+
+ Covariance::Options options;
+ CovarianceImpl covariance_impl(options);
+ EXPECT_TRUE(covariance_impl
+ .ComputeCovarianceSparsity(covariance_blocks, &problem));
+
+ const CompressedRowSparseMatrix* crsm = covariance_impl.covariance_matrix();
+
+ EXPECT_EQ(crsm->num_rows(), 7);
+ EXPECT_EQ(crsm->num_cols(), 7);
+ EXPECT_EQ(crsm->num_nonzeros(), 25);
+
+ const int* rows = crsm->rows();
+ for (int r = 0; r < crsm->num_rows() + 1; ++r) {
+ EXPECT_EQ(rows[r], expected_rows[r])
+ << r << " "
+ << rows[r] << " "
+ << expected_rows[r];
+ }
+
+ const int* cols = crsm->cols();
+ for (int c = 0; c < crsm->num_nonzeros(); ++c) {
+ EXPECT_EQ(cols[c], expected_cols[c])
+ << c << " "
+ << cols[c] << " "
+ << expected_cols[c];
+ }
+}
+
+TEST(CovarianceImpl, ComputeCovarianceSparsityWithFreeParameterBlock) {
+ double parameters[10];
+
+ double* block1 = parameters;
+ double* block2 = block1 + 1;
+ double* block3 = block2 + 2;
+ double* block4 = block3 + 3;
+
+ ProblemImpl problem;
+
+ // Add in random order
+ Vector junk_jacobian = Vector::Zero(10);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 1, junk_jacobian.data()), NULL, block1);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 4, junk_jacobian.data()), NULL, block4);
+ problem.AddParameterBlock(block3, 3);
+ problem.AddResidualBlock(
+ new UnaryCostFunction(1, 2, junk_jacobian.data()), NULL, block2);
+
+ // Sparsity pattern
+ //
+ // Note that the problem structure does not imply this sparsity
+ // pattern since all the residual blocks are unary. But the
+ // ComputeCovarianceSparsity function in its current incarnation
+ // does not pay attention to this fact and only looks at the
+ // parameter block pairs that the user provides.
+ //
+ // X . . X X X X
+ // . X X . . . .
+ // . X X . . . .
+ // . . . X X X X
+ // . . . X X X X
+ // . . . X X X X
+ // . . . X X X X
+
+ int expected_rows[] = {0, 5, 7, 9, 13, 17, 21, 25};
+ int expected_cols[] = {0, 3, 4, 5, 6,
+ 1, 2,
+ 1, 2,
+ 3, 4, 5, 6,
+ 3, 4, 5, 6,
+ 3, 4, 5, 6,
+ 3, 4, 5, 6};
+
+ vector<pair<const double*, const double*> > covariance_blocks;
+ covariance_blocks.push_back(make_pair(block1, block1));
+ covariance_blocks.push_back(make_pair(block4, block4));
+ covariance_blocks.push_back(make_pair(block2, block2));
+ covariance_blocks.push_back(make_pair(block3, block3));
+ covariance_blocks.push_back(make_pair(block2, block3));
+ covariance_blocks.push_back(make_pair(block4, block1)); // reversed
+
+ Covariance::Options options;
+ CovarianceImpl covariance_impl(options);
+ EXPECT_TRUE(covariance_impl
+ .ComputeCovarianceSparsity(covariance_blocks, &problem));
+
+ const CompressedRowSparseMatrix* crsm = covariance_impl.covariance_matrix();
+
+ EXPECT_EQ(crsm->num_rows(), 7);
+ EXPECT_EQ(crsm->num_cols(), 7);
+ EXPECT_EQ(crsm->num_nonzeros(), 25);
+
+ const int* rows = crsm->rows();
+ for (int r = 0; r < crsm->num_rows() + 1; ++r) {
+ EXPECT_EQ(rows[r], expected_rows[r])
+ << r << " "
+ << rows[r] << " "
+ << expected_rows[r];
+ }
+
+ const int* cols = crsm->cols();
+ for (int c = 0; c < crsm->num_nonzeros(); ++c) {
+ EXPECT_EQ(cols[c], expected_cols[c])
+ << c << " "
+ << cols[c] << " "
+ << expected_cols[c];
+ }
+}
+
class CovarianceTest : public ::testing::Test {
protected:
typedef map<const double*, pair<int, int> > BoundsMap;
