Add a function to merge point clouds

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 PointType>
-typename pcl::PointCloud<PointType>::Ptr MergePointClouds(
+typename pcl::PointCloud<PointType>::Ptr ScanIntegration(
   const SlidingWindow<typename pcl::PointCloud<PointType>::Ptr> & sliding_window,
   const OdometryIntegration & twist_integration)
 {
@@ -145,14 +146,15 @@ typename pcl::PointCloud<PointType>::Ptr MergePointClouds(
   const auto latest_pose = twist_integration.Get(latest_timestamp);
   const Eigen::Isometry3d inv_latest_pose = latest_pose.inverse();
 
-  typename pcl::PointCloud<PointType>::Ptr merged(new pcl::PointCloud<PointType>());
-  for (size_t i = 0; i < sliding_window.Size(); i++) {
+  std::vector<Eigen::Isometry3d> poses;
+  std::vector<typename pcl::PointCloud<PointType>::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<PointType>(inv_latest_pose * pose, cloud);
-    *merged += *transformed;
+    poses.push_back(inv_latest_pose * pose);
+    clouds.push_back(cloud);
   }
-  return merged;
+  return MergePointClouds<PointType>(poses, clouds);
 }
 
 template<typename PointType>
@@ -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<PointType>(sliding_window_, twist_integration_);
+    const auto merged = ScanIntegration<PointType>(sliding_window_, twist_integration_);
 
     const auto [edge, surface] = extraction_.Run(merged);
 

localization/lidar_feature_localization/include/lidar_feature_library/pcl_utils.hpp

@@ -76,10 +76,23 @@ typename pcl::PointCloud<T>::Ptr TransformPointCloud(
   return transformed;
 }
 
+template<typename PointType>
+typename pcl::PointCloud<PointType>::Ptr MergePointClouds(
+  const std::vector<Eigen::Isometry3d> & poses,
+  const std::vector<typename pcl::PointCloud<PointType>::Ptr> & clouds)
+{
+  assert(poses.size() == clouds.size());
+  typename pcl::PointCloud<PointType>::Ptr merged(new pcl::PointCloud<PointType>());
+  for (size_t i = 0; i < poses.size(); i++) {
+    const auto transformed = TransformPointCloud<PointType>(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_

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<pcl::PointXYZI>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZI>());
+  cloud0->push_back(pcl::PointXYZI(1., 2., 0., 0.5));
+  cloud0->push_back(pcl::PointXYZI(2., 4., 3., 0.3));
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZI>());
+  cloud1->push_back(pcl::PointXYZI(1., 2., 1., 0.1));
+  cloud1->push_back(pcl::PointXYZI(2., 4., 3., 0.9));
+
+  std::vector<Eigen::Isometry3d> poses{pose0, pose1};
+  std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> clouds{cloud0, cloud1};
+  const auto merged = MergePointClouds<pcl::PointXYZI>(poses, clouds);
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr expected(new pcl::PointCloud<pcl::PointXYZI>());
+  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)));
+  }
+}