HotFix a few gtests that did not pass test due to quaternion not being normalized

test/gtest_factory_state_block.cpp

@@ -108,7 +108,7 @@ TEST(FactoryStateBlock, creator_StateBlock)
 
 TEST(FactoryStateBlock, creator_Quaternion)
 {
-    auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+    auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4).normalized(), false);
 
     ASSERT_EQ(sbq->getSize()     , 4);
     ASSERT_EQ(sbq->getLocalSize(), 3);
@@ -144,7 +144,7 @@ TEST(FactoryStateBlock, creator_H)
 
 TEST(FactoryStateBlock, creator_O_is_quaternion)
 {
-    auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4), false);
+    auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4).normalized(), false);
 
     ASSERT_EQ(sbq->getSize()     , 4);
     ASSERT_EQ(sbq->getLocalSize(), 3);

test/gtest_problem.cpp

@@ -101,7 +101,7 @@ TEST(Problem, Installers)
 {
     std::string wolf_root = _WOLF_ROOT_DIR;
     ProblemPtr P = Problem::create("PO", 3);
-    Eigen::Vector7d xs;
+    Eigen::Vector7d xs; xs.setRandom(); xs.tail(4).normalize();
 
     SensorBasePtr    S = P->installSensor   ("SensorOdom3d", "odometer",        xs,         wolf_root + "/test/yaml/sensor_odom_3d.yaml");
 
@@ -183,7 +183,8 @@ TEST(Problem, SetOrigin_PO_3d)
 {
     ProblemPtr P = Problem::create("PO", 3);
     TimeStamp       t0(0);
-    Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
+    Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7;
+    vec7.tail(4).normalize();
     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);
@@ -242,9 +243,13 @@ TEST(Problem, emplaceFrame_factory)
 {
     ProblemPtr P = Problem::create("PO", 2);
 
-    FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2,  VectorXd(3)  );
-    FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3,  VectorXd(7)  );
-    FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3,  VectorXd(10) );
+    Vector3d xpo2;
+    Vector7d xpo3; xpo3 << 1,2,3,.5,.5,.5,.5;
+    VectorXd xpov3(10); xpov3 << 1,2,3,.5,.5,.5,.5,1,2,3;
+
+    FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2,  xpo2  );
+    FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3,  xpo3  );
+    FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3,  xpov3 );
 
 
     // check that all frames are effectively in the trajectory
@@ -324,7 +329,8 @@ TEST(Problem, Covariances)
     std::string wolf_root = _WOLF_ROOT_DIR;
 
     ProblemPtr P = Problem::create("PO", 3);
-    Eigen::Vector7d xs3d;
+    Eigen::Vector7d xs3d; xs3d.setRandom(); xs3d.tail(4).normalize();
+
     Eigen::Vector3d xs2d;
 
     SensorBasePtr    Sm = P->installSensor   ("SensorOdom3d", "odometer",       xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");

test/gtest_sensor_pose.cpp

@@ -38,7 +38,7 @@ TEST(Pose, constructor)
 {
 
     auto intr = std::make_shared<ParamsSensorPose>();
-    Vector7d extr; extr << 1,2,3,4,5,6,7;
+    Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
 
     auto sen = std::make_shared<SensorPose>(extr, intr);
 
@@ -54,7 +54,7 @@ TEST(Pose, getParams)
     intr->std_p = 2;
     intr->std_o = 3;
 
-    Vector7d extr; extr << 1,2,3,4,5,6,7;
+    Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
 
     auto sen = std::make_shared<SensorPose>(extr, intr);
 
@@ -70,7 +70,7 @@ TEST(Pose, create)
     intr->std_p = 2;
     intr->std_o = 3;
 
-    Vector7d extr; extr << 1,2,3,4,5,6,7;
+    Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
 
     auto sen_base = SensorPose::create("name", extr, intr);
 

test/gtest_tree_manager.cpp

@@ -114,7 +114,8 @@ TEST(TreeManager, keyFrameCallback)
 
     ASSERT_EQ(GM->n_KF_, 0);
 
-    auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) );
+    Vector7d x; x << 1,2,3,0,0,0,1;
+    auto F0 = P->emplaceFrame(0, "PO", 3, x );
     P->keyFrameCallback(F0, nullptr);
 
     ASSERT_EQ(GM->n_KF_, 1);