diff --git a/CMakeLists.txt b/CMakeLists.txt
index a94aa0e12b882d02666f5525f7d57f87657a8b03..d58be1667a76ee86a9123166174e49eca1927c8b 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 caae0f4c58b2df25ad7f4b807118254d1baeb598..5db953501ad44029b995ba8ba15f516209a02c6c 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 c008b58b97ccd6e4596341cba1f56f034f23ca53..1eb9bd33853b8201f971060689344d14c0fd1174 100644
--- a/include/core/feature/feature_base.h
+++ b/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);
diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h
new file mode 100644
index 0000000000000000000000000000000000000000..4721c3def15ec0f5cdf8371387daf989402dcc12
--- /dev/null
+++ b/include/core/math/covariance.h
@@ -0,0 +1,108 @@
+/**
+ * \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_ */
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index caeba3c2e91d0b641ee8bd7ffd7e1e951828198c..df5d1f1e7aa0f214adab2e0f74dc496623e7cf48 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,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");
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index e56f9cdd9654e160d5d53196a11e3f4445aa004f..5ad5e732d52937857a50e4ee3c4826ebfcf4f6d8 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 607ff99c37d4f0d1670cb617b25cb4b659d777e1..ce406cd1a8b21c4f63251b2f28336b0acca9e1ad 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;