From 9042ff0e6e4e48779fe9b0c7e667026c99655c28 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 25 Jun 2018 12:14:58 +0200
Subject: [PATCH] avoidSingularCovariance using while called in
setMeasurementInformation
---
src/feature_base.cpp | 22 +++++++++++++++-------
1 file changed, 15 insertions(+), 7 deletions(-)
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 71ceb6937..09e9c0c79 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -107,10 +107,9 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
// set (ensuring symmetry)
measurement_covariance_ = _meas_info.inverse().selfadjointView<Eigen::Upper>();
- WOLF_ASSERT_COVARIANCE_MATRIX(measurement_covariance_);
// Avoid singular covariance
- //avoidSingularCovariance();
+ avoidSingularCovariance();
// compute square root information upper matrix
measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
@@ -145,12 +144,21 @@ void FeatureBase::avoidSingularCovariance()
if (eigensolver.info() == Eigen::Success)
{
// All eigenvalues must be >= 0:
- //std::cout << "pre\n" << measurement_covariance_ << std::endl;
- if ((eigensolver.eigenvalues().array() < Constants::EPS).all())
+ Scalar epsilon = Constants::EPS;
+ while ((eigensolver.eigenvalues().array() < Constants::EPS).any())
+ {
+ std::cout << "----- any negative eigenvalue or too close to zero\n";
+ std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
+ std::cout << "previous determinant: " << measurement_covariance_.determinant() << std::endl;
measurement_covariance_= eigensolver.eigenvectors() *
- eigensolver.eigenvalues().cwiseMax(Constants::EPS).asDiagonal() *
+ eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
eigensolver.eigenvectors().transpose();
- //std::cout << "post\n" << measurement_covariance_ << std::endl;
+ eigensolver.compute(measurement_covariance_);
+ std::cout << "epsilon used: " << epsilon << std::endl;
+ std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
+ std::cout << "posterior determinant: " << measurement_covariance_.determinant() << std::endl;
+ epsilon *=10;
+ }
}
else
WOLF_ERROR("Couldn't compute covariance eigen decomposition");
@@ -161,7 +169,7 @@ void FeatureBase::avoidSingularCovariance()
measurement_covariance_ += Eigen::MatrixXs::Identity(measurement_covariance_.rows(), measurement_covariance_.cols()) * eps_scalar; // avoid singular covariance
eps_scalar*=10;
}*/
- assert(measurement_covariance_.determinant() > 0 && "Couldn't avoid singular covariance");
+ WOLF_ASSERT_COVARIANCE_MATRIX(measurement_covariance_);
}
} // namespace wolf
--
GitLab