From c8e00409c03555274c6ff26cc438e75144e1dcfd Mon Sep 17 00:00:00 2001
From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com>
Date: Sat, 28 Dec 2024 03:16:48 +0900
Subject: [PATCH] feat(pointpainting_fusion): enable cloud display on image
 (#9813)

---
 .../src/pointpainting_fusion/node.cpp         | 90 ++++++++++---------
 1 file changed, 48 insertions(+), 42 deletions(-)

diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
index 2837bac458541..25d9064ddbfc7 100644
--- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
@@ -316,54 +316,56 @@ dc   | dc dc dc  dc ||zc|
   // iterate points
   // Requires 'OMP_NUM_THREADS=N'
   omp_set_num_threads(omp_num_threads_);
-#pragma omp parallel for
-  for (int i = 0; i < iterations; i++) {
-    int stride = p_step * i;
-    unsigned char * data = &painted_pointcloud_msg.data[0];
-    unsigned char * output = &painted_pointcloud_msg.data[0];
-    // cppcheck-suppress-begin invalidPointerCast
-    float p_x = *reinterpret_cast<const float *>(&data[stride + x_offset]);
-    float p_y = *reinterpret_cast<const float *>(&data[stride + y_offset]);
-    float p_z = *reinterpret_cast<const float *>(&data[stride + z_offset]);
-    // cppcheck-suppress-end invalidPointerCast
-    point_lidar << p_x, p_y, p_z;
-    point_camera = lidar2cam_affine * point_lidar;
-    p_x = point_camera.x();
-    p_y = point_camera.y();
-    p_z = point_camera.z();
-
-    if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) {
-      continue;
-    }
+  std::vector<std::vector<Eigen::Vector2d>> local_vectors(omp_num_threads_);
+#pragma omp parallel
+  {
+#pragma omp for
+    for (int i = 0; i < iterations; i++) {
+      int stride = p_step * i;
+      unsigned char * data = &painted_pointcloud_msg.data[0];
+      unsigned char * output = &painted_pointcloud_msg.data[0];
+      // cppcheck-suppress-begin invalidPointerCast
+      float p_x = *reinterpret_cast<const float *>(&data[stride + x_offset]);
+      float p_y = *reinterpret_cast<const float *>(&data[stride + y_offset]);
+      float p_z = *reinterpret_cast<const float *>(&data[stride + z_offset]);
+      // cppcheck-suppress-end invalidPointerCast
+      point_lidar << p_x, p_y, p_z;
+      point_camera = lidar2cam_affine * point_lidar;
+      p_x = point_camera.x();
+      p_y = point_camera.y();
+      p_z = point_camera.z();
+
+      if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) {
+        continue;
+      }
 
-    // project
-    Eigen::Vector2d projected_point;
-    if (camera_projectors_[image_id].calcImageProjectedPoint(
-          cv::Point3d(p_x, p_y, p_z), projected_point)) {
-      // iterate 2d bbox
-      for (const auto & feature_object : objects) {
-        sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi;
-        // paint current point if it is inside bbox
-        int label2d = feature_object.object.classification.front().label;
-        if (
-          !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) {
-          // cppcheck-suppress invalidPointerCast
-          auto p_class = reinterpret_cast<float *>(&output[stride + class_offset]);
-          for (const auto & cls : isClassTable_) {
-            // add up the class values if the point belongs to multiple classes
-            *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class;
+      // project
+      Eigen::Vector2d projected_point;
+      if (camera_projectors_[image_id].calcImageProjectedPoint(
+            cv::Point3d(p_x, p_y, p_z), projected_point)) {
+        // iterate 2d bbox
+        for (const auto & feature_object : objects) {
+          sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi;
+          // paint current point if it is inside bbox
+          int label2d = feature_object.object.classification.front().label;
+          if (
+            !isUnknown(label2d) &&
+            isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) {
+            // cppcheck-suppress invalidPointerCast
+            auto p_class = reinterpret_cast<float *>(&output[stride + class_offset]);
+            for (const auto & cls : isClassTable_) {
+              // add up the class values if the point belongs to multiple classes
+              *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class;
+            }
           }
         }
+        if (debugger_) {
+          int thread_id = omp_get_thread_num();
+          local_vectors[thread_id].push_back(projected_point);
+        }
       }
-#if 0
-      // Parallelizing loop don't support push_back
-      if (debugger_) {
-        debug_image_points.push_back(projected_point);
-      }
-#endif
     }
   }
-
   if (debugger_) {
     std::unique_ptr<ScopedTimeTrack> inner_st_ptr;
     if (time_keeper_)
@@ -373,6 +375,10 @@ dc   | dc dc dc  dc ||zc|
       debug_image_rois.push_back(feature_object.feature.roi);
     }
 
+    for (const auto & local_vec : local_vectors) {
+      debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end());
+    }
+
     debugger_->image_rois_ = debug_image_rois;
     debugger_->obstacle_points_ = debug_image_points;
     debugger_->publishImage(image_id, input_roi_msg.header.stamp);