DOCSEARCHbyAlgolia
Submit Docs

hdl_graph_slam

repository·master·Indexed 25 days ago

https://github.com/koide3/hdl_graph_slam

An open-source ROS package for real-time 6DOF SLAM using 3D LIDAR. It implements 3D Graph SLAM with NDT scan matching-based odometry and supports constraints from GPS, IMU, and floor planes. The system consists of a pipeline of four nodelets: prefiltering, scan matching odometry, floor detection, and the core graph SLAM nodelet for loop detection and pose graph optimization. It supports ROS Melodic and Noetic, and provides services for dumping/loading graphs and saving maps as PCD files.

Tokens
5.6K
Snippets
6
Records
30
Agent score
80%

What's inside hdl_graph_slam

  1. Understand hdl_graph_slam nodelet architecture

    master

    The system operates as a pipeline of four nodelets:

    1. prefiltering_nodelet: Downsamples the input point cloud.
    2. scan_matching_odometry_nodelet: Estimates sensor pose via iterative scan matching between consecutive frames.
    3. floor_detection_nodelet: Detects floor planes using RANSAC.
    4. hdl_graph_slam_nodelet: Receives odometry and floor planes, performs loop detection, and optimizes the pose graph using various constraints.

  2. Tune hdl_graph_slam parameters for better mapping

    master

    Mapping quality depends heavily on scan matching parameters. Use these guidelines for tuning:

    • registration_method: Use FAST_GICP for most cases. Use FAST_VGICP or NDT_OMP if processing speed is a priority. Note: If using ROS Kinetic or earlier, do not use GICP due to a bug in initial guess handling.
    • ndt_resolution: Controls NDT voxel size.
      • Indoor: 0.5 - 2.0 [m]
      • Outdoor: 2.0 - 10.0 [m]
    • General Tuning: Copy a template launch file (e.g., hdl_graph_slam_501.launch for indoor or hdl_graph_slam_400.launch for outdoor) and modify parameters to suit your environment.

  4. Install hdl_graph_slam on ROS Melodic

    master

    To install hdl_graph_slam on ROS Melodic, you must install the required ROS dependencies, clone the necessary third-party repositories (ndt_omp and fast_gicp), and then build your workspace using catkin_make with the Release build type.

    Note: The bag_player.py script requires ProgressBar2 via pip.

    # for melodic
sudo apt-get install ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs ros-melodic-libg2o
cd catkin_ws/src
git clone https://github.com/koide3/ndt_omp.git -b melodic
git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
git clone https://github.com/koide3/hdl_graph_slam

cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release

  5. Run hdl_graph_slam on a host machine

    master

    To run hdl_graph_slam directly on your host machine (assuming ROS is installed), follow these steps:

    1. Start the ROS master:
      roscore
    2. Configure ROS to use simulation time:
      rosparam set use_sim_time true
    3. Launch RViz with the provided configuration file:
      cd hdl_graph_slam/rviz
rviz -d hdl_graph_slam.rviz
    4. Play the sample rosbag with the clock enabled:
      rosbag play --clock hdl_400.bag

    Note: You can download the sample bag file from here.

    roscore
rosparam set use_sim_time true
cd hdl_graph_slam/rviz
rviz -d hdl_graph_slam.rviz
rosbag play --clock hdl_400.bag

  6. Install hdl_graph_slam on ROS Noetic

    master

    To install hdl_graph_slam on ROS Noetic, install the required ROS dependencies, clone ndt_omp and fast_gicp (with submodules), and build your workspace using catkin_make with the Release build type.

    # for noetic
sudo apt-get install ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs ros-noetic-libg2o

cd catkin_ws/src
git clone https://github.com/koide3/ndt_omp.git
git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
git clone https://github.com/koide3/hdl_graph_slam

cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release

  7. Run hdl_graph_slam using the Docker image

    master

    To run hdl_graph_slam within the pre-built Docker container, use the provided run.sh script and then launch the ROS launch file inside the container.

    1. Start the Docker container:
      cd hdl_graph_slam/docker
./run.sh
    2. Inside the container, launch the SLAM node using the sample launch file:
      roslaunch hdl_graph_slam hdl_graph_slam_400.launch
    cd hdl_graph_slam/docker
./run.sh
roslaunch hdl_graph_slam hdl_graph_slam_400.launch

  8. Integrate hdl_graph_slam into your system

    master

    To use hdl_graph_slam with your own hardware:

    1. Transform Sensors: Define the static transformation between your sensors (LIDAR, IMU, GPS) and the base_link frame using static_transform_publisher. All sensor data must be transformed into the base_link frame before being fed to the SLAM algorithm.
    2. Remap Point Cloud Topic: Remap the input topic for the prefiltering_nodelet. For example, to use rslidar_points instead of velodyne_points:
    <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" ...
  <remap from="/velodyne_points" to="/rslidar_points"/>
