Skip to content

ROS 2 package of 3D lidar slam using ndt/gicp registration and pose-optimization

License

Notifications You must be signed in to change notification settings

rsasaki0109/lidarslam_ros2

Repository files navigation

lidarslam_ros2

humble
ros2 slam package of the frontend using OpenMP-boosted gicp/ndt scan matching and the backend using graph-based slam.

mobile robot mapping

Green: path with loopclosure
(the 25x25 grids in size of 10m × 10m)

Red and yellow: map

summary

lidarslam_ros2 is a ROS2 package of the frontend using OpenMP-boosted gicp/ndt scan matching and the backend using graph-based slam.
I found that even a four-core laptop with 16GB of memory could work in outdoor environments for several kilometers with only 16 line LiDAR.
(WIP)

requirement to build

You need ndt_omp_ros2 for scan-matcher

clone (If you forget to add the --recursive option when you do a git clone, run git submodule update --init --recursive in the lidarslam_ros2 directory)

cd ~/ros2_ws/src
git clone --recursive https://github.com/rsasaki0109/lidarslam_ros2
cd ..
rosdep install --from-paths src --ignore-src -r -y

build

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

io

frontend(scan-matcher)

  • input
    /input_cloud (sensor_msgs/PointCloud2)
    /tf(from "base_link" to LiDAR's frame)
    /initial_pose (geometry_msgs/PoseStamed)(optional)
    /imu (sensor_msgs/Imu)(optional)
    /tf(from "odom" to "base_link")(Odometry)(optional)

  • output
    /current_pose (geometry_msgs/PoseStamped)
    /map (sensor_msgs/PointCloud2)
    /path (nav_msgs/Path)
    /tf(from "map" to "base_link")
    /map_array(lidarslam_msgs/MapArray)

backend(graph-based-slam)

  • input
    /map_array(lidarslam_msgs/MapArray)

  • output
    /modified_path (nav_msgs/Path)
    /modified_map (sensor_msgs/PointCloud2)

  • srv
    /map_save (std_srvs/Empty)  

how to save the map

pose_graph.g2o and map.pcd are saved in loop closing or using the following service call.

ros2 service call /map_save std_srvs/Empty

params

  • frontend(scan-matcher)
Name Type Default value Description
registration_method string "NDT" "NDT" or "GICP"
ndt_resolution double 5.0 resolution size of voxel[m]
ndt_num_threads int 0 threads using ndt(if 0 is set, maximum alloawble threads are used.)(The higher the number, the better, but reduce it if the CPU processing is too large to estimate its own position.)
gicp_corr_dist_threshold double 5.0 the distance threshold between the two corresponding points of the source and target[m]
trans_for_mapupdate double 1.5 moving distance of map update[m]
vg_size_for_input double 0.2 down sample size of input cloud[m]
vg_size_for_map double 0.05 down sample size of map cloud[m]
use_min_max_filter bool false whether or not to use minmax filter
scan_max_range double 100.0 max range of input cloud[m]
scan_min_range double 1.0 min range of input cloud[m]
scan_period double 0.1 scan period of input cloud[sec](If you want to compound imu, you need to change this parameter.)
map_publish_period double 15.0 period of map publish[sec]
num_targeted_cloud int 10 number of targeted cloud in registration(The higher this number, the less distortion.)
debug_flag bool false Whether or not to display the registration information
set_initial_pose bool false whether or not to set the default pose value in the param file
initial_pose_x double 0.0 x-coordinate of the initial pose value[m]
initial_pose_y double 0.0 y-coordinate of the initial pose value[m]
initial_pose_z double 0.0 z-coordinate of the initial pose value[m]
initial_pose_qx double 0.0 Quaternion x of the initial pose value
initial_pose_qy double 0.0 Quaternion y of the initial pose value
initial_pose_qz double 0.0 Quaternion z of the initial pose value
initial_pose_qw double 1.0 Quaternion w of the initial pose value
publish_tf bool true Whether or not to publish tf from global frame to robot frame
use_odom bool false whether odom is used or not for initial attitude in point cloud registration
use_imu bool false whether 9-axis imu(Angular velocity, acceleration and orientation must be included.) is used or not for point cloud distortion correction.(Note that you must also set the scan_period.)
debug_flag bool false Whether or not to display the registration information
  • backend(graph-based-slam)
Name Type Default value Description
registration_method string "NDT" "NDT" or "GICP"
ndt_resolution double 5.0 resolution size of voxel[m]
ndt_num_threads int 0 threads using ndt(if 0 is set, maximum alloawble threads are used.)
voxel_leaf_size double 0.2 down sample size of input cloud[m]
loop_detection_period int 1000 period of searching loop detection[ms]
threshold_loop_closure_score double 1.0 fitness score of ndt for loop clousure
distance_loop_closure double 20.0 distance far from revisit candidates for loop clousure[m]
range_of_searching_loop_closure double 20.0 search radius for candidate points from the present for loop closure[m]
search_submap_num int 2 the number of submap points before and after the revisit point used for registration
num_adjacent_pose_cnstraints int 5 the number of constraints between successive nodes in a pose graph over time
use_save_map_in_loop bool true Whether to save the map when loop close(If the map saving process in loop close is too heavy and the self-position estimation fails, set this to false.)

demo

trial environment

demo data(ROS1) is hdl_400.bag in hdl_graph_slam
The Velodyne VLP-32 was used in this data.
To use rosbag in ROS1, use rosbags

ros2 launch lidarslam lidarslam.launch.py
ros2 bag play hdl_400/

Green: path with loopclosure, Yellow: path without loopclosure

The larger environment

(This data is not available at the moment...) demo data(ROS1) by Autoware Foundation
https://data.tier4.jp/rosbag_details/?id=212
The Velodyne VLP-16 was used in this data.

ros2 launch lidarslam lidarslam_tukuba.launch.py
ros2 bag play tc_2017-10-15-15-34-02_free_download/ 

Green: path

Red and yellow: map

Used Libraries

  • Eigen
  • PCL(BSD3)
  • g2o(BSD2 except a part)
  • ndt_omp (BSD2)

Related packages