From dc9ded6138ef28563c55aaec4943719a928ec79f Mon Sep 17 00:00:00 2001 From: taisa1 <38522559+taisa1@users.noreply.github.com> Date: Tue, 16 Jul 2024 16:32:32 +0900 Subject: [PATCH] perf(probabilistic_occupancy_grid_map): performance tuning for pointcloud based occupancy grid map (#7687) * perf: performance tuning Signed-off-by: taisa1 * fix: remove unnecessary change Signed-off-by: taisa1 * chore: edit comments Signed-off-by: taisa1 * fix: change the way of offset initialization Signed-off-by: taisa1 * fix: compile error and name Signed-off-by: taisa1 * fix: take common parts as func Signed-off-by: taisa1 * refactor: change variable name Signed-off-by: taisa1 * refactor: add some blank lines Signed-off-by: taisa1 * fix: made unapplied change Signed-off-by: taisa1 * fix: compile error Signed-off-by: taisa1 * refactor: simplified conditions Signed-off-by: taisa1 * fix: apply changes to projective Signed-off-by: taisa1 * fix: remove reserve from projective Signed-off-by: taisa1 * refactor: reduce too many arguments Signed-off-by: taisa1 * refactor: delete angle_increment Signed-off-by: taisa1 * fix: reflect review Signed-off-by: taisa1 * fix: build error Signed-off-by: taisa1 --------- Signed-off-by: taisa1 Co-authored-by: atsushi yano <55824710+atsushi421@users.noreply.github.com> --- .../costmap_2d/occupancy_grid_map_base.hpp | 43 ++++++++ .../costmap_2d/occupancy_grid_map_fixed.hpp | 1 + .../occupancy_grid_map_projective.hpp | 1 + .../utils/utils.hpp | 2 + .../costmap_2d/occupancy_grid_map_base.cpp | 28 ++++- .../costmap_2d/occupancy_grid_map_fixed.cpp | 103 ++++++++++++------ .../occupancy_grid_map_projective.cpp | 94 ++++++++++------ .../lib/utils/utils.cpp | 7 ++ ...intcloud_based_occupancy_grid_map_node.cpp | 55 ++++++---- 9 files changed, 240 insertions(+), 94 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index 2b2057af9cc12..09ad0a4d554a1 100644 --- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp +++ b/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 #include #include +#include #include #include @@ -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 diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp index 9dfd8b8689123..3301fd1987bd3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp +++ b/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; diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp index 1f0f92d933ff3..7569a60b36466 100644 --- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp +++ b/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; diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index 50b470d904ef6..a8288d2720f48 100644 --- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/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, diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp index 5e9b5598c8f63..39855ec36260c 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp +++ b/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::infinity(); + max_height_ = std::numeric_limits::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(std::floor((wx - origin_x_) / resolution_)); - my = static_cast(std::floor((wy - origin_y_) / resolution_)); + mx = static_cast(std::floor((wx - origin_x_) * resolution_inv_)); + my = static_cast(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 diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index dffbda18d0c8a..c8af97fcf3ba0 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/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> obstacle_pointcloud_angle_bins; - std::vector> raw_pointcloud_angle_bins; - obstacle_pointcloud_angle_bins.resize(angle_bin_size); - raw_pointcloud_angle_bins.resize(angle_bin_size); - for (PointCloud2ConstIterator 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> obstacle_pointcloud_angle_bins(angle_bin_size); + std::vector> 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(&raw_pointcloud.data[global_offset + x_offset_raw_]), + *reinterpret_cast(&raw_pointcloud.data[global_offset + y_offset_raw_]), + *reinterpret_cast(&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 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( + &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]), + *reinterpret_cast( + &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]), + *reinterpret_cast( + &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(), diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index 39224bc7e3a69..f5f2c70ae6dfc 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -59,21 +59,20 @@ void OccupancyGridMapProjectiveBlindSpot::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*/); + const size_t angle_bin_size = + ((max_angle_ - min_angle_) * angle_increment_inv_) + 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); + 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 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 points by range struct BinInfo3D @@ -100,57 +99,82 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( double projected_wy; }; - std::vector> obstacle_pointcloud_angle_bins; - std::vector> raw_pointcloud_angle_bins; - obstacle_pointcloud_angle_bins.resize(angle_bin_size); - raw_pointcloud_angle_bins.resize(angle_bin_size); - for (PointCloud2ConstIterator 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> obstacle_pointcloud_angle_bins(angle_bin_size); + std::vector> 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; + + // 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(&raw_pointcloud.data[global_offset + x_offset_raw_]), + *reinterpret_cast(&raw_pointcloud.data[global_offset + y_offset_raw_]), + *reinterpret_cast(&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(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy, *iter_wz); + .emplace_back(range, pt_map[0], pt_map[1], pt_map[2]); } + 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 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( + &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]), + *reinterpret_cast( + &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]), + *reinterpret_cast( + &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); 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); + // 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 } + 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::infinity(), + range, pt_map[0], pt_map[1], pt_map[2], std::numeric_limits::infinity(), std::numeric_limits::infinity(), std::numeric_limits::infinity()); } } + for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { std::sort( obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(), diff --git a/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp index e8c2cdb2617df..902de2148cffe 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -60,6 +60,13 @@ void transformPointcloud( output.header.frame_id = ""; } +Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose) +{ + auto transform = autoware::universe_utils::pose2transform(pose); + Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast(); + return tf_matrix; +} + 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, diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 3fe75263b3f28..0d772e73c1ad8 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -37,6 +37,7 @@ #endif #include +#include #include #include @@ -141,33 +142,46 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( scan_origin_frame_ = input_raw_msg->header.frame_id; } - // Apply height filter - PointCloud2 cropped_obstacle_pc{}; - PointCloud2 cropped_raw_pc{}; + PointCloud2 trans_input_raw{}, trans_input_obstacle{}; + bool is_raw_transformed = false; + bool is_obstacle_transformed = false; + + // Prepare for applying height filter if (use_height_filter_) { - if (!utils::cropPointcloudByHeight( - *input_obstacle_msg, *tf2_, base_link_frame_, min_height_, max_height_, - cropped_obstacle_pc)) { - return; + // Make sure that the frame is base_link + if (input_raw_msg->header.frame_id != base_link_frame_) { + if (!utils::transformPointcloud(*input_raw_msg, *tf2_, base_link_frame_, trans_input_raw)) { + return; + } + is_raw_transformed = true; } - if (!utils::cropPointcloudByHeight( - *input_raw_msg, *tf2_, base_link_frame_, min_height_, max_height_, cropped_raw_pc)) { - return; + if (input_obstacle_msg->header.frame_id != base_link_frame_) { + if (!utils::transformPointcloud( + *input_obstacle_msg, *tf2_, base_link_frame_, trans_input_obstacle)) { + return; + } + is_obstacle_transformed = true; } + occupancy_grid_map_ptr_->setHeightLimit(min_height_, max_height_); + } else { + occupancy_grid_map_ptr_->setHeightLimit( + -std::numeric_limits::infinity(), std::numeric_limits::infinity()); } - const PointCloud2 & filtered_obstacle_pc = - use_height_filter_ ? cropped_obstacle_pc : *input_obstacle_msg; - const PointCloud2 & filtered_raw_pc = use_height_filter_ ? cropped_raw_pc : *input_raw_msg; + + const PointCloud2::ConstSharedPtr input_raw_use = + is_raw_transformed ? std::make_shared(trans_input_raw) : input_raw_msg; + const PointCloud2::ConstSharedPtr input_obstacle_use = + is_obstacle_transformed ? std::make_shared(trans_input_obstacle) + : input_obstacle_msg; // Filter obstacle pointcloud by raw pointcloud - PointCloud2 filtered_obstacle_pc_common{}; + PointCloud2 input_obstacle_pc_common{}; + bool use_input_obstacle_pc_common = false; if (filter_obstacle_pointcloud_by_raw_pointcloud_) { - if (!utils::extractCommonPointCloud( - filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)) { - filtered_obstacle_pc_common = filtered_obstacle_pc; + if (utils::extractCommonPointCloud( + *input_obstacle_use, *input_raw_use, input_obstacle_pc_common)) { + use_input_obstacle_pc_common = true; } - } else { - filtered_obstacle_pc_common = filtered_obstacle_pc; } // Get from map to sensor frame pose @@ -191,7 +205,8 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( gridmap_origin.position.x - occupancy_grid_map_ptr_->getSizeInMetersX() / 2, gridmap_origin.position.y - occupancy_grid_map_ptr_->getSizeInMetersY() / 2); occupancy_grid_map_ptr_->updateWithPointCloud( - filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin); + *input_raw_use, (use_input_obstacle_pc_common ? input_obstacle_pc_common : *input_obstacle_use), + robot_pose, scan_origin); if (enable_single_frame_mode_) { // publish