saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 08:34:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix free parameter block handling in covariance computation

Browse Source 
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
This commit is contained in:
Sameer Agarwal
2015-12-04 07:26:33 -08:00
parent 992ae55e84
commit 6418b33f21
2 changed files with 273 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
internal/ceres/covariance_impl.cc
+20 -5
View File
@@ -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<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 @@ 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: "
internal/ceres/covariance_test.cc
+253 -79
View File
@@ -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<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 @@ 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<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;