From e5f095140229c4e826737ca98a3991c2dc89446b Mon Sep 17 00:00:00 2001 From: taisa1 Date: Tue, 25 Jun 2024 17:27:30 +0900 Subject: [PATCH 01/17] perf: performance tuning Signed-off-by: taisa1 --- .../occupancy_grid_map_base.hpp | 7 ++ ...intcloud_based_occupancy_grid_map_node.hpp | 1 + .../occupancy_grid_map_base.cpp | 13 +- .../occupancy_grid_map_fixed.cpp | 108 ++++++++++++----- .../occupancy_grid_map_projective.cpp | 113 +++++++++++++----- ...intcloud_based_occupancy_grid_map_node.cpp | 51 ++++---- .../src/utils/utils.cpp | 7 ++ 7 files changed, 219 insertions(+), 81 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index f01b2d74e160b..db70a86075565 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/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::infinity(); + double max_height_ = std::numeric_limits::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 diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 1d119102dc28d..3c0a3441de65b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/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( diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index 3d176e41583a0..bb9051d1327ec 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/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(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; @@ -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 diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index d4e177209f99d..4c5135913cdd2 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/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> 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; + std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); + std::vector> 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(&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); + // 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 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( + &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); + // 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( diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 3846b3a7d16e7..63360edea70d7 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/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> 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 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(&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); + // 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 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); + // 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::infinity(), + range, pt_map[0], pt_map[1], pt_map[2], std::numeric_limits::infinity(), std::numeric_limits::infinity(), std::numeric_limits::infinity()); } + global_offset += obstacle_pointcloud.point_step; } for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { std::sort( 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 d019280aefda0..5dcab2d221034 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,41 @@ 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 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_); } - 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 @@ -190,8 +199,10 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( occupancy_grid_map_ptr_->updateOrigin( 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 diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 209207fe10f34..b8ea952b702e8 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -58,6 +58,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, From b2c6ae3ed47b16b502f6ec62852ce94a4564b5c9 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Tue, 25 Jun 2024 17:42:02 +0900 Subject: [PATCH 02/17] fix: remove unnecessary change Signed-off-by: taisa1 --- .../pointcloud_based_occupancy_grid_map_node.hpp | 1 - .../occupancy_grid_map_fixed.cpp | 4 ++-- .../occupancy_grid_map_projective.cpp | 7 ++----- .../pointcloud_based_occupancy_grid_map_node.cpp | 1 - 4 files changed, 4 insertions(+), 9 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 3c0a3441de65b..1d119102dc28d 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -55,7 +55,6 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node { public: explicit PointcloudBasedOccupancyGridMapNode(const rclcpp::NodeOptions & node_options); - ~PointcloudBasedOccupancyGridMapNode(); private: void onPointcloudWithObstacleAndRaw( diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 4c5135913cdd2..4c46decb39e52 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -58,6 +58,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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 auto angle_increment_inv = 1.0 / angle_increment; const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform Matrix from base_link to map frame @@ -92,7 +93,6 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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) { @@ -102,7 +102,6 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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(&raw_pointcloud.data[global_offset + x_offset_raw]), @@ -134,6 +133,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( }); } global_offset = 0; + // Create obstacle angle bins and sort points by range for (size_t i = 0; i < obstacle_pointcloud_size; ++i) { Eigen::Vector4f pt( *reinterpret_cast( diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 63360edea70d7..6d45f768cd182 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -59,6 +59,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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 auto angle_increment_inv = 1.0 / angle_increment; const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame @@ -94,7 +95,6 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( double projected_wx; double projected_wy; }; - std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); std::vector> raw_pointcloud_angle_bins(angle_bin_size); const int x_offset_raw = raw_pointcloud.fields[pcl::getFieldIndex(raw_pointcloud, "x")].offset; @@ -108,7 +108,6 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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) { @@ -118,8 +117,6 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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(&raw_pointcloud.data[global_offset + x_offset_raw]), @@ -151,8 +148,8 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( return a.range < b.range; }); } - global_offset = 0; + // Create obstacle angle bins and sort points by range for (size_t i = 0; i < obstacle_pointcloud_size; i++) { Eigen::Vector4f pt( *reinterpret_cast( 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 5dcab2d221034..2a726eb50faec 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 @@ -199,7 +199,6 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( occupancy_grid_map_ptr_->updateOrigin( 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( *input_raw_use, (use_input_obstacle_pc_common ? input_obstacle_pc_common : *input_obstacle_use), robot_pose, scan_origin); From dda0c0d3bc8df3a726197da23fcd9ea80d2590c3 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Tue, 25 Jun 2024 17:50:31 +0900 Subject: [PATCH 03/17] chore: edit comments Signed-off-by: taisa1 --- .../occupancy_grid_map_fixed.cpp | 1 + .../occupancy_grid_map_projective.cpp | 1 + .../pointcloud_based_occupancy_grid_map_node.cpp | 2 +- 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 4c46decb39e52..139f7774b42b3 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -95,6 +95,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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 (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { raw_pointcloud_angle_bin.reserve(raw_reserve_size); } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 6d45f768cd182..8a609077b8d3c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -110,6 +110,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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 (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { raw_pointcloud_angle_bin.reserve(raw_reserve_size); } 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 2a726eb50faec..2f2b330e2df26 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 @@ -147,7 +147,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( bool is_obstacle_transformed = false; // Prepare for applying height filter if (use_height_filter_) { - // make sure that frame is base_link + // 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; From 29dc755aa32496e02b551de85abc3b6e47668201 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 27 Jun 2024 15:21:38 +0900 Subject: [PATCH 04/17] fix: change the way of offset initialization Signed-off-by: taisa1 --- .../occupancy_grid_map_base.hpp | 15 ++++++++++-- .../occupancy_grid_map_base.cpp | 15 ++++++++++++ .../occupancy_grid_map_fixed.cpp | 23 +++++++++--------- .../occupancy_grid_map_projective.cpp | 24 +++++++------------ ...intcloud_based_occupancy_grid_map_node.cpp | 3 +++ 5 files changed, 52 insertions(+), 28 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index db70a86075565..adb7ed70f3352 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -85,8 +85,19 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D void setHeightLimit(const double min_height, const double max_height); - double min_height_ = -std::numeric_limits::infinity(); - double max_height_ = std::numeric_limits::infinity(); + double min_height_; + double max_height_; + + void set_field_offsets( + const PointCloud2ConstPtr & input_raw, const PointCloud2ConstPtr & 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_; private: bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index bb9051d1327ec..a8b706dc56934 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -77,7 +77,10 @@ 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) { + min_height_ = -std::numeric_limits::infinity(); + max_height_ = std::numeric_limits::infinity(); resolution_inv_ = 1.0 / resolution_; + offset_initialized_ = false; } inline bool OccupancyGridMapInterface::worldToMap( @@ -236,4 +239,16 @@ void OccupancyGridMapInterface::setHeightLimit(const double min_height, const do max_height_ = max_height; } +inline void ScanGroundFilterComponent::set_field_offsets( + const PointCloud2ConstPtr & input_raw, const PointCloud2ConstPtr & 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 diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 139f7774b42b3..34b79568c6623 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -82,6 +82,9 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( }; std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); std::vector> raw_pointcloud_angle_bins(angle_bin_size); + if (!offset_initialized_) { + set_field_offsets(raw_pointcloud, obstacle_pointcloud); + } 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; @@ -96,18 +99,16 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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 (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); + 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); } 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); + *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); // Exclude invalid points if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { global_offset += raw_pointcloud.point_step; @@ -138,11 +139,11 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( for (size_t i = 0; i < obstacle_pointcloud_size; ++i) { Eigen::Vector4f pt( *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + x_offset_obstacle]), + &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]), *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + y_offset_obstacle]), + &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]), *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + z_offset_obstacle]), + &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])) { diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 8a609077b8d3c..1bfbc0fa08ae3 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -97,15 +97,9 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( }; std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); std::vector> 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; + if (!offset_initialized_) { + set_field_offsets(raw_pointcloud, obstacle_pointcloud); + } 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; @@ -120,9 +114,9 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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); + *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); // Exclude invalid points if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { global_offset += raw_pointcloud.point_step; @@ -154,11 +148,11 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( for (size_t i = 0; i < obstacle_pointcloud_size; i++) { Eigen::Vector4f pt( *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + x_offset_obstacle]), + &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]), *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + y_offset_obstacle]), + &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]), *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + z_offset_obstacle]), + &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])) { 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 2f2b330e2df26..ba5cf0b48f00f 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 @@ -162,6 +162,9 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( 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::ConstSharedPtr input_raw_use = is_raw_transformed ? std::make_shared(trans_input_raw) : input_raw_msg; From 4689449465bb59264ab8387162586e4e2ae014a3 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 27 Jun 2024 16:20:52 +0900 Subject: [PATCH 05/17] fix: compile error and name Signed-off-by: taisa1 --- .../occupancy_grid_map_base.hpp | 3 +-- .../occupancy_grid_map_base.cpp | 16 ++++++++-------- .../occupancy_grid_map_fixed.cpp | 11 +---------- .../occupancy_grid_map_projective.cpp | 2 +- 4 files changed, 11 insertions(+), 21 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index adb7ed70f3352..5f79b9aba9268 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -88,8 +88,7 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D double min_height_; double max_height_; - void set_field_offsets( - const PointCloud2ConstPtr & input_raw, const PointCloud2ConstPtr & input_obstacle); + void setFieldOffsets(const PointCloud2 & input_raw, const PointCloud2 & input_obstacle); int x_offset_raw_; int y_offset_raw_; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index a8b706dc56934..3931487fb8e67 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -239,15 +239,15 @@ void OccupancyGridMapInterface::setHeightLimit(const double min_height, const do max_height_ = max_height; } -inline void ScanGroundFilterComponent::set_field_offsets( - const PointCloud2ConstPtr & input_raw, const PointCloud2ConstPtr & input_obstacle) +inline 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; + 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; } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 34b79568c6623..3fdafe27819eb 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -83,17 +83,8 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); std::vector> raw_pointcloud_angle_bins(angle_bin_size); if (!offset_initialized_) { - set_field_offsets(raw_pointcloud, obstacle_pointcloud); + setFieldOffsets(raw_pointcloud, obstacle_pointcloud); } - 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; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 1bfbc0fa08ae3..5d40ae61e9c5e 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -98,7 +98,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); std::vector> raw_pointcloud_angle_bins(angle_bin_size); if (!offset_initialized_) { - set_field_offsets(raw_pointcloud, obstacle_pointcloud); + setFieldOffsets(raw_pointcloud, obstacle_pointcloud); } const size_t raw_pointcloud_size = raw_pointcloud.width * raw_pointcloud.height; const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height; From ba35f4cc65976451913b62325cdeb825f2ee75e0 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 27 Jun 2024 19:09:09 +0900 Subject: [PATCH 06/17] fix: take common parts as func Signed-off-by: taisa1 --- .../occupancy_grid_map_base.hpp | 6 ++ .../occupancy_grid_map_fixed.hpp | 6 ++ .../occupancy_grid_map_projective.hpp | 6 ++ .../occupancy_grid_map_base.cpp | 2 +- .../occupancy_grid_map_fixed.cpp | 79 +++++++++++-------- .../occupancy_grid_map_projective.cpp | 66 ++++++++-------- 6 files changed, 97 insertions(+), 68 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index 5f79b9aba9268..64ee9e03f5263 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -52,6 +52,7 @@ #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 #include #include @@ -98,6 +99,11 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D 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; + private: bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp index 298f327d632d7..97f3da66ea8eb 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp @@ -32,8 +32,14 @@ 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, const Eigen::Matrix4f & matmap, const Eigen::Matrix4f & matscan, + 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; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp index 67b51d6184c8c..eab024c8b9800 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp @@ -36,8 +36,14 @@ 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, const Eigen::Matrix4f & matmap, const Eigen::Matrix4f & matscan, + 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; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index 3931487fb8e67..76a029ec9f95f 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -239,7 +239,7 @@ void OccupancyGridMapInterface::setHeightLimit(const double min_height, const do max_height_ = max_height; } -inline void OccupancyGridMapInterface::setFieldOffsets( +void OccupancyGridMapInterface::setFieldOffsets( const PointCloud2 & input_raw, const PointCloud2 & input_obstacle) { x_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "x")].offset; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 3fdafe27819eb..43d416163652c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -43,6 +43,32 @@ OccupancyGridMapFixedBlindSpot::OccupancyGridMapFixedBlindSpot( { } +inline bool OccupancyGridMapFixedBlindSpot::isPointValid(const Eigen::Vector4f & pt) +{ + // Exclude invalid points + if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { + return false; + } + // Apply height filter + if (pt[2] < min_height_ || max_height_ < pt[2]) { + return false; + } + return true; +} + +// pt -> (pt_map, angle_bin_index, range) +inline void OccupancyGridMapFixedBlindSpot::transformPointAndCalculate( + const Eigen::Vector4f & pt, const Eigen::Matrix4f & matmap, const Eigen::Matrix4f & matscan, + Eigen::Vector4f & pt_map, int & angle_bin_index, double & range) +{ + // Calculate transformed points + pt_map = matmap * pt; + Eigen::Vector4f pt_scan(matscan * 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 * @@ -55,10 +81,6 @@ 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 auto angle_increment_inv = 1.0 / angle_increment; const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform Matrix from base_link to map frame @@ -80,11 +102,13 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( double wx; double wy; }; - std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); - std::vector> raw_pointcloud_angle_bins(angle_bin_size); if (!offset_initialized_) { setFieldOffsets(raw_pointcloud, obstacle_pointcloud); } + + 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; const size_t raw_reserve_size = raw_pointcloud_size / angle_bin_size; @@ -94,39 +118,35 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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); - // Exclude invalid points - if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { + if (!isPointValid(pt)) { 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::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]), pt_map[0], pt_map[1]); + transformPointAndCalculate(pt, matmap, matscan, 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; }); } - global_offset = 0; + // 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( @@ -136,22 +156,11 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( *reinterpret_cast( &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]) { + if (!isPointValid(pt)) { 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]); + transformPointAndCalculate(pt, matmap, matscan, 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 diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 5d40ae61e9c5e..e4e0c3488cdda 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -44,6 +44,31 @@ OccupancyGridMapProjectiveBlindSpot::OccupancyGridMapProjectiveBlindSpot( { } +bool OccupancyGridMapProjectiveBlindSpot::isPointValid(const Eigen::Vector4f & pt) +{ + // Exclude invalid points + if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { + return false; + } + // Apply height filter + if (pt[2] < min_height_ || max_height_ < pt[2]) { + return false; + } + return true; +} + +// pt -> (pt_map, angle_bin_index, range) +void OccupancyGridMapProjectiveBlindSpot::transformPointAndCalculate( + const Eigen::Vector4f & pt, const Eigen::Matrix4f & matmap, const Eigen::Matrix4f & matscan, + Eigen::Vector4f & pt_map, int & angle_bin_index, double & range) +{ + // Calculate transformed points + pt_map = matmap * pt; + Eigen::Vector4f pt_scan(matscan * 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 in 3D manner * @@ -56,10 +81,6 @@ 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 auto angle_increment_inv = 1.0 / angle_increment; const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame @@ -112,30 +133,22 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( obstacle_pointcloud_angle_bin.reserve(obstacle_reserve_size); } size_t global_offset = 0; + // updated inside filterAndTransform() + Eigen::Vector4f pt_map; + int angle_bin_index; + double range; 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); - // Exclude invalid points - if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { + if (!isPointValid(pt)) { 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; + transformPointAndCalculate(pt, matmap, matscan, pt_map, angle_bin_index, range); raw_pointcloud_angle_bins.at(angle_bin_index) - .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]); + .emplace_back(range, 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) { @@ -154,25 +167,14 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( *reinterpret_cast( &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]) { + if (!isPointValid(pt)) { global_offset += obstacle_pointcloud.point_step; continue; } - // Calculate transformed points - Eigen::Vector4f pt_map(matmap * pt); - Eigen::Vector4f pt_scan(matscan * pt_map); + transformPointAndCalculate(pt, matmap, matscan, pt_map, angle_bin_index, range); const double scan_z = scan_origin.position.z - 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(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 From d24fec21270569546c3f3e1c8ac4de6f0e49927d Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 27 Jun 2024 19:10:11 +0900 Subject: [PATCH 07/17] refactor: change variable name Signed-off-by: taisa1 --- .../occupancy_grid_map_fixed.hpp | 2 +- .../occupancy_grid_map_projective.hpp | 2 +- .../occupancy_grid_map_fixed.cpp | 14 +++++++------- .../occupancy_grid_map_projective.cpp | 14 +++++++------- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp index 97f3da66ea8eb..b82793fd43a49 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp @@ -34,7 +34,7 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface bool isPointValid(const Eigen::Vector4f & pt); void transformPointAndCalculate( - const Eigen::Vector4f & pt, const Eigen::Matrix4f & matmap, const Eigen::Matrix4f & matscan, + const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range); using OccupancyGridMapInterface::raytrace; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp index eab024c8b9800..52d91c6c80fd9 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp @@ -38,7 +38,7 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface bool isPointValid(const Eigen::Vector4f & pt); void transformPointAndCalculate( - const Eigen::Vector4f & pt, const Eigen::Matrix4f & matmap, const Eigen::Matrix4f & matscan, + const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range); using OccupancyGridMapInterface::raytrace; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 43d416163652c..7c7b8bf83110a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -58,12 +58,12 @@ inline bool OccupancyGridMapFixedBlindSpot::isPointValid(const Eigen::Vector4f & // pt -> (pt_map, angle_bin_index, range) inline void OccupancyGridMapFixedBlindSpot::transformPointAndCalculate( - const Eigen::Vector4f & pt, const Eigen::Matrix4f & matmap, const Eigen::Matrix4f & matscan, + const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range) { // Calculate transformed points - pt_map = matmap * pt; - Eigen::Vector4f pt_scan(matscan * pt_map); + 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]); @@ -84,11 +84,11 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform Matrix from base_link to map frame - Eigen::Matrix4f matmap = utils::getTransformMatrix(robot_pose); + Eigen::Matrix4f mat_map = utils::getTransformMatrix(robot_pose); const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose // Transform Matrix from map frame to scan frame - Eigen::Matrix4f matscan = utils::getTransformMatrix(scan2map_pose); + Eigen::Matrix4f mat_scan = utils::getTransformMatrix(scan2map_pose); // Create angle bins and sort by distance struct BinInfo @@ -134,7 +134,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( global_offset += raw_pointcloud.point_step; continue; } - transformPointAndCalculate(pt, matmap, matscan, pt_map, angle_bin_index, range); + transformPointAndCalculate(pt, mat_map, mat_scan, 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; } @@ -160,7 +160,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( global_offset += obstacle_pointcloud.point_step; continue; } - transformPointAndCalculate(pt, matmap, matscan, pt_map, angle_bin_index, range); + transformPointAndCalculate(pt, mat_map, mat_scan, 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 diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index e4e0c3488cdda..ee76844ccf8ae 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -59,12 +59,12 @@ bool OccupancyGridMapProjectiveBlindSpot::isPointValid(const Eigen::Vector4f & p // pt -> (pt_map, angle_bin_index, range) void OccupancyGridMapProjectiveBlindSpot::transformPointAndCalculate( - const Eigen::Vector4f & pt, const Eigen::Matrix4f & matmap, const Eigen::Matrix4f & matscan, + const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range) { // Calculate transformed points - pt_map = matmap * pt; - Eigen::Vector4f pt_scan(matscan * pt_map); + 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]); @@ -85,12 +85,12 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( // Transform from base_link to map frame PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud; // point cloud in map frame - Eigen::Matrix4f matmap = utils::getTransformMatrix(robot_pose); + Eigen::Matrix4f 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 - Eigen::Matrix4f matscan = utils::getTransformMatrix(scan2map_pose); + Eigen::Matrix4f mat_scan = utils::getTransformMatrix(scan2map_pose); // Create angle bins and sort points by range struct BinInfo3D @@ -146,7 +146,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( global_offset += raw_pointcloud.point_step; continue; } - transformPointAndCalculate(pt, matmap, matscan, pt_map, angle_bin_index, range); + transformPointAndCalculate(pt, mat_map, mat_scan, pt_map, angle_bin_index, range); raw_pointcloud_angle_bins.at(angle_bin_index) .emplace_back(range, pt_map[0], pt_map[1], pt_map[2]); global_offset += raw_pointcloud.point_step; @@ -171,7 +171,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( global_offset += obstacle_pointcloud.point_step; continue; } - transformPointAndCalculate(pt, matmap, matscan, pt_map, angle_bin_index, range); + transformPointAndCalculate(pt, mat_map, mat_scan, pt_map, angle_bin_index, range); const double scan_z = scan_origin.position.z - robot_pose.position.z; const double obstacle_z = (pt_map[2]) - robot_pose.position.z; const double dz = scan_z - obstacle_z; From 174a32ed5cba728afa930f6abd40445fee503be8 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 27 Jun 2024 19:18:55 +0900 Subject: [PATCH 08/17] refactor: add some blank lines Signed-off-by: taisa1 --- .../occupancy_grid_map_fixed.cpp | 13 ++++++--- .../occupancy_grid_map_projective.cpp | 27 ++++++++++++++----- ...intcloud_based_occupancy_grid_map_node.cpp | 2 ++ 3 files changed, 32 insertions(+), 10 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 7c7b8bf83110a..0f243a923779a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -87,9 +87,14 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( Eigen::Matrix4f mat_map = utils::getTransformMatrix(robot_pose); const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose + // Transform Matrix from map frame to scan frame Eigen::Matrix4f mat_scan = utils::getTransformMatrix(scan2map_pose); + if (!offset_initialized_) { + setFieldOffsets(raw_pointcloud, obstacle_pointcloud); + } + // Create angle bins and sort by distance struct BinInfo { @@ -102,9 +107,6 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( double wx; double wy; }; - if (!offset_initialized_) { - setFieldOffsets(raw_pointcloud, obstacle_pointcloud); - } std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); std::vector> raw_pointcloud_angle_bins(angle_bin_size); @@ -113,6 +115,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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); @@ -135,6 +138,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( continue; } transformPointAndCalculate(pt, mat_map, mat_scan, 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; } @@ -161,15 +165,18 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( continue; } transformPointAndCalculate(pt, mat_map, mat_scan, 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).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(), diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index ee76844ccf8ae..fcdf8ddfdb4b6 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -87,11 +87,16 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud; // point cloud in map frame Eigen::Matrix4f 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 + + // Transform from map frame to scan frame Eigen::Matrix4f mat_scan = utils::getTransformMatrix(scan2map_pose); + if (!offset_initialized_) { + setFieldOffsets(raw_pointcloud, obstacle_pointcloud); + } + // Create angle bins and sort points by range struct BinInfo3D { @@ -116,15 +121,15 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( double projected_wx; double projected_wy; }; + std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); std::vector> raw_pointcloud_angle_bins(angle_bin_size); - if (!offset_initialized_) { - setFieldOffsets(raw_pointcloud, obstacle_pointcloud); - } + 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 (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { raw_pointcloud_angle_bin.reserve(raw_reserve_size); @@ -132,11 +137,13 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { obstacle_pointcloud_angle_bin.reserve(obstacle_reserve_size); } - size_t global_offset = 0; - // updated inside filterAndTransform() + + // 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_]), @@ -147,17 +154,20 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( continue; } transformPointAndCalculate(pt, mat_map, mat_scan, pt_map, angle_bin_index, range); + raw_pointcloud_angle_bins.at(angle_bin_index) .emplace_back(range, 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; }); } - global_offset = 0; + // 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( @@ -175,12 +185,14 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const double scan_z = scan_origin.position.z - robot_pose.position.z; const double obstacle_z = (pt_map[2]) - robot_pose.position.z; const double dz = scan_z - obstacle_z; + // 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; @@ -197,6 +209,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( } 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(), 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 ba5cf0b48f00f..8cd8660db613d 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 @@ -145,6 +145,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( 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_) { // Make sure that the frame is base_link @@ -166,6 +167,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( occupancy_grid_map_ptr_->setHeightLimit( -std::numeric_limits::infinity(), std::numeric_limits::infinity()); } + const PointCloud2::ConstSharedPtr input_raw_use = is_raw_transformed ? std::make_shared(trans_input_raw) : input_raw_msg; const PointCloud2::ConstSharedPtr input_obstacle_use = From dd6b800d2929f9f4dd10032967ac4dba941dc5a5 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 27 Jun 2024 19:20:44 +0900 Subject: [PATCH 09/17] fix: made unapplied change Signed-off-by: taisa1 --- .../occupancy_grid_map_projective.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index fcdf8ddfdb4b6..aaef96198d31c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -84,10 +84,8 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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 Eigen::Matrix4f mat_map = utils::getTransformMatrix(robot_pose); - PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud; // point cloud in scan frame const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose // Transform from map frame to scan frame @@ -124,18 +122,16 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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; 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 (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); + 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() @@ -154,7 +150,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( continue; } transformPointAndCalculate(pt, mat_map, mat_scan, pt_map, angle_bin_index, range); - + raw_pointcloud_angle_bins.at(angle_bin_index) .emplace_back(range, pt_map[0], pt_map[1], pt_map[2]); global_offset += raw_pointcloud.point_step; From 6e55227a3b9e0336ccb3e245aa9d391dd7dac932 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 27 Jun 2024 19:46:49 +0900 Subject: [PATCH 10/17] fix: compile error Signed-off-by: taisa1 --- .../occupancy_grid_map_base.hpp | 1 + .../occupancy_grid_map_fixed.cpp | 2 +- .../pointcloud_based_occupancy_grid_map_node.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index 64ee9e03f5263..cf3848ae315ea 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 0f243a923779a..a6c35ade6ed41 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -176,7 +176,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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(), 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 8cd8660db613d..1be3f9cf3978e 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 @@ -167,7 +167,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( occupancy_grid_map_ptr_->setHeightLimit( -std::numeric_limits::infinity(), std::numeric_limits::infinity()); } - + const PointCloud2::ConstSharedPtr input_raw_use = is_raw_transformed ? std::make_shared(trans_input_raw) : input_raw_msg; const PointCloud2::ConstSharedPtr input_obstacle_use = From 2d2c73768ad3f372249c61e7977738b42bf784ec Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 4 Jul 2024 18:22:08 +0900 Subject: [PATCH 11/17] refactor: simplified conditions Signed-off-by: taisa1 --- .../occupancy_grid_map_fixed.cpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index a6c35ade6ed41..957f1213b0bc6 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -45,23 +45,16 @@ OccupancyGridMapFixedBlindSpot::OccupancyGridMapFixedBlindSpot( inline bool OccupancyGridMapFixedBlindSpot::isPointValid(const Eigen::Vector4f & pt) { - // Exclude invalid points - if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { - return false; - } - // Apply height filter - if (pt[2] < min_height_ || max_height_ < pt[2]) { - return false; - } - return true; + // 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]); } -// pt -> (pt_map, angle_bin_index, range) +// Transform pt to (pt_map, pt_scan), then calculate angle_bin_index and range inline void OccupancyGridMapFixedBlindSpot::transformPointAndCalculate( const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range) { - // Calculate transformed points pt_map = mat_map * pt; Eigen::Vector4f pt_scan(mat_scan * pt_map); const double angle = atan2(pt_scan[1], pt_scan[0]); From d9676487e1c7511784cbd0d0b95096824f85fb76 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 4 Jul 2024 19:10:06 +0900 Subject: [PATCH 12/17] fix: apply changes to projective Signed-off-by: taisa1 --- .../occupancy_grid_map_projective.cpp | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index aaef96198d31c..4ff8be38f5a12 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -44,31 +44,25 @@ OccupancyGridMapProjectiveBlindSpot::OccupancyGridMapProjectiveBlindSpot( { } -bool OccupancyGridMapProjectiveBlindSpot::isPointValid(const Eigen::Vector4f & pt) +inline bool OccupancyGridMapProjectiveBlindSpot::isPointValid(const Eigen::Vector4f & pt) { - // Exclude invalid points - if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { - return false; - } - // Apply height filter - if (pt[2] < min_height_ || max_height_ < pt[2]) { - return false; - } - return true; + // 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]); } -// pt -> (pt_map, angle_bin_index, range) -void OccupancyGridMapProjectiveBlindSpot::transformPointAndCalculate( +// Transform pt to (pt_map, pt_scan), then calculate angle_bin_index and range +inline void OccupancyGridMapProjectiveBlindSpot::transformPointAndCalculate( const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range) { - // Calculate transformed points 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 in 3D manner * From 9b3a47029122649986b0c4859c8ea7b17785bf71 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 4 Jul 2024 19:29:34 +0900 Subject: [PATCH 13/17] fix: remove reserve from projective Signed-off-by: taisa1 --- .../occupancy_grid_map_projective.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 4ff8be38f5a12..1129ed1ffd3d7 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -119,14 +119,6 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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; From d958c1caa929ec2e4b46ea72e274993f3a45e331 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Thu, 4 Jul 2024 19:53:07 +0900 Subject: [PATCH 14/17] refactor: reduce too many arguments Signed-off-by: taisa1 --- .../occupancy_grid_map_base.hpp | 2 ++ .../occupancy_grid_map_fixed.hpp | 3 +-- .../occupancy_grid_map_projective.hpp | 3 +-- .../occupancy_grid_map_fixed.cpp | 11 +++++------ .../occupancy_grid_map_projective.cpp | 11 +++++------ 5 files changed, 14 insertions(+), 16 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index cf3848ae315ea..34534c0e8d805 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -105,6 +105,8 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D 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; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp index b82793fd43a49..7870ac20618d9 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp @@ -34,8 +34,7 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface bool isPointValid(const Eigen::Vector4f & pt); void transformPointAndCalculate( - const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, - Eigen::Vector4f & pt_map, int & angle_bin_index, double & range); + const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range); using OccupancyGridMapInterface::raytrace; using OccupancyGridMapInterface::setCellValue; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp index 52d91c6c80fd9..76cd59d1b220e 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp @@ -38,8 +38,7 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface bool isPointValid(const Eigen::Vector4f & pt); void transformPointAndCalculate( - const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, - Eigen::Vector4f & pt_map, int & angle_bin_index, double & range); + const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index, double & range); using OccupancyGridMapInterface::raytrace; using OccupancyGridMapInterface::setCellValue; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 957f1213b0bc6..88dbfa44a1c0a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -52,8 +52,7 @@ inline bool OccupancyGridMapFixedBlindSpot::isPointValid(const Eigen::Vector4f & // Transform pt to (pt_map, pt_scan), then calculate angle_bin_index and range inline void OccupancyGridMapFixedBlindSpot::transformPointAndCalculate( - const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, - Eigen::Vector4f & pt_map, int & angle_bin_index, double & range) + 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); @@ -77,12 +76,12 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform Matrix from base_link to map frame - Eigen::Matrix4f mat_map = utils::getTransformMatrix(robot_pose); + mat_map = utils::getTransformMatrix(robot_pose); const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose // Transform Matrix from map frame to scan frame - Eigen::Matrix4f mat_scan = utils::getTransformMatrix(scan2map_pose); + mat_scan = utils::getTransformMatrix(scan2map_pose); if (!offset_initialized_) { setFieldOffsets(raw_pointcloud, obstacle_pointcloud); @@ -130,7 +129,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( global_offset += raw_pointcloud.point_step; continue; } - transformPointAndCalculate(pt, mat_map, mat_scan, pt_map, angle_bin_index, range); + 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; @@ -157,7 +156,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( global_offset += obstacle_pointcloud.point_step; continue; } - transformPointAndCalculate(pt, mat_map, mat_scan, pt_map, angle_bin_index, range); + 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()) { diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 1129ed1ffd3d7..f25c4d43091e8 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -53,8 +53,7 @@ inline bool OccupancyGridMapProjectiveBlindSpot::isPointValid(const Eigen::Vecto // Transform pt to (pt_map, pt_scan), then calculate angle_bin_index and range inline void OccupancyGridMapProjectiveBlindSpot::transformPointAndCalculate( - const Eigen::Vector4f & pt, const Eigen::Matrix4f & mat_map, const Eigen::Matrix4f & mat_scan, - Eigen::Vector4f & pt_map, int & angle_bin_index, double & range) + 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); @@ -78,12 +77,12 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame - Eigen::Matrix4f mat_map = utils::getTransformMatrix(robot_pose); + mat_map = utils::getTransformMatrix(robot_pose); const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose // Transform from map frame to scan frame - Eigen::Matrix4f mat_scan = utils::getTransformMatrix(scan2map_pose); + mat_scan = utils::getTransformMatrix(scan2map_pose); if (!offset_initialized_) { setFieldOffsets(raw_pointcloud, obstacle_pointcloud); @@ -135,7 +134,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( global_offset += raw_pointcloud.point_step; continue; } - transformPointAndCalculate(pt, mat_map, mat_scan, pt_map, angle_bin_index, range); + 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], pt_map[2]); @@ -163,7 +162,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( global_offset += obstacle_pointcloud.point_step; continue; } - transformPointAndCalculate(pt, mat_map, mat_scan, pt_map, angle_bin_index, range); + transformPointAndCalculate(pt, pt_map, angle_bin_index, range); const double scan_z = scan_origin.position.z - robot_pose.position.z; const double obstacle_z = (pt_map[2]) - robot_pose.position.z; const double dz = scan_z - obstacle_z; From 4cc661aa43db65da0e264e665a0699188a6d78e0 Mon Sep 17 00:00:00 2001 From: taisa1 Date: Tue, 9 Jul 2024 15:10:07 +0900 Subject: [PATCH 15/17] refactor: delete angle_increment Signed-off-by: taisa1 --- .../occupancy_grid_map_base.hpp | 7 +++---- .../occupancy_grid_map_fixed.cpp | 4 ++-- .../occupancy_grid_map_projective.cpp | 5 +++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index 34534c0e8d805..c3706512136b3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -100,10 +100,9 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D 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; + 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; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 88dbfa44a1c0a..d12f9e802a099 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -57,7 +57,7 @@ inline void OccupancyGridMapFixedBlindSpot::transformPointAndCalculate( 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; + angle_bin_index = (angle - min_angle_) * angle_increment_inv; range = std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]); } @@ -73,7 +73,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - 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 Matrix from base_link to map frame mat_map = utils::getTransformMatrix(robot_pose); diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index f25c4d43091e8..78010d40c050e 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -58,7 +58,7 @@ inline void OccupancyGridMapProjectiveBlindSpot::transformPointAndCalculate( 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; + angle_bin_index = (angle - min_angle_) * angle_increment_inv; range = std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]); } @@ -74,7 +74,8 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - 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 mat_map = utils::getTransformMatrix(robot_pose); From dd60264c5145a6619c599c07693cb0a9cb02d61c Mon Sep 17 00:00:00 2001 From: taisa1 Date: Tue, 9 Jul 2024 16:02:16 +0900 Subject: [PATCH 16/17] fix: reflect review Signed-off-by: taisa1 --- .../occupancy_grid_map_base.hpp | 20 ++++++++++- .../occupancy_grid_map_fixed.hpp | 4 --- .../occupancy_grid_map_projective.hpp | 4 --- .../occupancy_grid_map_fixed.cpp | 36 +++++-------------- .../occupancy_grid_map_projective.cpp | 30 +++------------- 5 files changed, 32 insertions(+), 62 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index c3706512136b3..8e149da68ee07 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -104,7 +104,25 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D 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; + 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; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp index 7870ac20618d9..b4a54d004626a 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp @@ -32,10 +32,6 @@ 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; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp index 76cd59d1b220e..43b54e62b936a 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp @@ -36,10 +36,6 @@ 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; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index d12f9e802a099..756cb14dc5e71 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -43,24 +43,6 @@ OccupancyGridMapFixedBlindSpot::OccupancyGridMapFixedBlindSpot( { } -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 * @@ -73,15 +55,16 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - const size_t angle_bin_size = ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/); + 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); + mat_map_ = utils::getTransformMatrix(robot_pose); const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose // Transform Matrix from map frame to scan frame - mat_scan = utils::getTransformMatrix(scan2map_pose); + mat_scan_ = utils::getTransformMatrix(scan2map_pose); if (!offset_initialized_) { setFieldOffsets(raw_pointcloud, obstacle_pointcloud); @@ -105,10 +88,10 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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 + 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); @@ -125,14 +108,13 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( *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)) { - 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) { @@ -152,8 +134,8 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( *reinterpret_cast( &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]), 1); + global_offset += obstacle_pointcloud.point_step; if (!isPointValid(pt)) { - global_offset += obstacle_pointcloud.point_step; continue; } transformPointAndCalculate(pt, pt_map, angle_bin_index, range); @@ -164,9 +146,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } 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).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) { diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 78010d40c050e..1545766faf60f 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -44,24 +44,6 @@ OccupancyGridMapProjectiveBlindSpot::OccupancyGridMapProjectiveBlindSpot( { } -inline bool OccupancyGridMapProjectiveBlindSpot::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 OccupancyGridMapProjectiveBlindSpot::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 in 3D manner * @@ -75,15 +57,15 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const Pose & robot_pose, const Pose & scan_origin) { const size_t angle_bin_size = - ((max_angle_ - min_angle_) / angle_increment_inv_) + size_t(1 /*margin*/); + ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/); // Transform from base_link to map frame - mat_map = utils::getTransformMatrix(robot_pose); + mat_map_ = utils::getTransformMatrix(robot_pose); const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose // Transform from map frame to scan frame - mat_scan = utils::getTransformMatrix(scan2map_pose); + mat_scan_ = utils::getTransformMatrix(scan2map_pose); if (!offset_initialized_) { setFieldOffsets(raw_pointcloud, obstacle_pointcloud); @@ -131,15 +113,14 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( *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)) { - 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], pt_map[2]); - global_offset += raw_pointcloud.point_step; } for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { @@ -159,8 +140,8 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( *reinterpret_cast( &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]), 1); + global_offset += obstacle_pointcloud.point_step; if (!isPointValid(pt)) { - global_offset += obstacle_pointcloud.point_step; continue; } transformPointAndCalculate(pt, pt_map, angle_bin_index, range); @@ -189,7 +170,6 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( range, pt_map[0], pt_map[1], pt_map[2], std::numeric_limits::infinity(), std::numeric_limits::infinity(), std::numeric_limits::infinity()); } - global_offset += obstacle_pointcloud.point_step; } for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { From 71974b31cb37c2ab4f2eb715b46819e2fb3a3f4b Mon Sep 17 00:00:00 2001 From: taisa1 Date: Tue, 16 Jul 2024 14:55:55 +0900 Subject: [PATCH 17/17] fix: build error Signed-off-by: taisa1 --- .../autoware/probabilistic_occupancy_grid_map/utils/utils.hpp | 2 ++ 1 file changed, 2 insertions(+) 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,