From f43f9f114564e9a8e52490be622ae2e9603088a7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 21 Jun 2018 10:32:10 +0200
Subject: [PATCH] not using avoidSingularCovariance
---
src/feature_base.cpp | 11 +++++------
1 file changed, 5 insertions(+), 6 deletions(-)
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index ea510640f..71ceb6937 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -91,11 +91,11 @@ void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
{
WOLF_ASSERT_COVARIANCE_MATRIX(_meas_cov);
- // set (ensuring strong symmetry)
+ // set (ensuring symmetry)
measurement_covariance_ = _meas_cov.selfadjointView<Eigen::Upper>();
// Avoid singular covariance
- avoidSingularCovariance();
+ //avoidSingularCovariance();
// compute square root information upper matrix
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
@@ -105,12 +105,12 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
{
WOLF_ASSERT_INFORMATION_MATRIX(_meas_info);
- // set (ensuring strong symmetry)
+ // 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);
@@ -119,8 +119,7 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const
{
// impose symmetry
- Eigen::MatrixXs info = ((_info + _info.transpose()) / 2);
- info = info.selfadjointView<Eigen::Upper>();
+ Eigen::MatrixXs info = _info.selfadjointView<Eigen::Upper>();
// Normal Cholesky factorization
Eigen::LLT<Eigen::MatrixXs> llt_of_info(info);
--
GitLab