Make test with a for loop to make more KFs

test/gtest_processor_odom_icp_3d.cpp

@@ -39,7 +39,6 @@
 #include <core/utils/utils_gtest.h>
 #include <core/utils/logging.h>
 
-
 // pcl includes
 #include <pcl/point_cloud.h>
 #include <pcl/io/pcd_io.h>
@@ -60,121 +59,68 @@ void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud)
 class Test_ProcessorOdomIcp3D_yaml : public testing::Test
 {
   public:
-    ProblemPtr                           P;
-    SensorLaser3dPtr                     S;
-    ProcessorOdomIcp3dPtr                p;
-    FrameBasePtr                         F0, F1, F;
-    CaptureLaser3dPtr                    C0, C1, C2, C3;
-    
-    VectorXd              data;
-    MatrixXd              data_cov;
+    ProblemPtr            P;
+    SensorLaser3dPtr      S;
+    ProcessorOdomIcp3dPtr p;
+    FrameBasePtr          F0, F1, F;
+    CaptureLaser3dPtr     C0, C1, C2, C3;
+
+    VectorXd data;
+    MatrixXd data_cov;
 
   protected:
     void SetUp() override
     {
-
         WOLF_INFO(laser_root_dir);
 
-        ParserYaml   parser    = ParserYaml("problem_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/");
-        ParamsServer server    = ParamsServer(parser.getParams());
-        P                      = Problem::autoSetup(server);
+        ParserYaml   parser = ParserYaml("problem_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/");
+        ParamsServer server = ParamsServer(parser.getParams());
+        P                   = Problem::autoSetup(server);
 
         S = std::static_pointer_cast<SensorLaser3d>(P->getHardware()->getSensorList().front());
         p = std::static_pointer_cast<ProcessorOdomIcp3d>(S->getProcessorList().front());
 
-
-        P->print(4,1,1,1);
-/* 
-        data     = VectorXd::Zero(12);  // [ a, w, f, t ]
-        data_cov = MatrixXd::Identity(12, 12);
-        C        = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
-
-        C->process();
-
-        C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
-        F0 = C0->getFrame();
-
-        ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp());
-        ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp());
-
-        C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
-        C->process();
-
-        C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
-        F1 = C1->getFrame();
-
-        F1->link(P);  // by the face
-
-        VectorXd delta_preint(VectorXd::Zero(19));
-        delta_preint.head<3>()     = -0.5 * gravity();
-        delta_preint.segment<3>(3) = -gravity();
-        delta_preint.segment<3>(6) = -0.5 * gravity();
-        delta_preint.segment<3>(9) = -gravity();
-        delta_preint(18)           = 1;
-        MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
-        VectorXd calib_preint(VectorXd::Zero(13));
-        MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
-        f1 = FeatureBase::emplace<FeatureMotion>(
-            C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
-
-        c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
-
-        F1->getStateBlock('P')->setState(Vector3d(0, 0, 0));
-        F1->getStateBlock('V')->setState(Vector3d(0, 0, 0));
-        F1->getStateBlock('O')->setState(Vector4d(0, 0, 0, 1));
-        F1->getStateBlock('L')->setState(Vector3d(0, 0, 0));
-        */
+        P->print(4, 1, 1, 1);
     }
 };
-    
 
 TEST(Init, register_in_factories)
 {
-FactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
-AutoConfFactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
-FactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
-AutoConfFactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
-
+    FactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
+    AutoConfFactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
+    FactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
+    AutoConfFactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
 }
 
-TEST_F(Test_ProcessorOdomIcp3D_yaml, align_known_transform)
+TEST_F(Test_ProcessorOdomIcp3D_yaml, process_make_keyframes_always_same_pcl)
 {
-    // Load data
-    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref0 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-    loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref0);
-    C0 = std::make_shared<CaptureLaser3d>(0.0, S, pcl_ref0);
-    C0->process();
-
-    VectorComposite x0 = P->getState();
-
-    // Load data
-    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref1 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-    loadData(laser_root_dir + "/test/data/2.pcd", *pcl_ref1);
-    C1 = std::make_shared<CaptureLaser3d>(1.0, S, pcl_ref1);
-    C1->process();
-
-    VectorComposite x1 = P->getState();
-
-    // Load data
-    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref2 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-    loadData(laser_root_dir + "/test/data/2.pcd", *pcl_ref2);
-    C2 = std::make_shared<CaptureLaser3d>(2.0, S, pcl_ref2);
-    C2->process();
-    
-    VectorComposite x2 = P->getState();
-
-    // Load data
-    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref3 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-    loadData(laser_root_dir + "/test/data/2.pcd", *pcl_ref3);
-    C3 = std::make_shared<CaptureLaser3d>(3.0, S, pcl_ref3);
-    C3->process();
-    
-    VectorComposite x3 = P->getState();
-
-    ASSERT_MATRIX_APPROX(x1.vector("PO"), x2.vector("PO"), 1e-8); // pointclouds 1 and 2 are the same
-    ASSERT_MATRIX_APPROX(x2.vector("PO"), x3.vector("PO"), 1e-8); // pointclouds 2 and 3 are the same
-
-    P->print(4,1,1,1);
+    std::vector<VectorComposite> X(10);
+    unsigned int                 i = 0;
+    for (TimeStamp t = 0; t < 6; t += 0.5)  // will make one KF every 2 captures
+    {
+        // Load pointcloud from file
+        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref0 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+        loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref0);
+
+        // make Capture and process
+        C0 = std::make_shared<CaptureLaser3d>(t, S, pcl_ref0);
+        C0->process();
+
+        // store state at integer timestamps (where the KFs are)
+        if (std::fabs(t.get() - (double)i) < 0.1)
+        {
+            X[i] = p->getLast()->getFrame()->getState("PO");
+            i++;
+        }
+    }
+
+    P->print(4, 1, 1, 1);
+
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(1).vector("PO"), 1e-8);  // pointclouds are the same
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(2).vector("PO"), 1e-8);  // pointclouds are the same
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(3).vector("PO"), 1e-8);  // pointclouds are the same
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(4).vector("PO"), 1e-8);  // pointclouds are the same
+    ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(5).vector("PO"), 1e-8);  // pointclouds are the same
 }
 
 // // Test to see if the constructor works (not yaml files)
@@ -332,7 +278,7 @@ TEST_F(Test_ProcessorOdomIcp3D_yaml, align_known_transform)
 //     ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
 // }
 
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
     testing::InitGoogleTest(&argc, argv);
     //::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";