[ROS2][feat] Improve obstacle clustering

Goal

In the previous MR !428 (merged), extraction and clustering of obstacles are added into aggregation node. The goal of this MR is to improve some edge cases when objects overlap.

• Cluster merging problem: Previously, when a moving object reaches a static object, the two clusters will be merged in the current version which is not expected.

Changes

• To improve the cluster merging problem, we add a step to compute the centroid of the two clusters before merging them.
• To improve the time decay behavior, a spatial threshold (DistanceFOV) is added to forget old clusters only if they are in the defined FOV (DistanceFOV). It avoids to forget the important static objects when the sensor goes far away from them.
• To improve the cluster separation problem, we check if a cluster has a large time standard deviation. If so, the old parts which split the cluster are removed and various subclusters are recreated.

Results

Reproduce

Dataset 1

• reference maps: kitware_office
• data: kitware_office2

Dataset 2

The same as in the MR !428 (merged)

Build

colcon build --base-paths slam/ros2_wrapping --cmake-args -DCMAKE_BUILD_TYPE=Release

Parameters

Parameters files for dataset 1 link

Parameters files for dataset 2 link

Run

ros2 launch lidar_slam slam_velodyne.py aggregate:=true

ros2 bag play --clock --start-offset 35 -d 2 'data/path/to/bag/zig-zag-between-rigs'

Output

Please find a demo of dataset 1 here

Please find results of dataset 2:

Tackle the cluster merging problem

The top image represents the result before changes in this MR. The bottom image represents the result of this MR to forbid the merging of 2 big clusters which should represent two different objects

Tackle the cluster separation problem

The top image represents the result after forbidding the merge of 2 big clusters. The bottom image represents the result to separate a cluster to several sub clusters. A demo of this step is here.

Improve time decay

The left image represents the result before changes in this MR. The right image represents the result of this MR.

Performances (optional)

• The overall SLAM process before (on master) was taking 10s
• The overall process takes 9s

Checklist

• Camel case everywhere except for ROS variables/parameters
• Lower case for local variables and lambda functions
• Upper case for members, methods and tool functions (in Utils)
• Precise namespace when calling a function (or this->X or classe.X)
• Align code (for multiline if and while, "&&" or "||" go in upper line to ensure alignement)
• Check your spaces
  • between if, while, for and parenthesis
  • between operators and variables: e.g. a + b
  • after ","
• Mind your commit titles/desc (plurals, he/she + "s", correct tags, title should begin by a verb...)
• Function names should start with a verb, variable names should start with a name
• Macros should be between {}
• Do not use negative boolean (i.e. noJoe)
• Check minimal size of the types (double -> float -> int -> uint)
• Check const and ref in functions arguments
• References should be written "type& name", not "type &name"
• Update documentation
• Add MR labels [ROS]/[ROS2]/[PV]
• If ros/ros2, update task table here
• Add a comment over each non trivial function in header files
• Add a header to each new file