Merge pull request #2863 from furushchev/fix/jsk-ros-pkg/particle-cuboid

[ROS-O] Add ParticleCuboid::weightedAverage method
jsk-ros-pkg · Feb 21, 2025 · 7d5d682 · 7d5d682
2 parents d24721c + 146824c
commit 7d5d682
Showing 1 changed file with 19 additions and 8 deletions.
diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/pcl/particle_cuboid.h b/jsk_pcl_ros/include/jsk_pcl_ros/pcl/particle_cuboid.h
@@ -302,15 +302,26 @@ namespace pcl
       }
 
       inline jsk_pcl_ros::Cube::Ptr toCube() const
-    {
-      Eigen::Affine3f pose = toEigenMatrix();
-      Eigen::Vector3f dimensions(dx, dy, dz);
-      jsk_pcl_ros::Cube::Ptr cube(new jsk_pcl_ros::Cube(Eigen::Vector3f(pose.translation()),
-                                                        Eigen::Quaternionf(pose.rotation()),
-                                                        dimensions));
-      return cube;
-    }
+      {
+        Eigen::Affine3f pose = toEigenMatrix();
+        Eigen::Vector3f dimensions(dx, dy, dz);
+        jsk_pcl_ros::Cube::Ptr cube(new jsk_pcl_ros::Cube(Eigen::Vector3f(pose.translation()),
+                                                          Eigen::Quaternionf(pose.rotation()),
+                                                          dimensions));
+        return cube;
+      }
 
+      // As of https://github.com/PointCloudLibrary/pcl/pull/5538,
+      // trackers must implements averaging method
+      template <class InputIterator>
+        static ParticleCuboid weightedAverage(InputIterator first, InputIterator last)
+      {
+        ParticleCuboid wa;
+        for (auto cuboid = first; cuboid != last; ++cuboid) {
+          wa = wa + *(cuboid) * cuboid->weight;
+        }
+        return wa;
+      }
 
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     };