Fast LIMO

Table of Contents

1. Disclaimer
2. Dependencies
3. Future Work
4. Quick Start
5. Approach
6. Docker
7. Configuration
8. References

A real-time, tightly coupled LiDAR-Inertial SLAM algorithm developed on top of IKFoM and ikd-Tree C++ libraries. This project's implementation is based on the existing algorithms FASTLIO2, LIMO-Velo and DLIO.

Fast-LIMO has been developed as a thread-safe C++ library with Eigen3 and PCL as its only dependencies. This way, it can be used outside the ROS framework without any changes, making it more portable. This project acts as a ROS wrapper of the self-developed fast_limo's library.

Fast-LIMO stands for a multithreaded version of the approach Localize Intensively Map Offline (LIMO) stated in LIMO-Velo 's algorithm developed by Huget57.

Formula Student race car CAT15X. Velocity in straights (~12m/s) and really tight turns (~100deg/s).

KITTI dataset (0071). Dynamic objects being added to the map. Still robust enough.

Formula Student race car XALOC. High velocity in straights (~15m/s) and tight turns (~80deg/s).

KITTI dataset (0034). High velocity (~20m/s), smooth turns (~35deg/s).

ONA robot. Double-ackermann autonomous delivery device.

Disclaimer

If you plan to use Fast-LIMO please make sure to give some love to LIMO-Velo, FASTLIO2 and DLIO projects, which greatly influenced this work.

Dependencies

Fast-LIMO C++14 library:

1. Eigen3
2. PCL (1.8)

ROS (Noetic) wrapper:

1. fast_limo
2. pcl_ros
3. sensor_msgs
4. geometry_msgs
5. tf2
6. visualization_msgs
7. nav_msgs

ROS2 (Humble) wrapper:

1. fast_limo
2. pcl_conversions
3. sensor_msgs
4. geometry_msgs
5. tf2
6. tf2_ros
7. visualization_msgs
8. nav_msgs

Future Work (To Do)

DevOps

• ROS2 branch.

New Features

• Take into account GPS data. Pose-graph optimization using GTSAM
• Add loop closure strategy. Loop Closure detection with ScanContext.
• Relocalize in previously saved pcl map. Probably ICP-based correction for initial pose.

📩 Feel free to reach out for new ideas or questions! 📩

Quick Start

0. Cloning the repo

git clone https://github.com/fetty31/fast_LIMO

More than half of the storage space needed to clone this repo is due to the README gifs. There's a branch updated with the master but without all this media files called no_media.

1. Building fast LIMO

Use default catkin_make or catkin build to build the code. By default it will compile under the CMAKE_BUILD_TYPE="Release" flag.

For ros2-humble branch, use default colcon build.

2. Running fast LIMO

roslaunch fast_limo fast_limo.launch

Afterwards, you should be seeing this output (if verbose param is set to true):

You can also run Fast-LIMO together with an rviz instance with:

roslaunch fast_limo fast_limo.launch rviz:=true

4. Quickly check its performance

IN THIS FOLDER you can find the rosbag file (850.41 MB) of this CAT15X trackdrive. Download it and try it out!

roslaunch fast_limo cat.launch rviz:=true

Note that this algorithm's precision greatly depends on the pointcloud & IMU timestamps, so remember to run the rosbag with use_sim_time=true and --clock flag.

5. Loop Closure

If you're interested in having loop closure for long-term odometry drift correction checkout to the loop/scancontext branch.

Apart from the steps 1-4, the looper node has to be launched:

roslaunch fast_limo loop.launch robot:=cat

Docker

A Dockerfile is provided in order to build a Fast-LIMO image on top of ros2-humble or ros-noetic desktop image.

Note that some bash scripts are given in order to quickly build & run the ROS docker container (also enabling GUI applications).

cd docker/
chmod +x build run
./build # build fastlimo docker image
./run   # deploy a fastlimo container

Finally, once inside the docker container run:

# ROS2 Humble
cd /home/colcon_ws/
colcon build --symlink-install

# ROS Noetic
cd /home/catkin_ws/
catkin_make

Approach

If you are interested in truly understanding the working principle of this SLAM algorithm, please read the FASTLIO paper. This project is merely an alternative implementation of this outstanding work, still relying upon IKFoM and ikd-Tree open-source projects.

This project implements the same concept as LIMO-Velo but without any accumulation procedure. Instead, Fast-LIMO operates with two concurrent threads. One thread handles the propagation of newly received IMU measurements through the iKFoM (prediction stage), while the other thread uses these propagated states to deskew the received point cloud, match the deskewed scan to the map, and update the iKFoM (measurement stage) by minimizing point-to-plane distances.

