A simple localization framework that can re-localize in built maps based on FAST-LIO.

Overview

FAST-LIO-LOCALIZATION

A simple localization framework that can re-localize in built maps based on FAST-LIO.

News

  • 2021-08-11: Add Open3D 0.7 support.

  • 2021-08-09: Migrate to Open3D for better performance.

1. Features

  • Realtime 3D global localization in a pre-built point cloud map. By fusing low-frequency global localization (about 0.5~0.2Hz), and high-frequency odometry from FAST-LIO, the entire system is computationally efficient.
  • Eliminate the accumulative error of the odometry.
  • The initial localization can be provided either by rough manual estimation from RVIZ, or pose from another sensor/algorithm.

2. Prerequisites

2.1 Dependencies for FAST-LIO

Technically, if you have built and run FAST-LIO before, you may skip section 2.1.

This part of dependency is consistent with FAST-LIO, please refer to the documentation https://github.com/hku-mars/FAST_LIO#1-prerequisites

2.2 Dependencies for localization module

sudo apt install ros-$ROS_DISTRO-ros-numpy
pip install open3d==0.9

Notice that, there may be issue when installing Open3D directly using pip in Python2.7:

ERROR: Package 'pyrsistent' requires a different Python: 2.7.18 not in '>=3.5'

you may firstly install pyrsistent:

pip install pyrsistent==0.15

Then

pip install open3d==0.9

3. Build

Clone the repository and catkin_make:

    cd ~/$A_ROS_DIR$/src
    git clone https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git
    cd FAST_LIO_LOCALIZATION
    git submodule update --init
    cd ../..
    catkin_make
    source devel/setup.bash
  • Remember to source the livox_ros_driver before build (follow livox_ros_driver)
  • If you want to use a custom build of PCL, add the following line to ~/.bashrc export PCL_ROOT={CUSTOM_PCL_PATH}

4. Run Localization

4.1 Sample Dataset

Demo rosbag in a large underground garage: Google Drive | Baidu Pan (Code: ne8d);

Corresponding map: Google Drive | Baidu Pan (Code: kw6f)

The map can be built using LIO-SAM or FAST-LIO-SLAM.

4.2 Run

  1. First, please make sure you're using the Python 2.7 environment;

  2. Run localization, here we take Livox AVIA as an example:

roslaunch fast_lio_localization localization_avia.launch map:=/path/to/your/map.pcd

Please modify /path/to/your/map.pcd to your own map point cloud file path.

Wait for 3~5 seconds until the map cloud shows up in RVIZ;

  1. If you are testing with the sample rosbag data:
rosbag play localization_test_scene_1.bag

Or if you are running realtime

roslaunch livox_ros_driver livox_lidar_msg.launch

Please set the publish_freq in livox_lidar_rviz.launch to 10Hz, to ensure there are enough points for global localization in a single scan. Support for higher frequency is coming soon.

  1. Provide initial pose
rosrun fast_lio_localization publish_initial_pose.py 14.5 -7.5 0 -0.25 0 0 

The numerical value 14.5 -7.5 0 -0.25 0 0 denotes 6D pose x y z yaw pitch roll in map frame, which is a rough initial guess for localization_test_scene_1.bag.

The initial guess can also be provided by the '2D Pose Estimate' Tool in RVIZ.

Note that, during the initialization stage, it's better to keep the robot still. Or if you play bags, fistly play the bag for about 0.5s, and then pause the bag until the initialization succeed.

Related Works

  1. FAST-LIO: A computationally efficient and robust LiDAR-inertial odometry (LIO) package
  2. ikd-Tree: A state-of-art dynamic KD-Tree for 3D kNN search.
  3. FAST-LIO-SLAM: The integration of FAST-LIO with Scan-Context loop closure module.
  4. LIO-SAM_based_relocalization: A simple system that can relocalize a robot on a built map based on LIO-SAM.

Acknowledgments

Thanks for the authors of FAST-LIO and LIO-SAM_based_relocalization.

