From 94450e2be426ce4d654126fb82f120c02a816b13 Mon Sep 17 00:00:00 2001 From: Yukihito Saito Date: Tue, 12 Mar 2024 01:15:46 +0900 Subject: [PATCH] add approximate extract common pointcloud function Signed-off-by: Yukihito Saito --- .../utils/utils.hpp | 4 + ...intcloud_based_occupancy_grid_map_node.cpp | 4 +- .../src/utils/utils.cpp | 139 ++++++++++++++++++ 3 files changed, 145 insertions(+), 2 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp index 0950272dac470..de6bc254e08c3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -116,6 +116,10 @@ bool extractCommonPointCloud( const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); +bool extractApproximateCommonPointCloud( + const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, + const float voxel_size, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); + unsigned char getApproximateOccupancyState(const unsigned char & value); } // namespace utils 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 a8cdff5b27907..e143c35b51fa9 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 @@ -157,8 +157,8 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( // Filter obstacle pointcloud by raw pointcloud PointCloud2 filtered_obstacle_pc_common{}; if (filter_obstacle_pointcloud_by_raw_pointcloud_) { - if (!utils::extractCommonPointCloud( - filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)) { + if (!utils::extractApproximateCommonPointCloud( + filtered_obstacle_pc, filtered_raw_pc, 0.1f, filtered_obstacle_pc_common)) { filtered_obstacle_pc_common = filtered_obstacle_pc; } } else { diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 8009a503167ea..db24f26ff9a2d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -18,6 +18,35 @@ #include +namespace +{ +/** + * @brief Hash function for voxel keys. + * Utilizes prime numbers to calculate a unique hash for each voxel key. + */ +struct VoxelKeyHash +{ + std::size_t operator()(const std::array & k) const + { + // Primes based on the following paper: 'Investigating the Use of Primes in Hashing for + // Volumetric Data'. + return (k[0] * 73856093 ^ k[1] * 19349663 ^ k[2] * 83492791); + } +}; + +/** + * @brief Equality function for voxel keys. + * Checks if two voxel keys are equal. + */ +struct VoxelKeyEqual +{ + bool operator()(const std::array & a, const std::array & b) const + { + return a == b; + } +}; +} // namespace + namespace utils { @@ -182,6 +211,116 @@ bool extractCommonPointCloud( return true; } +/** + * @brief extract Common Pointcloud between obstacle pc and raw pc + * @param obstacle_pc + * @param raw_pc + * @param voxel_size + * @param output_obstacle_pc + */ +bool extractApproximateCommonPointCloud( + const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, + const float voxel_size, sensor_msgs::msg::PointCloud2 & output_obstacle_pc) +{ + using VoxelKey = std::array; + std::unordered_map obstacle_voxel_map; + std::unordered_map raw_voxel_map; + + constexpr float large_num_offset = 100000.0; + const float & voxel_size_x = voxel_size; + const float & voxel_size_y = voxel_size; + const float & voxel_size_z = voxel_size; + const float inverse_voxel_size_x = 1.0 / voxel_size_x; + const float inverse_voxel_size_y = 1.0 / voxel_size_y; + const float inverse_voxel_size_z = 1.0 / voxel_size_z; + + { + const int x_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "x")].offset; + const int y_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "y")].offset; + const int z_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "z")].offset; + + // Process each point in the point cloud + for (size_t global_offset = 0; global_offset + raw_pc.point_step <= raw_pc.data.size(); + global_offset += raw_pc.point_step) { + const float & x = *reinterpret_cast(&raw_pc.data[global_offset + x_offset]); + const float & y = *reinterpret_cast(&raw_pc.data[global_offset + y_offset]); + const float & z = *reinterpret_cast(&raw_pc.data[global_offset + z_offset]); + + // The reason for adding a large value is that when converting from float to int, values + // around -1 to 1 are all rounded down to 0. Therefore, to prevent the numbers from becoming + // negative, a large value is added. It has been tuned to reduce computational costs, and + // deliberately avoids using round or floor functions. + VoxelKey key = { + static_cast((x + large_num_offset) * inverse_voxel_size_x), + static_cast((y + large_num_offset) * inverse_voxel_size_y), + static_cast((z + large_num_offset) * inverse_voxel_size_z)}; + + if (raw_voxel_map.find(key) == raw_voxel_map.end()) { + raw_voxel_map[key] = global_offset; + } + } + } + { + const int x_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "x")].offset; + const int y_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "y")].offset; + const int z_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "z")].offset; + + for (size_t global_offset = 0; + global_offset + obstacle_pc.point_step <= obstacle_pc.data.size(); + global_offset += obstacle_pc.point_step) { + const float & x = + *reinterpret_cast(&obstacle_pc.data[global_offset + x_offset]); + const float & y = + *reinterpret_cast(&obstacle_pc.data[global_offset + y_offset]); + const float & z = + *reinterpret_cast(&obstacle_pc.data[global_offset + z_offset]); + + // The reason for adding a large value is that when converting from float to int, values + // around -1 to 1 are all rounded down to 0. Therefore, to prevent the numbers from becoming + // negative, a large value is added. It has been tuned to reduce computational costs, and + // deliberately avoids using round or floor functions. + VoxelKey key = { + static_cast((x + large_num_offset) * inverse_voxel_size_x), + static_cast((y + large_num_offset) * inverse_voxel_size_y), + static_cast((z + large_num_offset) * inverse_voxel_size_z)}; + + if (raw_voxel_map.find(key) == raw_voxel_map.end()) { + obstacle_voxel_map[key] = global_offset; + } + } + + // Populate the output point cloud + size_t output_global_offset = 0; + output_obstacle_pc.data.resize(obstacle_voxel_map.size() * obstacle_pc.point_step); + for (const auto & kv : obstacle_voxel_map) { + std::memcpy( + &output_obstacle_pc.data[output_global_offset + x_offset], + &obstacle_pc.data[kv.second + x_offset], sizeof(float)); + std::memcpy( + &output_obstacle_pc.data[output_global_offset + y_offset], + &obstacle_pc.data[kv.second + y_offset], sizeof(float)); + std::memcpy( + &output_obstacle_pc.data[output_global_offset + z_offset], + &obstacle_pc.data[kv.second + z_offset], sizeof(float)); + output_global_offset += obstacle_pc.point_step; + } + + // Set the output point cloud metadata + output_obstacle_pc.header.frame_id = obstacle_pc.header.frame_id; + output_obstacle_pc.height = 1; + output_obstacle_pc.fields = obstacle_pc.fields; + output_obstacle_pc.is_bigendian = obstacle_pc.is_bigendian; + output_obstacle_pc.point_step = obstacle_pc.point_step; + output_obstacle_pc.is_dense = obstacle_pc.is_dense; + output_obstacle_pc.width = static_cast( + output_obstacle_pc.data.size() / output_obstacle_pc.height / output_obstacle_pc.point_step); + output_obstacle_pc.row_step = + static_cast(output_obstacle_pc.data.size() / output_obstacle_pc.height); + } + + return true; +} + /** * @brief Convert unsigned char value to closest cost value * @param cost Cost value