Skip to content
Snippets Groups Projects
Commit 2d0e51e0 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

[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)
parent b1b79b65
No related branches found
No related tags found
1 merge request!366Resolve "Complete state vector new data structure?"
Pipeline #5551 failed
......@@ -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);
......
......@@ -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);
}
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment