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

Overview

LIMO-Velo [Alpha]

🔴 [16 February 2022] 🔴 The project is on alpha stage, so be sure to open Issues and Discussions and give all the feedback you can! Better to have 20 people giving lots of feedback than 1000 not saying anything.

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

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

Perfomance of the algorithm
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.

Perfomance of the algorithm
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.

Map comparison - Cones
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.

Map comparison - Cones
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.

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

Work in progress (branch hdmaps)

Prelocalizing with a given HD map
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
Xaloc's "fast" dataset. High velocity in the straights (~15m/s) and tight turns (~80deg/s).

Try xaloc.launch with Xaloc's own rosbags.

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

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
Comments
  • [Velodyne@20Hz] Drift during initialization

    [[email protected]] 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.

    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!

    Screenshot from 2022-02-28 14-34-29

    opened by halops 7
  • Commit [08c7093] has better accuracy than [main]

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

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

    我会进入它。

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

    Originally posted by @qpc001 in https://github.com/Huguet57/LIMO-Velo/issues/12#issuecomment-1051441887

    opened by Huguet57 6
  • [OpenMP] Colon for loop syntax fails with #pragma parallel for

    [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

    opened by arihantb2 4
  • High Quality Map Publish

    High Quality Map Publish

    Seeing (literally) for the first time indoor data, I've seen it's relevant to add the ability to change the downsampling precision. Indoor scenarios have more details and open spaces tend to have repeated normals and create degenerate scenarios.

    The downsampling_prec parameter is added such that the lower the value is, less spatial downsampling there is.

    Results using @qpc001's data (hope you don't mind!).

    downsampling_prec: 0.5

    Screenshot from 2022-02-24 23-40-37

    downsampling_prec: 0.1

    Screenshot from 2022-02-24 23-40-02

    opened by Huguet57 2
  • If you read this, remember to give feedback.

    If you read this, remember to give feedback.

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

    If you are not trying the algorithm, that means there something stopping you to do it :face_with_spiral_eyes:. 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 :nerd_face:

    • 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! :smile:

    opened by Huguet57 1
  • Mapping Offline underdelivers

    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:

    • [x] Fixing the "compensating fully" bug
    • [ ] Fixing the "offline mapping" bug
    • [x] Reducing complexity of the code
    • [ ] Replacing PointCloud for Points, its canonic representation.
    bug 
    opened by Huguet57 1
  • Compensate: it works bad.

    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.

    Bad-1 Bad-2 Bad-3

    A better result is given when trying global = true:

    Good-1 Good-2 Good-3

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

    Shift Xt2 - Xtj (1) Shift Xt2 - Xtj (2)

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

    -vyaw

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

    Position error

    bug 
    opened by Huguet57 1
  • Is LIMO-Velo drifting? (Parameter tuning tips)

    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?

    https://user-images.githubusercontent.com/6130834/160896788-8e2d248a-f394-4421-aeba-afe74bd09bda.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?

    https://user-images.githubusercontent.com/6130834/160899190-8c3fc9be-b193-404f-9b22-4938f7a71f97.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).

    Announcement 
    opened by Huguet57 0
  • Added HESAI support

    Added HESAI support

    First support of another LiDAR. Point object has gained more presence instead of using PointType and PointCloud, still work to be done. The PointCloud Processor has also been rewritten.

    opened by Huguet57 0
  • /camera_info for the Xaloc dataset

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

    opened by Reschivon 0
  • Leaf size is too small for the input dataset

    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.

    opened by wpwebrobotics 2
  • Getting The Map pcd file?

    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.

    opened by wienans 2
Releases(v0.4-alpha)
  • v0.4-alpha(Dec 30, 2021)

  • v0.3-alpha(Dec 29, 2021)

    Real-time localization and offline mapping that matches Fast-LIO2's performance. Stable beginnings and can go to really low deltas. Still has quick fixes and computation cost has to be revised.

    Source code(tar.gz)
    Source code(zip)
  • v0.1-alpha(Dec 28, 2021)

Owner
Andreu Huguet
Visiting Intern Researcher @ HKUST. Working on Autonomous Cars.
Andreu Huguet
Tightly coupled GNSS-Visual-Inertial system for locally smooth and globally consistent state estimation in complex environment.

GVINS GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation. paper link Authors: Shaozu CAO, Xiuyuan LU and Sh

HKUST Aerial Robotics Group 572 Nov 27, 2022
Lidar-with-velocity - Lidar with Velocity: Motion Distortion Correction of Point Clouds from Oscillating Scanning Lidars

Lidar with Velocity A robust camera and Lidar fusion based velocity estimator to undistort the pointcloud. This repository is a barebones implementati

ISEE Research Group 160 Nov 29, 2022
Livox-Mapping - An all-in-one and ready-to-use LiDAR-inertial odometry system for Livox LiDAR