TODO

  1. Go over the timestamp issue of the published odometry and tf;
  2. Using integrated points for global localization;
  3. Fuse global localization with the state estimation of FAST-LIO, and smooth the localization trajectory;
  4. Updating...
Comments
  • problem with initial pose

    problem with initial pose

    Hi, I test your algorithm with my own dataset. I first use this dataset to create a map by fast-lio2, then use the same dataset to do localization.

    After I play the rosbag, cur_scan is at correct position in the point cloud map. Then I run rosrun fast_lio_localization publish_initial_pose.py 0.0 0.0 0.0 0.0 0.0 0.0 The system Initialize successfully!!! but the pose of current scan immediately jump to a wrong position and direction. The same happens if I set the inital pose by using 2d pose estimate from rviz.

    I test many times and this phenomenan happens always. Do you know why?

    ps: My dataset is collected with a equipment where there is distance and angle difference between lidar and imu. But I have set correct extrinsics in the code.

    opened by ZeyingXuHuaWei 3
  • using offline map for odometry

    using offline map for odometry

    Hi author, I think you only use offline map for global localization, can we also use offline map to replace the online map for odometry? Do you think this a good idea?

    opened by ZeyingXuHuaWei 2
  • Localization with Gigabyte Map

    Localization with Gigabyte Map

    Hi,

    Thanks for your great package.

    I tried to test with my pcd map which is about 8gb size. Once it is loaded, the node would died. Is it because the map is too big?

    If yes, how could I reduce the map size? The map is generated from fast lio package. Possible to use fast lio to generate map with smaller size?

    Thanks.

    David

    opened by David9696 2
  • Updated common_lib.h

    Updated common_lib.h

    Error fast_lio/Pose6D.h: No such file or directory

    Solved by changing line 36 in common_lib.h typedef fast_lio::Pose6D Pose6D; to typedef fast_lio_localization::Pose6D Pose6D;

    opened by MarcoMollo 0
  • "Not Matching" means fail to localization?

    My terminal shows like follow:

    [INFO] [1663312150.392011]: Global localization by scan-to-map matching...... [INFO] [1663312151.285043]: Time: 0.88362288475 [INFO] [1663312151.286561]: [WARN] [1663312151.287811]: Not match!!!! [WARN] [1663312151.289611]: [[ 0.98088933 0.19430204 0.01014116 7.2997208 ] [-0.1944272 0.9808267 0.01330566 -0.4114994 ] [-0.0073614 -0.0150231 0.99986005 0.54942485] [ 0. 0. 0. 1. ]] [WARN] [1663312151.290827]: fitness score:0.364882312047 [WARN] [1663312151.292033]: Waiting for initial pose....

    Fitness score is only 0.364882312047. It seems fail to localization. How to solve this problem?

    opened by caomengnb 3
  • Transform of localization.twist

    Transform of localization.twist

    Thank you for this great job. I have a question when I reading transform_fushion.py

                localization.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
                localization.twist = cur_odom.twist
    

    It transform the pose, but did not transform the twist, can you tell me the reason? actually, I need the twist data, but i find it's all 0 Thank you every much!

    opened by InterKnight 0
  • What is the relationship between the curvature of the point cloud and time?

    What is the relationship between the curvature of the point cloud and time?

    Hi, in the func of “bool sync_packages(MeasureGroup &meas)” , lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); Why curvature divided by 1000 is the interval time?

    opened by LiquoriceH 0
Owner
KINO
Failed person.
KINO
A simple Unreal 5 plugin with demo content that shows how audio DSP can be implemented as Metasound, SourceEffect and SubmixEffect

A simple Unreal 5 plugin with demo content that shows how audio DSP can be implemented as Metasound, SourceEffect and SubmixEffect. The intention is to use it as a template to create more complex audio DSP effects.

Alex Perez San Elias 17 Jan 2, 2023
Cute Framework (CF for short) is the cutest framework available for making 2D games in C/C++

Cute Framework (CF for short) is the cutest framework available for making 2D games in C/C++. CF comprises of different features, where the various features avoid inter-dependencies. In this way using CF is about picking and choosing which pieces are needed for your game

