finished gtest for processor landmark external

test/gtest_processor_tracker_feature_landmark_external.cpp

@@ -57,8 +57,9 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
         ProcessorMotionPtr processor_motion;
         std::vector<LandmarkBasePtr> landmarks;
 
-        // Groundtruth poses
-        VectorXd p_robot, o_robot, p_sensor, o_sensor;
+        // Groundtruth states
+        VectorComposite state_robot, state_sensor;
+        std::vector<VectorComposite> state_landmarks;
 
         virtual void SetUp() {};
         void initProblem(int _dim,
@@ -77,6 +78,7 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
                                int track_length, 
                                double time_span, 
                                bool add_landmarks);
+        void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
 };
 
 void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, 
@@ -179,13 +181,12 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
                                                         nullptr));
         lmk->setId(i);
         landmarks.push_back(lmk);
+        state_landmarks.push_back(lmk->getState());
     }
 
     // Store groundtruth poses
-    p_robot  = F0->getP()->getState();
-    o_robot  = F0->getO()->getState();
-    p_sensor = sensor->getP()->getState();
-    o_sensor = sensor->getO()->getState();
+    state_robot  = F0->getState("PO");
+    state_sensor = sensor->getState("PO");
 }
 
 void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
@@ -210,38 +211,31 @@ void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
     cap_odom->process();
 
     // update groundtruth pose with random movement
-    auto state = processor_motion->getState("PO");
-    p_robot = state.vector("P");
-    o_robot = state.vector("O");
+    state_robot = processor_motion->getState("PO");
 }
 
 CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::computeCaptureLandmarks() const
 {
     // Detections
     auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
-    VectorXd p_lmk, o_lmk;
     for (auto i = 0; i < landmarks.size(); i++)
     {
-        p_lmk = landmarks.at(i)->getP()->getState();
-        if (orientation)
-            o_lmk = landmarks.at(i)->getO()->getState();
-
         // compute detection
         VectorXd meas(orientation?(dim==2?3:7):(dim==2?2:3));
         if (dim == 2)
         {
-            meas.head(2) = Rotation2Dd(-o_sensor(0))*(Rotation2Dd(-o_robot(0))*(p_lmk - p_robot) - p_sensor);
+            meas.head(2) = Rotation2Dd(-state_sensor.at('O')(0))*(Rotation2Dd(-state_robot.at('O')(0))*(state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
             if (orientation)
-                meas(2) = o_lmk(0) - o_robot(0) - o_sensor(0);
+                meas(2) = state_landmarks.at(i).at('O')(0) - state_robot.at('O')(0) - state_sensor.at('O')(0);
         }
         else
         {
-            auto q_sensor = Quaterniond(Vector4d(o_sensor));
-            auto q_robot  = Quaterniond(Vector4d(o_robot));
+            auto q_sensor = Quaterniond(Vector4d(state_sensor.at('O')));
+            auto q_robot  = Quaterniond(Vector4d(state_robot.at('O')));
 
-            meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (p_lmk - p_robot) - p_sensor);
+            meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
             if (orientation)
-                meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(o_lmk))).coeffs();
+                meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(state_landmarks.at(i).at('O')))).coeffs();
         }
         // cov
         MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
@@ -323,20 +317,8 @@ void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim,
                 for (auto lmk_map : landmarks_map)
                 {
                     ASSERT_TRUE(lmk_map->id() < landmarks.size());
-                    auto lmk_gt = landmarks.at(lmk_map->id());
-                    ASSERT_EQ(lmk_map->id(), lmk_gt->id());
-                    if (not orientation)
-                    {
-                        ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS);
-                    }
-                    else if (dim == 2)
-                    {
-                        ASSERT_POSE2d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
-                    }
-                    else if (dim == 3)
-                    {
-                        ASSERT_POSE3d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
-                    }
+                    ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id())->id());
+                    assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
                 }
             }
         }
@@ -350,24 +332,55 @@ void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim,
             ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks);
         }
 
+        // step with random movement
+        t+=dt;
+        randomStep();
+
         // solve
