SSL_SLAM2: Lightweight 3-D Localization and Mapping for Solid-State LiDAR (mapping and localization separated) ICRA 2021

Overview

SSL_SLAM2

Lightweight 3-D Localization and Mapping for Solid-State LiDAR (Intel Realsense L515 as an example)

This repo is an extension work of SSL_SLAM. Similar to RTABMAP, SSL_SLAM2 separates the mapping module and localization module. Map saving and map optimization is enabled in the mapping unit. Map loading and localization is enabled in the localziation unit.

This code is an implementation of paper "Lightweight 3-D Localization and Mapping for Solid-State LiDAR", published in IEEE Robotics and Automation Letters, 2021 paper

A summary video demo can be found at Video

Modifier: Wang Han, Nanyang Technological University, Singapore

Running speed: 20 Hz on Intel NUC, 30 Hz on PC

1. Solid-State Lidar Sensor Example

1.1 Scene reconstruction example

1.2 Localization with built map

1.3 Comparison

2. Prerequisites

2.1 Ubuntu and ROS

Ubuntu 64-bit 18.04.

ROS Melodic. ROS Installation

2.2. Ceres Solver

Follow Ceres Installation.

2.3. PCL

Follow PCL Installation.

Tested with 1.8.1

2.4. GTSAM

Follow GTSAM Installation.

2.5. Trajectory visualization

For visualization purpose, this package uses hector trajectory sever, you may install the package by

sudo apt-get install ros-melodic-hector-trajectory-server

Alternatively, you may remove the hector trajectory server node if trajectory visualization is not needed

3. Sensor Setup

If you have new Realsense L515 sensor, you may follow the below setup instructions

3.1 L515

3.2 Librealsense

Follow Librealsense Installation

3.3 Realsense_ros

Copy realsense_ros package to your catkin folder

    cd ~/catkin_ws/src
    git clone https://github.com/IntelRealSense/realsense-ros.git
    cd ..
    catkin_make

4. Build SSL_SLAM2

4.1 Clone repository:

    cd ~/catkin_ws/src
    git clone https://github.com/wh200720041/ssl_slam2.git
    cd ..
    catkin_make
    source ~/catkin_ws/devel/setup.bash

4.2 Download test rosbag

You may download our recorded data: MappingTest.bag (3G) and LocalizationTest.bag (6G)if you dont have realsense L515, and by defult the file should be under home/user/Downloads

unzip the file (it may take a while to unzip)

cd ~/Downloads
unzip LocalizationTest.zip
unzip MappingTest.zip

4.3 Map Building

map optimization and building

    roslaunch ssl_slam2 ssl_slam2_mapping.launch

The map optimization is performed based on loop closure, you have to specify the loop clousre manually in order to trigger global optimization. To save map, open a new terminal and

  rosservice call /save_map

Upon calling the serviece, the map will be automatically saved. It is recommended to have a loop closure to reduce the drifts. Once the service is called, loop closure will be checked. For example, in the rosbag provided, the loop closure appears at frame 1060-1120, thus, when you see "total_frame 1070" or "total_frame 1110" you may immediately type

  rosservice call /save_map

Since the current frame is between 1060 and 1120, the loop closure will be triggered automatically and the global map will be optimized and saved

4.4 Localization

Type

    roslaunch ssl_slam2 ssl_slam2_localization.launch

If your map is large, it may takes a while to load

4.5 Parameters Explanation

The map size depends on number of keyframes used. The more keyframes used for map buildin, the larger map will be.

min_map_update_distance: distance threshold to add a keyframe. higher means lower update rate. min_map_update_angle: angle threshold to add a keyframe. higher means lower update rate. min_map_update_frame: time threshold to add a keyframe. higher means lower update rate.

4.6 Relocalization

The relocalization module under tracking loss is still under development. You must specify the robot init pose w.r.t. the map coordinate if the starting position is not the origin of map. You can set this by

    
    
    

4.7 Running speed

The realsense is running at 30Hz and some computer may not be able to support such high processing rate. You may reduce the processing rate by skipping frames. You can do thid by setting the


1 implies no skip frames, i.e., 30Hz; implies skip 1 frames, i.e., 15Hz. For small map building, you can do it online. however, it is recommended to record a rosbag and build map offline for large mapping since the dense map cannot be generated in real-time.

