3D Mapping with Graph SLAM using LiDAR in ROS2

I wrote a program for Graph SLAM using 3D LiDAR in ROS2.

The code I wrote is on Github.

rsasaki0109/lidarslam_ros2

Path modified by loop closure

Map

Algorithm Description

• frontend(scan-matcher)

Sequential scan-matching is performed between the previous multiple input points and the current input point cloud. It also generates and stores a partial map (and its center) as a candidate for the loop search at a fixed distance.

• backend(graph-based-slam)

At a certain period of time, ndt scan matching is performed at the last candidate point and all previous candidate points, and if the matching score is less than the threshold, loop constraints are generated there, and pose optimization is performed.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)

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

demo

demo data(ROS1) is hdl_400.bag in hdl_graph_slam
The Velodyne VLP-32 was used in this data.

rviz2 -d src/graphslam_ros2/scanmatcher/config/mapping.rviz
ros2 launch lidarslam lidarslam.launch.py
ros2 bag play -s rosbag_v2 hdl_400.bag

The results are as follows! I can confirm that the loop is closing nicely!

Green: path with loopclosure, Yellow: path without loopclosure

Used Libraries

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

Related packages

pcl_localization_ros2 — ROS2 package of 3D LIDAR-based Localization using the static map
li_slam_ros2 — A lidar inertial slam version of lidarslam_ros2