Remove dynamic objects from the point cloud passed to localization
Description
This is stemming from #942 (comment 572886370). Currently, the point clouds get sent to the localization (currently NDT) module without modification. That means that some part of those point clouds corresponds to dynamic points and while localization can be robust to these "outliers" it would be best if these points would not be taken into account when performing localization.
Desired Behavior
I see two ways of doing this:
- Have a node that cleans up the incoming point cloud by subscribing to the LiDAR point cloud, the tracks from the tracker and outputs a new point cloud without the points that fall into the tracks.
- Have a library that can be used in the localization framework to do this. This avoids copying the data but is less generic.
Additional consideration should be taken about the runtime.
-
A naive way to perform this filtering is to find for every point of the point cloud if it fits into any track, which is an
O(n * m)
operation withn
- number of points in the point cloud andm
number of tracks. -
An alternative way is to perform these checks on a 2D grid. The gist of the method is the following:
- store the pointers to the 3D points within the 2D grid. All the points whose XY coordinate falls within such a cell is stored there.
- for each track, "walk" over all 2D cells within the shape of this tracked object and
clear()
the corresponding cells
The complexity of this solution is
O(n + k * m)
assuming all track shapes are of reasonable small size bound by a small constantk
.
Definition of Done
-
The need for such functionality is analyzed -
An issue is created to track the implementation of one of the solutions above (or an alternative one)