Autoware.Auto


Autoware.Auto requires a method to distinguish ground points from nonground points in point clouds. This is meant to act as a filtering step for object detection algorithms (working on nonground points), and free space estimation algorithms (working on ground points). This method is very fast and allows the filtered point cloud to be exposed while the packets for a scan are still coming in.
Concretely, the original implementation of the ray ground filter batches points in a pointcloud according to azimuth slice (that is, the angle between x and y in the vehicle frame). These are typically binned into angular segments of 0.1 degrees rad. This roughly corresponds to one firing of the VLP16's.
Once the point cloud is restructured into rays, each point in a ray is annotated with the planar distance (euclidean distance on xy place only, again in vehicle frame). Points with height too high, or radius too low are ignored. The ray is then sorted in order of increasing radial distance.
Once the rays are ordered, the algorithm scans through the points in the ray in order of increasing radial distance. The algorithm defines two cones: a global cone and a local cone, where the local cone has a wider aperture than the global cone. The global cone is rooted at the point cloud's footprint (e.g. projected down to the ground plane). The local cone is rooted at the last point processed.
The ray ground filter has two main state variables: the last point processed, and whether the last point was labeled as a ground point. These variables are initialized as the root of the global cone and false (not ground) initially. For each subsequent point, the following logic is then used to label the point:
The first branch gives near points (in the z sense) the same label. The second branch provides a mechanism to switch back to ground points. In the last branch, the current point is already outside of the local cone (in the z sense), so it must first be nonlocal (in the r sense) before it can be checked against the global cone.
A new point structure was added. This was added to precompute the projected radius of a point, and save a little computational work during the processing of a ray (i.e. labeling, sorting).
Inputs:
A block of points with at least the following fields:
x
y
z
ray id
Such that all points with the same ray are contiguous. Within a single block, ray id
may not repeat in non contiguous sets.
Outputs:
Two output blocks of points (one for ground, and one for not ground) with at least the following fields:
x
y
z
ray id
Additionally, streams are delineated by uninitialized points with the spatial ray id
velodyne::END OF SCAN ID
.
For more details, see the API documentation.
The above stated algorithm was modified to work directly on the point stream coming from the device and parsed by the (Velodyne) driver. Autoware.Auto does the following:
ray id
at the driver level according to its position in the LiDAR data packetray id
of a new point is different from the ID currently being tracked, the buffer of points that make of the current ray is sorted, and passed through the above logic in radial order, or in increasing height in the case of equal radial distancestd::partial_sort
is used for sortingrather than std::sort
or std::stable_sort
even though it is less performant because it is not recursiveIn addition, to make classification of distant points more robust, a limit is added to the global height threshold. This was added to prevent the global height threshold from growing unbounded with increasing distance, resulting in most, if not all distant points to be classified as ground.
Additional rules were also added to improve the performance of the algorithm:
The time complexity of filtering a single ray is O(k + k log(k) + k)
for inserting and calculating radius, sorting by radius, and then scanning through points to label as ground or nonground.
The operation is thus O(k log k)
. k
is the size of a laser scan ray, which in this implementation is a hardware defined constant.
Filtering an entire point cloud of size n
consequently takes n log k
time.
The space complexity for this module is O(k)=O(1)
, where k
is the size of a laser scan ray, which is a hardware defined constant.
The core filter has three floating point state variables fully determined by the last point processed:
last_point_ground
last_point_height
last_point_radius
The following configuration state is introduced by the configuration class, Config.
The main potential failure modes are due to undefined behavior from std::partial_sort
, e.g. if the comparator is semantically incorrect, and indexing outside of an array. The former should be correct, and there are checks to prevent the latter.
Additionally, the RayGroundClassifier internally makes use of a pointer during a function call, and an array internally for storage. These two elements bring the potential of errors, though accessing these elements is guarded by proper checks.
Next, the primary invariant of the algorithm is that the core labeling function requires points to come in order of increasing radial distance. The list is sorted by increasing height if radial distance is approximately the same. This invariant is checked to not be broken at the beginning of the core labeling method. It is worth noting that there is some threshold to compensate for floating point error. The radial distance is allowed to shrink by as much as [FEPS](autoware::common::types::FEPS) in order to handle the case when radial distance is approximately equal. While this allows for an invariant to be broken, in practice this should not cause issues, because if the maximum recession occurs for 1000 points, the net recession would be no more than 1 mm, which is negligible, considering the VLP16HiRes' precision is no greater than 1 cm.
If configuration parameters are semantically incorrect with respect to one another, then an error will be thrown during construction of the configuration class.
TBD by security expert.
The design of the ray ground filter is based off an implementation provided in CPFL's Autoware
Correction + estimation using global groundplane estimate from the last frame.