[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

@@ -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);

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);
 }

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);