null 314 Dec 26, 2022
DOSBox Pure is a new fork of DOSBox built for RetroArch/Libretro aiming for simplicity and ease of use.

DOSBox Pure is a fork of DOSBox, an emulator for DOS games, built for RetroArch/Libretro aiming for simplicity and ease of use.

Bernhard Schelling 565 Dec 27, 2022
Project DELTA - An open-source trainer built on the Void Engine for Toby Fox's games and their spin-offs.

Project DELTA v3 Project DELTA - An open-source, modular mod menu for Toby Fox's games and their spin-offs. Important note to Grossley: Yes, it is out

Archie 8 Oct 15, 2022
Engine-3D is a 3D-Graphics Renderer built from scratch in C++

Engine3D Engine-3D is a 3D-Graphics Renderer built from scratch in C++, as an Undergraduate Computer Engineering Project for 5th Semester, assigned by

Chirag Lamsal 3 Oct 24, 2022
Own mod. Should be usable with VS v4 API (built with v3).

Own mod. Should be usable with VS v4 API (built with v3).

null 85 Nov 26, 2022
gloom game, built with conan

gloom game, built with conan

null 2 Oct 14, 2022
Care race game built in c++ with sfml. this project is done in 3rd semester of csit for demonstration of computer graphics.

Car Race Car race is simple game which has been built by using SFML in c++ implementing concepts of Computer Graphics for project Work of 3rd semester

subash kc 4 Dec 18, 2021
A minecraft clone built in c++ opengl for the purpose of prefecting graphics programming skills.

LearnOpenGL - CLion This is a project template for OpenGL development with JetBrains CLion IDE. It was created mainly for LearnOpenGL tutorials. Inclu

Jeremy Dellock 1 Dec 28, 2021
FPS Game built from scratch using C++ and Legacy OpenGL.

A small game made by a couple of students as a university project. Built from scratch using C++ and Legacy OpenGL, hence the name.

Yaman Qassas 55 Dec 11, 2022
Simple, fast, easy to get started mid-level game engine written in Zig

Alka Game engine written in zig, compatible with master branch. This engine does provide a toolset for you but generally you have to implement how the

Kiakra 23 Dec 5, 2022
A Tiny 2D OpenGL based C++ Game Engine that is fast, lightweight and comes with a level editor.

A Tiny 2D OpenGL based C++ Game Engine that is fast, lightweight and comes with a level editor.

Samuel Rasquinha 59 Jan 3, 2023
A simple game framework written in C++ using SDL

SGF SGF (Simple Game Framework) is, as the name implies; a very simple and easy to use game framework written in C++ using SDL. Currently the project

Cam K. 1 Nov 4, 2021
sdl based game framework

Hallow Engine an sdl-based game framework Install Procedure Install SDL2[https://libsdl.org] and take the .h(header) files from the include/ folder, t

Hallow - The Game Framework 6 Nov 24, 2021
So_Long project is a 2D game that can read/parse any given map.

SO_LONG So_Long project is a 2D game builded with <MLX> that can read/parse any given map. COMPILE cd path/to/SO_LONG && make or for bonus part cd pat

Mhamed Ajjig 4 Nov 15, 2022
A simple text-based adventure game

The Lighthouse of Doom This repository contain a simple text-based adventure game, implemented twice, once in portable C, and once in Z80 assembly lan

Steve Kemp 55 Nov 1, 2022
Simple Ball-Based Game, made using SFML.

Ball Ball is a basic windows-only, ball-based game, made using C++ SFML. The goal of the game is to claim 5 balls every 10 seconds. Ball can never be

orlando 2 Dec 20, 2021
Gaming meets modern C++ - a fast and reliable entity component system (ECS) and much more

EnTT is a header-only, tiny and easy to use library for game programming and much more written in modern C++. Among others, it's used in Minecraft by

Michele Caini 7.6k Jan 4, 2023
A fast entity component system (ECS) for C & C++

Flecs is a fast and lightweight Entity Component System with a focus on high performance game development (join the Discord!). Highlights of the frame

Sander Mertens 3.5k Jan 8, 2023