perf: performance tuning

Author: taisa1

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

@@ -83,11 +83,18 @@ 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_ = -std::numeric_limits<double>::infinity();
+  double max_height_ = std::numeric_limits<double>::infinity();
+
 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/pointcloud_based_occupancy_grid_map_node.hpp

@@ -55,6 +55,7 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node
 {
 public:
   explicit PointcloudBasedOccupancyGridMapNode(const rclcpp::NodeOptions & node_options);
+  ~PointcloudBasedOccupancyGridMapNode();
 
 private:
   void onPointcloudWithObstacleAndRaw(

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp

@@ -77,17 +77,18 @@ 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, occupancy_cost_value::NO_INFORMATION)
 {
+  resolution_inv_ = 1.0 / resolution_;
 }
 
-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 +230,10 @@ 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;
+}
+
 }  // namespace costmap_2d

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp

@@ -60,16 +60,12 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud(
   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
+  Eigen::Matrix4f matmap = 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
+  Eigen::Matrix4f matscan = utils::getTransformMatrix(scan2map_pose);
 
   // Create angle bins and sort by distance
   struct BinInfo
@@ -83,40 +79,94 @@ 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;
+  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 int x_offset_raw = raw_pointcloud.fields[pcl::getFieldIndex(raw_pointcloud, "x")].offset;
+  const int y_offset_raw = raw_pointcloud.fields[pcl::getFieldIndex(raw_pointcloud, "y")].offset;
+  const int z_offset_raw = raw_pointcloud.fields[pcl::getFieldIndex(raw_pointcloud, "z")].offset;
+  const int x_offset_obstacle =
+    obstacle_pointcloud.fields[pcl::getFieldIndex(obstacle_pointcloud, "x")].offset;
+  const int y_offset_obstacle =
+    obstacle_pointcloud.fields[pcl::getFieldIndex(obstacle_pointcloud, "y")].offset;
+  const int z_offset_obstacle =
+    obstacle_pointcloud.fields[pcl::getFieldIndex(obstacle_pointcloud, "z")].offset;
+  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;
+  for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
+    raw_pointcloud_angle_bin.reserve(raw_reserve_size);
+  }
+  for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
+    obstacle_pointcloud_angle_bin.reserve(obstacle_reserve_size);
+  }
+  size_t global_offset = 0;
+  const auto angle_increment_inv = 1.0 / angle_increment;
+  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);
+    // Exclude invalid points
+    if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
+      global_offset += raw_pointcloud.point_step;
+      continue;
+    }
+    // Apply height filter
+    if (pt[2] < min_height_ || max_height_ < pt[2]) {
+      global_offset += raw_pointcloud.point_step;
+      continue;
+    }
+    // Calculate transformed points
+    Eigen::Vector4f pt_map(matmap * pt);
+    Eigen::Vector4f pt_scan(matscan * pt_map);
+    const double angle = atan2(pt_scan[1], pt_scan[0]);
+    const int angle_bin_index = (angle - min_angle) * angle_increment_inv;
     raw_pointcloud_angle_bins.at(angle_bin_index)
-      .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy));
+      .emplace_back(
+        std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]), 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);
+  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);
+    // Exclude invalid points
+    if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
+      global_offset += obstacle_pointcloud.point_step;
+      continue;
+    }
+    // Apply height filter
+    if (pt[2] < min_height_ || max_height_ < pt[2]) {
+      global_offset += obstacle_pointcloud.point_step;
+      continue;
+    }
+    // Calculate transformed points
+    Eigen::Vector4f pt_map(matmap * pt);
+    Eigen::Vector4f pt_scan(matscan * pt_map);
+    const double angle = atan2(pt_scan[1], pt_scan[0]);
+    const int angle_bin_index = (angle - min_angle) * angle_increment_inv;
+    const double range = std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]);
     // 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(

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp

@@ -63,14 +63,12 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
 
   // 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);
+  Eigen::Matrix4f matmap = 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);
+  Eigen::Matrix4f matscan = utils::getTransformMatrix(scan2map_pose);
 
   // Create angle bins and sort points by range
   struct BinInfo3D
@@ -97,36 +95,92 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
     double projected_wy;
   };
 
