From 2d0e51e09a6b0c08fcfde3565a2c575342eec1e4 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Fri, 5 Jun 2020 11:49:20 +0200
Subject: [PATCH] [WIP] Fix tests to use Composites
All tests compile, 10 tests failing
18 - gtest_problem (Child aborted)
19 - gtest_processor_base (Child aborted)
20 - gtest_processor_motion (Child aborted)
31 - gtest_tree_manager (Failed)
35 - gtest_factor_block_difference (Child aborted)
36 - gtest_factor_diff_drive (Child aborted)
46 - gtest_processor_diff_drive (Child aborted)
47 - gtest_processor_loopclosure (Child aborted)
48 - gtest_odom_2d (Child aborted)
53 - gtest_tree_manager_sliding_window (Failed)
---
test/gtest_odom_2d.cpp | 5 +++--
test/gtest_problem.cpp | 27 ++++++++++++++++-----------
test/gtest_processor_motion.cpp | 18 ++++++++++++------
3 files changed, 31 insertions(+), 19 deletions(-)
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 1afb7c474..be169a04a 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -120,7 +120,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
TimeStamp t0(0.0), t = t0;
double dt = .01;
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- VectorComposite P0(Vector3d(0.1,0.1,0.1), "PO", {2,1});
+ VectorComposite P0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
Vector3d delta (2,0,0);
Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
@@ -171,7 +171,8 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
ASSERT_POSE2d_APPROX(F0->getStateVector(), Vector3d(0,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
+ auto P0_vector = P0.vector("PO");
+ ASSERT_MATRIX_APPROX(P0_solver, (P0_vector.array() * P0_vector.array()).matrix(), 1e-6);
ASSERT_POSE2d_APPROX(F1->getStateVector(), Vector3d(2,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index f2eef1fbe..11517881f 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -119,7 +119,7 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_MATRIX_APPROX(x0, P->getState().vector("PO"), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
@@ -147,12 +147,14 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_FALSE(fac->getCaptureOther());
ASSERT_FALSE(fac->getFeatureOther());
}
-
+ auto x0_vector = x0.vector("PO");
+ auto P0_vector = P0.vector("PO");
+ auto P0_matrix = (P0_vector.array() * P0_vector.array()).matrix();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(x0.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(x0.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -173,7 +175,7 @@ TEST(Problem, SetOrigin_PO_3d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_TRUE((x0 - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((x0.vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL));
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
@@ -202,11 +204,14 @@ TEST(Problem, SetOrigin_PO_3d)
ASSERT_FALSE(fac->getFeatureOther());
}
+ auto x0_vector = x0.vector("PO");
+ auto P0_vector = P0.vector("PO");
+ auto P0_matrix = (P0_vector.array() * P0_vector.array()).matrix();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(x0.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(x0.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 4ed782610..7704bd572 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -133,8 +133,10 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite P0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFix(x0, t, 0.01);
processor->setOrigin(KF_0);
@@ -208,8 +210,10 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite P0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFix(x0, t, 0.01);
processor->setOrigin(KF_0);
@@ -319,8 +323,10 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite P0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFix(x0, t, 0.01);
processor->setOrigin(KF_0);
--
GitLab