diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc index 907f8ad08..ff65c77f0 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 @@ bool CovarianceImpl::ComputeCovarianceSparsity( vector all_parameter_blocks; problem->GetParameterBlocks(&all_parameter_blocks); const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map(); + HashSet parameter_blocks_in_use; + vector 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& 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 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() { 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 f50cc5768..6b90d6ef6 100644 --- a/internal/ceres/covariance_test.cc +++ b/internal/ceres/covariance_test.cc @@ -50,85 +50,6 @@ using std::map; 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 > 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 @@ class PolynomialParameterization : public LocalParameterization { 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 > 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 > 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 > 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 > BoundsMap;