From 065362093360940a61459035b1c337f6cb03304c Mon Sep 17 00:00:00 2001
From: seb <spelletier1996@gmail.com>
Date: Tue, 11 Jun 2024 20:59:58 +0000
Subject: [PATCH] multi threaded transform

---
 src/utils/tf2_sensor_msgs.cpp | 46 +++++++++++++++++++++++++++++------
 1 file changed, 38 insertions(+), 8 deletions(-)

diff --git a/src/utils/tf2_sensor_msgs.cpp b/src/utils/tf2_sensor_msgs.cpp
index c3acc80..8686276 100644
--- a/src/utils/tf2_sensor_msgs.cpp
+++ b/src/utils/tf2_sensor_msgs.cpp
@@ -9,6 +9,8 @@
 
 #include <unordered_set>
 
+#include <boost/asio.hpp>
+
 namespace robot_body_filter {
 
 const static std::unordered_map<std::string, CloudChannelType> XYZ_CHANNELS({
@@ -32,6 +34,31 @@ bool fieldNameMatchesChannel(const std::string& fieldName, const std::string& ch
   }
 }
 
+void ApplyTransform(CloudConstIter& x_in, CloudConstIter& y_in, CloudConstIter& z_in, CloudIter& x_out,
+                    CloudIter& y_out, CloudIter& z_out, const Eigen::Isometry3f& t, size_t sp, size_t ep) {
+  Eigen::Vector3f point;
+  for (size_t i = sp; i < ep; ++i) {
+    point = t * Eigen::Vector3f(*(x_in + i), *(y_in + i), *(z_in + i));  // apply the whole transform
+    *(x_out + i) = point.x();
+    *(y_out + i) = point.y();
+    *(z_out + i) = point.z();
+  }
+}
+
+void SegmentTransform(size_t np, size_t threads, CloudConstIter& x_in, CloudConstIter& y_in, CloudConstIter& z_in,
+                      CloudIter& x_out, CloudIter& y_out, CloudIter& z_out, const Eigen::Isometry3f& t) {
+  //Given a number of points and a number of threads, this function will divide the points into segments
+  //Making this threadpool static is causing issues with the transform?
+  boost::asio::thread_pool pool(threads);
+  for (size_t i = 0; i < threads; ++i) {
+    size_t sp = i * np / threads;
+    size_t ep = (i + 1) * np / threads;
+    boost::asio::post(pool, std::bind(ApplyTransform, std::ref(x_in), std::ref(y_in), std::ref(z_in), std::ref(x_out),
+                                      std::ref(y_out), std::ref(z_out), std::ref(t), sp, ep));
+  }
+  pool.join();
+}
+
 void transformChannel(const sensor_msgs::msg::PointCloud2& cloudIn, sensor_msgs::msg::PointCloud2& cloudOut,
                       const Eigen::Isometry3f& t, const std::string& channelPrefix, const CloudChannelType type)
 {
@@ -48,18 +75,21 @@ void transformChannel(const sensor_msgs::msg::PointCloud2& cloudIn, sensor_msgs:
   CloudIter z_out(cloudOut, channelPrefix + "z");
 
   Eigen::Vector3f point;
+  size_t np = num_points(cloudIn);
+
   // the switch has to be outside the for loop for performance reasons
   switch (type)
   {
-    case CloudChannelType::POINT:
-      for (; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out)
-      {
-        point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);  // apply the whole transform
-        *x_out = point.x();
-        *y_out = point.y();
-        *z_out = point.z();
-      }
+    case CloudChannelType::POINT: {
+      // for (; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
+      //   point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);  // apply the whole transform
+      //   *x_out = point.x();
+      //   *y_out = point.y();
+      //   *z_out = point.z();
+      // }
+      SegmentTransform(np, 16, std::ref(x_in), std::ref(y_in), std::ref(z_in), std::ref(x_out), std::ref(y_out), std::ref(z_out), t);
       break;
+    }
     case CloudChannelType::DIRECTION:
       for (; x_out != x_out.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out)
       {