|
| 1 | +// Copyright 2024 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp" |
| 16 | + |
| 17 | +#include "robin_hood.h" |
| 18 | + |
| 19 | +namespace |
| 20 | +{ |
| 21 | +/** |
| 22 | + * @brief Hash function for voxel keys. |
| 23 | + * Utilizes prime numbers to calculate a unique hash for each voxel key. |
| 24 | + */ |
| 25 | +struct VoxelKeyHash |
| 26 | +{ |
| 27 | + std::size_t operator()(const std::array<int, 3> & k) const |
| 28 | + { |
| 29 | + // Primes based on the following paper: 'Investigating the Use of Primes in Hashing for |
| 30 | + // Volumetric Data'. |
| 31 | + return (k[0] * 73856093 ^ k[1] * 19349663 ^ k[2] * 83492791); |
| 32 | + // In general, the performance of the search may be improved by restricting the hashkey to the |
| 33 | + // following However, the risk of key collisions also increases, so the value must be |
| 34 | + // appropriate. Enable the following code depending on the situation. return ((1 << 16) - 1) & |
| 35 | + // (k[0] * 73856093 ^ k[1] * 19349663 ^ k[2] * 83492791); |
| 36 | + } |
| 37 | +}; |
| 38 | + |
| 39 | +/** |
| 40 | + * @brief Equality function for voxel keys. |
| 41 | + * Checks if two voxel keys are equal. |
| 42 | + */ |
| 43 | +struct VoxelKeyEqual |
| 44 | +{ |
| 45 | + bool operator()(const std::array<int, 3> & a, const std::array<int, 3> & b) const |
| 46 | + { |
| 47 | + return a == b; |
| 48 | + } |
| 49 | +}; |
| 50 | +} // namespace |
| 51 | + |
| 52 | +namespace pointcloud_preprocessor |
| 53 | +{ |
| 54 | +PickupBasedVoxelGridDownsampleFilterComponent::PickupBasedVoxelGridDownsampleFilterComponent( |
| 55 | + const rclcpp::NodeOptions & options) |
| 56 | +: Filter("PickupBasedVoxelGridDownsampleFilterComponent", options) |
| 57 | +{ |
| 58 | + // initialize debug tool |
| 59 | + { |
| 60 | + using tier4_autoware_utils::DebugPublisher; |
| 61 | + using tier4_autoware_utils::StopWatch; |
| 62 | + stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>(); |
| 63 | + debug_publisher_ = std::make_unique<DebugPublisher>(this, this->get_name()); |
| 64 | + stop_watch_ptr_->tic("cyclic_time"); |
| 65 | + stop_watch_ptr_->tic("processing_time"); |
| 66 | + } |
| 67 | + |
| 68 | + // Initialization of voxel sizes from parameters |
| 69 | + voxel_size_x_ = static_cast<float>(declare_parameter("voxel_size_x", 1.0)); |
| 70 | + voxel_size_y_ = static_cast<float>(declare_parameter("voxel_size_y", 1.0)); |
| 71 | + voxel_size_z_ = static_cast<float>(declare_parameter("voxel_size_z", 1.0)); |
| 72 | + |
| 73 | + using std::placeholders::_1; |
| 74 | + set_param_res_ = this->add_on_set_parameters_callback( |
| 75 | + std::bind(&PickupBasedVoxelGridDownsampleFilterComponent::paramCallback, this, _1)); |
| 76 | +} |
| 77 | + |
| 78 | +void PickupBasedVoxelGridDownsampleFilterComponent::filter( |
| 79 | + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, |
| 80 | + PointCloud2 & output) |
| 81 | +{ |
| 82 | + std::scoped_lock lock(mutex_); |
| 83 | + |
| 84 | + stop_watch_ptr_->toc("processing_time", true); |
| 85 | + |
| 86 | + using VoxelKey = std::array<int, 3>; |
| 87 | + // std::unordered_map<VoxelKey, size_t, VoxelKeyHash, VoxelKeyEqual> voxel_map; |
| 88 | + robin_hood::unordered_map<VoxelKey, size_t, VoxelKeyHash, VoxelKeyEqual> voxel_map; |
| 89 | + |
| 90 | + voxel_map.reserve(input->data.size() / input->point_step); |
| 91 | + |
| 92 | + constexpr float large_num_offset = 100000.0; |
| 93 | + const float inverse_voxel_size_x = 1.0 / voxel_size_x_; |
| 94 | + const float inverse_voxel_size_y = 1.0 / voxel_size_y_; |
| 95 | + const float inverse_voxel_size_z = 1.0 / voxel_size_z_; |
| 96 | + |
| 97 | + const int x_offset = input->fields[pcl::getFieldIndex(*input, "x")].offset; |
| 98 | + const int y_offset = input->fields[pcl::getFieldIndex(*input, "y")].offset; |
| 99 | + const int z_offset = input->fields[pcl::getFieldIndex(*input, "z")].offset; |
| 100 | + |
| 101 | + // Process each point in the point cloud |
| 102 | + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); |
| 103 | + global_offset += input->point_step) { |
| 104 | + const float & x = *reinterpret_cast<const float *>(&input->data[global_offset + x_offset]); |
| 105 | + const float & y = *reinterpret_cast<const float *>(&input->data[global_offset + y_offset]); |
| 106 | + const float & z = *reinterpret_cast<const float *>(&input->data[global_offset + z_offset]); |
| 107 | + |
| 108 | + // The reason for adding a large value is that when converting from float to int, values around |
| 109 | + // -1 to 1 are all rounded down to 0. Therefore, to prevent the numbers from becoming negative, |
| 110 | + // a large value is added. It has been tuned to reduce computational costs, and deliberately |
| 111 | + // avoids using round or floor functions. |
| 112 | + VoxelKey key = { |
| 113 | + static_cast<int>((x + large_num_offset) * inverse_voxel_size_x), |
| 114 | + static_cast<int>((y + large_num_offset) * inverse_voxel_size_y), |
| 115 | + static_cast<int>((z + large_num_offset) * inverse_voxel_size_z)}; |
| 116 | + |
| 117 | + voxel_map.emplace(key, global_offset); |
| 118 | + } |
| 119 | + |
| 120 | + // Populate the output point cloud |
| 121 | + size_t output_global_offset = 0; |
| 122 | + output.data.resize(voxel_map.size() * input->point_step); |
| 123 | + for (const auto & kv : voxel_map) { |
| 124 | + std::memcpy( |
| 125 | + &output.data[output_global_offset + x_offset], &input->data[kv.second + x_offset], |
| 126 | + sizeof(float)); |
| 127 | + std::memcpy( |
| 128 | + &output.data[output_global_offset + y_offset], &input->data[kv.second + y_offset], |
| 129 | + sizeof(float)); |
| 130 | + std::memcpy( |
| 131 | + &output.data[output_global_offset + z_offset], &input->data[kv.second + z_offset], |
| 132 | + sizeof(float)); |
| 133 | + output_global_offset += input->point_step; |
| 134 | + } |
| 135 | + |
| 136 | + // Set the output point cloud metadata |
| 137 | + output.header.frame_id = input->header.frame_id; |
| 138 | + output.height = 1; |
| 139 | + output.fields = input->fields; |
| 140 | + output.is_bigendian = input->is_bigendian; |
| 141 | + output.point_step = input->point_step; |
| 142 | + output.is_dense = input->is_dense; |
| 143 | + output.width = static_cast<uint32_t>(output.data.size() / output.height / output.point_step); |
| 144 | + output.row_step = static_cast<uint32_t>(output.data.size() / output.height); |
| 145 | + |
| 146 | + // add processing time for debug |
| 147 | + if (debug_publisher_) { |
| 148 | + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); |
| 149 | + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); |
| 150 | + debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>( |
| 151 | + "debug/cyclic_time_ms", cyclic_time_ms); |
| 152 | + debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>( |
| 153 | + "debug/processing_time_ms", processing_time_ms); |
| 154 | + |
| 155 | + auto pipeline_latency_ms = |
| 156 | + std::chrono::duration<double, std::milli>( |
| 157 | + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) |
| 158 | + .count(); |
| 159 | + |
| 160 | + debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>( |
| 161 | + "debug/pipeline_latency_ms", pipeline_latency_ms); |
| 162 | + } |
| 163 | +} |
| 164 | + |
| 165 | +rcl_interfaces::msg::SetParametersResult |
| 166 | +PickupBasedVoxelGridDownsampleFilterComponent::paramCallback( |
| 167 | + const std::vector<rclcpp::Parameter> & p) |
| 168 | +{ |
| 169 | + std::scoped_lock lock(mutex_); |
| 170 | + |
| 171 | + // Handling dynamic updates for the voxel sizes |
| 172 | + if (get_param(p, "voxel_size_x", voxel_size_x_)) { |
| 173 | + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_x_); |
| 174 | + } |
| 175 | + if (get_param(p, "voxel_size_y", voxel_size_y_)) { |
| 176 | + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_y_); |
| 177 | + } |
| 178 | + if (get_param(p, "voxel_size_z", voxel_size_z_)) { |
| 179 | + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_z_); |
| 180 | + } |
| 181 | + |
| 182 | + rcl_interfaces::msg::SetParametersResult result; |
| 183 | + result.successful = true; |
| 184 | + result.reason = "success"; |
| 185 | + |
| 186 | + return result; |
| 187 | +} |
| 188 | + |
| 189 | +} // namespace pointcloud_preprocessor |
| 190 | + |
| 191 | +#include <rclcpp_components/register_node_macro.hpp> |
| 192 | +RCLCPP_COMPONENTS_REGISTER_NODE( |
| 193 | + pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent) |
![pre-commit-ci[bot]](https://avatars.githubusercontent.com/in/68672?v=4&size=40){kind=avatar}
0 commit comments