-        if (not problem->getMap()->getLandmarkList().empty())
+        if (should_emplace_KF)
         {
-            // TODO store gt values
+            // perturb landmarks
+            for (auto lmk : problem->getMap()->getLandmarkList())
+                lmk->perturb();
 
-            // TODO perturb
+            // fix frame and extrinsics
+            sensor->fix();
+            problem->getLastFrame()->fix();
 
             // solve
-            // auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
-            // WOLF_INFO(report);
+            auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
+            WOLF_INFO(report);
 
-            // TODO check values
+            // check values                    
+            //assertVectorComposite(sensor->getState("PO"), state_sensor);
+            //assertVectorComposite(problem->getState("PO"), state_robot);
+            for (auto lmk_map : problem->getMap()->getLandmarkList())
+            {
+                assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+            }
         }
+    }
+}
 
-        // step with random movement
-        t+=dt;
-        randomStep();
+void ProcessorTrackerFeatureLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
+{
+    if (v1.includesStructure("PO") and v2.includesStructure("PO"))
+    {
+        if (dim == 2)
+        {
+            ASSERT_POSE2d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+        }
+        else
+        {
+            ASSERT_POSE3d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+        }
     }
+    else if (v1.includesStructure("P") and v2.includesStructure("P"))
+    {
+        ASSERT_MATRIX_APPROX(v1.vector("P"), v2.vector("P"), Constants::EPS);
+    }
+    else
+        throw std::runtime_error("wrong vector composite");
 }
 
 // / TESTS //////////////////////////////////////////
@@ -397,10 +410,10 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks_quality)
 {
     testConfiguration(2,      //int dim
                       false,  //bool orientation
-                      0.3,      //double quality_th
+                      0.3,    //double quality_th
                       1e6,    //double dist_th
                       6,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       false); //bool add_landmarks
 }
 
@@ -411,7 +424,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_existing_lmks)
                       0,      //double quality_th
                       1e6,    //double dist_th
                       3,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       true);  //bool add_landmarks
 }
 
@@ -422,7 +435,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks)
                       0,      //double quality_th
                       1e6,    //double dist_th
                       1,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       false); //bool add_landmarks
 }
 
@@ -430,10 +443,10 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks_quality)
 {
     testConfiguration(2,      //int dim
                       true,   //bool orientation
-                      0.3,      //double quality_th
+                      0.3,    //double quality_th
                       1e6,    //double dist_th
                       0,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       false); //bool add_landmarks
 }
 
@@ -444,7 +457,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks)
                       0,      //double quality_th
                       1e6,    //double dist_th
                       7,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       true);  //bool add_landmarks
 }
 
@@ -454,8 +467,8 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks)
                       false,  //bool orientation
                       0,      //double quality_th
                       1e6,    //double dist_th
-                      53,      //int track_length
-                      4.5*dt,   //double time_span
+                      53,     //int track_length
+                      4.5*dt, //double time_span
                       false); //bool add_landmarks
 }
 
@@ -463,10 +476,10 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks_quality)
 {
     testConfiguration(3,      //int dim
                       false,  //bool orientation
-                      0.3,      //double quality_th
+                      0.3,    //double quality_th
                       1e6,    //double dist_th
                       2,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       false); //bool add_landmarks
 }
 
@@ -477,7 +490,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_existing_lmks)
                       0,      //double quality_th
                       1e6,    //double dist_th
                       1,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       true);  //bool add_landmarks
 }
 
@@ -488,7 +501,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks)
                       0,      //double quality_th
                       1e6,    //double dist_th
                       4,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       false); //bool add_landmarks
 }
 
@@ -496,10 +509,10 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks_quality)
 {
     testConfiguration(3,      //int dim
                       true,   //bool orientation
-                      0.2,      //double quality_th
+                      0.2,    //double quality_th
                       1e6,    //double dist_th
                       5,      //int track_length
-                      4.5*dt,   //double time_span
+                      4.5*dt, //double time_span
                       false); //bool add_landmarks
 }