autoware_pointcloud_preprocessor#
Purpose#
The autoware_pointcloud_preprocessor is a package that includes the following filters:
- removing outlier points
- cropping
- concatenating pointclouds
- correcting distortion
- downsampling
- densifying pointclouds
Inner-workings / Algorithms#
Detail description of each filter's algorithm is in the following links.
| Filter Name | Description | Detail |
|---|---|---|
| concatenate_data | subscribe multiple pointclouds and concatenate them into a pointcloud | link |
| crop_box_filter | remove points within a given box | link |
| distortion_corrector | compensate pointcloud distortion caused by ego vehicle's movement during 1 scan | link |
| downsample_filter | downsampling input pointcloud | link |
| outlier_filter | remove points caused by hardware problems, rain drops and small insects as a noise | link |
| passthrough_filter | remove points on the outside of a range in given field (e.g. x, y, z, intensity) | link |
| pointcloud_accumulator | accumulate pointclouds for a given amount of time | link |
| pointcloud_densifier | enhance sparse point clouds by using information from previous frames | link |
| vector_map_filter | remove points on the outside of lane by using vector map | link |
| vector_map_inside_area_filter | remove points inside of vector map area that has given type by parameter | link |
Inputs / Outputs#
Input#
| Name | Type | Description |
|---|---|---|
~/input/points |
sensor_msgs::msg::PointCloud2 |
reference points |
~/input/indices |
pcl_msgs::msg::Indices |
reference indices |
Output#
| Name | Type | Description |
|---|---|---|
~/output/points |
sensor_msgs::msg::PointCloud2 |
filtered points |
Parameters#
| Name | Type | Description | Default | Range |
|---|---|---|---|---|
| voxel_size_x | float | voxel size along the x-axis [m] | 0.3 | ≥0.0 |
| voxel_size_y | float | voxel size along the y-axis [m] | 0.3 | ≥0.0 |
| voxel_size_z | float | voxel size along the z-axis [m] | 0.1 | ≥0.0 |
| Name | Type | Description | Default | Range |
| ----------------------------- | --------- | -------------------------------------------------------------------------------------------------------------------------------- | -------------- | ------------- |
| blockage_ratio_threshold | float | The threshold of blockage area ratio. If the blockage value exceeds this threshold, the diagnostic state will be set to ERROR. | 0.1 | ≥0 |
| blockage_count_threshold | float | The threshold of number continuous blockage frames | 50 | ≥0 |
| blockage_buffering_frames | integer | The number of buffering about blockage detection [range:1-200] | 2 | ≥1 ≤200 |
| blockage_buffering_interval | integer | The interval of buffering about blockage detection | 1 | ≥0 |
| enable_dust_diag | boolean | enable dust diagnostic | False | N/A |
| publish_debug_image | boolean | publish debug image | False | N/A |
| dust_ratio_threshold | float | The threshold of dusty area ratio | 0.2 | ≥0 |
| dust_count_threshold | integer | The threshold of number continuous frames include dusty area | 10 | ≥0 |
| dust_kernel_size | integer | The kernel size of morphology processing in dusty area detection | 2 | ≥0 |
| dust_buffering_frames | integer | The number of buffering about dusty area detection [range:1-200] | 10 | ≥1 ≤200 |
| dust_buffering_interval | integer | The interval of buffering about dusty area detection | 1 | ≥0 |
| max_distance_range | float | Maximum view range for the LiDAR | 200.0 | ≥0 |
| horizontal_resolution | float | The horizontal resolution of depth map image [deg/pixel] | 0.4 | ≥0 |
| blockage_kernel | integer | The kernel size of morphology processing the detected blockage area | 10 | ≥0 |
| angle_range | array | The effective range of LiDAR | [0.0, 360.0] | N/A |
| vertical_bins | integer | The LiDAR channel | 40 | ≥0 |
| is_channel_order_top2down | boolean | If the lidar channels are indexed from top to down | True | N/A |
| horizontal_ring_id | integer | The id of horizontal ring of the LiDAR | 18 | ≥0 |
| Name | Type | Description | Default | Range |
| ---------------- | --------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ----------- | --------- |
| timeout_sec | float | Tolerance of time to publish the next pointcloud [s]. When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | 0.1 | ≥0 |
| input_offset | array | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers. | [] | N/A |
| output_frame | string | Output frame id. | base_link | N/A |
| input_topics | array | List of input topics. | [] | N/A |
| max_queue_size | integer | Max queue size of input/output topics. | 5 | ≥1 |
| Name | Type | Description | Default | Range |
| ------------------------------- | --------- | ------------------------------------------------------------------------------------------------------------------- | ----------- | --------- |
| min_x | float | minimum x-coordinate value for crop range in meters | -1.0 | N/A |
| min_y | float | minimum y-coordinate value for crop range in meters | -1.0 | N/A |
| min_z | float | minimum z-coordinate value for crop range in meters | -1.0 | N/A |
| max_x | float | maximum x-coordinate value for crop range in meters | 1.0 | N/A |
| max_y | float | maximum y-coordinate value for crop range in meters | 1.0 | N/A |
| max_z | float | maximum z-coordinate value for crop range in meters | 1.0 | N/A |
| negative | boolean | if true, remove points within the box from the pointcloud; otherwise, remove points outside the box. | false | N/A |
| 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 |
| 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 |
| update_azimuth_and_distance | boolean | Flag to update the azimuth and distance values of each point after undistortion. If set to false, the azimuth and distance values will remain unchanged after undistortion, resulting in a mismatch with the updated x, y, z coordinates. | false | N/A |
| 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 |
| Name | Type | Description | Default | Range |
| ---------------------------------- | --------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------------- | -------------------------------------------------- |
| x_max | float | Maximum of x in meters for Fixed_xyz_ROI mode |
18.0 | N/A |
| x_min | float | Minimum of x in meters for Fixed_xyz_ROI mode |
-12.0 | N/A |
| y_max | float | Maximum of y in meters for Fixed_xyz_ROI mode |
2.0 | N/A |
| y_min | float | Minimum of y in meters for Fixed_xyz_ROI mode |
-2.0 | N/A |
| z_max | float | Maximum of z in meters for Fixed_xyz_ROI mode |
10.0 | N/A |
| z_min | float | Minimum of z in meters for Fixed_xyz_ROI mode |
0.0 | N/A |
| min_azimuth_deg | float | The left limit of azimuth in degrees for Fixed_azimuth_ROI mode |
135.0 | ≥0 ≤360 |
| max_azimuth_deg | float | The right limit of azimuth in degrees for Fixed_azimuth_ROI mode |
225.0 | ≥0 ≤360 |
| max_distance | float | The limit distance in meters for Fixed_azimuth_ROI mode |
12.0 | ≥0.0 |
| vertical_bins | integer | The number of vertical bins for the visibility histogram | 128 | ≥1 |
| max_azimuth_diff | float | The azimuth difference threshold in degrees for ring_outlier_filter | 50.0 | ≥0.0 |
| weak_first_distance_ratio | float | The maximum allowed ratio between consecutive weak point distances | 1.004 | ≥0.0 |
| general_distance_ratio | float | The maximum allowed ratio between consecutive normal point distances | 1.03 | ≥0.0 |
| weak_first_local_noise_threshold | integer | If the number of outliers among weak points is less than the weak_first_local_noise_threshold in the (max_azimuth - min_azimuth) / horizontal_bins interval, all points within the interval will not be filtered out. | 2 | ≥0 |
| roi_mode | string | roi mode | Fixed_xyz_ROI | ['Fixed_xyz_ROI', 'No_ROI', 'Fixed_azimuth_ROI'] |
| visibility_error_threshold | float | When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR | 0.5 | ≥0.0 ≤1.0 |
| visibility_warn_threshold | float | When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | 0.7 | ≥0.0 ≤1.0 |
| Name | Type | Description | Default | Range |
| -------------- | -------- | ----------------------------- | ----------- | --------- |
| voxel_size_x | float | voxel size along x-axis [m] | 0.04 | ≥0 |
| voxel_size_y | float | voxel size along y-axis [m] | 0.04 | ≥0 |
| Name | Type | Description | Default | Range |
| ----------------------- | --------- | -------------------------------------------------------- | ----------- | --------- |
| filter_limit_min | integer | minimum allowed field value | 0 | ≥0 |
| filter_limit_max | integer | maximum allowed field value | 127 | ≥0 |
| filter_field_name | string | filtering field name | channel | N/A |
| keep_organized | boolean | flag to keep indices structure | False | N/A |
| filter_limit_negative | boolean | flag to return whether the data is inside limit or not | False | N/A |
| Name | Type | Description | Default | Range |
| -------------- | -------- | --------------------------------- | ----------- | --------- |
| voxel_size_x | float | voxel size along the x-axis [m] | 1 | >0.0 |
| voxel_size_y | float | voxel size along the y-axis [m] | 1 | >0.0 |
| voxel_size_z | float | voxel size along the z-axis [m] | 1 | >0.0 |
| Name | Type | Description | Default | Range |
| ------------------------ | --------- | ------------------------- | ----------- | --------- |
| accumulation_time_sec | float | accumulation period [s] | 2 | ≥0 |
| pointcloud_buffer_size | integer | buffer size | 50 | ≥0 |
| Name | Type | Description | Default | Range |
| --------------- | --------- | ------------------------------------------------------------------------------------------------------------------------ | ----------- | --------- |
| min_neighbors | integer | If points in the circle centered on reference point is less than min_neighbors, a reference point is judged as outlier | 5 | ≥0 |
| search_radius | float | Searching number of points included in search_radius | 0.2 | ≥0 |
| Name | Type | Description | Default | Range |
| ------------ | --------- | --------------------------------- | ----------- | --------- |
| sample_num | integer | number of indices to be sampled | 1500 | ≥0 |
| Name | Type | Description | Default | Range |
| ------------------------------- | --------- | ------------------------------------------------------------------------------------------------------------------------------- | ----------- | ----------------- |
| distance_ratio | float | distance_ratio | 1.03 | ≥0.0 |
| object_length_threshold | float | object_length_threshold | 0.05 | ≥0.0 |
| max_rings_num | integer | max_rings_num | 128 | ≥1 |
| max_points_num_per_ring | integer | Set this value large enough such that HFoV / resolution < max_points_num_per_ring | 4000 | ≥0 |
| publish_outlier_pointcloud | boolean | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. | false | N/A |
| min_azimuth_deg | float | The left limit of azimuth for visibility score calculation | 0.0 | ≥0.0 |
| max_azimuth_deg | float | The right limit of azimuth for visibility score calculation | 360.0 | ≥0.0 ≤360.0 |
| max_distance | float | The limit distance for visibility score calculation | 12.0 | ≥0.0 |
| vertical_bins | integer | The number of vertical bin for visibility histogram | 128 | ≥1 |
| horizontal_bins | integer | The number of horizontal bin for visibility histogram | 36 | ≥1 |
| noise_threshold | integer | The threshold value for distinguishing noise from valid points in the frequency image | 2 | ≥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 |
| Name | Type | Description | Default | Range |
| --------------------------------------------- | --------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------ | ------------------- |
| timeout_sec | float | Tolerance of time to publish the next pointcloud [s]. When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | 0.033 | ≥0 |
| input_offset | array | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers. | [] | N/A |
| input_twist_topic_type | string | Topic type for twist. Currently supports 'twist' or 'odom'. | twist | ['twist', 'odom'] |
| output_frame | string | Output frame. | base_link | N/A |
| keep_input_frame_in_synchronized_pointcloud | boolean | Flag to indicate if input frame should be kept in synchronized point cloud. | True | N/A |
| input_topics | array | List of input topics. | [] | N/A |
| synchronized_pointcloud_postfix | string | Postfix for the synchronized point cloud. | pointcloud | N/A |
| max_queue_size | integer | Max queue size of input/output topics. | 5 | ≥1 |
| Name | Type | Description | Default | Range |
| -------------- | --------- | ----------------------------- | ------------------------------- | --------- |
| polygon_type | string | polygon type to be filtered | no_obstacle_segmentation_area | N/A |
| use_z_filter | boolean | use z value for filtering | false | N/A |
| z_threshold | float | z threshold for filtering | 0.0 | N/A |
| Name | Type | Description | Default | Range |
| -------------- | -------- | --------------------------------- | ----------- | --------- |
| voxel_size_x | float | the voxel size along x-axis [m] | 0.3 | ≥0 |
| voxel_size_y | float | the voxel size along y-axis [m] | 0.3 | ≥0 |
| voxel_size_z | float | the voxel size along z-axis [m] | 0.1 | ≥0 |
| Name | Type | Description | Default | Range |
| ------------------------ | --------- | -------------------------------------------- | ----------- | --------- |
| voxel_size_x | float | the voxel size along x-axis [m] | 0.3 | ≥0 |
| voxel_size_y | float | the voxel size along y-axis [m] | 0.3 | ≥0 |
| voxel_size_z | float | the voxel size along z-axis [m] | 0.1 | ≥0 |
| voxel_points_threshold | integer | the minimum number of points in each voxel | 2 | ≥1 |
Assumptions / Known limits#
autoware::pointcloud_preprocessor::Filter is implemented based on pcl_perception [1] because
of this issue.
Measuring the performance#
In Autoware, point cloud data from each LiDAR sensor undergoes preprocessing in the sensing pipeline before being input into the perception pipeline. The preprocessing stages are illustrated in the diagram below:
Each stage in the pipeline incurs a processing delay. Mostly, we've used ros2 topic delay /topic_name to measure
the time between the message header and the current time. This approach works well for small-sized messages. However,
when dealing with large point cloud messages, this method introduces an additional delay. This is primarily because
accessing these large point cloud messages externally impacts the pipeline's performance.
Our sensing/perception nodes are designed to run within composable node containers, leveraging intra-process communication. External subscriptions to these messages (like using ros2 topic delay or rviz2) impose extra delays and can even slow down the pipeline by subscribing externally. Therefore, these measurements will not be accurate.
To mitigate this issue, we've adopted a method where each node in the pipeline reports its pipeline latency time. This approach ensures the integrity of intra-process communication and provides a more accurate measure of delays in the pipeline.
Benchmarking The Pipeline#
The nodes within the pipeline report the pipeline latency time, indicating the duration from the sensor driver's pointcloud output to the node's output. This data is crucial for assessing the pipeline's health and efficiency.
When running Autoware, you can monitor the pipeline latency times for each node in the pipeline by subscribing to the following ROS 2 topics:
/sensing/lidar/LidarX/crop_box_filter_self/debug/pipeline_latency_ms/sensing/lidar/LidarX/crop_box_filter_mirror/debug/pipeline_latency_ms/sensing/lidar/LidarX/distortion_corrector/debug/pipeline_latency_ms/sensing/lidar/LidarX/ring_outlier_filter/debug/pipeline_latency_ms/sensing/lidar/concatenate_data_synchronizer/debug/sensing/lidar/LidarX/pointcloud/pipeline_latency_ms
These topics provide the pipeline latency times, giving insights into the delays at various stages of the pipeline from the sensor output of LidarX to each subsequent node.
(Optional) Error detection and handling#
(Optional) Performance characterization#
References/External links#
[1] https://github.com/ros-perception/perception_pcl/blob/ros2/pcl_ros/src/pcl_ros/filters/filter.cpp