diff --git a/test/gtest_processor_odom_icp_3d.cpp b/test/gtest_processor_odom_icp_3d.cpp index 4da139bd0e186062c1395dde7b2d2edb55fe3e8e..00313cade8eba4eb10f234c696789d4560076d15 100644 --- a/test/gtest_processor_odom_icp_3d.cpp +++ b/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)