5 Map Building with multiple loop closure places

5.1 Dataset

You may download a larger dataset LargeMappingTest.bag (10G), and by defult the file should be under home/user/Downloads

unzip the file (it may take a while to unzip)

cd ~/Downloads
unzip LargeMappingTest.zip

5.2 Map Building

Two loop closure places appear at frame 0-1260 and 1270-3630, i.e., frame 0 and frame 1260 are the same place, frame 1270 adn 3630 are the same place. Run

    roslaunch ssl_slam2 ssl_slam2_large_mapping.launch

open a new terminal, when you see "total_frame 1260", immediately type

  rosservice call /save_map

when you see "total_frame 3630", immediately type again

  rosservice call /save_map

6. Citation

If you use this work for your research, you may want to cite the paper below, your citation will be appreciated

@article{wang2021lightweight,
  author={H. {Wang} and C. {Wang} and L. {Xie}},
  journal={IEEE Robotics and Automation Letters}, 
  title={Lightweight 3-D Localization and Mapping for Solid-State LiDAR}, 
  year={2021},
  volume={6},
  number={2},
  pages={1801-1807},
  doi={10.1109/LRA.2021.3060392}}
Issues
  • How to apply the trajectory to point cloud from the bags?

    How to apply the trajectory to point cloud from the bags?

    Hi,@wh200720041. I am trying to apply your work to do some 3D reconstructions. But I am not familiar with the ROS, so maybe I make some basic mistakes. I output the trajectory from /odom as your said https://github.com/wh200720041/ssl_slam/issues/14#issuecomment-792251212 And I also output the original point from the bag. However, the pcl::transformPointCloud for output pose and the original point goes completely wrong, what I am miss? The pose and pcd are obtained through odomEstimationLocalizationNode.cpp

    I make a fork, maybe you can point out the problem. Thanks.

    https://github.com/cdb0y511/ssl_slam2/blob/fec5de0e21c49801e9a5ccfb6c921929ada04855/src/odomEstimationLocalizationNode.cpp#L127

    https://github.com/cdb0y511/ssl_slam2/blob/fec5de0e21c49801e9a5ccfb6c921929ada04855/src/odomEstimationLocalizationNode.cpp#L99

    opened by cdb0y511 18
  • Problem with L515

    Problem with L515

    Hello, first of all impresive work. I have my L515 connected and facing the following problem after running the " roslaunch ssl_slam ssl_slam_mapping.launch". image

    I am completely new in ROS. Thank you very much for your time.

    opened by avgilias 6
  • About function LaseProcessingClass::featureExtraction

    About function LaseProcessingClass::featureExtraction

    Hi @wh200720041

    I'm still studying your masterpiece. but there is a question I can`t understand.
    

    Code block in function LaseProcessingClass::featureExtraction

    double last_angle = atan2(pc_in->points[0].z,pc_in->points[0].y) * 180 / M_PI; double angle = atan2(pc_in->points[i].x,pc_in->points[i].z) * 180 / M_PI; ... if(fabs(angle - last_angle) > 0.05){ .... } ...

    Why could angle and last_angle subtract? What`s the meaning?

    BRs

    opened by e04su3184 1
  • double free or corruption (out) error when start the launch

    double free or corruption (out) error when start the launch

    process[ssl_slam2_map_optimization_node-3]: started with pid [29347]
    double free or corruption (out)
    process[baselink2cam_tf-4]: started with pid [29391]
    process[rviz-5]: started with pid [29393]
    process[ssl_slam2/trajectory_server_ssl_slam-6]: started with pid [29398]
    [ssl_slam2_map_optimization_node-3] process has died [pid 29347, exit code -6, cmd /home/jzx/l515_slam/devel/lib/ssl_slam2/ssl_slam2_map_optimization_node __name:=ssl_slam2_map_optimization_node __log:=/home/jzx/.ros/log/4f8913b2-bf8b-11ec-b172-000ec672fdee/ssl_slam2_map_optimization_node-3.log].
    
    opened by jzx-gooner 0
  • ERROR: service [/save_map]

    ERROR: service [/save_map]

    Hello, i have a problem with the command "rosservice call /save map".

    • i am following the "4.3 Map Building" procedure, but when i use the "rosservice call /save map" command in my second terminal tab, i get the following error:

    ERROR: service [/save_map] responded with an error: : [pcl::PCDWriter::writeASCII] Could not open file for writing!

    • also in the first terminal tab that i am using the command "roslaunch ssl_slam2 ssl_slam2_mapping.launch" i get the following error for a moment:

    [ERROR] [1642414934.582664157, 1642414849.721206964]: Exception thrown while processing service call: : [pcl::PCDWriter::writeASCII] Could not open file for writing!

    Picture of my terminal:

    error

    • i solved it by typing "rosrun pcl_ros pointcloud_to_pcd input:=/map" instead of "rosservice call /save map" but i would like to check if i get better results with the loop closure that you describe.

    Could you help me? Thank you in advance for your time.

    opened by BellosC 2
  • use with a camera towards the ceiling

    use with a camera towards the ceiling

    hi I install a L515 upper on the robot, it's towards the ceiling. Now I always get a rotable map and transform. I see in the laserProcessingClass.cpp and modify the line 20~27, for change the coordinate transform for my install style, but it lead to feature Extraction fail . and then I try to change the final tranform result in odomEstimationLocalizationClass.cpp and add another transform to change what br.sendTransform(), but it also can not get the right map or transform.Do you have any idea for my case?

    opened by gongyue666 0
  • Build problem

    Build problem

    Hi!

    I could build ssl_slam without problems, but when I try with ssl_slam2, I can't build. Have you changed some library version?

    I would like to copy the log error, but is too long.

    opened by Carlosmct 1
Owner
Wang Han 王晗
I am currently a Phd Candidate at Nanyang Technological University, Singapore, specialize in computer vision and robotics
Wang Han 王晗
This robot lcoalisation package for lidar-map based localisation using multi-sensor state estimation.

A ROS-based NDT localizer with multi-sensor state estimation This repo is a ROS based multi-sensor robot localisation. An NDT localizer is loosely-cou

null 40 Jul 1, 2022
LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

A real-time lidar-inertial odometry package. We strongly recommend the users read this document thoroughly and test the package with the provided dataset first.

Tixiao Shan 1.9k Aug 8, 2022
A Modular Framework for LiDAR-based Lifelong Mapping

LT-mapper News July 2021 A preprint manuscript is available (download the preprint). LT-SLAM module is released.

Giseop Kim 241 Aug 1, 2022
Yggdrasil Decision Forests (YDF) is a collection of state-of-the-art algorithms for the training, serving and interpretation of Decision Forest models.

Yggdrasil Decision Forests (YDF) is a collection of state-of-the-art algorithms for the training, serving and interpretation of Decision Forest models. The library is developed in C++ and available in C++, CLI (command-line-interface, i.e. shell commands) and in TensorFlow under the name TensorFlow Decision Forests (TF-DF).

Google 188 Aug 5, 2022
A lightweight C++ machine learning library for embedded electronics and robotics.

Fido Fido is an lightweight, highly modular C++ machine learning library for embedded electronics and robotics. Fido is especially suited for robotic

The Fido Project 412 Jun 25, 2022
[RSS 2021] An End-to-End Differentiable Framework for Contact-Aware Robot Design

DiffHand This repository contains the implementation for the paper An End-to-End Differentiable Framework for Contact-Aware Robot Design (RSS 2021). I

Jie Xu 48 Jul 29, 2022
Scalable, Portable and Distributed Gradient Boosting (GBDT, GBRT or GBM) Library, for Python, R, Java, Scala, C++ and more. Runs on single machine, Hadoop, Spark, Dask, Flink and DataFlow

eXtreme Gradient Boosting Community | Documentation | Resources | Contributors | Release Notes XGBoost is an optimized distributed gradient boosting l

Distributed (Deep) Machine Learning Community 23k Aug 3, 2022
A toolkit for making real world machine learning and data analysis applications in C++

dlib C++ library Dlib is a modern C++ toolkit containing machine learning algorithms and tools for creating complex software in C++ to solve real worl

Davis E. King 11.3k Aug 3, 2022
A RGB-D SLAM system for structural scenes, which makes use of point-line-plane features and the Manhattan World assumption.

This repo proposes a RGB-D SLAM system specifically designed for structured environments and aimed at improved tracking and mapping accuracy by relying on geometric features that are extracted from the surrounding.

Yanyan Li 233 Jul 28, 2022
MITIE: library and tools for information extraction

MITIE: MIT Information Extraction This project provides free (even for commercial use) state-of-the-art information extraction tools. The current rele

null 2.8k Aug 1, 2022
MATLAB and C++ implementations of sideslip angle estimators

sideslip-angle-vehicle-estimation MATLAB and C++ implementations of sideslip angle estimators Factor graph sideslip angle estimator Papers: "A Factor

Libraries for Multibody Dynamics Simulation (MBDS) 9 Jan 20, 2022
ICRA 2021 - Robust Place Recognition using an Imaging Lidar

Robust Place Recognition using an Imaging Lidar A place recognition package using high-resolution imaging lidar. For best performance, a lidar equippe

Tixiao Shan 275 Jul 28, 2022
R3live - A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package

R3LIVE A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package News [Dec 31, 2021] Release of cod

HKU-Mars-Lab 971 Aug 5, 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 201 Jul 31, 2022
C++ Implementation of "An Equivariant Filter for Visual Inertial Odometry", ICRA 2021

EqF VIO (Equivariant Filter for Visual Inertial Odometry) This repository contains an implementation of an Equivariant Filter (EqF) for Visual Inertia

null 53 Jul 28, 2022
《Graph Optimization Approach to Range-based Localization》; UWB localization

This is modified from localization . Thanks for his work for uwb localizaiton. 代码1:https://github.com/qxiaofan/awesome-uwb-localization 代码2:https://gi

3D视觉工坊 32 Aug 1, 2022
Rock solid distributed database specializing in active/active automatic failover and WAN replication

Expensify/Bedrock/ This public repo contains the Bedrock database server. For more information on what that is, please see http://bedrockdb.com. The d

Expensify, Inc 848 Aug 5, 2022
This project aims to provide a framework and a solid implementation of different techniques

This project aims to provide a framework and a solid implementation of different techniques for generating complete seamless procedural cities with interiors for all buildings.

Tobias Elinder 570 Jul 31, 2022
SOLID principles using modern C++

SOLID principles in modern C++ Description We will try to explain SOLID principles with an example. We will start with a non optimised scenario of a s

angelos plastropoulos 4 Jan 4, 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 288 Aug 4, 2022
A ROS package for mobile robot localization with 2D LiDAR

mcl_ros About mcl_ros mcl_ros is a ROS package for mobile robot localization with 2D LiDAR. To implement localization, Monte Carlo localization (MCL)

Naoki Akai 45 May 13, 2022
R2LIVE is a robust, real-time tightly-coupled multi-sensor fusion framework, which fuses the measurement from the LiDAR, inertial sensor, visual camera to achieve robust, accurate state estimation.

R2LIVE is a robust, real-time tightly-coupled multi-sensor fusion framework, which fuses the measurement from the LiDAR, inertial sensor, visual camera to achieve robust, accurate state estimation.

HKU-Mars-Lab 572 Aug 2, 2022
This robot lcoalisation package for lidar-map based localisation using multi-sensor state estimation.

A ROS-based NDT localizer with multi-sensor state estimation This repo is a ROS based multi-sensor robot localisation. An NDT localizer is loosely-cou

null 40 Jul 1, 2022
Official page of ERASOR RA-L with ICRA'21

?? ERASOR (RA-L'21 with ICRA Option) Official page of "ERASOR: Egocentric Ratio of Pseudo Occupancy-based Dynamic Object Removal for Static 3D Point C

Hyungtae Lim 174 Aug 2, 2022
ERASOR - Official page of ERASOR (RA-L'21 with ICRA'21)

?? ERASOR (RA-L'21 with ICRA Option) Official page of "ERASOR: Egocentric Ratio of Pseudo Occupancy-based Dynamic Object Removal for Static 3D Point C

Hyungtae Lim 173 Jul 26, 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 300 Aug 2, 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 134 Jul 31, 2022