From dc4b531bee604f3469738b4381e2b2ee29476ce3 Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Fri, 4 Aug 2017 16:07:49 +0200
Subject: [PATCH] add isCovariance @todo mv to proper file
---
src/wolf.h | 32 ++++++++++++++++++++++++++++++++
1 file changed, 32 insertions(+)
diff --git a/src/wolf.h b/src/wolf.h
index e6196474d..dc63786c3 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -329,6 +329,38 @@ WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
inline const Eigen::Vector3s gravity(void) {
return Eigen::Vector3s(0,0,-9.806);
}
+
+template <typename T, int N>
+bool isSymmetric(const Eigen::Matrix<T, N, N>& M,
+ const T eps = wolf::Constants::EPS)
+{
+ return M.isApprox(M.transpose(), eps);
+}
+
+template <typename T, int N>
+bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N>& M)
+{
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N> > eigensolver(M);
+
+ if (eigensolver.info() == Eigen::Success)
+ {
+ // All eigenvalues must be >= 0:
+ return (eigensolver.eigenvalues().array() >= T(0)).all();
+ }
+
+ return false;
+}
+
+template <typename T, int N>
+bool isCovariance(const Eigen::Matrix<T, N, N>& M)
+{
+ return isSymmetric(M) && isPositiveSemiDefinite(M);
+}
+
+#define WOLF_ASSERT_COVARIANCE_MATRIX(x) \
+ assert(x.determinant() > 0 && "Not positive definite measurement covariance"); \
+ assert(isCovariance(x) && "Not a covariance");
+
//===================================================
} // namespace wolf
--
GitLab