From b9d39987a095f22d134cebcfd071810b4e5ffee3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 9 Aug 2022 23:19:51 +0200
Subject: [PATCH] Use more KFs in gtest
---
test/gtest_processor_odom_icp_3d.cpp | 39 ++++++++++++++++++----------
1 file changed, 26 insertions(+), 13 deletions(-)
diff --git a/test/gtest_processor_odom_icp_3d.cpp b/test/gtest_processor_odom_icp_3d.cpp
index 4da139bd0..00313cade 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)
--
GitLab