-  std::vector</*angle bin*/ std::vector<BinInfo3D>> obstacle_pointcloud_angle_bins;
-  std::vector</*angle bin*/ std::vector<BinInfo3D>> 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_wz(map_raw_pointcloud, "z");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy, ++iter_wz) {
-    const double angle = atan2(*iter_y, *iter_x);
-    const int angle_bin_index = (angle - min_angle) / angle_increment;
+  std::vector</*angle bin*/ std::vector<BinInfo3D>> obstacle_pointcloud_angle_bins(angle_bin_size);
+  std::vector</*angle bin*/ std::vector<BinInfo3D>> raw_pointcloud_angle_bins(angle_bin_size);
+  const int x_offset_raw = raw_pointcloud.fields[pcl::getFieldIndex(raw_pointcloud, "x")].offset;
+  const int y_offset_raw = raw_pointcloud.fields[pcl::getFieldIndex(raw_pointcloud, "y")].offset;
+  const int z_offset_raw = raw_pointcloud.fields[pcl::getFieldIndex(raw_pointcloud, "z")].offset;
+  const int x_offset_obstacle =
+    obstacle_pointcloud.fields[pcl::getFieldIndex(obstacle_pointcloud, "x")].offset;
+  const int y_offset_obstacle =
+    obstacle_pointcloud.fields[pcl::getFieldIndex(obstacle_pointcloud, "y")].offset;
+  const int z_offset_obstacle =
+    obstacle_pointcloud.fields[pcl::getFieldIndex(obstacle_pointcloud, "z")].offset;
+  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;
+  for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
+    raw_pointcloud_angle_bin.reserve(raw_reserve_size);
+  }
+  for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
+    obstacle_pointcloud_angle_bin.reserve(obstacle_reserve_size);
+  }
+  size_t global_offset = 0;
+  const auto angle_increment_inv = 1.0 / angle_increment;
+
+  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);
+    // Exclude invalid points
+    if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
+      global_offset += raw_pointcloud.point_step;
+      continue;
+    }
+    // Apply height filter
+    if (pt[2] < min_height_ || max_height_ < pt[2]) {
+      global_offset += raw_pointcloud.point_step;
+      continue;
+    }
+    // Calculate transformed points
+    Eigen::Vector4f pt_map(matmap * pt);
+    Eigen::Vector4f pt_scan(matscan * pt_map);
+    const double angle = atan2(pt_scan[1], pt_scan[0]);
+    const int angle_bin_index = (angle - min_angle) * angle_increment_inv;
     raw_pointcloud_angle_bins.at(angle_bin_index)
-      .emplace_back(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy, *iter_wz);
+      .emplace_back(
+        std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]), pt_map[0], pt_map[1],
+        pt_map[2]);
+    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 obstacle angle bins and sort points by range
-  for (PointCloud2ConstIterator<float> iter_x(scan_obstacle_pointcloud, "x"),
-       iter_y(scan_obstacle_pointcloud, "y"), iter_z(scan_obstacle_pointcloud, "z"),
-       iter_wx(map_obstacle_pointcloud, "x"), iter_wy(map_obstacle_pointcloud, "y"),
-       iter_wz(map_obstacle_pointcloud, "z");
-       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_wx, ++iter_wy, ++iter_wz) {
+
+  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);
+    // Exclude invalid points
+    if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
+      global_offset += obstacle_pointcloud.point_step;
+      continue;
+    }
+    // Apply height filter
+    if (pt[2] < min_height_ || max_height_ < pt[2]) {
+      global_offset += obstacle_pointcloud.point_step;
+      continue;
+    }
+    // Calculate transformed points
+    Eigen::Vector4f pt_map(matmap * pt);
+    Eigen::Vector4f pt_scan(matscan * pt_map);
     const double scan_z = scan_origin.position.z - robot_pose.position.z;
-    const double obstacle_z = (*iter_wz) - robot_pose.position.z;
+    const double obstacle_z = (pt_map[2]) - robot_pose.position.z;
     const double dz = scan_z - obstacle_z;
-    const double angle = atan2(*iter_y, *iter_x);
-    const int angle_bin_index = (angle - min_angle) / angle_increment;
-    const double range = std::hypot(*iter_x, *iter_y);
+    const double angle = atan2(pt_scan[1], pt_scan[0]);
+    const int angle_bin_index = (angle - min_angle) * angle_increment_inv;
+    const double range = std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]);
     // 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
@@ -136,17 +190,18 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
     if (dz > projection_dz_threshold_) {
       const double ratio = obstacle_z / dz;
       const double projection_length = range * ratio;
-      const double projected_wx = (*iter_wx) + ((*iter_wx) - scan_origin.position.x) * ratio;
-      const double projected_wy = (*iter_wy) + ((*iter_wy) - scan_origin.position.y) * ratio;
+      const double projected_wx = (pt_map[0]) + ((pt_map[0]) - scan_origin.position.x) * ratio;
+      const double projected_wy = (pt_map[1]) + ((pt_map[1]) - scan_origin.position.y) * ratio;
       obstacle_pointcloud_angle_bins.at(angle_bin_index)
         .emplace_back(
-          range, *iter_wx, *iter_wy, *iter_wz, projection_length, projected_wx, projected_wy);
+          range, pt_map[0], pt_map[1], pt_map[2], projection_length, projected_wx, projected_wy);
     } else {
       obstacle_pointcloud_angle_bins.at(angle_bin_index)
         .emplace_back(
-          range, *iter_wx, *iter_wy, *iter_wz, std::numeric_limits<double>::infinity(),
+          range, pt_map[0], pt_map[1], pt_map[2], std::numeric_limits<double>::infinity(),
           std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
     }
+    global_offset += obstacle_pointcloud.point_step;
   }
   for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
     std::sort(