Merge branch 'devel' of...

Merge branch 'devel' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf into devel

test/gtest_problem.cpp

@@ -46,6 +46,7 @@
 #include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_angle.h"
 #include "core/landmark/landmark_base.h"
+#include "core/math/SE3.h"
 
 
 #include <iostream>
@@ -627,7 +628,8 @@ TEST(Problem, transform)
     auto S = P->getHardware()->getSensorList().front();
 
     // Add dynamic sensor params
-    S->addStateBlock('b', std::make_shared<StateParams3>(Vector3d(3, 2, 1), false), true); // Estimated ; Dynamic
+    Vector3d param_sensor = Vector3d(3, 2, 1);
+    S->addStateBlock('b', std::make_shared<StateParams3>(param_sensor, false), true); // Estimated ; Dynamic
 
     Vector6d data;
     data << 0, 0.2, 0, 0, 0, 0;  // advance on Y
@@ -659,33 +661,78 @@ TEST(Problem, transform)
                                         std::make_shared<StatePoint3d>(Vector3d(1, 2, 3)),
                                         std::make_shared<StateQuaternion>(Vector4d(0, 0, 0, 1)));
 
-    // define transformations
-    VectorComposite traslation("PO",                                                          //
-                               {Vector3d(1, 0, 0),                                            // translation X: 1m
-                                Quaterniond::Identity().coeffs()});                           // no rotation
-    VectorComposite rotation("PO",                                                            //
-                             {Vector3d(0, 0, 0),                                              // no translation
-                              Quaterniond(AngleAxisd(M_PI_2, Vector3d(1, 0, 0))).coeffs()});  // rotation X: 90deg
-    VectorComposite transformation(
-        "PO",                                                             //
-        {Vector3d(1, 0, 0),                                               // translation X: 1m
-         Quaterniond(AngleAxisd(-M_PI_2, Vector3d(1, 0, 0))).coeffs()});  // rotation X: -90deg
-
-    // Perform transformations and print WOLF trees
-    WOLF_INFO("Initial WOLF tree state")
-    P->print(2, 0, 1, 1);  // reference
-
-    WOLF_INFO("Translated 1m along X")
-    P->transform(traslation);  // translation
-    P->print(2, 0, 1, 1);
-
-    WOLF_INFO("Rotated 90deg around X")
-    P->transform(rotation);  // rotation
-    P->print(2, 0, 1, 1);
-
-    WOLF_INFO("Translated 1m along X and rotated -90deg around X")
-    P->transform(transformation);  // translation and rotation
-    P->print(2, 0, 1, 1);
+    // P->print(2, 0, 1, 1);  // reference
+    
+    ///////////////////////////
+    // Define transformations
+
+    // translation
+    Quaterniond q_w1_w0 = Quaterniond::Identity();                            // no rotation
+    VectorComposite pose_w1_w0("PO", {Vector3d(1, 0, 0), q_w1_w0.coeffs()});  // translation X: 1m
+
+    // rotation
+    Quaterniond q_w2_w1(AngleAxisd(M_PI_2, Vector3d(1, 0, 0)));               // rotation X: 90deg
+    VectorComposite pose_w2_w1("PO", {Vector3d(0, 0, 0), q_w2_w1.coeffs()}); // no translation, 
+    
+    // rotation + translation
+    Quaterniond q_w3_w2(AngleAxisd(-M_PI_2, Vector3d(1, 0, 0)));              // rotation X: -90deg
+    VectorComposite pose_w3_w2("PO", {Vector3d(1, 0, 0), q_w3_w2.coeffs()});  // translation X: 1m
+
+    // State of the problem in the initial frame "w0"
+    VectorComposite pose_w0_r0 = P->getTrajectory()->getFirstFrame()->getState();
+    VectorComposite pose_w0_r1 = P->getTrajectory()->getLastFrame()->getState();
+    Vector3d p_w0_l0 = P->getMap()->getLandmarkList().front()->getState("P").vector("P");  // Point3d
+    VectorComposite pose_w0_l1 = P->getMap()->getLandmarkList().back()->getState();
+
+    //////////////////////
+    // Apply successive changes of reference frame w0->w1->w2->w3 and compare with transformed states
+    //////////////////////
+
+    //////////////////////
+    // w0->w1: Translation
+    P->transform(pose_w1_w0);
+    // Expected values
+    VectorComposite pose_w1_r0 = SE3::compose(pose_w1_w0, pose_w0_r0);
+    VectorComposite pose_w1_r1 = SE3::compose(pose_w1_w0, pose_w0_r1);
+    Vector3d p_w1_l0 = pose_w1_w0.at('P') + q_w1_w0 * p_w0_l0;
+    VectorComposite pose_w1_l1 = SE3::compose(pose_w1_w0, pose_w0_l1);
+    ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w1_r0.vector("PO"), 1e-10);
+    ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w1_r1.vector("PO"), 1e-10);
+    ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w1_l0, 1e-10);
+    ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w1_l1.vector("PO"), 1e-10);
+    // P->print(2, 0, 1, 1);
+
+    //////////////////////
+    // w1->w2: Rotation
+    P->transform(pose_w2_w1);
+    // Expected values
+    VectorComposite pose_w2_r0 = SE3::compose(pose_w2_w1, pose_w1_r0);
+    VectorComposite pose_w2_r1 = SE3::compose(pose_w2_w1, pose_w1_r1);
+    Vector3d p_w2_l0 = pose_w2_w1.at('P') + q_w2_w1 * p_w1_l0;
+    VectorComposite pose_w2_l1 = SE3::compose(pose_w2_w1, pose_w1_l1);
+    ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w2_r0.vector("PO"), 1e-10);
+    ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w2_r1.vector("PO"), 1e-10);
+    ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w2_l0, 1e-10);
+    ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w2_l1.vector("PO"), 1e-10);
+    // P->print(2, 0, 1, 1);
+
+    //////////////////////
+    // w2->w3: Translation + Rotation
+    P->transform(pose_w3_w2);
+    // Expected values
+    VectorComposite pose_w3_r0 = SE3::compose(pose_w3_w2, pose_w2_r0);
+    VectorComposite pose_w3_r1 = SE3::compose(pose_w3_w2, pose_w2_r1);
+    Vector3d p_w3_l0 = pose_w3_w2.at('P') + q_w3_w2 * p_w2_l0;
+    VectorComposite pose_w3_l1 = SE3::compose(pose_w3_w2, pose_w2_l1);
+    ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w3_r0.vector("PO"), 1e-10);
+    ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w3_r1.vector("PO"), 1e-10);
+    ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w3_l0, 1e-10);
+    ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w3_l1.vector("PO"), 1e-10);
+    // P->print(2, 0, 1, 1);
+
+    // sensor parameters should not be transformed
+    ASSERT_MATRIX_APPROX(S->getStateBlock('b')->getState(), param_sensor, 1e-10);
+
 }
 
 int main(int argc, char **argv)