Fix test with composite prior

test/gtest_problem.cpp

@@ -6,7 +6,7 @@
  */
 
 #include "core/utils/utils_gtest.h"
-#include "core/utils/logging.h"
+//#include "core/utils/logging.h"
 
 #include "core/problem/problem.h"
 #include "core/sensor/sensor_base.h"
@@ -109,9 +109,9 @@ TEST(Problem, SetOrigin_PO_2d)
     // Eigen::VectorXd x0(3); x0 << 1,2,3;
     VectorComposite x0(Vector3d(1,2,3), "PO", {2,1});
     // Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
-    VectorComposite P0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+    VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
 
-    P->setPriorFactor(x0, P0, t0, 1.0);
+    P->setPriorFactor(x0, s0, t0, 1.0);
 
     P->print(4,1,1,1);
 
@@ -125,8 +125,8 @@ TEST(Problem, SetOrigin_PO_2d)
     TrajectoryBasePtr T = P->getTrajectory();
     FrameBasePtr F = P->getLastFrame();
     CaptureBasePtr C = F->getCaptureList().front();
-    FeatureBasePtr fp = C->getFeatureList().front();
-    FeatureBasePtr fo = C->getFeatureList().back();
+    FeatureBasePtr fo = C->getFeatureList().front();
+    FeatureBasePtr fp = C->getFeatureList().back();
     FactorBasePtrList fac_list;
     F->getFactorList(fac_list);
 
@@ -148,8 +148,20 @@ TEST(Problem, SetOrigin_PO_2d)
         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();
+    auto s0_vector = s0.vector("PO");
+    MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
+
+    WOLF_DEBUG("x0 ", x0);
+    WOLF_DEBUG("x0_vector ", x0_vector.transpose());
+    WOLF_DEBUG("s0 ", s0);
+    WOLF_DEBUG("s0_vector ", s0_vector.transpose());
+    WOLF_DEBUG("P0_matrix ", P0_matrix);
+    WOLF_DEBUG("fp meas ", fp->getMeasurement().transpose());
+    WOLF_DEBUG("fp cova ", fp->getMeasurementCovariance());
+    WOLF_DEBUG("fo meas ", fo->getMeasurement().transpose());
+    WOLF_DEBUG("fo cova ", fo->getMeasurementCovariance());
+
+
     // check that the Feature measurement and covariance are the ones provided
     ASSERT_MATRIX_APPROX(x0_vector.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
     ASSERT_MATRIX_APPROX(x0_vector.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
@@ -167,9 +179,9 @@ TEST(Problem, SetOrigin_PO_3d)
     VectorComposite x0(vec7, "PO", {3,4});
     // Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
     Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
-    VectorComposite P0(vec6, "PO", {3,3});
+    VectorComposite s0(vec6, "PO", {3,3});
 
-    P->setPriorFactor(x0, P0, t0, 1.0);
+    P->setPriorFactor(x0, s0, t0, 1.0);
 
     // check that no sensor has been added
     ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
@@ -181,8 +193,8 @@ TEST(Problem, SetOrigin_PO_3d)
     TrajectoryBasePtr T = P->getTrajectory();
     FrameBasePtr F = P->getLastFrame();
     CaptureBasePtr C = F->getCaptureList().front();
-    FeatureBasePtr fp = C->getFeatureList().front();
-    FeatureBasePtr fo = C->getFeatureList().back();
+    FeatureBasePtr fo = C->getFeatureList().front();
+    FeatureBasePtr fp = C->getFeatureList().back();
     FactorBasePtrList fac_list;
     F->getFactorList(fac_list);
 
@@ -205,8 +217,8 @@ TEST(Problem, SetOrigin_PO_3d)
     }
 
     auto x0_vector = x0.vector("PO");
-    auto P0_vector = P0.vector("PO");
-    auto P0_matrix = (P0_vector.array() * P0_vector.array()).matrix();
+    auto s0_vector = s0.vector("PO");
+    MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
     // check that the Feature measurement and covariance are the ones provided
     ASSERT_MATRIX_APPROX(x0_vector.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
     ASSERT_MATRIX_APPROX(x0_vector.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );