perf(occupancy_grid_map_outlier_filter): improve performance (autowar…

…efoundation#5980) * improve perfomance Signed-off-by: Taiga Takano <[email protected]> * style(pre-commit): autofix Signed-off-by: Taiga Takano <[email protected]> * fixed the bug and corrected the spelling mistake. Signed-off-by: Taiga Takano <[email protected]> * style(pre-commit): autofix Signed-off-by: Taiga Takano <[email protected]> * fix bug Signed-off-by: Taiga Takano <[email protected]> * style(pre-commit): autofix Signed-off-by: Taiga Takano <[email protected]> * change filter Signed-off-by: Taiga Takano <[email protected]> --------- Signed-off-by: Taiga Takano <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yoshi Ri <[email protected]>
badai-nguyen · Apr 19, 2024 · 1d957c9 · 1d957c9
1 parent 360fd3c
commit 1d957c9
Showing 1 changed file with 32 additions and 9 deletions.
diff --git a/...ption/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/...ption/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp
@@ -239,19 +239,42 @@ 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<float> x(*input_pc, "x"), y(*input_pc, "y"),
-       z(*input_pc, "z");
-       x != x.end(); ++x, ++y, ++z) {
+  size_t front_count = 0;
+  size_t behind_count = 0;
+
+  for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"); x != x.end(); ++x) {
     if (*x < 0.0) {
-      tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z));
+      behind_count++;
     } else {
-      tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z));
+      front_count++;
     }
   }
-  pcl::toROSMsg(tmp_front_pc, front_pc);
-  pcl::toROSMsg(tmp_behind_pc, behind_pc);
+
+  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<float> fr_iter(front_pc, "x");
+  sensor_msgs::PointCloud2Iterator<float> be_iter(behind_pc, "x");
+
+  for (sensor_msgs::PointCloud2ConstIterator<float> 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;
+    }
+  }
+
   front_pc.header = input_pc->header;
   behind_pc.header = input_pc->header;
 }