LIMO-Velo

Contact me at andreuhuguet@gmail.com for questions or ideas.

News! 🎉

• Carlos is working on LIMOncello that improves the Lie Algebra of the Kalman Filter, is written in functional code and has thread safety just like Fast-LIMO. Better, safer and simpler!
• Oriol just released Fast-LIMO - a thread-safe and fast implementation of LIMO-Velo!
• Now with a ROS2 branch, ros2 - thanks to bertaveira. Broader LiDARs support is needed.
• bertaveira is also working on a LIMO-Velo++ with python bindings. Check out his work in progress!

A real-time LiDAR-Inertial SLAM for high velocities in spinning LiDARs.

<p align="center"> <img src="./config/docs/img/Localization.gif" alt="Perfomance of the algorithm" /><br /> Visualization of the algorithm with <code>delta = 0.01</code> (100Hz) </p>

Designed for easy modifying via modular and easy to understand code. Relying upon HKU-Mars's IKFoM and ikd-Tree open-source libraries. Based also on their FAST_LIO2.

Tested and made for racing at Formula Student Driverless

Common working speeds are 20m/s in straights and 100deg/s in the turns.

<p align="center"> <img src="./config/docs/img/xaloc.gif" alt="Perfomance of the algorithm" /><br /> Tested on and made for Barcelona's own "<a href="https://www.youtube.com/watch?v=ly_ax8w-T7E&feature=emb_logo">Xaloc</a>". </p>

Centimeter-level accuracy is kept under racing speeds

Only algorithm that can deliver centimeter-level resolution on real-time. See the part of my thesis where I explain the algorithm and its results: LIMOVelo + Results.

<p align="center"> <img src="./config/docs/img/cones-comparison.png" alt="Map comparison - Cones" /><br /> Comparison of cones under racing speeds running all algorithms in real-time, except for LIO-SAM (-r 0.5). It failed otherwise. </p>

Designed to be easily understood even by beginners

Developing an algorithm for a team requires the algorithm to be easy enough to understand being passed through generations.

<p align="center"> <img src="./config/docs/img/pipeline.png" alt="Map comparison - Cones" /><br /> LIMO-Velo's pipeline. Here are seen the different modules (blue), data (orange) and libraries (dark green). </p>

LiDARs supported

• Velodyne
• Hesai
• Ouster
• Livox (check livox git branch)

Dependencies

• Ubuntu (tested on 18.04, 20.04)
• ROS (tested on Melodic, Noetic)
• Eigen
• PCL (tested on 1.8)

Newest additions

High Quality Maps

Sometimes a higher map quality is needed, now a new high_quality_publish parameter has been added to yield results like this below.

<p align="center"> <img src="./config/docs/img/high-quality-cones.png" alt="High quality cones" /><br /> Sometimes Xaloc needs more definition to see if a cluster of points is actually a cone. </p>

Work in progress (branch hdmaps)

<p align="center"> <img src="./config/docs/img/prelocalization-hdmaps.gif" alt="Prelocalizing with a given HD map" /><br /> Prelocalization with a previoulsy saved HD map. Still work in progress on the <code>hdmaps</code> branch. Official release will be in a couple of days. </p>

Sample datasets

<p align="center"> <img src="./config/docs/img/rosbag-xaloc.gif" alt="Xaloc's fast dataset" /><br /> Xaloc's "fast" dataset. High velocity in the straights (~15m/s) and tight turns (~80deg/s). </p>

Try xaloc.launch with Xaloc's own rosbags.

• :checkered_flag: Find a slow (818MB) and a fast (1.71GB) run in this Dropbox folder.

See Issue #10 to see other sample datasets found in the web.
Don't hesitate to ask there for more data on specific scenarios/cases.

Using LIMO-Velo

0. Cloning the repository

When cloning the repository, we also need to clone the IKFoM and ikd-Tree submodules. Hence we will use the --recurse-submodules tag.

git clone --recurse-submodules https://github.com/Huguet57/LIMO-Velo.git

1. Compiling the code

We either can do catkin_make or catkin build to compile the code. By default it will compile it optimized already

2. Running LIMO-Velo

To run LIMO-Velo, we can run the launch file roslaunch limovelo test.launch if we want a visualization or roslaunch limovelo run.launch if we want it without.

2.1 Debugging LIMO-Velo

An additional launch file roslaunch limovelo debug.launch is added that uses Valgrind as a analysing tool to check for leaks and offers detailed anaylsis of program crashes.

3. Changing parameters

To adapt LIMO-Velo to our own hardware infrastructure, a YAML file config/params.yaml is available and we need to change it to our own topic names and sensor specs.

Relevant parameters are:

• real_time if you want to get real time experience.
• mapping_offline is on an pre-alpha stage and it does not work 100% as it should of.
• initialization which you can choose how you want the initialization of the pointcloud sizes (sizes =: deltas, in seconds).

4. Modifying the LiDAR driver to get true real-time performance

TODO - This section is intended to explain how to modify the LiDAR driver to increase its frequency by publishing parts of the pointcloud instead of waiting for all of it.

References

• IKFoM: Iterated Kalman Filters on Manifolds
• ikd-Tree: Incremental KD-Tree for Robotic Applications
• FAST-LIO2: Fast and Direct LIO SLAM

TODO list

Fixes in progress

• Work with 9-DOF IMUs
• Saving and loading HD-Maps (needs 9-DOF done)
• Adding GPS as extra input
• Rethink mapping_offline (see Discussions)

Design choices

• Renew Buffer private structure. Interesting answer in StackOverflow: https://stackoverflow.com/a/67236232
• Simplify the upsampling in the Compensator.

Fixes to investigate

• Interpolation and smoothing of states when mapping offline
• Erase unused (potentially dangerous) points in the map
• Check if need to add point in map
• Try to add a module for removing dynamic objects such as people or vehicles
• Use UKF instead of EKF
• Add vision buffer and ability to paint the map's points
• Initialize IMU measurements