Livox-Mapping This repository implements an all-in-one and ready-to-use LiDAR-inertial odometry system for Livox LiDAR. The system is developed based

null 248 Nov 27, 2022
A Robust LiDAR-Inertial Odometry for Livox LiDAR

LIO-Livox (A Robust LiDAR-Inertial Odometry for Livox LiDAR) This respository implements a robust LiDAR-inertial odometry system for Livox LiDAR. The

livox 353 Dec 6, 2022
Direct LiDAR Odometry: Fast Localization with Dense Point Clouds

Direct LiDAR Odometry: Fast Localization with Dense Point Clouds DLO is a lightweight and computationally-efficient frontend LiDAR odometry solution w

VECTR at UCLA 359 Nov 30, 2022
Robust LiDAR SLAM

SC-A-LOAM What is SC-A-LOAM? A real-time LiDAR SLAM package that integrates A-LOAM and ScanContext. A-LOAM for odometry (i.e., consecutive motion esti

Giseop Kim 242 Nov 29, 2022
Fast and Accurate Extrinsic Calibration for Multiple LiDARs and Cameras

Fast and Accurate Extrinsic Calibration for Multiple LiDARs and Cameras The pre-print version of our paper is available here. The pre-release code has

HKU-Mars-Lab 240 Nov 24, 2022
This repo contains source code of our paper presented in IROS2021 "Single-Shot is Enough: Panoramic Infrastructure Based Calibration of Multiple Cameras and 3D LiDARs"

Single-Shot is Enough: Panoramic Infrastructure Based Calibration of Multiple Cameras and 3D LiDARs Updates [2021/09/01] first commit, source code of

Alibaba 78 Nov 25, 2022
Real Time, High performance BOT detection and protection

REAL-TIME BOT PROTECTION CHALLENGE IronFox https://innovera.ir IronFox is a real-time and high performance bot protection, that using Nginx as a reve

Khalegh Salehi 3 Jun 5, 2022
Continuous-Time Spline Visual-Inertial Odometry

Continuous-Time Spline Visual-Inertial Odometry Related Publications Direct Sparse Odometry, J. Engel, V. Koltun, D. Cremers, In IEEE Transactions on

Minnesota Interactive Robotics and Vision Laboratory 68 Nov 18, 2022
Updates the Wii's current system time with the real world time.

Fix Wii System Time This is a homebrew tool I made for the Wii a while ago. It updates the current system time with the real world time via worldtimea

Puzzle 2 Nov 9, 2022
DG-Mesh-Optimization - Discontinuous Galerkin (DG) solver coupled with a Quasi-Newton line-search algorithm to optimize the DG mesh.

Date written: December 2020 This project was pursued as my final project for MECH 579 (Multidisciplinary Design Optimization) at McGill University, ta

Julien Brillon 8 Sep 18, 2022
This repository is used for automatic calibration between high resolution LiDAR and camera in targetless scenes.

livox_camera_calib livox_camera_calib is a robust, high accuracy extrinsic calibration tool between high resolution LiDAR (e.g. Livox) and camera in t

HKU-Mars-Lab 470 Nov 26, 2022
Continuous Time LiDAR odometry

CT-ICP: Elastic SLAM for LiDAR sensors This repository implements the SLAM CT-ICP (see our article), a lightweight, precise and versatile pure LiDAR o

null 365 Dec 2, 2022
An easy to build CO2 Monitor/Meter with Android and iOS App for real time visualization and charting of air data, data logger, a variety of communication options (BLE, WIFI, MQTT, ESP-Now) and many supported sensors.

CO2-Gadget An easy to build CO2 Monitor/Meter with cell phone App for real time visualization and charting of air data, datalogger, a variety of commu

Mariete 29 Nov 15, 2022
The Pizza Compass will determine your location and direct you to the nearest pizza place. It’s like a regular compass, but better!

Pizza_Compass A Particle project named Pizza_Compass Welcome to your project! Every new Particle project is composed of 3 important elements that you'

Joe Grand 68 Aug 16, 2022
Some source code to demonstrate avoiding certain direct syscall detections by locating and JMPing to a legitimate syscall instruction within NTDLL.

hiding-your-syscalls What is this? This repository contains all of the source code from my blog post about avoiding direct syscall detections, which y

null 198 Dec 1, 2022
Demonstrates implementation of the Windows 10 Acrylic Effect on C++ Win32 Apps using DWM Private APIs and Direct Composition

Win32 Acrylic Effect A Demonstration of Acrylic Effect on C++ Win32 applications using Direct Composition and DWM private APIs. Table of Contents Over

Selastin 125 Nov 30, 2022
This repo includes SVO Pro which is the newest version of Semi-direct Visual Odometry (SVO) developed over the past few years at the Robotics and Perception Group (RPG).

rpg_svo_pro This repo includes SVO Pro which is the newest version of Semi-direct Visual Odometry (SVO) developed over the past few years at the Robot

Robotics and Perception Group 1k Nov 29, 2022