From 20b083367b7392d0393053fc99b65ba75f4e4554 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Wed, 11 Jan 2023 22:45:39 +0900 Subject: [PATCH] Add a function to merge point clouds --- .../app/localization.cpp | 16 ++++--- .../lidar_feature_library/pcl_utils.hpp | 15 +++++- .../test/lib/test_pcl_utils.cpp | 46 +++++++++++++++++++ 3 files changed, 69 insertions(+), 8 deletions(-) diff --git a/localization/lidar_feature_localization/app/localization.cpp b/localization/lidar_feature_localization/app/localization.cpp index 1a46ee93fd848..d4d8a38cfe3b6 100644 --- a/localization/lidar_feature_localization/app/localization.cpp +++ b/localization/lidar_feature_localization/app/localization.cpp @@ -49,6 +49,7 @@ #include "lidar_feature_extraction/hyper_parameter.hpp" #include "lidar_feature_extraction/ring.hpp" +#include "lidar_feature_library/pcl_utils.hpp" #include "lidar_feature_library/point_type.hpp" #include "lidar_feature_library/qos.hpp" #include "lidar_feature_library/ros_msg.hpp" @@ -136,7 +137,7 @@ class DebugCloudPublisher }; template -typename pcl::PointCloud::Ptr MergePointClouds( +typename pcl::PointCloud::Ptr ScanIntegration( const SlidingWindow::Ptr> & sliding_window, const OdometryIntegration & twist_integration) { @@ -145,14 +146,15 @@ typename pcl::PointCloud::Ptr MergePointClouds( const auto latest_pose = twist_integration.Get(latest_timestamp); const Eigen::Isometry3d inv_latest_pose = latest_pose.inverse(); - typename pcl::PointCloud::Ptr merged(new pcl::PointCloud()); - for (size_t i = 0; i < sliding_window.Size(); i++) { + std::vector poses; + std::vector::Ptr> clouds; + for (size_t i = 0; i < size; i++) { const auto [timestamp, cloud] = sliding_window.Get(i); const Eigen::Isometry3d pose = twist_integration.Get(timestamp); - const auto transformed = TransformPointCloud(inv_latest_pose * pose, cloud); - *merged += *transformed; + poses.push_back(inv_latest_pose * pose); + clouds.push_back(cloud); } - return merged; + return MergePointClouds(poses, clouds); } template @@ -236,7 +238,7 @@ class LocalizationNode : public rclcpp::Node const rclcpp::Time stamp = cloud_msg->header.stamp; sliding_window_.Slide(Seconds(stamp), input_cloud); - const auto merged = MergePointClouds(sliding_window_, twist_integration_); + const auto merged = ScanIntegration(sliding_window_, twist_integration_); const auto [edge, surface] = extraction_.Run(merged); diff --git a/localization/lidar_feature_localization/include/lidar_feature_library/pcl_utils.hpp b/localization/lidar_feature_localization/include/lidar_feature_library/pcl_utils.hpp index 0e70f851c1f6f..119e529b96f8c 100644 --- a/localization/lidar_feature_localization/include/lidar_feature_library/pcl_utils.hpp +++ b/localization/lidar_feature_localization/include/lidar_feature_library/pcl_utils.hpp @@ -76,10 +76,23 @@ typename pcl::PointCloud::Ptr TransformPointCloud( return transformed; } +template +typename pcl::PointCloud::Ptr MergePointClouds( + const std::vector & poses, + const std::vector::Ptr> & clouds) +{ + assert(poses.size() == clouds.size()); + typename pcl::PointCloud::Ptr merged(new pcl::PointCloud()); + for (size_t i = 0; i < poses.size(); i++) { + const auto transformed = TransformPointCloud(poses[i], clouds[i]); + *merged += *transformed; + } + return merged; +} + inline Eigen::Vector3d GetXYZ(const pcl::PointXYZ & point) { return Eigen::Vector3d(point.x, point.y, point.z); } - #endif // LIDAR_FEATURE_LIBRARY__PCL_UTILS_HPP_ diff --git a/localization/lidar_feature_localization/test/lib/test_pcl_utils.cpp b/localization/lidar_feature_localization/test/lib/test_pcl_utils.cpp index 7fb13d1cfac75..98b2d8e371a73 100644 --- a/localization/lidar_feature_localization/test/lib/test_pcl_utils.cpp +++ b/localization/lidar_feature_localization/test/lib/test_pcl_utils.cpp @@ -72,3 +72,49 @@ TEST(PCL_UTILS, TransformPointCloud) (q * to_vector(cloud->at(1)) + t - to_vector(transformed->at(1))).norm(), testing::Le(1e-4)); } + +TEST(PCL_UTILS, MergePointClouds) +{ + auto almost_equal = [](const pcl::PointXYZI & a, const pcl::PointXYZI & b) { + return + std::abs(a.x - b.x) + + std::abs(a.y - b.y) + + std::abs(a.z - b.z) + + std::abs(a.intensity - b.intensity) < 1e-6; + }; + + Eigen::Isometry3d pose0; + pose0.linear() = (Eigen::Matrix3d() << + 0., 1., 0., + 1., 0., 0., + 0., 0., -1. + ).finished(); + pose0.translation() = Eigen::Vector3d::Zero(); + + Eigen::Isometry3d pose1; + pose1.linear() = Eigen::Matrix3d::Identity(); + pose1.translation() = Eigen::Vector3d(1., -1., 0.); + + pcl::PointCloud::Ptr cloud0(new pcl::PointCloud()); + cloud0->push_back(pcl::PointXYZI(1., 2., 0., 0.5)); + cloud0->push_back(pcl::PointXYZI(2., 4., 3., 0.3)); + + pcl::PointCloud::Ptr cloud1(new pcl::PointCloud()); + cloud1->push_back(pcl::PointXYZI(1., 2., 1., 0.1)); + cloud1->push_back(pcl::PointXYZI(2., 4., 3., 0.9)); + + std::vector poses{pose0, pose1}; + std::vector::Ptr> clouds{cloud0, cloud1}; + const auto merged = MergePointClouds(poses, clouds); + + pcl::PointCloud::Ptr expected(new pcl::PointCloud()); + expected->push_back(pcl::PointXYZI(2., 1., 0., 0.5)); + expected->push_back(pcl::PointXYZI(4., 2., -3., 0.3)); + expected->push_back(pcl::PointXYZI(2., 1., 1., 0.1)); + expected->push_back(pcl::PointXYZI(3., 3., 3., 0.9)); + + EXPECT_EQ(merged->size(), expected->size()); + for (size_t i = 0; i < expected->size(); i++) { + EXPECT_TRUE(almost_equal(merged->at(i), expected->at(i))); + } +}