diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index f7c8d1c0d158f13cc8065dd8a077b8bc7a0c49bf..7eabd88301bad8fdb4e5297e259c410353241f32 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -246,9 +246,17 @@ int main()
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
- WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
+ {
+ Eigen::MatrixXs cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
for (auto lmk : problem->getMap()->getLandmarkList())
- WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance());
+ {
+ Eigen::MatrixXs cov;
+ lmk->getCovariance(cov);
+ WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+ }
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
diff --git a/include/base/frame_base.h b/include/base/frame_base.h
index be97eb80a333fac1391f7b19bf12d1895e9dbb6d..0177bd8e9253dbd9840ec8fe3a765720a0754fc6 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame_base.h
@@ -113,7 +113,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
void getState(Eigen::VectorXs& _state) const;
unsigned int getSize() const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
- Eigen::MatrixXs getCovariance() const;
// Wolf tree access ---------------------------------------------------
public:
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index 9019ee85efd04f54ddfa920e98413c8aabf3846e..e3b61d65bf86a7690a9350d3d089bec156637092 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -72,7 +72,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& _state) const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
- Eigen::MatrixXs getCovariance() const;
protected:
virtual void removeStateBlocks();
diff --git a/include/base/problem.h b/include/base/problem.h
index 84b51c2a6ce9638fef140c84c86240ebb596738d..a9f8e5e4a28c857a4061ec2296cb81cf31858dfa 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -236,10 +236,8 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov);
bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
- Eigen::MatrixXs getFrameCovariance(FrameBaseConstPtr _frame_ptr);
- Eigen::MatrixXs getLastKeyFrameCovariance();
+ bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance);
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
- Eigen::MatrixXs getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr);
// Solver management ----------------------------------------
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 71e4867117fcaaee040718336af0ca1759835744..32c65515203711405d50b7b95946a76d3dbe10bd 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -216,11 +216,6 @@ bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
-Eigen::MatrixXs FrameBase::getCovariance() const
-{
- return getProblem()->getFrameCovariance(shared_from_this());
-}
-
FrameBasePtr FrameBase::getPreviousFrame() const
{
//std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 8ee7568aba6b36a4e1d1be37406dd735d791b3ac..705cb97b45ef6796d1441630c010e366f657d281 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -94,11 +94,6 @@ void LandmarkBase::registerNewStateBlocks()
}
}
-Eigen::MatrixXs LandmarkBase::getCovariance() const
-{
- return getProblem()->getLandmarkCovariance(shared_from_this());
-}
-
bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
{
return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
diff --git a/src/problem.cpp b/src/problem.cpp
index c95a725a57f654f75c5fe40bbd7f62d3015e2b25..4fe1df930f72885805988ad8038bbe760e865982 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -558,6 +558,16 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
const auto& state_bloc_vec = _frame_ptr->getStateBlockVec();
+ // computing size
+ SizeEigen sz = 0;
+ for (const auto& sb : state_bloc_vec)
+ if (sb)
+ sz += sb->getSize();
+
+ // resizing
+ _covariance = Eigen::MatrixXs(sz, sz);
+
+ // filling covariance
for (const auto& sb_i : state_bloc_vec)
{
if (sb_i)
@@ -577,23 +587,10 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
return success;
}
-Eigen::MatrixXs Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr)
-{
- SizeEigen sz = 0;
- for (const auto& sb : _frame_ptr->getStateBlockVec())
- if (sb)
- sz += sb->getSize();
-
- Eigen::MatrixXs covariance(sz, sz);
-
- getFrameCovariance(_frame_ptr, covariance);
- return covariance;
-}
-
-Eigen::MatrixXs Problem::getLastKeyFrameCovariance()
+bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
{
FrameBasePtr frm = getLastKeyFrame();
- return getFrameCovariance(frm);
+ return getFrameCovariance(frm, cov);
}
bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
@@ -603,6 +600,17 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
+ // computing size
+ SizeEigen sz = 0;
+ for (const auto& sb : state_bloc_vec)
+ if (sb)
+ sz += sb->getSize();
+
+ // resizing
+ _covariance = Eigen::MatrixXs(sz, sz);
+
+ // filling covariance
+
for (const auto& sb_i : state_bloc_vec)
{
if (sb_i)
@@ -622,19 +630,6 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
return success;
}
-Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr)
-{
- SizeEigen sz = 0;
- for (const auto& sb : _landmark_ptr->getStateBlockVec())
- if (sb)
- sz += sb->getSize();
-
- Eigen::MatrixXs covariance(sz, sz);
-
- getLandmarkCovariance(_landmark_ptr, covariance);
- return covariance;
-}
-
MapBasePtr Problem::getMap()
{
return map_ptr_;
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index 769cf871be36a1969a0e2c4883b044f9cabdbda3..7c4e63a75031de399473c9fa1c6e764fa28331a9 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -200,8 +200,12 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& f
Eigen::Vector5s& ellipse)
{
// get 3x3 covariance matrix AKA variance
- const Eigen::MatrixXs covariance =
- getProblem()->getFrameCovariance(frame_ptr);
+ Eigen::MatrixXs covariance;
+ if (!getProblem()->getFrameCovariance(frame_ptr, covariance))
+ {
+ WOLF_WARN("Frame covariance not found!");
+ return false;
+ }
if (!isCovariance(covariance))
{
@@ -245,9 +249,18 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr&
ellipsoid(2) = frame_position(2);
// get 9x9 covariance matrix AKA variance
- const Eigen::MatrixXs covariance = getProblem()->getFrameCovariance(frame_pointer);
+ Eigen::MatrixXs covariance;
+ if (!getProblem()->getFrameCovariance(frame_pointer, covariance))
+ {
+ WOLF_WARN("Frame covariance not found!");
+ return false;
+ }
- if (!isCovariance(covariance)) return false;
+ if (!isCovariance(covariance))
+ {
+ WOLF_WARN("Covariance is not proper !");
+ return false;
+ }
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3s> solver(covariance.block(0,0,3,3));
const Scalar eigenvalue1 = solver.eigenvalues()[0]; // smaller value
@@ -464,10 +477,11 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
query_pose << query->getP()->getState(),
query->getO()->getState();
- const Eigen::MatrixXs traj_covariance = getProblem()->getFrameCovariance(trajectory);
- const Eigen::MatrixXs query_covariance = getProblem()->getFrameCovariance(query);
-
- if ( !isCovariance(traj_covariance) || !isCovariance(query_covariance))
+ Eigen::MatrixXs traj_covariance, query_covariance;
+ if ( !getProblem()->getFrameCovariance(trajectory, traj_covariance) ||
+ !getProblem()->getFrameCovariance(query, query_covariance) ||
+ !isCovariance(traj_covariance) ||
+ !isCovariance(query_covariance))
return distance;
const Eigen::MatrixXs covariance = traj_covariance * query_covariance.transpose();
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 5f479b19ec09a04c604aaca010c98cfd6bd37c6f..df5d418e017d2862941212fd3f94ff173fdaf13a 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -92,7 +92,9 @@ void show(const ProblemPtr& problem)
}
}
cout << " state: \n" << F->getState().transpose() << endl;
- cout << " covariance: \n" << problem->getFrameCovariance(F) << endl;
+ Eigen::MatrixXs cov;
+ problem->getFrameCovariance(F,cov);
+ cout << " covariance: \n" << cov << endl;
}
}
}
@@ -163,12 +165,18 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
0, 1.91, 0.48,
0, 0.48, 0.14;
+ // get covariances
+ MatrixXs P0_solver, P1_solver, P2_solver;
+ ASSERT_TRUE(Pr->getFrameCovariance(F0, P0_solver));
+ ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
+ ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
+
ASSERT_POSE2D_APPROX(F0->getState(), Vector3s(0,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F0), P0, 1e-6);
+ ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
ASSERT_POSE2D_APPROX(F1->getState(), Vector3s(2,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F1), P1, 1e-6);
+ ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
ASSERT_POSE2D_APPROX(F2->getState(), Vector3s(4,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(Pr->getFrameCovariance(F2), P2, 1e-6);
+ ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
}
TEST(Odom2D, VoteForKfAndSolve)
@@ -285,7 +293,9 @@ TEST(Odom2D, VoteForKfAndSolve)
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector[5], 1e-6);
+ MatrixXs computed_cov;
+ ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+ ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[5], 1e-6);
}
TEST(Odom2D, KF_callback)
@@ -413,7 +423,9 @@ TEST(Odom2D, KF_callback)
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
- ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6);
+ MatrixXs computed_cov;
+ ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+ ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
////////////////////////////////////////////////////////////////
// Split between keyframes, exact timestamp
@@ -449,12 +461,16 @@ TEST(Odom2D, KF_callback)
problem->print(4,1,1,1);
// check the split KF
- ASSERT_POSE2D_APPROX(keyframe_1->getState() , integrated_pose_vector[m_split], 1e-6);
- ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6);
+ MatrixXs KF1_cov;
+ ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
+ ASSERT_POSE2D_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
// check other KF in the future of the split KF
- ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
- ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_2) , integrated_cov_vector [n_split], 1e-6);
+ MatrixXs KF2_cov;
+ ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
+ ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
t = t0;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 8ee7623dc68b7f637677fd24ceb6f90b083a4b39..e7a1d6a32d705fef8c5085ef475e3a358a4624e7 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -262,11 +262,20 @@ TEST(Problem, Covariances)
St->unfixExtrinsics();
FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
- Eigen::MatrixXs Cov = P->getFrameCovariance(F);
+ // set covariance (they are not computed without a solver)
+ P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
+ P->addCovarianceBlock(F->getO(), Eigen::Matrix4s::Identity());
+ P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix<Scalar,3,4>::Zero());
+
+ // get covariance
+ Eigen::MatrixXs Cov;
+ ASSERT_TRUE(P->getFrameCovariance(F, Cov));
// FIXME Frame covariance should be 6x6, but it is actually 7x7 (the state of the state blocks, not of the local parametrizations)
+ // JV: The local parameterization projects the covariance to the state size.
ASSERT_EQ(Cov.cols() , 7);
ASSERT_EQ(Cov.rows() , 7);
+ ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix7s::Identity(), 1e-12);
}