huguet57/limo-velo

A real-time, direct and tightly-coupled LiDAR-Inertial SLAM for high velocities with spinning LiDARs

LIMO-Velo

Contact me at [email protected] for questions or ideas.

News! 🎉

• 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.

Visualization of the algorithm with delta = 0.01 (100Hz)

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.

Tested on and made for Barcelona's own "Xaloc".

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.

Comparison of cones under racing speeds running all algorithms in real-time, except for LIO-SAM (-r 0.5). It failed otherwise.

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.

LIMO-Velo's pipeline. Here are seen the different modules (blue), data (orange) and libraries (dark green).

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.

Sometimes Xaloc needs more definition to see if a cluster of points is actually a cone.

Work in progress (branch hdmaps)

Prelocalization with a previoulsy saved HD map. Still work in progress on the hdmaps branch. Official release will be in a couple of days.

Sample datasets

Xaloc's "fast" dataset. High velocity in the straights (~15m/s) and tight turns (~80deg/s).

Try xaloc.launch with Xaloc's own rosbags.

• 🏁 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

limo-velo's Issues

livox support ros2

Does the ros2 branch support livox?

thanks

How soon the [hdmap] branch will be release?

If you read this, remember to give feedback.

If you are reading this, that means you are worried something is not working 😏. Try it yourself!

If you are not trying the algorithm, that means there something stopping you to do it 😵‍💫. Tell me what it is: here in this conversation or at [email protected].

What I need now is alpha testers, not alpha watchers! Feedback, feedback, feedback.

Feel free to ask whatever you want 🤓

• How does it work? How does it compare to Fast-LIO, LIO-SAM, LOAM...? (See my thesis extract first)
• Is your LiDAR brand unsupported? Tell me which one is it (Livox is on their way).
• Do you need rosbags to test the algorithm?
• Does a function in the code confuse you? Could it need a rewrite?
• Does LIMO-Velo fail in your setup? Open an Issue explaining the setup and showing the result!

This code is aimed to be understood from beginners to experts and made to be compatible of all types of LiDARs. Let's make it happen! 😄

[OpenMP] Colon for loop syntax fails with #pragma parallel for

I am building LIMO-Velo on my machine

Host OS: Ubuntu 18.04
CPU Architecture: x86-64
Build tool - catkin tools

The error I am getting when building from a clean workspace is,

Errors     << limovelo:make /path/to/ws/logs/limovelo/build.make.003.log                                                                                                                                                                                                                                                                                                            
/path/to/ws/src/limo-velo/src/Modules/Mapper.cpp: In member function ‘Matches Mapper::match(const State&, const Points&)’:
/path/to/ws/src/limo-velo/src/Modules/Mapper.cpp:47:26: error: expected ‘=’ before ‘:’ token
             for (Point p : points) {
                          ^
/path/to/ws/src/limo-velo/src/Modules/Mapper.cpp:53:13: error: expected ‘;’ before ‘return’
             return matches;
             ^~~~~~
/path/to/ws/src/limo-velo/src/Modules/Mapper.cpp:53:13: error: expected primary-expression before ‘return’
/path/to/ws/src/limo-velo/src/Modules/Mapper.cpp:54:9: error: expected primary-expression before ‘}’ token
         }
         ^
