From 899e9ff564d4eb9d419bec218d8e730d324dcadb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 11 Apr 2019 12:11:45 +0200
Subject: [PATCH] adapted tests,processors and hello wolf
---
hello_wolf/hello_wolf.cpp | 12 +++++--
...rocessor_frame_nearest_neighbor_filter.cpp | 30 +++++++++++-----
test/gtest_odom_2D.cpp | 36 +++++++++++++------
test/gtest_problem.cpp | 11 +++++-
4 files changed, 68 insertions(+), 21 deletions(-)
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index f7c8d1c0d..7eabd8830 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/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index 769cf871b..7c4e63a75 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 5f479b19e..df5d418e0 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 8ee7623dc..e7a1d6a32 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);
}
--
GitLab