Skip to content

[feat] Add moving objects handling

Julia Sanchez requested to merge MovingKpts into master

Changes

  • Refactor to have a custom inner voxel grid to downsample the maps (one point/inner voxel).
    • New downsampling modes were added to select the remaining point in each voxel (see first commit description).
  • Add update modes for the maps (map never modified / map expanded but initial points not modified / map can be entirely updated).
  • Add a time of life for some keypoints in the map to handle low frequency moving object (changes in the scenes) that should be used to localize on a short period of time.
  • Add a confidence threshold (MinFramesPerVoxel) to extract the points from maps insuring they are not laying on a high frequency moving object (people/car/bike). A counter is added to the inner voxels and is updated when keypoints are added to the map.
  • [PV] Add the possibility to visualize the submaps in PV in the maps output to observe the moving objects removal
    • As it is not possible to add more filter outputs, the same port is used and a list button allows to choose which maps to display.
  • [ROS] Add a submap output in ROS (rviz config is updated)

Results and limits

  • Localization time is slightly reduced :

    • Less points are extracted
    • The inner voxels are not redefined between scan inputs
  • This cleans quite well the map. Here is a before/after example (min_frames_per_voxel = 5 frames)

comparison

  • /!\ A lot of useful points are removed in front of the LiDAR because of the confidence threshold constraint. Indeed, these areas have not been explored yet. This leads to some failure cases mostly in narrow corridors:

fail

  • Maybe this needs more heuristics to be used safely :

    • A distance threshold not to apply the filtering to far points where moving objects are less problematic ?
  • /!\ Other parameters are linked with the MinFramesPerVoxel threshold

    • The motion to define a keyframe. As the voxel counters are updated when adding the points to the map, all other frames are not taken into account so when updating this motion threshold, the min number of frames per voxel has to be updated too... If we want to update the counter for each new input frame, the time gained with the keyframe feature will be lost and more refactoring would be needed. What do you think?
    • The leaf size. If the leaf size is reduced, more points are added to the map but less points are extracted in the submap because the frames threshold is stricter. Maybe the user should put a density of points which will be converted in points number for the different leaf sizes...?

Other ideas

  • If the thresholds are not sufficient (for time of life + for the min frames per voxel), we may add a soft constraint as a weight in the optimization.

  • A time threshold may seem more intuitive to parameterize rather than the min frames per voxels (minimal time of life of a voxel to add its keypoint to the submap). However, we cannot ensure the visibility time was continuous and so it may lead to too many moving objects in a dynamic environment as soon as 2 frames have access to one voxel visibility.

@nick.laurenson PTAL

Edited by Julia Sanchez

Merge request reports