[ROS2][feat] Integrate loop closure into ros2 wrapping

Goal

Integrate loop closure into ros2 wrapping. The changes are for ros2_wrapping

Changes

• Add function ReadLoopIndices to read loop closure indices from an external csv file
• Add command LOAD_LOOP_INDICES to load loop closure indices and add 'Load loop indices' button in rviz
• Make OptimizeGraph take loop closure constraint into account
• Add PGO constraint parameters for pose graph
• Add buttons 'Optimize graph' in rviz
• Add command RESET_TRAJECTORY to reload a slam trajectory and add Reset trajectory button in rviz

Results

Reproduce

Data

car(hesai)

Build

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

Launch file

• Use `slam_velodyne.py' but comment velodyne points conversion part.
• change "velodyne" to "hesai" in tf_base_to_velo_node

Parameters

• change the parameters:
• loop_closure.enable_offset: true
• n_threads: 4
• distance_threshold: 0.1
• leaf_size.edges: 0.1
• leaf_size.planes: 0.1
• decaying_threshold: 900
• min_frames_per_voxel: 0
• voxel_grid_resolution: 1.
• neighbors_radius: 0.05
• edge_depth_gap_threshold: 2.
• edge_nb_gap_points: 20

Run

ros2 run lidar_conversions generic_conversion_node \
ros2 launch ros2 launch lidar_slam slam_velodyne.py \
ros2 bag play --clock path/to/bag --remap /hesai/pandar:=/generic_points

External sensors and PGO information will display when launching lidar_slam node:

Result

Demo available here.

An example of pose graph optimization with loop closure:

Befor PGO:

After PGO:

TODO

• Save trajectory with index, add a CheckCSVField function in another MR
• Test with a bag of loop closure that change after optimization is more visible
• Implementation in ros_wrapping
• Rebase on feat/ROS2 after MR!318 merged
• Update documentation
• make a demo