Pay attention to condition number in covariance estimation.
1. Sparse covariance estimation now uses cholmod_rcond to
detect singular Jacobians.
2. Dense covariance estimation now uses relative magnitude
of singular/eigen values to compute the pseudoinverse.
3. Truncation logic is now unified with Solver::Options::null_space_rank.
Change-Id: I095bd737510c836b4251255926190a7f31d64bce
diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
index 1a96193..966ba35 100644
--- a/internal/ceres/covariance_impl.cc
+++ b/internal/ceres/covariance_impl.cc
@@ -397,9 +397,23 @@
cholmod_factor* factor = ss_.AnalyzeCholesky(&cholmod_jacobian_view);
event_logger.AddEvent("Symbolic Factorization");
- bool status = ss_.Cholesky(&cholmod_jacobian_view, factor);
+ bool factorization_succeeded = ss_.Cholesky(&cholmod_jacobian_view, factor);
+ if (factorization_succeeded) {
+ const double reciprocal_condition_number =
+ cholmod_rcond(factor, ss_.mutable_cc());
+ if (reciprocal_condition_number <
+ options_.min_reciprocal_condition_number) {
+ LOG(WARNING) << "Cholesky factorization of J'J is not reliable. "
+ << "Reciprocal condition number: "
+ << reciprocal_condition_number << " "
+ << "min_reciprocal_condition_number : "
+ << options_.min_reciprocal_condition_number;
+ factorization_succeeded = false;
+ }
+ }
+
event_logger.AddEvent("Numeric Factorization");
- if (!status) {
+ if (!factorization_succeeded) {
ss_.Free(factor);
LOG(WARNING) << "Cholesky factorization failed.";
return false;
@@ -520,22 +534,39 @@
Eigen::JacobiSVD<Matrix> svd(dense_jacobian,
Eigen::ComputeThinU | Eigen::ComputeThinV);
- Vector inverse_singular_values = svd.singularValues();
- event_logger.AddEvent("SVD");
+ event_logger.AddEvent("SingularValueDecomposition");
- for (int i = 0; i < inverse_singular_values.rows(); ++i) {
- if (inverse_singular_values[i] > options_.min_singular_value_threshold &&
- i < (inverse_singular_values.rows() - options_.null_space_rank)) {
- inverse_singular_values[i] =
- 1.0 / (inverse_singular_values[i] * inverse_singular_values[i]);
- } else {
- inverse_singular_values[i] = 0.0;
+ const Vector singular_values = svd.singularValues();
+ const int num_singular_values = singular_values.rows();
+ Vector inverse_squared_singular_values(num_singular_values);
+ inverse_squared_singular_values.setZero();
+
+ const double max_singular_value = singular_values[0];
+ const double min_singular_value_ratio =
+ sqrt(options_.min_reciprocal_condition_number);
+
+ const bool automatic_truncation = (options_.null_space_rank < 0);
+ const int max_rank = min(num_singular_values,
+ num_singular_values - options_.null_space_rank);
+
+ for (int i = 0; i < max_rank; ++i) {
+ const double singular_value_ratio = singular_values[i] / max_singular_value;
+ if (singular_value_ratio >= min_singular_value_ratio) {
+ inverse_squared_singular_values[i] =
+ 1.0 / (singular_values[i] * singular_values[i]);
+ } else if (!automatic_truncation) {
+ LOG(WARNING) << "Cholesky factorization of J'J is not reliable. "
+ << "Reciprocal condition number: "
+ << singular_value_ratio * singular_value_ratio << " "
+ << "min_reciprocal_condition_number : "
+ << options_.min_reciprocal_condition_number;
+ return false;
}
}
Matrix dense_covariance =
svd.matrixV() *
- inverse_singular_values.asDiagonal() *
+ inverse_squared_singular_values.asDiagonal() *
svd.matrixV().transpose();
event_logger.AddEvent("PseudoInverse");
diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc
index b74efda..05fb234 100644
--- a/internal/ceres/covariance_test.cc
+++ b/internal/ceres/covariance_test.cc
@@ -532,15 +532,24 @@
-1.6076e-02, 1.2549e-02, -3.3329e-04, -6.6659e-04, -9.9988e-04, 3.9539e-02
};
- Covariance::Options options;
- options.use_dense_linear_algebra = true;
- options.null_space_rank = 1;
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
- options.use_dense_linear_algebra = true;
- options.null_space_rank = 0;
- options.min_singular_value_threshold = sqrt(3.5);
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+ {
+ Covariance::Options options;
+ options.use_dense_linear_algebra = true;
+ // Force dropping of the smallest eigenvector.
+ options.null_space_rank = 1;
+ ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+ }
+
+ {
+ Covariance::Options options;
+ options.use_dense_linear_algebra = true;
+ // Force dropping of the smallest eigenvector via the ratio but
+ // automatic truncation.
+ options.min_reciprocal_condition_number = 0.044494;
+ options.null_space_rank = -1;
+ ComputeAndCompareCovarianceBlocks(options, expected_covariance);
+ }
}
class RankDeficientCovarianceTest : public CovarianceTest {
@@ -598,7 +607,7 @@
}
};
-TEST_F(RankDeficientCovarianceTest, MinSingularValueTolerance) {
+TEST_F(RankDeficientCovarianceTest, AutomaticTruncation) {
// J
//
// 1 0 0 0 0 0
@@ -631,15 +640,7 @@
Covariance::Options options;
options.use_dense_linear_algebra = true;
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
-
- options.null_space_rank = 1;
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
-
- options.null_space_rank = 2;
- ComputeAndCompareCovarianceBlocks(options, expected_covariance);
-
- options.null_space_rank = 3;
+ options.null_space_rank = -1;
ComputeAndCompareCovarianceBlocks(options, expected_covariance);
}
