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

perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp

@@ -52,8 +52,10 @@
 #ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_
 #define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__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>
@@ -83,11 +85,35 @@ 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 = autoware::universe_utils::deg2rad(0.1);
+  const double angle_increment_inv = 1.0 / angle_increment;
+
+  Eigen::Matrix4f mat_map, mat_scan;
+
 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/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp

@@ -32,8 +32,13 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface
     const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud,
     const Pose & robot_pose, const Pose & scan_origin) override;
 
+  bool isPointValid(const Eigen::Vector4f & pt);
+  void transformPointAndCalculate(
+    const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range);
+
   using OccupancyGridMapInterface::raytrace;
   using OccupancyGridMapInterface::setCellValue;
+  using OccupancyGridMapInterface::setFieldOffsets;
   using OccupancyGridMapInterface::updateOrigin;
 
   void initRosParam(rclcpp::Node & node) override;

perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp

@@ -36,8 +36,13 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface
     const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud,
     const Pose & robot_pose, const Pose & scan_origin) override;
 
+  bool isPointValid(const Eigen::Vector4f & pt);
+  void transformPointAndCalculate(
+    const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range);
+
   using OccupancyGridMapInterface::raytrace;
   using OccupancyGridMapInterface::setCellValue;
+  using OccupancyGridMapInterface::setFieldOffsets;
   using OccupancyGridMapInterface::updateOrigin;
 
   void initRosParam(rclcpp::Node & node) override;

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -77,17 +77,21 @@
   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, occupancy_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;
@@ -229,4 +233,22 @@
   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

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp

@@ -43,6 +43,24 @@
 {
 }
 
+inline bool OccupancyGridMapFixedBlindSpot::isPointValid(const Eigen::Vector4f & pt)
+{
+  // 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
+inline void OccupancyGridMapFixedBlindSpot::transformPointAndCalculate(
+  const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range)
+{
+  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]);
+}
+
 /**
  * @brief update Gridmap with PointCloud
  *
@@ -54,70 +72,103 @@
 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 Matrix from base_link to map frame
+  mat_map = utils::getTransformMatrix(robot_pose);
 
-  // Transform from map frame to scan frame
-  PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud;      // point cloud in scan frame
   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
   {
     BinInfo() = default;
     BinInfo(const double _range, const double _wx, const double _wy)
     : range(_range), wx(_wx), wy(_wy)
     {
     }
     double range;
     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;
+  const size_t raw_reserve_size = raw_pointcloud_size / angle_bin_size;
+  const size_t obstacle_reserve_size = obstacle_pointcloud_size / angle_bin_size;
+
+  // Reserve a certain amount of memory in advance for performance reasons
+  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);
+    if (!isPointValid(pt)) {
+      global_offset += raw_pointcloud.point_step;
+      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]);
+    global_offset += raw_pointcloud.point_step;
+  }
+
   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);
+    if (!isPointValid(pt)) {
+      global_offset += obstacle_pointcloud.point_step;
+      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]);
+    global_offset += obstacle_pointcloud.point_step;
   }
+
   for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
     std::sort(
       obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(),