...

  9. Configure IMU and Robot Odometry for Initial Guess

    master

    The nodelet can use external pose data to provide an initial guess for the scan matching process.

    IMU Frontend

    To use MSF (Multi-Sensor Fusion) pose data, set enable_imu_frontend to true. The nodelet subscribes to:

    • /msf_core/pose
    • /msf_core/pose_after_update

    Robot Odometry

    To use the robot's internal odometry as an initial guess, set enable_robot_odometry_init_guess to true. The nodelet will look up the transform between the current point cloud frame and the robot_odom_frame_id (configured via robot_odom_frame_id) over the time interval since the last frame.

  10. Use the bag_player utility to play ROS bags

    master
    The bag_player.py script is a utility for playing back ROS bag files. It provides a way to play messages from a bag file while publishing to the /clock topic, which is useful for simulating time in ROS nodes. It also includes a curses-based terminal interface to monitor topic progress and processing speed.

  11. Use hdl_graph_slam services to save data

    master

    The package provides two services for data persistence:

    • /hdl_graph_slam/dump (hdl_graph_slam/DumpGraph): Saves all internal data (point clouds, floor coeffs, odoms, and pose graph) to a directory.
    • /hdl_graph_slam/save_map (hdl_graph_slam/SaveMap): Saves the generated map as a PCD file.
    rosservice call /hdl_graph_slam/save_map "resolution: 0.05
destination: '/full_path_directory/map.pcd'"

  12. Configure hdl_graph_slam nodelet parameters

    master

    The HdlGraphSlamNodelet uses ROS private parameters for configuration. Key parameters include:

    General Configuration

    • published_odom_topic (string): Topic for odometry input. Default: /odom.
    • map_frame_id (string): The frame ID for the map. Default: map.
    • odom_frame_id (string): The frame ID for odometry. Default: odom.
    • map_cloud_resolution (double): Resolution for the map point cloud. Default: 0.05.
    • max_keyframes_per_update (int): Maximum number of keyframes to process in one update cycle. Default: 10.
    • g2o_solver_type (string): The type of g2o solver to use. Default: lm_var.
    • graph_update_interval (double): Interval in seconds between graph optimizations. Default: 3.0.
    • map_cloud_update_interval (double): Interval in seconds between map point cloud publications. Default: 10.0.
    • g2o_solver_num_iterations (int): Number of iterations for the g2o optimizer. Default: 1024.

    GPS Configuration

    • enable_gps (bool): Whether to enable GPS integration. Default: true.
    • gps_time_offset (double): Time offset for GPS data. Default: 0.0.
    • gps_edge_stddev_xy (double): Standard deviation for GPS XY constraints. Default: 10000.0.
    • gps_edge_stddev_z (double): Standard deviation for GPS Z constraints. Default: 10.0.
    • gps_edge_robust_kernel (string): Robust kernel type for GPS edges. Default: NONE.
    • gps_edge_robust_kernel_size (double): Size for the GPS robust kernel. Default: 1.0.

    IMU Configuration

    • imu_time_offset (double): Time offset for IMU data. Default: 0.0.
    • enable_imu_orientation (bool): Enable IMU orientation constraints. Default: false.
    • enable_imu_acceleration (bool): Enable IMU acceleration constraints. Default: false.
    • imu_orientation_edge_stddev (double): Standard deviation for IMU orientation. Default: 0.1.
    • imu_acceleration_edge_stddev (double): Standard deviation for IMU acceleration. Default: 3.0.
    • imu_orientation_edge_robust_kernel (string): Robust kernel for IMU orientation. Default: NONE.
    • imu_acceleration_edge_robust_kernel (string): Robust kernel for IMU acceleration. Default: NONE.

    Floor and Odometry Constraints

    • floor_edge_stddev (double): Standard deviation for floor plane constraints. Default: 10.0.
    • floor_edge_robust_kernel (string): Robust kernel for floor edges. Default: NONE.
    • odometry_edge_robust_kernel (string): Robust kernel for odometry edges. Default: NONE.
    • odometry_edge_robust_kernel_size (double): Size for the odometry robust kernel. Default: 1.0.
    • loop_closure_edge_robust_kernel (string): Robust kernel for loop closure edges. Default: NONE.
    • loop_closure_edge_robust_kernel_size (double): Size for the loop closure robust kernel. Default: 1.0.

    Anchor/First Node Configuration

    • fix_first_node (bool): Whether to fix the first node in the graph. Default: false.
    • fix_first_node_stddev (string): Space-separated string of 6 standard deviations for the first node. Default: 1 1 1 1 1 1.
    • fix_first_node_adaptive (bool): Whether to adaptively move the anchor node to the current estimate of the first node. Default: true.