Localization (built on top of #428)

[akevinge]

DO NOT SUBMIT. Built off of #428. This contains the code for global localization using SAC-IA. Needs to be run with an additional repo on the dev branch of Navigator. Deleted the rest of Navigator files so colcon builds the proper version of PCL and dependents. A copy of README.md is provided below.

Findings

• KISS ICP + MapClosures produces a map that drifts. KISS-ICP is unable to reconcile drift that occurs during turns and map closures cannot seem to pull together the scan. This problem may be solved with more frequent scans with contiguous scans that are more closely aligned.
• GLIM also produces a map that drifts (even more than KISS-ICP + MapClosures) despite accounting for additional IMU data via a factor graph. It also warns that lidar / imu contigous measurements are too far apart in terms of time, suggesting that scans are not frequent enough. GLIM attempts to bridge this gap with a factor in the graph but to no avail. One thing we should try is setting the correct covariance values for the IMU and Lidar in the GLIM configuration to test if that fixes the problem, but the map drifts so significantly that I doubt it.
• This code implements SAC-IA alignment (which is built on top of RANSAC alignment) to attempt to initially globally localize on the offline map. It loads a folder of *.ply files and a file with poses that align these individual local maps. The maps that were tested on were produced by KISS-ICP and MapClosures on the same bag used to test live Lidar scan alignment, so in theory the alignments should be near perfect. This was not the case however, with fitness scores of 5-20 being produced (good alignments should be ~0).

Recommendations:

• Check the frequency on the raw lidar and IMU points /ouster/points and /ouster/imu. Ideally, the frequency should be high AND consistent. I recommend making this a priority because running Ouster samples with KISS-ICP suggests that KISS-ICP SHOULD be able to handle turns. Try getting a ROS bag of a HDR LiDAR from a public dataset and running it on our KISS-ICP pipeline.
• Try converting our bags to a OusterSDK-compatible input file and running their SLAM pipeline (see resources section below).
• Try GLIM with covariance values of the IMU and Lidar. Consider implementing a filter or factor graph to stabilize IMU values.
• Try producing a non-drift map on a straight road using KISS-ICP + MapClosures and running localization on the same bag to test with SAC-IA alignment works (it should).
• I highly discourage developing SLAM/Localization in Carla because conditions with the real Lidar are likely significantly different than those in simulation.
• Look into factor graphs to improve offline SLAM and consider using initial alignment (such as SAC-IA) and ICP to do live localization.
• Use the Dockerfile in this repo when considering developing with PCL because the Naviagtor PCL is on v1.12.1 with some major segmentation fault issues and it's dependency hell to update it.

Resources

• Lidar odometry smoothing using ES EKF and KissICP for Ouster sensors with IMUs
• Factor Graphs and GTSAM
• Factor Graphs and GTSAM: A Hands-on Introduction
• HDL Localization with FPFH and RANSAC
• GLIM
• Run OusterSDK SLAM (uses KISS-ICP)
• Mapping with G2O or GTSAM
• SLAM Toolbox
• FAST-LOCALIZATION (relocalization based on FAST-LIO)
• Offline Mapping with LIO-SAM
• Localization with NDT
• Mapping with NDT + GTSAM Loop Closures
• Localization Using NDT

How-To-Run Instructions

Navigator KISS-ICP

In the root navigator directory:

docker compose run navigator_carla
# Inside the docker container
python3 src/mapping/offline_lidar_slam/offline_lidar_slam.py <bag_path> <topic_name> <save_dir>
# To view the map you can run (or you can run localization_node with visualize_global_map:=true later)
python3 src/mapping/offline_lidar_slam/view_map.py <save_dir>

Navigator Localization

In this directory:

docker compose run navigator_localization
# Inside the docker container
colcon build --symlink-install --packages-select localization_cc
. install/setup.bash
# Running for the first time (without existing FPFH features for the map)
# This will create a point cloud of FPFH features for the global map at
# global_map_features.pcd inside this directory.
ros2 run localization_cc localization_node \
  --ros-args \
    --log-level info \
    -p visualize_global_map:=true \
    -p local_maps_directory:=<save_dir>/local_maps \
    -p local_maps_pose_file:=<save_dir>/optimized_poses.txt

# Re-running with FPFH features
ros2 run localization_cc localization_node \
  --ros-args \
    --log-level info \
    -p visualize_global_map:=true \
    -p local_maps_directory:=<save_dir>/local_maps \
    -p local_maps_pose_file:=<save_dir>/optimized_poses.txt \
    -p fpfh_features_file:=/navigator/global_map_features.pcd

# Optional parameters to adjust SAC-IA alignment
-p max_correspondence_distance:=1.0 \
-p similarity_threshold:=0.5 \
-p inlier_fraction:= 0.25 \
-p max_iterations:=10000 \
-p num_samples:=3 \
-p correspondence_randomness:=2

GLIM

Run anywhere

docker run \
  -it \
  --rm \
  --net=host \
  --ipc=host \
  --pid=host \
  -e=DISPLAY \
  -e=ROS_DOMAIN_ID \
  -v $(realpath config):/glim/config \
  koide3/glim_ros2:humble \
  ros2 run glim_ros glim_rosnode --ros-args -p config_path:=/glim/config

Then run your ROS bag (ensure you're on the same ROS_DOMAIN_ID).

docker compose run navigator_carla
# Inside the docker container
ros2 bag play <bag_path>