[Velodyne@20Hz] Drift during initialization

[halops]

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.

https://user-images.githubusercontent.com/38696249/155993618-c269cd8c-7454-4bb7-a3e2-6086685a9f79.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!

[Huguet57]

Hey! Thank you for the feedback :smile:

Does the drift happen if you do it without the Heuristic part in the YAML? If it does not, try to modify the times vector. Putting a [1.0, 1.5] would mean changing the deltas size at seconds 1.0 and 1.5, giving more stability to the initialization.

[Huguet57]

I add as TODO explaining better the "Heuristic" definition; since it's causing confusion...

[Huguet57]

Actually, I just realized what might be happening. You mentioned a Velodyne running at 20Hz; that means 0.05s rotation time so the deltas should never contain deltas (t2-t1) higher than rotation time (0.1 > 0.05).

I will also add putting a check as a TODO...

[halops]

I am facing the same issue with the parameters that you suggested (even without using the heuristic). Would you suggest modifying the degeneracy threshold? How should I fine-tune it?

[Huguet57]

So just to be clear, you used something like that:

Heuristic:
    # No heuristic
    times: []
    deltas: [0.05]

or:

    # With heuristic
    times: [1.0]
    deltas: [0.05, 0.025]

For the heuristic, right? It's important that the biggest delta value is less than the rotation time.

[Huguet57]

For degeneracy, what I would suggest is to activate print_degeneracy_values to true and that will show the 6-DOF eigenvalues. Usually what you would want is a threshold that only is crossed by 1 or 2 eigenvalues when the space is clearly degenerated (tunnels, very open spaces...).

Otherwise, you would not want the degeneracy fix (since you are taking the IMU's guess as solution through the degenerated axes).

[Huguet57]

I am assuming this was either a problem of the initialization deltas or a buggy 9-DOF support (now removed in recent commits).

So closing this until further notice