From b3343999c76d77c5dbb3ca18c4f701618cc8840a Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Wed, 26 Feb 2020 13:22:09 +0100
Subject: [PATCH] Move all cov functions to math/covariance.h
---
CMakeLists.txt | 1 +
include/core/common/wolf.h | 66 -----------------------------
include/core/feature/feature_base.h | 6 +--
src/feature/feature_base.cpp | 43 ++++++++++---------
src/processor/processor_odom_2D.cpp | 2 +
test/gtest_make_posdef.cpp | 1 +
6 files changed, 29 insertions(+), 90 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a94aa0e12..d58be1667 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -174,6 +174,7 @@ SET(HDRS_MATH
include/core/math/SE2.h
include/core/math/SE3.h
include/core/math/rotations.h
+ include/core/math/covariance.h
)
SET(HDRS_UTILS
include/core/utils/converter.h
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index caae0f4c5..5db953501 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -276,72 +276,6 @@ inline const Eigen::Vector3d gravity(void) {
return Eigen::Vector3d(0,0,-9.806);
}
-template <typename T, int N, int RC>
-bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M,
- const T eps = wolf::Constants::EPS)
-{
- return M.isApprox(M.transpose(), eps);
-}
-
-template <typename T, int N, int RC>
-bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
-{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M);
-
- if (eigensolver.info() == Eigen::Success)
- {
- // All eigenvalues must be >= 0:
- return (eigensolver.eigenvalues().array() >= eps).all();
- }
-
- return false;
-}
-
-template <typename T, int N, int RC>
-bool isCovariance(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
-{
- return isSymmetric(M) && isPositiveSemiDefinite(M, eps);
-}
-
-#define WOLF_ASSERT_COVARIANCE_MATRIX(x) \
- assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance");
-
-#define WOLF_ASSERT_INFORMATION_MATRIX(x) \
- assert(isCovariance(x, double(0.0)) && "Not an information matrix");
-
-template <typename T, int N, int RC>
-bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS)
-{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T,N,N,RC> > eigensolver(M);
-
- if (eigensolver.info() == Eigen::Success)
- {
- // All eigenvalues must be >= 0:
- double epsilon = eps;
- while ((eigensolver.eigenvalues().array() < 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: " << M.determinant() << std::endl;
- M = eigensolver.eigenvectors() *
- eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
- eigensolver.eigenvectors().transpose();
- eigensolver.compute(M);
- //std::cout << "epsilon used: " << epsilon << std::endl;
- //std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
- //std::cout << "posterior determinant: " << M.determinant() << std::endl;
- epsilon *=10;
- }
- WOLF_ASSERT_COVARIANCE_MATRIX(M);
-
- return epsilon != eps;
- }
- else
- WOLF_ERROR("Couldn't compute covariance eigen decomposition");
-
- return false;
-}
-
} // namespace wolf
#endif /* WOLF_H_ */
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index c008b58b9..8964df792 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -101,9 +101,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
- protected:
-
- Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd& _M) const;
+// protected:
+//
+// Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd& _M) const;
private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index caeba3c2e..68ddcba42 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -1,6 +1,7 @@
#include "core/feature/feature_base.h"
#include "core/factor/factor_base.h"
#include "core/capture/capture_base.h"
+#include "core/math/covariance.h"
namespace wolf {
@@ -133,27 +134,27 @@ void FeatureBase::setProblem(ProblemPtr _problem)
fac->setProblem(_problem);
}
-Eigen::MatrixXd FeatureBase::computeSqrtUpper(const Eigen::MatrixXd & _info) const
-{
- // impose symmetry
- Eigen::MatrixXd info = _info.selfadjointView<Eigen::Upper>();
-
- // Normal Cholesky factorization
- Eigen::LLT<Eigen::MatrixXd> llt_of_info(info);
- Eigen::MatrixXd R = llt_of_info.matrixU();
-
- // Good factorization
- if (info.isApprox(R.transpose() * R, Constants::EPS))
- return R;
-
- // Not good factorization: SelfAdjointEigenSolver
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(info);
- Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
-
- R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
-
- return R;
-}
+//Eigen::MatrixXd FeatureBase::computeSqrtUpper(const Eigen::MatrixXd & _info) const
+//{
+// // impose symmetry
+// Eigen::MatrixXd info = _info.selfadjointView<Eigen::Upper>();
+//
+// // Normal Cholesky factorization
+// Eigen::LLT<Eigen::MatrixXd> llt_of_info(info);
+// Eigen::MatrixXd R = llt_of_info.matrixU();
+//
+// // Good factorization
+// if (info.isApprox(R.transpose() * R, Constants::EPS))
+// return R;
+//
+// // Not good factorization: SelfAdjointEigenSolver
+// Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(info);
+// Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
+//
+// R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
+//
+// return R;
+//}
void FeatureBase::link(CaptureBasePtr _cpt_ptr)
{
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index e56f9cdd9..5ad5e732d 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -1,4 +1,6 @@
#include "core/processor/processor_odom_2D.h"
+#include "core/math/covariance.h"
+
namespace wolf
{
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index 607ff99c3..ce406cd1a 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -1,5 +1,6 @@
#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
+#include "core/math/covariance.h"
using namespace Eigen;
using namespace wolf;
--
GitLab