From 6df44b812491f87cfd3d16dc5bc39898950e2cac Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 12 Apr 2024 00:20:09 +0900 Subject: [PATCH 1/9] fix(occupancy_grid_map_outlier_filter): add intensity field Signed-off-by: badai-nguyen --- .../occupancy_grid_map_outlier_filter_nodelet.hpp | 2 +- .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 1d7d01bfbab55..53c8f29fcc12b 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -46,7 +46,7 @@ using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; -using PclPointCloud = pcl::PointCloud; +using PclPointCloud = pcl::PointCloud; class RadiusSearch2dFilter { diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 65eb336269fe4..3bc4b302b1537 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -354,17 +354,17 @@ void OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap( PclPointCloud & high_confidence, PclPointCloud & low_confidence, PclPointCloud & out_ogm) { for (sensor_msgs::PointCloud2ConstIterator x(pointcloud, "x"), y(pointcloud, "y"), - z(pointcloud, "z"); - x != x.end(); ++x, ++y, ++z) { + z(pointcloud, "z"), intensity(pointcloud, "intensity"); + x != x.end(); ++x, ++y, ++z, ++intensity) { const auto cost = getCost(occupancy_grid_map, *x, *y); if (cost) { if (cost_threshold_ < *cost) { - high_confidence.push_back(pcl::PointXYZ(*x, *y, *z)); + high_confidence.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); } else { - low_confidence.push_back(pcl::PointXYZ(*x, *y, *z)); + low_confidence.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); } } else { - out_ogm.push_back(pcl::PointXYZ(*x, *y, *z)); + out_ogm.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); } } } From 3eb1608679fd9328fdabedfd1c46d78aa7242dcc Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 12 Apr 2024 09:19:13 +0900 Subject: [PATCH 2/9] fix: add intensity Signed-off-by: badai-nguyen --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 44 ++++--------------- 1 file changed, 9 insertions(+), 35 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 3bc4b302b1537..7302e3b8850f9 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -234,48 +234,24 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( if (enable_debugger) { debugger_ptr_ = std::make_shared(*this); } - published_time_publisher_ = std::make_unique(this); } void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc) { - size_t front_count = 0; - size_t behind_count = 0; - - for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"); x != x.end(); ++x) { + PclPointCloud tmp_behind_pc; + PclPointCloud tmp_front_pc; + for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"), y(*input_pc, "y"), + z(*input_pc, "z"), intensity(*input_pc, "intensity"); + x != x.end(); ++x, ++y, ++z, ++intensity) { if (*x < 0.0) { - behind_count++; + tmp_behind_pc.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); } else { - front_count++; + tmp_front_pc.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); } } - - sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc); - sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc); - front_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); - behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); - front_pc_modifier.resize(front_count); - behind_pc_modifier.resize(behind_count); - - sensor_msgs::PointCloud2Iterator fr_iter(front_pc, "x"); - sensor_msgs::PointCloud2Iterator be_iter(behind_pc, "x"); - - for (sensor_msgs::PointCloud2ConstIterator in_iter(*input_pc, "x"); - in_iter != in_iter.end(); ++in_iter) { - if (*in_iter < 0.0) { - *be_iter = in_iter[0]; - be_iter[1] = in_iter[1]; - be_iter[2] = in_iter[2]; - ++be_iter; - } else { - *fr_iter = in_iter[0]; - fr_iter[1] = in_iter[1]; - fr_iter[2] = in_iter[2]; - ++fr_iter; - } - } - + pcl::toROSMsg(tmp_front_pc, front_pc); + pcl::toROSMsg(tmp_behind_pc, behind_pc); front_pc.header = input_pc->header; behind_pc.header = input_pc->header; } @@ -329,8 +305,6 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( return; } pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); - published_time_publisher_->publish_if_subscribed( - pointcloud_pub_, ogm_frame_filtered_pc.header.stamp); } if (debugger_ptr_) { debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); From 2d5017df2a6812994a3d1f9d85ae475572a34d32 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 12 Apr 2024 09:48:12 +0900 Subject: [PATCH 3/9] fix: using empty padding field Signed-off-by: badai-nguyen --- ...upancy_grid_map_outlier_filter_nodelet.hpp | 17 +- ...upancy_grid_map_outlier_filter_nodelet.cpp | 212 +++++++++++++----- 2 files changed, 163 insertions(+), 66 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 53c8f29fcc12b..56cc087ff2260 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -56,8 +56,8 @@ class RadiusSearch2dFilter const PclPointCloud & input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier); void filter( - const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose, - PclPointCloud & output, PclPointCloud & outlier); + const PointCloud2 & high_conf_input, const PointCloud2 & low_conf_input, const Pose & pose, + PointCloud2 & output, PointCloud2 & outlier); private: float search_radius_; @@ -79,22 +79,25 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node const PointCloud2::ConstSharedPtr & input_pointcloud); void filterByOccupancyGridMap( const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud, - PclPointCloud & high_confidence, PclPointCloud & low_confidence, PclPointCloud & out_ogm); + PointCloud2 & high_confidence, PointCloud2 & low_confidence, PointCloud2 & out_ogm); void splitPointCloudFrontBack( const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc); + void initializerPointCloud2(const PointCloud2 & input, PointCloud2 & output); + void finalizePointCloud2(const PointCloud2 & input, PointCloud2 & output); + void concatPointCloud2(PointCloud2 & output, const PointCloud2 & input); private: class Debugger { public: explicit Debugger(OccupancyGridMapOutlierFilterComponent & node); - void publishOutlier(const PclPointCloud & input, const Header & header); - void publishHighConfidence(const PclPointCloud & input, const Header & header); - void publishLowConfidence(const PclPointCloud & input, const Header & header); + void publishOutlier(const PointCloud2 & input, const Header & header); + void publishHighConfidence(const PointCloud2 & input, const Header & header); + void publishLowConfidence(const PointCloud2 & input, const Header & header); private: void transformToBaseLink( - const PclPointCloud & input, const Header & header, PointCloud2 & output); + const PointCloud2 & input, const Header & header, PointCloud2 & output); rclcpp::Publisher::SharedPtr outlier_pointcloud_pub_; rclcpp::Publisher::SharedPtr low_confidence_pointcloud_pub_; rclcpp::Publisher::SharedPtr high_confidence_pointcloud_pub_; diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 7302e3b8850f9..e35215dfbca67 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -147,34 +147,45 @@ void RadiusSearch2dFilter::filter( } void RadiusSearch2dFilter::filter( - const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose, - PclPointCloud & output, PclPointCloud & outlier) + const PointCloud2 & high_conf_xyz_cloud, const PointCloud2 & low_conf_xyz_cloud, + const Pose & pose, PointCloud2 & output, PointCloud2 & outlier) { - const auto & high_conf_xyz_cloud = high_conf_input; - const auto & low_conf_xyz_cloud = low_conf_input; // check the limit points number - if (low_conf_xyz_cloud.points.size() > max_filter_points_nb_) { + if (low_conf_xyz_cloud.width > max_filter_points_nb_) { RCLCPP_WARN( rclcpp::get_logger("OccupancyGridMapOutlierFilterComponent"), "Skip outlier filter since too much low_confidence pointcloud!"); return; } - + int x_offset = low_conf_xyz_cloud.fields[pcl::getFieldIndex(low_conf_xyz_cloud, "x")].offset; + int y_offset = low_conf_xyz_cloud.fields[pcl::getFieldIndex(low_conf_xyz_cloud, "y")].offset; + int point_step = low_conf_xyz_cloud.point_step; pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); - xy_cloud->points.resize(low_conf_xyz_cloud.points.size() + high_conf_xyz_cloud.points.size()); - for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) { - xy_cloud->points[i].x = low_conf_xyz_cloud.points[i].x; - xy_cloud->points[i].y = low_conf_xyz_cloud.points[i].y; + xy_cloud->points.resize(low_conf_xyz_cloud.width + high_conf_xyz_cloud.width); + for (size_t i = 0; i < low_conf_xyz_cloud.width; ++i) { + std::memcpy( + &xy_cloud->points[i].x, &low_conf_xyz_cloud.data[i * point_step + x_offset], sizeof(float)); + std::memcpy( + &xy_cloud->points[i].y, &low_conf_xyz_cloud.data[i * point_step + y_offset], sizeof(float)); } - for (size_t i = low_conf_xyz_cloud.points.size(); i < xy_cloud->points.size(); ++i) { - xy_cloud->points[i].x = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].x; - xy_cloud->points[i].y = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].y; + + for (size_t i = low_conf_xyz_cloud.width; i < xy_cloud->points.size(); ++i) { + size_t high_conf_xyz_cloud_index = i - low_conf_xyz_cloud.width; + std::memcpy( + &xy_cloud->points[i].x, + &high_conf_xyz_cloud.data[high_conf_xyz_cloud_index * point_step + x_offset], sizeof(float)); + std::memcpy( + &xy_cloud->points[i].y, + &high_conf_xyz_cloud.data[high_conf_xyz_cloud_index * point_step + y_offset], sizeof(float)); } std::vector k_indices(xy_cloud->points.size()); std::vector k_distances(xy_cloud->points.size()); kd_tree_->setInputCloud(xy_cloud); - for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) { + + size_t output_size = 0; + size_t outlier_size = 0; + for (size_t i = 0; i < low_conf_xyz_cloud.data.size() / low_conf_xyz_cloud.point_step; ++i) { const float distance = std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y); const int min_points_threshold = std::min( @@ -184,11 +195,20 @@ void RadiusSearch2dFilter::filter( kd_tree_->radiusSearch(i, search_radius_, k_indices, k_distances, min_points_threshold); if (min_points_threshold <= points_num) { - output.points.push_back(low_conf_xyz_cloud.points.at(i)); + std::memcpy( + &output.data[output_size], &low_conf_xyz_cloud.data[i * low_conf_xyz_cloud.point_step], + low_conf_xyz_cloud.point_step); + output_size += low_conf_xyz_cloud.point_step; } else { - outlier.points.push_back(low_conf_xyz_cloud.points.at(i)); + std::memcpy( + &outlier.data[outlier_size], &low_conf_xyz_cloud.data[i * low_conf_xyz_cloud.point_step], + low_conf_xyz_cloud.point_step); + outlier_size += low_conf_xyz_cloud.point_step; } } + + output.data.resize(output_size); + outlier.data.resize(outlier_size); } OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( @@ -239,32 +259,45 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc) { - PclPointCloud tmp_behind_pc; - PclPointCloud tmp_front_pc; - for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"), y(*input_pc, "y"), - z(*input_pc, "z"), intensity(*input_pc, "intensity"); - x != x.end(); ++x, ++y, ++z, ++intensity) { - if (*x < 0.0) { - tmp_behind_pc.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); + int x_offset = input_pc->fields[pcl::getFieldIndex(*input_pc, "x")].offset; + int point_step = input_pc->point_step; + size_t front_count = 0; + size_t behind_count = 0; + + for (size_t global_offset = 0; global_offset < input_pc->data.size(); + global_offset += point_step) { + float x; + std::memcpy(&x, &input_pc->data[global_offset + x_offset], sizeof(float)); + if (x < 0.0) { + std::memcpy( + &behind_pc.data[behind_count * point_step], &input_pc->data[global_offset], + input_pc->point_step); + behind_count++; } else { - tmp_front_pc.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); + std::memcpy( + &front_pc.data[front_count * point_step], &input_pc->data[global_offset], + input_pc->point_step); + front_count++; } } - pcl::toROSMsg(tmp_front_pc, front_pc); - pcl::toROSMsg(tmp_behind_pc, behind_pc); - front_pc.header = input_pc->header; - behind_pc.header = input_pc->header; } void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( const OccupancyGrid::ConstSharedPtr & input_ogm, const PointCloud2::ConstSharedPtr & input_pc) { stop_watch_ptr_->toc("processing_time", true); // Transform to occupancy grid map frame - PointCloud2 ogm_frame_pc{}; - PointCloud2 input_front_pc{}; + PointCloud2 input_behind_pc{}; - PointCloud2 ogm_frame_input_behind_pc{}; + PointCloud2 input_front_pc{}; + initializerPointCloud2(*input_pc, input_front_pc); + initializerPointCloud2(*input_pc, input_behind_pc); + // Split pointcloud into front and behind of the vehicle to reduce the calculation cost splitPointCloudFrontBack(input_pc, input_front_pc, input_behind_pc); + finalizePointCloud2(*input_pc, input_front_pc); + finalizePointCloud2(*input_pc, input_behind_pc); + + PointCloud2 ogm_frame_pc{}; + PointCloud2 ogm_frame_input_behind_pc{}; if ( !transformPointcloud(input_front_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc) || !transformPointcloud( @@ -272,16 +305,22 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( return; } // Occupancy grid map based filter - PclPointCloud high_confidence_pc{}; - PclPointCloud low_confidence_pc{}; - PclPointCloud out_ogm_pc{}; - PclPointCloud ogm_frame_behind_pc; - pcl::fromROSMsg(ogm_frame_input_behind_pc, ogm_frame_behind_pc); + PointCloud2 high_confidence_pc{}; + PointCloud2 low_confidence_pc{}; + PointCloud2 out_ogm_pc{}; + initializerPointCloud2(ogm_frame_pc, high_confidence_pc); + initializerPointCloud2(ogm_frame_pc, low_confidence_pc); + initializerPointCloud2(ogm_frame_pc, out_ogm_pc); + // split front pointcloud into high and low confidence and out of map pointcloud filterByOccupancyGridMap( *input_ogm, ogm_frame_pc, high_confidence_pc, low_confidence_pc, out_ogm_pc); // Apply Radius search 2d filter for low confidence pointcloud - PclPointCloud filtered_low_confidence_pc{}; - PclPointCloud outlier_pc{}; + PointCloud2 filtered_low_confidence_pc{}; + PointCloud2 outlier_pc{}; + initializerPointCloud2(low_confidence_pc, outlier_pc); + initializerPointCloud2(low_confidence_pc, filtered_low_confidence_pc); + initializerPointCloud2(low_confidence_pc, outlier_pc); + if (radius_search_2d_filter_ptr_) { auto pc_frame_pose_stamped = getPoseStamped( *tf2_, input_ogm->header.frame_id, input_pc->header.frame_id, input_ogm->header.stamp); @@ -289,16 +328,18 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( high_confidence_pc, low_confidence_pc, pc_frame_pose_stamped.pose, filtered_low_confidence_pc, outlier_pc); } else { - outlier_pc = low_confidence_pc; + std::memcpy(&outlier_pc.data[0], &low_confidence_pc.data[0], low_confidence_pc.data.size()); + outlier_pc.data.resize(low_confidence_pc.data.size()); } // Concatenate high confidence pointcloud from occupancy grid map and non-outlier pointcloud - PclPointCloud concat_pc = - high_confidence_pc + filtered_low_confidence_pc + out_ogm_pc + ogm_frame_behind_pc; - // Convert to ros msg + PointCloud2 ogm_frame_filtered_pc{}; + concatPointCloud2(ogm_frame_filtered_pc, high_confidence_pc); + concatPointCloud2(ogm_frame_filtered_pc, filtered_low_confidence_pc); + concatPointCloud2(ogm_frame_filtered_pc, out_ogm_pc); + concatPointCloud2(ogm_frame_filtered_pc, ogm_frame_input_behind_pc); + finalizePointCloud2(ogm_frame_pc, ogm_frame_filtered_pc); { - PointCloud2 ogm_frame_filtered_pc{}; auto base_link_frame_filtered_pc_ptr = std::make_unique(); - pcl::toROSMsg(concat_pc, ogm_frame_filtered_pc); ogm_frame_filtered_pc.header = ogm_frame_pc.header; if (!transformPointcloud( ogm_frame_filtered_pc, *tf2_, base_link_frame_, *base_link_frame_filtered_pc_ptr)) { @@ -307,6 +348,9 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); } if (debugger_ptr_) { + finalizePointCloud2(ogm_frame_pc, filtered_low_confidence_pc); + finalizePointCloud2(ogm_frame_pc, outlier_pc); + finalizePointCloud2(ogm_frame_pc, high_confidence_pc); debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); debugger_ptr_->publishLowConfidence(filtered_low_confidence_pc, ogm_frame_pc.header); debugger_ptr_->publishOutlier(outlier_pc, ogm_frame_pc.header); @@ -323,24 +367,77 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( } } +void OccupancyGridMapOutlierFilterComponent::initializerPointCloud2( + const PointCloud2 & input, PointCloud2 & output) +{ + output.point_step = input.point_step; + output.data.resize(input.data.size()); +} + +void OccupancyGridMapOutlierFilterComponent::finalizePointCloud2( + const PointCloud2 & input, PointCloud2 & output) +{ + output.header = input.header; + output.point_step = input.point_step; + output.fields = input.fields; + output.height = input.height; + output.is_bigendian = input.is_bigendian; + output.is_dense = input.is_dense; + output.width = output.data.size() / output.point_step / output.height; + output.row_step = output.data.size() / output.height; +} + +void OccupancyGridMapOutlierFilterComponent::concatPointCloud2( + PointCloud2 & output, const PointCloud2 & input) +{ + size_t output_size = output.data.size(); + output.data.resize(output.data.size() + input.data.size()); + std::memcpy(&output.data[output_size], &input.data[0], input.data.size()); +} void OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap( const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud, - PclPointCloud & high_confidence, PclPointCloud & low_confidence, PclPointCloud & out_ogm) + PointCloud2 & high_confidence, PointCloud2 & low_confidence, PointCloud2 & out_ogm) { - for (sensor_msgs::PointCloud2ConstIterator x(pointcloud, "x"), y(pointcloud, "y"), - z(pointcloud, "z"), intensity(pointcloud, "intensity"); - x != x.end(); ++x, ++y, ++z, ++intensity) { - const auto cost = getCost(occupancy_grid_map, *x, *y); + int x_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "x")].offset; + int y_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "y")].offset; + // int z_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "z")].offset; + // int intensity_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "intensity")].offset; + size_t high_confidence_size = 0; + size_t low_confidence_size = 0; + size_t out_ogm_size = 0; + + for (size_t global_offset = 0; global_offset < pointcloud.data.size(); + global_offset += pointcloud.point_step) { + float x; + float y; + std::memcpy(&x, &pointcloud.data[global_offset + x_offset], sizeof(float)); + std::memcpy(&y, &pointcloud.data[global_offset + y_offset], sizeof(float)); + + const auto cost = getCost(occupancy_grid_map, x, y); if (cost) { if (cost_threshold_ < *cost) { - high_confidence.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); + std::memcpy( + &high_confidence.data[high_confidence_size], &pointcloud.data[global_offset], + pointcloud.point_step); + high_confidence_size += pointcloud.point_step; } else { - low_confidence.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); + std::memcpy( + &low_confidence.data[low_confidence_size], &pointcloud.data[global_offset], + pointcloud.point_step); + low_confidence_size += pointcloud.point_step; } } else { - out_ogm.push_back(pcl::PointXYZI(*x, *y, *z, *intensity)); + std::memcpy( + &out_ogm.data[out_ogm_size], &pointcloud.data[global_offset], pointcloud.point_step); + out_ogm_size += pointcloud.point_step; } } + high_confidence.data.resize(high_confidence_size); + low_confidence.data.resize(low_confidence_size); + out_ogm.data.resize(out_ogm_size); + finalizePointCloud2(pointcloud, high_confidence); + finalizePointCloud2(pointcloud, low_confidence); + finalizePointCloud2(pointcloud, out_ogm); } OccupancyGridMapOutlierFilterComponent::Debugger::Debugger( @@ -356,14 +453,14 @@ OccupancyGridMapOutlierFilterComponent::Debugger::Debugger( } void OccupancyGridMapOutlierFilterComponent::Debugger::publishOutlier( - const PclPointCloud & input, const Header & header) + const PointCloud2 & input, const Header & header) { auto output_ptr = std::make_unique(); transformToBaseLink(input, header, *output_ptr); outlier_pointcloud_pub_->publish(std::move(output_ptr)); } void OccupancyGridMapOutlierFilterComponent::Debugger::publishHighConfidence( - const PclPointCloud & input, const Header & header) + const PointCloud2 & input, const Header & header) { auto output_ptr = std::make_unique(); transformToBaseLink(input, header, *output_ptr); @@ -371,7 +468,7 @@ void OccupancyGridMapOutlierFilterComponent::Debugger::publishHighConfidence( } void OccupancyGridMapOutlierFilterComponent::Debugger::publishLowConfidence( - const PclPointCloud & input, const Header & header) + const PointCloud2 & input, const Header & header) { auto output_ptr = std::make_unique(); transformToBaseLink(input, header, *output_ptr); @@ -379,11 +476,8 @@ void OccupancyGridMapOutlierFilterComponent::Debugger::publishLowConfidence( } void OccupancyGridMapOutlierFilterComponent::Debugger::transformToBaseLink( - const PclPointCloud & pcl_input, const Header & header, PointCloud2 & output) + const PointCloud2 & ros_input, [[maybe_unused]] const Header & header, PointCloud2 & output) { - PointCloud2 ros_input{}; - pcl::toROSMsg(pcl_input, ros_input); - ros_input.header = header; transformPointcloud(ros_input, *(node_.tf2_), node_.base_link_frame_, output); } From fcf6b616ee3b246b78f381ef887d6599400206b6 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 12 Apr 2024 18:27:31 +0900 Subject: [PATCH 4/9] refactor Signed-off-by: badai-nguyen --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 63 ++++++++++++++----- 1 file changed, 47 insertions(+), 16 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index e35215dfbca67..4d3d0fc58d673 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -261,6 +261,16 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( { int x_offset = input_pc->fields[pcl::getFieldIndex(*input_pc, "x")].offset; int point_step = input_pc->point_step; + + front_pc.data.resize(input_pc->data.size()); + behind_pc.data.resize(input_pc->data.size()); + front_pc.point_step = input_pc->point_step; + behind_pc.point_step = input_pc->point_step; + front_pc.fields = input_pc->fields; + behind_pc.fields = input_pc->fields; + front_pc.height = input_pc->height; + behind_pc.height = input_pc->height; + size_t front_count = 0; size_t behind_count = 0; @@ -280,24 +290,34 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( front_count++; } } + front_pc.data.resize(front_count * point_step); + front_pc.width = front_count; + front_pc.row_step = front_count * front_pc.point_step; + front_pc.header = input_pc->header; + front_pc.is_bigendian = input_pc->is_bigendian; + front_pc.is_dense = input_pc->is_dense; + + behind_pc.data.resize(behind_count * point_step); + behind_pc.width = behind_count; + behind_pc.row_step = behind_count * behind_pc.point_step; + behind_pc.header = input_pc->header; + behind_pc.is_bigendian = input_pc->is_bigendian; + behind_pc.is_dense = input_pc->is_dense; } void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( const OccupancyGrid::ConstSharedPtr & input_ogm, const PointCloud2::ConstSharedPtr & input_pc) { stop_watch_ptr_->toc("processing_time", true); // Transform to occupancy grid map frame - - PointCloud2 input_behind_pc{}; - PointCloud2 input_front_pc{}; - initializerPointCloud2(*input_pc, input_front_pc); - initializerPointCloud2(*input_pc, input_behind_pc); - // Split pointcloud into front and behind of the vehicle to reduce the calculation cost - splitPointCloudFrontBack(input_pc, input_front_pc, input_behind_pc); - finalizePointCloud2(*input_pc, input_front_pc); - finalizePointCloud2(*input_pc, input_behind_pc); - PointCloud2 ogm_frame_pc{}; + PointCloud2 input_front_pc{}; + PointCloud2 input_behind_pc{}; PointCloud2 ogm_frame_input_behind_pc{}; + PointCloud2 ogm_frame_pc; + PointCloud2 input_front_pc; + PointCloud2 input_behind_pc; + PointCloud2 ogm_frame_input_behind_pc; + splitPointCloudFrontBack(input_pc, input_front_pc, input_behind_pc); if ( !transformPointcloud(input_front_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc) || !transformPointcloud( @@ -311,7 +331,8 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( initializerPointCloud2(ogm_frame_pc, high_confidence_pc); initializerPointCloud2(ogm_frame_pc, low_confidence_pc); initializerPointCloud2(ogm_frame_pc, out_ogm_pc); - // split front pointcloud into high and low confidence and out of map pointcloud + // PclPointCloud ogm_frame_behind_pc; + // pcl::fromROSMsg(ogm_frame_input_behind_pc, ogm_frame_behind_pc); filterByOccupancyGridMap( *input_ogm, ogm_frame_pc, high_confidence_pc, low_confidence_pc, out_ogm_pc); // Apply Radius search 2d filter for low confidence pointcloud @@ -319,8 +340,11 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( PointCloud2 outlier_pc{}; initializerPointCloud2(low_confidence_pc, outlier_pc); initializerPointCloud2(low_confidence_pc, filtered_low_confidence_pc); - initializerPointCloud2(low_confidence_pc, outlier_pc); - + PointCloud2 outlier_pc; + outlier_pc.data.resize(low_confidence_pc.data.size()); + outlier_pc.point_step = low_confidence_pc.point_step; + outlier_pc.fields = low_confidence_pc.fields; + outlier_pc.height = 1; if (radius_search_2d_filter_ptr_) { auto pc_frame_pose_stamped = getPoseStamped( *tf2_, input_ogm->header.frame_id, input_pc->header.frame_id, input_ogm->header.stamp); @@ -337,7 +361,12 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( concatPointCloud2(ogm_frame_filtered_pc, filtered_low_confidence_pc); concatPointCloud2(ogm_frame_filtered_pc, out_ogm_pc); concatPointCloud2(ogm_frame_filtered_pc, ogm_frame_input_behind_pc); + finalizePointCloud2(ogm_frame_pc, ogm_frame_filtered_pc); + finalizePointCloud2(ogm_frame_pc, filtered_low_confidence_pc); + finalizePointCloud2(ogm_frame_pc, outlier_pc); + finalizePointCloud2(ogm_frame_pc, high_confidence_pc); + // Convert to ros msg { auto base_link_frame_filtered_pc_ptr = std::make_unique(); ogm_frame_filtered_pc.header = ogm_frame_pc.header; @@ -348,9 +377,6 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); } if (debugger_ptr_) { - finalizePointCloud2(ogm_frame_pc, filtered_low_confidence_pc); - finalizePointCloud2(ogm_frame_pc, outlier_pc); - finalizePointCloud2(ogm_frame_pc, high_confidence_pc); debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); debugger_ptr_->publishLowConfidence(filtered_low_confidence_pc, ogm_frame_pc.header); debugger_ptr_->publishOutlier(outlier_pc, ogm_frame_pc.header); @@ -370,7 +396,12 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( void OccupancyGridMapOutlierFilterComponent::initializerPointCloud2( const PointCloud2 & input, PointCloud2 & output) { + output.header = input.header; output.point_step = input.point_step; + output.fields = input.fields; + output.height = input.height; + output.is_bigendian = input.is_bigendian; + output.is_dense = input.is_dense; output.data.resize(input.data.size()); } From aa09cf4f37283486c2beaaf47760711144e68bc6 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 12 Apr 2024 18:27:40 +0900 Subject: [PATCH 5/9] Revert "refactor" This reverts commit d16b3e0721b63f4085de128ed72aa7b8671701b2. --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 63 +++++-------------- 1 file changed, 16 insertions(+), 47 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 4d3d0fc58d673..e35215dfbca67 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -261,16 +261,6 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( { int x_offset = input_pc->fields[pcl::getFieldIndex(*input_pc, "x")].offset; int point_step = input_pc->point_step; - - front_pc.data.resize(input_pc->data.size()); - behind_pc.data.resize(input_pc->data.size()); - front_pc.point_step = input_pc->point_step; - behind_pc.point_step = input_pc->point_step; - front_pc.fields = input_pc->fields; - behind_pc.fields = input_pc->fields; - front_pc.height = input_pc->height; - behind_pc.height = input_pc->height; - size_t front_count = 0; size_t behind_count = 0; @@ -290,34 +280,24 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( front_count++; } } - front_pc.data.resize(front_count * point_step); - front_pc.width = front_count; - front_pc.row_step = front_count * front_pc.point_step; - front_pc.header = input_pc->header; - front_pc.is_bigendian = input_pc->is_bigendian; - front_pc.is_dense = input_pc->is_dense; - - behind_pc.data.resize(behind_count * point_step); - behind_pc.width = behind_count; - behind_pc.row_step = behind_count * behind_pc.point_step; - behind_pc.header = input_pc->header; - behind_pc.is_bigendian = input_pc->is_bigendian; - behind_pc.is_dense = input_pc->is_dense; } void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( const OccupancyGrid::ConstSharedPtr & input_ogm, const PointCloud2::ConstSharedPtr & input_pc) { stop_watch_ptr_->toc("processing_time", true); // Transform to occupancy grid map frame - PointCloud2 ogm_frame_pc{}; - PointCloud2 input_front_pc{}; + PointCloud2 input_behind_pc{}; - PointCloud2 ogm_frame_input_behind_pc{}; - PointCloud2 ogm_frame_pc; - PointCloud2 input_front_pc; - PointCloud2 input_behind_pc; - PointCloud2 ogm_frame_input_behind_pc; + PointCloud2 input_front_pc{}; + initializerPointCloud2(*input_pc, input_front_pc); + initializerPointCloud2(*input_pc, input_behind_pc); + // Split pointcloud into front and behind of the vehicle to reduce the calculation cost splitPointCloudFrontBack(input_pc, input_front_pc, input_behind_pc); + finalizePointCloud2(*input_pc, input_front_pc); + finalizePointCloud2(*input_pc, input_behind_pc); + + PointCloud2 ogm_frame_pc{}; + PointCloud2 ogm_frame_input_behind_pc{}; if ( !transformPointcloud(input_front_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc) || !transformPointcloud( @@ -331,8 +311,7 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( initializerPointCloud2(ogm_frame_pc, high_confidence_pc); initializerPointCloud2(ogm_frame_pc, low_confidence_pc); initializerPointCloud2(ogm_frame_pc, out_ogm_pc); - // PclPointCloud ogm_frame_behind_pc; - // pcl::fromROSMsg(ogm_frame_input_behind_pc, ogm_frame_behind_pc); + // split front pointcloud into high and low confidence and out of map pointcloud filterByOccupancyGridMap( *input_ogm, ogm_frame_pc, high_confidence_pc, low_confidence_pc, out_ogm_pc); // Apply Radius search 2d filter for low confidence pointcloud @@ -340,11 +319,8 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( PointCloud2 outlier_pc{}; initializerPointCloud2(low_confidence_pc, outlier_pc); initializerPointCloud2(low_confidence_pc, filtered_low_confidence_pc); - PointCloud2 outlier_pc; - outlier_pc.data.resize(low_confidence_pc.data.size()); - outlier_pc.point_step = low_confidence_pc.point_step; - outlier_pc.fields = low_confidence_pc.fields; - outlier_pc.height = 1; + initializerPointCloud2(low_confidence_pc, outlier_pc); + if (radius_search_2d_filter_ptr_) { auto pc_frame_pose_stamped = getPoseStamped( *tf2_, input_ogm->header.frame_id, input_pc->header.frame_id, input_ogm->header.stamp); @@ -361,12 +337,7 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( concatPointCloud2(ogm_frame_filtered_pc, filtered_low_confidence_pc); concatPointCloud2(ogm_frame_filtered_pc, out_ogm_pc); concatPointCloud2(ogm_frame_filtered_pc, ogm_frame_input_behind_pc); - finalizePointCloud2(ogm_frame_pc, ogm_frame_filtered_pc); - finalizePointCloud2(ogm_frame_pc, filtered_low_confidence_pc); - finalizePointCloud2(ogm_frame_pc, outlier_pc); - finalizePointCloud2(ogm_frame_pc, high_confidence_pc); - // Convert to ros msg { auto base_link_frame_filtered_pc_ptr = std::make_unique(); ogm_frame_filtered_pc.header = ogm_frame_pc.header; @@ -377,6 +348,9 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); } if (debugger_ptr_) { + finalizePointCloud2(ogm_frame_pc, filtered_low_confidence_pc); + finalizePointCloud2(ogm_frame_pc, outlier_pc); + finalizePointCloud2(ogm_frame_pc, high_confidence_pc); debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); debugger_ptr_->publishLowConfidence(filtered_low_confidence_pc, ogm_frame_pc.header); debugger_ptr_->publishOutlier(outlier_pc, ogm_frame_pc.header); @@ -396,12 +370,7 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( void OccupancyGridMapOutlierFilterComponent::initializerPointCloud2( const PointCloud2 & input, PointCloud2 & output) { - output.header = input.header; output.point_step = input.point_step; - output.fields = input.fields; - output.height = input.height; - output.is_bigendian = input.is_bigendian; - output.is_dense = input.is_dense; output.data.resize(input.data.size()); } From 454d560797f2a4f4ae6e2a1c73f5dba6ac0c33fe Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Sat, 13 Apr 2024 00:33:28 +0900 Subject: [PATCH 6/9] refactor Signed-off-by: badai-nguyen --- .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index e35215dfbca67..250a94bab9c5c 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -280,6 +280,8 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( front_count++; } } + front_pc.data.resize(front_count * point_step); + behind_pc.data.resize(behind_count * point_step); } void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( const OccupancyGrid::ConstSharedPtr & input_ogm, const PointCloud2::ConstSharedPtr & input_pc) @@ -348,9 +350,9 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); } if (debugger_ptr_) { + finalizePointCloud2(ogm_frame_pc, high_confidence_pc); finalizePointCloud2(ogm_frame_pc, filtered_low_confidence_pc); finalizePointCloud2(ogm_frame_pc, outlier_pc); - finalizePointCloud2(ogm_frame_pc, high_confidence_pc); debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); debugger_ptr_->publishLowConfidence(filtered_low_confidence_pc, ogm_frame_pc.header); debugger_ptr_->publishOutlier(outlier_pc, ogm_frame_pc.header); @@ -370,7 +372,12 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( void OccupancyGridMapOutlierFilterComponent::initializerPointCloud2( const PointCloud2 & input, PointCloud2 & output) { + output.header = input.header; output.point_step = input.point_step; + output.fields = input.fields; + output.height = input.height; + output.is_bigendian = input.is_bigendian; + output.is_dense = input.is_dense; output.data.resize(input.data.size()); } From fa0543c2cefbf510bf0f19dd2e5573c5372cac98 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Sat, 13 Apr 2024 01:02:37 +0900 Subject: [PATCH 7/9] fix: radius filter Signed-off-by: badai-nguyen --- ...upancy_grid_map_outlier_filter_nodelet.hpp | 3 +- ...upancy_grid_map_outlier_filter_nodelet.cpp | 31 ++++++++++--------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 56cc087ff2260..53965c00b48f5 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -53,8 +53,7 @@ class RadiusSearch2dFilter public: explicit RadiusSearch2dFilter(rclcpp::Node & node); void filter( - const PclPointCloud & input, const Pose & pose, PclPointCloud & output, - PclPointCloud & outlier); + const PointCloud2 & input, const Pose & pose, PointCloud2 & output, PointCloud2 & outlier); void filter( const PointCloud2 & high_conf_input, const PointCloud2 & low_conf_input, const Pose & pose, PointCloud2 & output, PointCloud2 & outlier); diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 250a94bab9c5c..836fd2b873276 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -116,19 +116,23 @@ RadiusSearch2dFilter::RadiusSearch2dFilter(rclcpp::Node & node) } void RadiusSearch2dFilter::filter( - const PclPointCloud & input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier) + const PointCloud2 & input, const Pose & pose, PointCloud2 & output, PointCloud2 & outlier) { - const auto & xyz_cloud = input; pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); - xy_cloud->points.resize(xyz_cloud.points.size()); - for (size_t i = 0; i < xyz_cloud.points.size(); ++i) { - xy_cloud->points[i].x = xyz_cloud.points[i].x; - xy_cloud->points[i].y = xyz_cloud.points[i].y; + int point_step = input.point_step; + int x_offset = input.fields[pcl::getFieldIndex(input, "x")].offset; + int y_offset = input.fields[pcl::getFieldIndex(input, "y")].offset; + xy_cloud->points.resize(input.data.size() / point_step); + for (size_t i = 0; i < input.data.size() / point_step; ++i) { + std::memcpy(&xy_cloud->points[i].x, &input.data[i * x_offset], sizeof(float)); + std::memcpy(&xy_cloud->points[i].y, &input.data[i * y_offset], sizeof(float)); } std::vector k_indices(xy_cloud->points.size()); std::vector k_distances(xy_cloud->points.size()); kd_tree_->setInputCloud(xy_cloud); + size_t output_size = 0; + size_t outlier_size = 0; for (size_t i = 0; i < xy_cloud->points.size(); ++i) { const float distance = std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y); @@ -139,11 +143,15 @@ void RadiusSearch2dFilter::filter( kd_tree_->radiusSearch(i, search_radius_, k_indices, k_distances, min_points_threshold); if (min_points_threshold <= points_num) { - output.points.push_back(xyz_cloud.points.at(i)); + std::memcpy(&output.data[output_size], &input.data[i * point_step], point_step); + output_size += point_step; } else { - outlier.points.push_back(xyz_cloud.points.at(i)); + std::memcpy(&outlier.data[outlier_size], &input.data[i * point_step], point_step); + outlier_size += point_step; } } + output.data.resize(output_size); + outlier.data.resize(outlier_size); } void RadiusSearch2dFilter::filter( @@ -372,12 +380,7 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( void OccupancyGridMapOutlierFilterComponent::initializerPointCloud2( const PointCloud2 & input, PointCloud2 & output) { - output.header = input.header; output.point_step = input.point_step; - output.fields = input.fields; - output.height = input.height; - output.is_bigendian = input.is_bigendian; - output.is_dense = input.is_dense; output.data.resize(input.data.size()); } @@ -407,8 +410,6 @@ void OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap( { int x_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "x")].offset; int y_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "y")].offset; - // int z_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "z")].offset; - // int intensity_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "intensity")].offset; size_t high_confidence_size = 0; size_t low_confidence_size = 0; size_t out_ogm_size = 0; From 0aed0adbef96b83620f0d2983fd370e6315266ff Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Sat, 13 Apr 2024 21:56:50 +0900 Subject: [PATCH 8/9] chore: typo Signed-off-by: badai-nguyen --- .../occupancy_grid_map_outlier_filter_nodelet.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 53965c00b48f5..0faa40255a1ca 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -46,7 +46,6 @@ using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; -using PclPointCloud = pcl::PointCloud; class RadiusSearch2dFilter { From e2330822d5b3d8d3ec516145d1ef40b07efbc2a7 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 7 May 2024 09:10:40 +0900 Subject: [PATCH 9/9] fix: typo Signed-off-by: badai-nguyen --- .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 836fd2b873276..bbd138030d76f 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -124,8 +124,8 @@ void RadiusSearch2dFilter::filter( int y_offset = input.fields[pcl::getFieldIndex(input, "y")].offset; xy_cloud->points.resize(input.data.size() / point_step); for (size_t i = 0; i < input.data.size() / point_step; ++i) { - std::memcpy(&xy_cloud->points[i].x, &input.data[i * x_offset], sizeof(float)); - std::memcpy(&xy_cloud->points[i].y, &input.data[i * y_offset], sizeof(float)); + std::memcpy(&xy_cloud->points[i].x, &input.data[i * point_step + x_offset], sizeof(float)); + std::memcpy(&xy_cloud->points[i].y, &input.data[i * point_step + y_offset], sizeof(float)); } std::vector k_indices(xy_cloud->points.size());