Skip to content

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

License

Notifications You must be signed in to change notification settings

TiNredmc/ssl_slam2_ros2

 
 

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

40 Commits
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

SSL_SLAM2_ROS2

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

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

ROS Humble. ROS Installation

2.2. Ceres Solver

sudo apt install libceres2 libceres-dev

2.3. PCL

Follow PCL Installation.

Tested with 1.8.1

2.4. GTSAM

sudo add-apt-repository ppa:borglab/gtsam-release-4.1
sudo apt update
sudo apt install libgtsam-dev libgtsam-unstable-dev

2.5. Trajectory visualization (Proting in progress)

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_ROS2

4.1 Clone repository:

    cd ~/<your_ros2_ws>/src
    git clone https://github.com/TiNredmc/ssl_slam2_ros2.git
    cd ..
    colcon build --symlink-install --packages-select ssl_slam2  --parallel-workers 4
    source ~/<your_ros2_ws>/install/setup.bash

if you encounter c++: fatal error: Killed signal terminated program cc1plus you might need to rebuild again using

    colcon build --symlink-install --packages-select ssl_slam2  --executor sequential

or

    MAKEFLAGS="-j3 -l1" colcon build --symlink-install --packages-select ssl_slam2 --executor sequential

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

  ros2 launch ssl_slam2 ssl_slam2_mapping.launch.py

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

  ros2 service call /save_map std_srvs/srv/Trigger

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

  ros2 service call /save_map std_srvs/srv/Trigger

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

    <param name="offset_x" type="double" value="0.0" />
    <param name="offset_y" type="double" value="0.0" />
    <param name="offset_yaw" type="double" value="0.0" />

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

<param name="skip_frames" type="int" value="1" />

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}}

About

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

Topics

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published

Languages

  • C++ 93.7%
  • CMake 3.2%
  • Python 3.1%