perf(probabilistic_occupancy_grid_map): performance tuning for pointcloud based occupancy grid map

perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp

@@ -52,8 +52,10 @@
 #ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_
 #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_
 
+#include <autoware/universe_utils/math/unit_conversion.hpp>
 #include <nav2_costmap_2d/costmap_2d.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
@@ -85,11 +87,52 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D
 
   virtual void initRosParam(rclcpp::Node & node) = 0;
 
+  void setHeightLimit(const double min_height, const double max_height);
+
+  double min_height_;
+  double max_height_;
+
+  void setFieldOffsets(const PointCloud2 & input_raw, const PointCloud2 & input_obstacle);
+
+  int x_offset_raw_;
+  int y_offset_raw_;
+  int z_offset_raw_;
+  int x_offset_obstacle_;
+  int y_offset_obstacle_;
+  int z_offset_obstacle_;
+  bool offset_initialized_;
+
+  const double min_angle_ = autoware::universe_utils::deg2rad(-180.0);
+  const double max_angle_ = autoware::universe_utils::deg2rad(180.0);
+  const double angle_increment_inv_ = 1.0 / autoware::universe_utils::deg2rad(0.1);
+
+  Eigen::Matrix4f mat_map_, mat_scan_;
+
+  bool isPointValid(const Eigen::Vector4f & pt) const
+  {
+    // Apply height filter and exclude invalid points
+    return min_height_ < pt[2] && pt[2] < max_height_ && std::isfinite(pt[0]) &&
+           std::isfinite(pt[1]) && std::isfinite(pt[2]);
+  }
+  // Transform pt to (pt_map, pt_scan), then calculate angle_bin_index and range
+  void transformPointAndCalculate(
+    const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index,
+    double & range) const
+  {
+    pt_map = mat_map_ * pt;
+    Eigen::Vector4f pt_scan(mat_scan_ * pt_map);
+    const double angle = atan2(pt_scan[1], pt_scan[0]);
+    angle_bin_index = (angle - min_angle_) * angle_increment_inv_;
+    range = std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]);
+  }
+
 private:
   bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const;
 
   rclcpp::Logger logger_{rclcpp::get_logger("pointcloud_based_occupancy_grid_map")};
   rclcpp::Clock clock_{RCL_ROS_TIME};
+
+  double resolution_inv_;
 };
 
 }  // namespace costmap_2d

perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp

@@ -36,6 +36,7 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface
 
   using OccupancyGridMapInterface::raytrace;
   using OccupancyGridMapInterface::setCellValue;
+  using OccupancyGridMapInterface::setFieldOffsets;
   using OccupancyGridMapInterface::updateOrigin;
 
   void initRosParam(rclcpp::Node & node) override;

perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp

@@ -40,6 +40,7 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface
 
   using OccupancyGridMapInterface::raytrace;
   using OccupancyGridMapInterface::setCellValue;
+  using OccupancyGridMapInterface::setFieldOffsets;
   using OccupancyGridMapInterface::updateOrigin;
 
   void initRosParam(rclcpp::Node & node) override;

perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp

@@ -97,6 +97,8 @@ void transformPointcloud(
   const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose,
   sensor_msgs::msg::PointCloud2 & output);
 
+Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose);
+
 bool cropPointcloudByHeight(
   const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2,
   const std::string & target_frame, const float min_height, const float max_height,

perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp

@@ -80,17 +80,21 @@ OccupancyGridMapInterface::OccupancyGridMapInterface(
   const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution)
 : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION)
 {
+  min_height_ = -std::numeric_limits<double>::infinity();
+  max_height_ = std::numeric_limits<double>::infinity();
+  resolution_inv_ = 1.0 / resolution_;
+  offset_initialized_ = false;
 }
 
-bool OccupancyGridMapInterface::worldToMap(
+inline bool OccupancyGridMapInterface::worldToMap(
   double wx, double wy, unsigned int & mx, unsigned int & my) const
 {
   if (wx < origin_x_ || wy < origin_y_) {
     return false;
   }
 
-  mx = static_cast<int>(std::floor((wx - origin_x_) / resolution_));
-  my = static_cast<int>(std::floor((wy - origin_y_) / resolution_));
+  mx = static_cast<int>(std::floor((wx - origin_x_) * resolution_inv_));
+  my = static_cast<int>(std::floor((wy - origin_y_) * resolution_inv_));
 
   if (mx < size_x_ && my < size_y_) {
     return true;
@@ -232,5 +236,23 @@ void OccupancyGridMapInterface::raytrace(
   raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
 }
 
+void OccupancyGridMapInterface::setHeightLimit(const double min_height, const double max_height)
+{
+  min_height_ = min_height;
+  max_height_ = max_height;
+}
+
+void OccupancyGridMapInterface::setFieldOffsets(
+  const PointCloud2 & input_raw, const PointCloud2 & input_obstacle)
+{
+  x_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "x")].offset;
+  y_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "y")].offset;
+  z_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "z")].offset;
+  x_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "x")].offset;
+  y_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "y")].offset;
+  z_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "z")].offset;
+  offset_initialized_ = true;
+}
+
 }  // namespace costmap_2d
 }  // namespace autoware::occupancy_grid_map

perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp

@@ -58,21 +58,20 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud(
   const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud,
   const Pose & robot_pose, const Pose & scan_origin)
 {
-  constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0);
-  constexpr double max_angle = autoware::universe_utils::deg2rad(180.0);
-  constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1);
-  const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/);
-
-  // Transform from base_link to map frame
-  PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud;  // point cloud in map frame
-  utils::transformPointcloud(raw_pointcloud, robot_pose, map_raw_pointcloud);
-  utils::transformPointcloud(obstacle_pointcloud, robot_pose, map_obstacle_pointcloud);
-
-  // Transform from map frame to scan frame
-  PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud;      // point cloud in scan frame
+  const size_t angle_bin_size =
+    ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/);
+
+  // Transform Matrix from base_link to map frame
+  mat_map_ = utils::getTransformMatrix(robot_pose);
+
   const auto scan2map_pose = utils::getInversePose(scan_origin);  // scan -> map transform pose
-  utils::transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud);
-  utils::transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud);
+
+  // Transform Matrix from map frame to scan frame
+  mat_scan_ = utils::getTransformMatrix(scan2map_pose);
+
+  if (!offset_initialized_) {
+    setFieldOffsets(raw_pointcloud, obstacle_pointcloud);
+  }
 
   // Create angle bins and sort by distance
   struct BinInfo
@@ -86,41 +85,73 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud(
     double wx;
     double wy;
   };
-  std::vector</*angle bin*/ std::vector<BinInfo>> obstacle_pointcloud_angle_bins;
-  std::vector</*angle bin*/ std::vector<BinInfo>> raw_pointcloud_angle_bins;
-  obstacle_pointcloud_angle_bins.resize(angle_bin_size);
-  raw_pointcloud_angle_bins.resize(angle_bin_size);
-  for (PointCloud2ConstIterator<float> iter_x(scan_raw_pointcloud, "x"),
-       iter_y(scan_raw_pointcloud, "y"), iter_wx(map_raw_pointcloud, "x"),
-       iter_wy(map_raw_pointcloud, "y");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) {
-    const double angle = atan2(*iter_y, *iter_x);
-    const int angle_bin_index = (angle - min_angle) / angle_increment;
-    raw_pointcloud_angle_bins.at(angle_bin_index)
-      .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy));
+
+  std::vector</*angle bin*/ std::vector<BinInfo>> obstacle_pointcloud_angle_bins(angle_bin_size);
+  std::vector</*angle bin*/ std::vector<BinInfo>> raw_pointcloud_angle_bins(angle_bin_size);
+
+  const size_t raw_pointcloud_size = raw_pointcloud.width * raw_pointcloud.height;
+  const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height;
+
+  // Reserve a certain amount of memory in advance for performance reasons
+  const size_t raw_reserve_size = raw_pointcloud_size / angle_bin_size;
+  const size_t obstacle_reserve_size = obstacle_pointcloud_size / angle_bin_size;
+  for (size_t i = 0; i < angle_bin_size; i++) {
+    raw_pointcloud_angle_bins[i].reserve(raw_reserve_size);
+    obstacle_pointcloud_angle_bins[i].reserve(obstacle_reserve_size);
+  }
+
+  // Updated every loop inside transformPointAndCalculate()
+  Eigen::Vector4f pt_map;
+  int angle_bin_index;
+  double range;
+
+  size_t global_offset = 0;
+  for (size_t i = 0; i < raw_pointcloud_size; ++i) {
+    Eigen::Vector4f pt(
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + x_offset_raw_]),
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + y_offset_raw_]),
+      *reinterpret_cast<const float *>(&raw_pointcloud.data[global_offset + z_offset_raw_]), 1);
+    global_offset += raw_pointcloud.point_step;
+    if (!isPointValid(pt)) {
+      continue;
+    }
+    transformPointAndCalculate(pt, pt_map, angle_bin_index, range);
+
+    raw_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]);
   }
+
   for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
     std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) {
       return a.range < b.range;
     });
   }
-  // create and sort bin for obstacle pointcloud
-  for (PointCloud2ConstIterator<float> iter_x(scan_obstacle_pointcloud, "x"),
-       iter_y(scan_obstacle_pointcloud, "y"), iter_wx(map_obstacle_pointcloud, "x"),
-       iter_wy(map_obstacle_pointcloud, "y");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) {
-    const double angle = atan2(*iter_y, *iter_x);
-    const int angle_bin_index = (angle - min_angle) / angle_increment;
-    const double range = std::hypot(*iter_y, *iter_x);
+
+  // Create obstacle angle bins and sort points by range
+  global_offset = 0;
+  for (size_t i = 0; i < obstacle_pointcloud_size; ++i) {
+    Eigen::Vector4f pt(
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]),
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]),
+      *reinterpret_cast<const float *>(
+        &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]),
+      1);
+    global_offset += obstacle_pointcloud.point_step;
+    if (!isPointValid(pt)) {
+      continue;
+    }
+    transformPointAndCalculate(pt, pt_map, angle_bin_index, range);
+
     // Ignore obstacle points exceed the range of the raw points
     if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) {
       continue;  // No raw point in this angle bin
     } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) {
       continue;  // Obstacle point exceeds the range of the raw points
     }
-    obstacle_pointcloud_angle_bins.at(angle_bin_index)
-      .push_back(BinInfo(range, *iter_wx, *iter_wy));
+    obstacle_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]);
   }
+
   for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
     std::sort(
       obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(),