Use more KFs in gtest

test/gtest_processor_odom_icp_3d.cpp

@@ -64,7 +64,7 @@ class Test_ProcessorOdomIcp3D_yaml : public testing::Test
     SensorLaser3dPtr                     S;
     ProcessorOdomIcp3dPtr                p;
     FrameBasePtr                         F0, F1, F;
-    CaptureLaser3dPtr                    C0, C1, C2;
+    CaptureLaser3dPtr                    C0, C1, C2, C3;
     
     VectorXd              data;
     MatrixXd              data_cov;
@@ -137,31 +137,44 @@ AutoConfFactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp
 
 }
 
-TEST_F(Test_ProcessorOdomIcp3D_yaml, constructor)
+TEST_F(Test_ProcessorOdomIcp3D_yaml, align_known_transform)
 {
     // Load data
-    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref1 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-    loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref1);
-    C0 = std::make_shared<CaptureLaser3d>(0.0, S, pcl_ref1);
+    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();
-    P->print(4,1,1,1);
+
+    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);
-    C1 = std::make_shared<CaptureLaser3d>(1.0, S, pcl_ref2);
-    C1->process();
-    P->print(4,1,1,1);
+    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);
-    C2 = std::make_shared<CaptureLaser3d>(2.0, S, pcl_ref3);
-    C2->process();
-    P->print(4,1,1,1);
+    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);
 }
 
 // // Test to see if the constructor works (not yaml files)