Fast-LIMO supports the standard IMU-LiDAR configuration, where the IMU provides new measurements at a rate of 100-500 Hz and the LiDAR sends a new point cloud approximately at 10 Hz. However, Fast-LIMO has been developed with the purpose to be used with a modified LiDAR driver capable of sending each scan packet as soon as it is ready, instead of waiting for the LiDAR to complete a full rotation.

NOTE: for a modified Velodyne driver, check this fork.

Configuration

Here, the configuration file for Fast-LIMO is explained. Note that some parameters relate to the sensor type and extrinsics. The remaining parameters generally do not require modification for standard use, as they are associated with computational load limits or precision thresholds.

Parameter	Units	Summary
num_threads	-	OpenMP threads (will be equal to $(nproc) if it is set higher).
sensor_type	-	LiDAR type (0: OUSTER \ 1: VELODYNE \ 2: HESAI \ 3: LIVOX).
debug	-	Save useful intermediate pcl for visualization purposes (pc2match, deskewed...).
verbose	-	Print debugging board with performance stats.
estimate_extrinsics	-	Enable continuous estimation of LiDAR-IMU extrinsics as in FASTLIOv2.
time_offset	-	Whether to take into account a possible sync offset between IMU and LiDAR (set to true if they are not properly synchronized).
end_of_sweep	-	Whether the sweep reference time is w.r.t. the start or the end of the scan (only applies to VELODYNE/OUSTER sensors, which have relative timestamp).
calibration/gravity_align	-	Estimate gravity vector while the robot is at stand still.
calibration/accel	-	Estimate linear accel. bias while the robot is at stand still.
calibration/gyro	-	Estimate angular velocity while the robot is at stand still.
calibration/time	s	Time at the beggining to estimate the quantities above. The robot MUST be at stand still. If all three flags above are set to false, no estimate will be done.
extrinsics/imu	SI	IMU pose with respect to base_link coordinate frame.
extrinsics/lidar	SI	LiDAR pose with respect to base_link coord. frame.
intrinsics	SI	IMU lin. acc. + gyro. biases.
filters/cropBox	m	Prismatic crop. Useful for removing LiDAR points that fall into the robot itself. Should be a 3D rectangle envolving the robot.
filters/voxelGrid	m	Voxel Grid filter. Pointcloud downsampling strategy.
filters/minDistance	m	Could be interpreted as a sphere crop. Removes all points closer than its value. Useful for avoiding having too many ground points.
filters/FoV	deg	Field of View (FoV) crop filter. Removes all points that are not inside the given angle. Useful for reducing the number of points to account for.
filters/rateSampling	-	Quick downsampling method. Only takes into account 1 in every value points. Useful for reducing the computational load.
iKFoM/MAX_NUM_ITERS	-	The Extended Kalman Filter will do MAX_NUM_ITERS+1 iterations. Useful for reducing the computational load.
iKFoM/MAX_NUM_MATCHES	-	Max. number of matches to account for when computing the Jacobians. Useful for reducing the computational load.
iKFoM/MAX_NUM_PC2MATCH	-	Max. number of points to consider when matching the current scan with the map. Useful for reducing the computational load.
iKFoM/Mapping/NUM_MATCH_POINTS	-	Number of points that constitute a match.
iKFoM/Mapping/MAX_DIST_PLANE	m	Max. distance between all points of a match.
iKFoM/Mapping/PLANES_THRESHOLD	m	Threshold to consider if a match point is part of a plane.
iKFoM/Mapping/LocalMapping	-	Whether to keep a fixed size map (moving with the robot) or a limitless map. Useful for limiting the memory used (in long runs).
iKFoM/iKDTree/balance	-	Incremental KD Tree balancing param.
iKFoM/iKDTree/delete	-	Incremental KD Tree deletion param.
iKFoM/iKDTree/voxel	-	Incremental KD Tree downsampling param. Currently not used
iKFoM/iKDTree/bb_size	m	Local Map's bounding box dimension (actually a cube). When LocalMapping is active, the local map won't include points outside this cube. Note that the local map is always defined relative to the robot's current position.
iKFoM/iKDTree/bb_range	m	Local Map's bounding box moving range (if the robot is closer than bb_range to any local map's edge, the map will "move").
iKFoM/covariance	m^2	Covariance of IMU measurements.

References

This project relies upon HKU-Mars' open-source C++ libraries:

• Iterative Kalman Filters on Manifolds (IKFoM)
• Incremental KD-Tree (ikd-Tree)