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.

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?

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?

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

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.

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!

我明白你现在的意思了。确实，现在它可以避免从 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)

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.

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

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)

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

Amazing jobs, could you please supply some dataset?

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! 😄

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.

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:

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.

Does the ros2 branch support livox?

thanks

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

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

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.