From d66647015b16b8dae4bf4cd27f15dba11b12eec9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 18 Jun 2018 16:18:06 +0200
Subject: [PATCH] changes proposed in covariance and sqrt upper information
matrix
---
src/feature_base.cpp | 75 ++++++++++++++++++++++++++++++--------------
src/feature_base.h | 8 +++++
2 files changed, 60 insertions(+), 23 deletions(-)
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 41a23ad30..570563055 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -89,40 +89,69 @@ void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list)
void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
{
- if (_meas_cov.determinant() < Constants::EPS_SMALL)
- {
- Eigen::MatrixXs eps = Eigen::MatrixXs::Identity(_meas_cov.rows(), _meas_cov.cols()) * 1e-10;
- measurement_covariance_ = _meas_cov + eps; // avoid singular covariance
- }
- else
- measurement_covariance_ = _meas_cov;
+ // Check symmetry (soft)
+ assert((_meas_cov - _meas_cov.transpose()).cwiseAbs().maxCoeff() < Constants::EPS && "Not symmetric measurement covariance");
+
+ // set (ensuring strong symmetry)
+ measurement_covariance_ = _meas_cov.selfadjointView<Eigen::Upper>();
+
+ // Avoid singular covariance
+ avoidSingleCovariance();
- measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_cov);
+ // compute square root information upper matrix
+ measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
}
void FeatureBase::setMeasurementInfo(const Eigen::MatrixXs & _meas_info)
{
- assert(_meas_info.determinant() > 0 && "Not positive definite measurement information");
+ assert(_meas_info.determinant() > Constants::EPS_SMALL && "Not positive definite measurement information");
+ assert((_meas_info - _meas_info.transpose()).cwiseAbs().maxCoeff() < Constants::EPS && "Not symmetric measurement information");
- measurement_covariance_ = _meas_info.inverse();
- measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info.inverse());
+ // set (ensuring strong symmetry)
+ measurement_covariance_ = _meas_info.inverse().selfadjointView<Eigen::Upper>();
+
+ // Avoid singular covariance
+ avoidSingleCovariance();
+
+ // compute square root information upper matrix
+ measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
}
-Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _covariance) const
+Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const
{
- if (_covariance.determinant() < Constants::EPS_SMALL)
- {
- // Avoid singular covariances matrix
- Eigen::MatrixXs cov = _covariance + 1e-8 * Eigen::MatrixXs::Identity(_covariance.rows(), _covariance.cols());
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(cov.inverse());
- return llt_of_info.matrixU();
- }
- else
+ assert(_info.determinant() > Constants::EPS_SMALL && "Matrix is not positive definite!");
+ assert((_info - _info.transpose()).cwiseAbs().maxCoeff() < Constants::EPS && "Matrix is not symmetric!");
+
+ // impose symmetry
+ Eigen::MatrixXs info = ((_info + _info.transpose()) / 2);
+ info = info.selfadjointView<Eigen::Upper>();
+
+ // Normal Cholesky factorization
+ Eigen::LLT<Eigen::MatrixXs> llt_of_info(info);
+ Eigen::MatrixXs R = llt_of_info.matrixU();
+
+ // Good factorization
+ if ((R.transpose() * R - info).cwiseAbs().maxCoeff() < Constants::EPS)
+ return R;
+
+ // Not good factorization: SelfAdjointEigenSolver
+ Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> es(info);
+ Eigen::VectorXs eval = es.eigenvalues().real().cwiseMax(0);
+
+ R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
+
+ return R;
+}
+
+void FeatureBase::avoidSingleCovariance()
+{
+ Scalar eps_scalar = 1e-10;
+ while (measurement_covariance_.determinant() < Constants::EPS_SMALL && eps_scalar < 1e-3)
{
- // Normal operation
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(_covariance.inverse());
- return llt_of_info.matrixU();
+ measurement_covariance_ += Eigen::MatrixXs::Identity(measurement_covariance_.rows(), measurement_covariance_.cols()) * eps_scalar; // avoid singular covariance
+ eps_scalar*=10;
}
+ assert(measurement_covariance_.determinant() > Constants::EPS_SMALL && "Couldn't avoid singular covariance");
}
} // namespace wolf
diff --git a/src/feature_base.h b/src/feature_base.h
index 495d7d58f..0f6d539d7 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -66,6 +66,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov);
void setMeasurementInfo(const Eigen::MatrixXs & _meas_info);
const Eigen::MatrixXs& getMeasurementCovariance() const;
+ Eigen::MatrixXs getMeasurementInformation() const;
const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const;
const Eigen::VectorXs& getExpectation() const;
@@ -90,6 +91,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
private:
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
+ void avoidSingleCovariance();
+
};
}
@@ -135,6 +138,11 @@ inline const Eigen::MatrixXs& FeatureBase::getMeasurementCovariance() const
return measurement_covariance_;
}
+inline Eigen::MatrixXs FeatureBase::getMeasurementInformation() const
+{
+ return measurement_sqrt_information_upper_.transpose() * measurement_sqrt_information_upper_;
+}
+
inline const Eigen::MatrixXs& FeatureBase::getMeasurementSquareRootInformationUpper() const
{
return measurement_sqrt_information_upper_;
--
GitLab