Skip to content

cuda_pointcloud_preprocessor#

Purpose#

This node implements all standard pointcloud preprocessing algorithms applied to a single LiDAR's pointcloud in CUDA. In particular, this node implements:

  • box cropping (ego-vehicle and ego-vehicle's mirrors)
  • distortion correction
  • ring-based outlier filtering

Inner-workings / Algorithms#

As this node reimplements the functionalities of the CPU-version algorithms, please have a look at the documentations of crop-box, distortion correction, and ring-based outlier filter for more information about these algorithms.

In addition to the individual algorithms previously mentioned, this node uses the cuda_blackboard, a cuda transport layer that enables a zero-copy mechanism between GPU and GPU memory for both input and output.

Inputs / Outputs#

Input#

Name Type Description
~/input/pointcloud sensor_msgs::msg::PointCloud2 Input pointcloud's topic.
~/input/pointcloud/cuda negotiated_interfaces/msg/NegotiatedTopicsInfo Input pointcloud's type negotiation topic
~/input/twist geometry_msgs::msg::TwistWithCovarianceStamped Topic of the twist information.
~/input/imu sensor_msgs::msg::Imu Topic of the IMU data.

Output#

Name Type Description
~/output/pointcloud sensor_msgs::msg::PointCloud2 Processed pointcloud's topic
~/output/pointcloud/cuda negotiated_interfaces/msg/NegotiatedTopicsInfo Processed pointcloud's negotiation topic

Parameters#

Core Parameters#

Name Type Description Default Range
base_frame string The undistortion algorithm is based on a base frame, which must be the same as the twist frame. base_link N/A
use_imu boolean Use IMU angular velocity, otherwise, use twist angular velocity. true N/A
use_3d_distortion_correction boolean Use 3d distortion correction algorithm, otherwise, use 2d distortion correction algorithm. false N/A
distance_ratio float distance ratio between consecutive points to determine is they belong to the same walk 1.03 ≥0.0
object_length_threshold float minimum object length to be considered a valid cluster in meters 0.05 ≥0.0
processing_time_threshold_sec float Threshold in seconds. If the processing time of the node exceeds this value, a diagnostic warning will be issued. 0.01 N/A
timestamp_mismatch_fraction_threshold float Threshold for the fraction of points that lack corresponding twist or IMU data within the allowed timestamp tolerance. 0.01 N/A
enable_ring_outlier_filter boolean Enable/disable ring outlier filter in the cuda_pointcloud_preprocessor pipeline. true N/A
crop_box.min_x array An array in which each element is the minimum x value in a crop box [] N/A
crop_box.min_y array An array in which each element is the minimum y value in a crop box [] N/A
crop_box.min_z array An array in which each element is the minimum z value in a crop box [] N/A
crop_box.max_x array An array in which each element is the maximum x value in a crop box [] N/A
crop_box.max_y array An array in which each element is the maximum y value in a crop box [] N/A
crop_box.max_z array An array in which each element is the maximum z value in a crop box [] N/A

Assumptions / Known limits#

  • The CUDA implementations, while following the original CPU ones, will not offer the same numerical results, and small approximations were needed to maximize GPU usage.
  • This node expects that the input pointcloud follows the autoware::point_types::PointXYZIRCAEDT layout and the output pointcloud will use the autoware::point_types::PointXYZIRC layout defined in the autoware_point_types package.