/path/to/ws/src/limo-velo/src/Modules/Mapper.cpp:54:9: error: expected ‘)’ before ‘}’ token
/path/to/ws/src/limo-velo/src/Modules/Mapper.cpp:54:9: error: expected primary-expression before ‘}’ token
/path/to/ws/src/limo-velo/src/Modules/Mapper.cpp:47:13: error: invalid controlling predicate
             for (Point p : points) {
             ^~~
At global scope:
cc1plus: warning: unrecognized command line option ‘-Wno-dev’
make[2]: *** [CMakeFiles/limovelo.dir/src/Modules/Mapper.cpp.o] Error 1
make[1]: *** [CMakeFiles/limovelo.dir/all] Error 2
make: *** [all] Error 2

Looks like a trivial build issue but I am not able to find the solution myself. Thank you for your help @Huguet57

Is LIMO-Velo drifting? (Parameter tuning tips)

Tips for hyper-parameter tuning

If the algorithm is drifting in your recorded bag file, follow this guide and see if it improves.

Is it drifting right away?

limovelo-drift-2022-03-30_19.35.29.mp4

This means that the initial_gravity parameter has been wrongly set. Try to change the sign of the gravity value (-9.807 -> +9.807) or check that your IMU gives the linear accelerations in m/s^2. Some, such as the livox ones, don't.

Other times, it can mean that the I_Rotation_L rotation matrix is not the identity as default. Check the xaloc.yaml parameter file as an example. Normally, IMUs have different axes than the LiDAR.

Does it drift in a degenerate scenario?

limovelo-drift-2022-03-30_19.48.39.mp4

Indoor cases

Usually in indoor places there's higher frequency details (therefore, smaller planes) that need to be captured with precision. It's recommended to decrease the downsample_rate, optimally to 1. Also, decreasing downsample_prec to lower values like 0.2 works as well.

Indoor and outdoor cases

In the case of racing, higher frequency of corrected results is strongly needed. However, it's wrong to want lower deltas (higher frequency) and more correction iterations at the same time. Higher frequency already gives more iterations, and adding more iterations with less data can lead to wrong results (and drifting).

So, low deltas (<0.3) should be met with low MAX_NUM_ITERS (1 or 2).

Also, be really cautious with the degeneracy_threshold and low deltas, since the smaller the data, the smaller the threshold will need to be. Print the values on the command line putting print_degeneracy_values to true and choose carefully values that only cross the threshold when there's a real case of degeneration.

There should always be a low degeneracy_threshold (or none) with low deltas.

Lower deltas give more frequency of results but can give more instability if the parameters are badly tuned. If high frequency of results is not needed for control, go for higher deltas (i.e. deltas: [0.1], if 0.1 is the full rotation time).

/camera_info for the Xaloc dataset

Hello, I'm building a racing stack involving lidar/visual fusion and came across the Xaloc rosbags on your Dropbox. We are not yet at the stage where physical tests are feasible, and I have been having trouble finding bags of full fsae runs, so these were pretty useful. However, through both stereo channels are available, would it be possible to also include /camera_info for the people who want to do projections and such? (or just the camera model itself)

Leaf size is too small for the input dataset

Hi, thanks for sharing the code.
I am trying the slam using a rosbag made from Velodyne VLP-16 output and IMU Vectornav 100. The rosbag seems to me to have nothing special but I get this error:
[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.
I have tried different values for downsample_prec (from 0.1 to 3) to no avail.
What could it be? Thank you.

May be we can integrate the method of <Robust and Online LiDAR-inertial Initialization.pdf> to initialize.

Getting The Map pcd file?

Hi,
thanks for your contribution to the SLAM space.
Actually i was wondering if the map can be saved? Or is this this offline mapping bug?

Moreover i wanted to ask if you have already experience with z stability of the algorithm, as many algorithms tend to struggle there which don't have any global height reference like GPS.

If i have a 9-dof imu, how can I set the init rotation?

[Velodyne@20Hz] Drift during initialization

Hi, I am having issues during initialization with my own data. As you can see in the video I attached, the robot drifts a significant amount before it starts to localize properly.

limovelo_init.mp4

I have a Velodyne-vlp16 running at 20Hz, driver configured so that each message contains a full 360º rotation with per-point timestamp, as well as a Microstrain 3dm-gx5-45 IMU running at 250Hz. Find my config below.

# Online/Offline
mapping_online: true     # set to 'true' until mapping offline fixed (see Discussions in GitHub)
real_time: false         # in a slow CPU, real_time ensures to always output to latest odometry (possibly skipping points)

# Topics
points_topic: "/hlidar_points"
imus_topic: "/microstrain/imu/data"

# Publishers
high_quality_publish: false   # true: Publishes the map without downsampling, can be slower. false: Publishes the downsampled map.  

# Extrinsics
estimate_extrinsics: false
print_extrinsics: false
initial_gravity: [0.0, 0.0, -9.807]
# Extrinsics LiDAR -> IMU
I_Translation_L: [0.03524776, -0.00758786, -0.17033461]
I_Rotation_L: [
-0.99981262, -0.01763221,  0.00799684, 
-0.01773889,  0.99975185, -0.01347297,
-0.00775726, -0.01361233, -0.99987721
]

# Delays
empty_lidar_time: 0.05      # Should be at least [FULL_ROTATION_TIME]
real_time_delay: 0.05       # Should be at least [FULL_ROTATION_TIME] (without a modificated LiDAR driver)

# LiDAR
LiDAR_type: velodyne       # Options: velodyne, hesai, ouster, custom
stamp_beginning: false     # (Usually: false) Is the pointcloud's stamp the last point's timestamp (end of rotation) or the first's (beggining of rotation)?
offset_beginning: false    # (Usual values: Velodyne = false, Ouster = true, HESAI = indiferent) Is the offset with respect the beginning of the rotation (i.e. point.time ~ [0, 0.1]) or with respect the end (i.e. point.time ~ [-0.1, 0])? For more information see Issue #14: https://github.com/Huguet57/LIMO-Velo/issues/14
LiDAR_noise: 0.001
full_rotation_time: 0.05
min_dist: 0.5                # Minimum distance: doesn't use points closer than this radius
downsample_rate: 1         # Downsampling rate: results show that this one can be up to 32 and still work, try it if you need a speedup
downsample_prec: 0.1       # Downsampling precision: Indoors, lower values (~0.2) work better. Outdoors, higher values (~0.5) lead to less degeneracy.

# IMU
imu_rate: 250              # Approximated IMU rate: only used to estimate when to start the algorithm
covariance_acceleration: 1.e-2
covariance_gyroscope: 1.e-4
covariance_bias_acceleration: 1.e-4
covariance_bias_gyroscope: 1.e-5

# Localizator
MAX_NUM_ITERS: 3
# LIMITS: [0.001] * 23
NUM_MATCH_POINTS: 5
MAX_DIST_PLANE: 2.0
PLANES_THRESHOLD: 5.e-2
# Localizator - Degeneracy
degeneracy_threshold: 7.           # Its magnitude depends on delta (see below), keeping it too high can cause blurry results
print_degeneracy_values: false     # Print the degeneracy eigenvalues to guess what the threshold must be for you

# Delta refinement
# Choose a set of times and field of view sizes (deltas) for the initialization.
# The delta (t2 - t1) that will be used through the algorithm therefore is the last one in the 'deltas' vector
# Tick the 'Localizator' box in RViz to see the initialization in action
Heuristic:
    # No heuristic
    # times: []
    # deltas: [0.05]
    
    # With heuristic
    times: [0.5, 1.0, 2.0]
    deltas: [0.1, 0.05, 0.025]

PS: Other than that, the overall results are great! Congrats for the good work and thanks for the contribution!

['time' field in LiDAR data] Time offset is with respect to the end of the rotation

When I start to play the bag, LIMO-Velo run a few scan (n<5), then stuck.

ros2 foxy please help

Currently, I'm going to proceed with the slam using Ooster. The following nodes are shown, and the situation is as follows. by the way
' ros2 launch limovelo ouster.launch.py '
It doesn't seem to go any further with the command.
Even if I try topic echo, there is no data. Did I do something wrong? Please help. The parameters are also as below. Please help me. Thank you.

compensator assert killing the process

I have been converting this to ROS2 and noticed that the assert in the compensator upsample function was killing the process. Upon further investigation, I think this assert is a bug.
Line: https://github.com/Huguet57/LIMO-Velo/blob/main/src/Modules/Compensator.cpp#L72

As it says in the "path" function the states are between "just before t1 to t2" and the imus are between "first state to just after t2".

Then in the assert we check if imus.front().time <= states.front().time which should always throw an error since the oldest imu should not be older than the older state.

I had to comment this line and then it works.

Is there a plan to add loop closer to generate a map?

Compensate: it works bad.

It has a huge error in rotation and small error in position. The rotation error is fixed with global = true but the position error still remains.

These are the results of the compensation with vx = 200, vy = 0, vyaw = -10*PI during 0.1 seconds.

A better result is given when trying global = true:

This is the result when switching the Xtj - Xt2 for Xt2 - Xtj (with the original circle shown for reference). The orientation gets flipped (?):

It obtained the same result as with global = true when negating vyaw:

Finally, this is the subtle position error, comparing with the states:

can it remove dynamic obstacle????

hello, can it remove dynamic obstacle???? thanks for your reply

Master / Livox diffeerences

Hi, I can use the Livox branch or the master one with Velodyne, is there any advantage or improvements that master have over Livox branch, have they the same level of development?

Thanks

Could you please supply some dataset?

Amazing jobs, could you please supply some dataset?

Commit [08c7093] has better accuracy than [main]

我明白你现在的意思了。确实，现在它可以避免从 null 到 initialized 的跳转。

我会进入它。

for more test, I think commit [08c7093] is better than branch [main]. I mean , the accuracy and fluency.

Originally posted by @qpc001 in #12 (comment)

Mapping Offline underdelivers

After the big bug resolved, the compensator still has the old structure made for surpassing it (with the "global" variable), it is overly complex for what it does and now it has two known bugs:

• It doesn't compensate all points it's given. Seems like the ones after last IMU are ignored or similar. Easy to see when lowering delta to 1/200 or lower.
• Accumulating the pointcloud it's supposed to map when mapping online and then mapping the accumulated result offline works way better than mapping offline. It's not using the known states well to compensate.

Apart from the bugs and the over-complexity, it's the main reason that PointCloud types are around the code leading to unnecessary and time-consuming conversions.

So a general rewrite is needed. Objectives:

• Fixing the "compensating fully" bug
• Fixing the "offline mapping" bug
• Reducing complexity of the code
• Replacing PointCloud for Points, its canonic representation.

[BUG] branch (main): Output Topic [limo_velo/full_pcl] has no timestamp.