Merge branch '274-covariances-manipulations-in-a-specific-file' into 'devel'

Resolve "Covariances manipulations in a specific file"

Closes #274

See merge request !335

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

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_ */

include/core/feature/feature_base.h

@@ -101,10 +101,6 @@ 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;
-
     private:
         void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
         FactorBasePtr addFactor(FactorBasePtr _co_ptr);

include/core/math/covariance.h

+/**
+ * \file covariance.h
+ *
+ *  Created on: Feb 26, 2020
+ *      \author: jsola
+ */
+
+#ifndef MATH_COVARIANCE_H_
+#define MATH_COVARIANCE_H_
+
+#include <Eigen/Dense>
+
+namespace wolf{
+
+template <typename T, int N, int RC>
+inline 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>
+inline 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>
+inline 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>
+inline 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;
+}
+
+inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd & _info)
+{
+    // 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;
+}
+
+
+}
+
+
+
+#endif /* MATH_COVARIANCE_H_ */

src/feature/feature_base.cpp

 #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,28 +134,6 @@ 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;
-}
-
 void FeatureBase::link(CaptureBasePtr _cpt_ptr)
 {
     assert(!is_removing_ && "linking a removed feature");

src/processor/processor_odom_2D.cpp

 #include "core/processor/processor_odom_2D.h"
+#include "core/math/covariance.h"
+
 namespace wolf
 {
 

test/gtest_make_posdef.cpp

 #include "core/utils/utils_gtest.h"
 #include "core/common/wolf.h"
+#include "core/math/covariance.h"
 
 using namespace Eigen;
 using namespace wolf;