From 0576ebb96d072585071f15f0516556339de46411 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 17 Mar 2026 00:09:27 +0100 Subject: [PATCH] Limit map size by subsampling points in point3di_to_pointcloud2 Signed-off-by: Michal Pelka --- include/hdmapping_lio/conversions.hpp | 2 +- src/conversions.cpp | 12 +++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/include/hdmapping_lio/conversions.hpp b/include/hdmapping_lio/conversions.hpp index fa1a139..295ea36 100644 --- a/include/hdmapping_lio/conversions.hpp +++ b/include/hdmapping_lio/conversions.hpp @@ -34,6 +34,6 @@ geometry_msgs::msg::TransformStamped affine_to_transform( sensor_msgs::msg::PointCloud2 point3di_to_pointcloud2( const std::vector& points, const rclcpp::Time& stamp, - const std::string& frame_id); + const std::string& frame_id, int max_points = 2'000'000); } diff --git a/src/conversions.cpp b/src/conversions.cpp index ca8d1bc..c1a0d84 100644 --- a/src/conversions.cpp +++ b/src/conversions.cpp @@ -163,7 +163,7 @@ geometry_msgs::msg::TransformStamped affine_to_transform( sensor_msgs::msg::PointCloud2 point3di_to_pointcloud2( const std::vector& points, const rclcpp::Time& stamp, - const std::string& frame_id) + const std::string& frame_id, int max_points) { sensor_msgs::msg::PointCloud2 msg; msg.header.stamp = stamp; @@ -175,14 +175,20 @@ sensor_msgs::msg::PointCloud2 point3di_to_pointcloud2( "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); - modifier.resize(points.size()); + const size_t point_skip = points.size() > static_cast(max_points) ? points.size() / max_points : 1; + const size_t output_points = (points.size() + point_skip - 1) / point_skip; + modifier.resize(output_points); sensor_msgs::PointCloud2Iterator it_x(msg, "x"); sensor_msgs::PointCloud2Iterator it_y(msg, "y"); sensor_msgs::PointCloud2Iterator it_z(msg, "z"); sensor_msgs::PointCloud2Iterator it_i(msg, "intensity"); - for (const auto& p : points) { + + for (size_t i = 0; i < output_points; ++i) { + const int idx = i * point_skip; + assert(idx < points.size()); + const Point3Di& p = points[idx]; *it_x = static_cast(p.point.x()); *it_y = static_cast(p.point.y()); *it_z = static_cast(p.point.z());