Convert Figma logo to code with AI

koide3 logohdl_graph_slam

3D LIDAR-based Graph SLAM

1,974
727
1,974
122
View on GitHub

Top Related Projects

HKUST-Aerial-Robotics's avatar

A-LOAM

2,089

Advanced implementation of LOAM

cartographer-project's avatar

cartographer

7,089

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.

RobustFieldAutonomyLab's avatar

LeGO-LOAM

2,354

LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

Quick Overview

HDL Graph SLAM is an open-source ROS package for real-time 3D simultaneous localization and mapping using 3D LiDAR. It provides a robust and efficient solution for creating accurate 3D maps of environments and estimating the sensor's pose in real-time, making it suitable for various robotics and autonomous navigation applications.

Pros

  • High accuracy and robustness in large-scale outdoor and indoor environments
  • Real-time performance, suitable for online SLAM applications
  • Supports various types of 3D LiDAR sensors and IMUs
  • Includes loop closure detection for improved map consistency

Cons

  • Requires relatively high computational resources for real-time operation
  • Dependency on ROS (Robot Operating System) may limit its use in non-ROS projects
  • Limited documentation and examples for advanced usage scenarios
  • May struggle in environments with limited distinct features or highly repetitive structures

Code Examples

  1. Launching HDL Graph SLAM:
roslaunch hdl_graph_slam hdl_graph_slam_501.launch

This command launches the HDL Graph SLAM node with default parameters for the Velodyne VLP-16 LiDAR.

  1. Customizing parameters in the launch file:
<param name="points_topic" value="/velodyne_points" />
<param name="odom_topic" value="/odom" />
<param name="imu_topic" value="/imu/data" />

These lines in the launch file specify the topics for point cloud data, odometry, and IMU information.

  1. Saving the generated map:
rosservice call /hdl_graph_slam/save_map "resolution: 0.05
destination: '/path/to/map.pcd'"

This ROS service call saves the current map as a PCD file with the specified resolution.

Getting Started

  1. Clone the repository:

    git clone https://github.com/koide3/hdl_graph_slam.git

  2. Install dependencies:

    rosdep install --from-paths . --ignore-src -r -y

  3. Build the package:

    catkin_make

  4. Launch HDL Graph SLAM:

    roslaunch hdl_graph_slam hdl_graph_slam_501.launch

  5. Visualize the results in RViz:

    rviz -d hdl_graph_slam/rviz/hdl_graph_slam.rviz

Competitor Comparisons

HKUST-Aerial-Robotics logo

A-LOAM

2,089

Advanced implementation of LOAM

Pros of A-LOAM

  • Lightweight implementation, optimized for aerial robotics applications
  • Faster processing speed, suitable for real-time SLAM in aerial vehicles
  • Simplified codebase, making it easier to understand and modify

Cons of A-LOAM

  • Less robust in complex environments compared to hdl_graph_slam
  • Limited to LiDAR-only SLAM, while hdl_graph_slam supports multi-sensor fusion
  • Fewer advanced features like loop closure and global optimization

Code Comparison

A-LOAM (scanRegistration.cpp):

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    if (!systemInited)
    {
        systemInitCount++;
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        return;
    }
    // ... (processing code)
}

hdl_graph_slam (hdl_graph_slam_nodelet.cpp):

void HDLGraphSLAMNodelet::cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
  if(!ros::ok()) {
    return;
  }

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
  pcl::fromROSMsg(*cloud_msg, *cloud);

  // ... (processing code)
}

Both repositories handle point cloud data, but A-LOAM focuses on lightweight processing for aerial applications, while hdl_graph_slam offers more comprehensive SLAM capabilities with multi-sensor support.

cartographer-project logo

cartographer

7,089

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.

Pros of Cartographer

  • More comprehensive and actively maintained by Google
  • Supports both 2D and 3D SLAM
  • Offers real-time loop closure and global optimization

Cons of Cartographer

  • Higher computational requirements
  • Steeper learning curve due to complexity
  • May require more parameter tuning for optimal performance

Code Comparison

hdl_graph_slam:

pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
pcl::io::loadPCDFile(filename, *cloud);
registration->setInputTarget(cloud);

Cartographer:

sensor::PointCloud point_cloud;
for (const auto& point : range_data.returns) {
  point_cloud.push_back(point);
}
AddRangeData(sensor_id, point_cloud);

Key Differences

  • hdl_graph_slam is primarily designed for 3D LiDAR SLAM, while Cartographer supports both 2D and 3D SLAM
  • Cartographer uses a submap-based approach, whereas hdl_graph_slam employs a graph-based method
  • hdl_graph_slam is more lightweight and easier to integrate, while Cartographer offers more features and robustness

Use Cases

  • hdl_graph_slam: Suitable for projects requiring quick integration and focused on 3D LiDAR data
  • Cartographer: Ideal for complex robotics applications needing versatile SLAM capabilities and long-term mapping
RobustFieldAutonomyLab logo

LeGO-LOAM

2,354

LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

Pros of LeGO-LOAM

  • Optimized for ground vehicles, providing better performance in urban and structured environments
  • Includes ground segmentation and feature extraction specifically designed for LiDAR odometry
  • Lightweight implementation, suitable for real-time processing on less powerful hardware

Cons of LeGO-LOAM

  • Limited to 3D LiDAR sensors, while hdl_graph_slam supports various sensor types
  • Less flexible in terms of loop closure and graph optimization compared to hdl_graph_slam
  • May struggle in environments with limited features or highly unstructured scenes

Code Comparison

LeGO-LOAM:

void BasicLaserOdometry::extractFeatures()
{
    cornerPointsSharp->clear();
    cornerPointsLessSharp->clear();
    surfPointsFlat->clear();
    surfPointsLessFlat->clear();
    // ... (feature extraction logic)
}

hdl_graph_slam:

void ScanMatchingOdometryEstimator::estimate(const pcl::PointCloud<PointT>::ConstPtr& cloud, const Eigen::Matrix4f& prev_pose, Eigen::Matrix4f& pose) {
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample(cloud, filtered);
    // ... (scan matching logic)
}

Both repositories implement SLAM algorithms, but LeGO-LOAM focuses on feature extraction for ground vehicles, while hdl_graph_slam provides a more general-purpose approach with support for various sensors and environments.

Convert Figma logo designs to code with AI

Visual Copilot

Introducing Visual Copilot: A new AI model to turn Figma designs to high quality code using your components.

Try Visual Copilot

README

New SLAM package is released

A new 3D SLAM package is released: https://github.com/koide3/glim.

hdl_graph_slam

hdl_graph_slam is an open source ROS package for real-time 6DOF SLAM using a 3D LIDAR. It is based on 3D Graph SLAM with NDT scan matching-based odometry estimation and loop detection. It also supports several graph constraints, such as GPS, IMU acceleration (gravity vector), IMU orientation (magnetic sensor), and floor plane (detected in a point cloud). We have tested this package with Velodyne (HDL32e, VLP16) and RoboSense (16 channels) sensors in indoor and outdoor environments.

Build on melodic & noetic

Third-party extensions

See also the following nice works built upon hdl_graph_slam. Feel free to request to include your work in the list :)

Nodelets

hdl_graph_slam consists of four nodelets.

  • prefiltering_nodelet
  • scan_matching_odometry_nodelet
  • floor_detection_nodelet
  • hdl_graph_slam_nodelet

The input point cloud is first downsampled by prefiltering_nodelet, and then passed to the next nodelets. While scan_matching_odometry_nodelet estimates the sensor pose by iteratively applying a scan matching between consecutive frames (i.e., odometry estimation), floor_detection_nodelet detects floor planes by RANSAC. The estimated odometry and the detected floor planes are sent to hdl_graph_slam. To compensate the accumulated error of the scan matching, it performs loop detection and optimizes a pose graph which takes various constraints into account.

Constraints (Edges)

You can enable/disable each constraint by changing params in the launch file, and you can also change the weight (*_stddev) and the robust kernel (*_robust_kernel) of each constraint.

  • Odometry

  • Loop closure

  • GPS

    • /gps/geopoint (geographic_msgs/GeoPointStamped)
    • /gps/navsat (sensor_msgs/NavSatFix)
    • /gpsimu_driver/nmea_sentence (nmea_msgs/Sentence)

hdl_graph_slam supports several GPS message types. All the supported types contain (latitude, longitude, and altitude). hdl_graph_slam converts them into the UTM coordinate, and adds them into the graph as 3D position constraints. If altitude is set to NaN, the GPS data is treated as a 2D constrait. GeoPoint is the most basic one, which consists of only (lat, lon, alt). Although NavSatFix provides many information, we use only (lat, lon, alt) and ignore all other data. If you're using HDL32e, you can directly connect hdl_graph_slam with velodyne_driver via /gpsimu_driver/nmea_sentence.

  • IMU acceleration (gravity vector)
    • /gpsimu_driver/imu_data (sensor_msgs/Imu)

This constraint rotates each pose node so that the acceleration vector associated with the node becomes vertical (as the gravity vector). This is useful to compensate for accumulated tilt rotation errors of the scan matching. Since we ignore acceleration by sensor motion, you should not give a big weight for this constraint.

  • IMU orientation (magnetic sensor)

    • /gpsimu_driver/imu_data (sensor_msgs/Imu)

    If your IMU has a reliable magnetic orientation sensor, you can add orientation data to the graph as 3D rotation constraints. Note that, magnetic orientation sensors can be affected by external magnetic disturbances. In such cases, this constraint should be disabled.

  • Floor plane

    • /floor_detection/floor_coeffs (hdl_graph_slam/FloorCoeffs)

This constraint optimizes the graph so that the floor planes (detected by RANSAC) of the pose nodes becomes the same. This is designed to compensate the accumulated rotation error of the scan matching in large flat indoor environments.

Parameters

All the configurable parameters are listed in launch/hdl_graph_slam.launch as ros params.

Services

  • /hdl_graph_slam/dump (hdl_graph_slam/DumpGraph)
    • save all the internal data (point clouds, floor coeffs, odoms, and pose graph) to a directory.
  • /hdl_graph_slam/save_map (hdl_graph_slam/SaveMap)
    • save the generated map as a PCD file.

Requirements

hdl_graph_slam requires the following libraries:

  • OpenMP
  • PCL
  • g2o
  • suitesparse

The following ROS packages are required:

# 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

# 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

[optional] bag_player.py script requires ProgressBar2.

sudo pip install ProgressBar2

Example1 (Indoor)

Bag file (recorded in a small room):

rosparam set use_sim_time true
roslaunch hdl_graph_slam hdl_graph_slam_501.launch

roscd hdl_graph_slam/rviz
rviz -d hdl_graph_slam.rviz

rosbag play --clock hdl_501_filtered.bag

We also provide bag_player.py which automatically adjusts the playback speed and processes data as fast as possible.

rosrun hdl_graph_slam bag_player.py hdl_501_filtered.bag

You'll see a point cloud like:

You can save the generated map by:

rosservice call /hdl_graph_slam/save_map "resolution: 0.05
destination: '/full_path_directory/map.pcd'"

Example2 (Outdoor)

Bag file (recorded in an outdoor environment):

rosparam set use_sim_time true
roslaunch hdl_graph_slam hdl_graph_slam_400.launch

roscd hdl_graph_slam/rviz
rviz -d hdl_graph_slam.rviz

rosbag play --clock hdl_400.bag

Example with GPS

Ford Campus Vision and Lidar Data Set [URL]

The following script converts the Ford Lidar Dataset to a rosbag and plays it. In this example, hdl_graph_slam utilizes the GPS data to correct the pose graph.

cd IJRR-Dataset-2
rosrun hdl_graph_slam ford2bag.py dataset-2.bag
rosrun hdl_graph_slam bag_player.py dataset-2.bag

Use hdl_graph_slam in your system

  1. Define the transformation between your sensors (LIDAR, IMU, GPS) and base_link of your system using static_transform_publisher (see line #11, hdl_graph_slam.launch). All the sensor data will be transformed into the common base_link frame, and then fed to the SLAM algorithm.

  2. Remap the point cloud topic of prefiltering_nodelet. Like:

  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" ...
    <remap from="/velodyne_points" to="/rslidar_points"/>
  ...

Common Problems

Parameter tuning guide

The mapping quality largely depends on the parameter setting. In particular, scan matching parameters have a big impact on the result. Tune the parameters accoding to the following instructions:

  • registration_method [updated] In short, use FAST_GICP for most cases and FAST_VGICP or NDT_OMP if the processing speed matters This parameter allows to change the registration method to be used for odometry estimation and loop detection. Note that GICP in PCL1.7 (ROS kinetic) or earlier has a bug in the initial guess handling. If you are on ROS kinectic or earlier, do not use GICP.

  • ndt_resolution This parameter decides the voxel size of NDT. Typically larger values are good for outdoor environements (0.5 - 2.0 [m] for indoor, 2.0 - 10.0 [m] for outdoor). If you chose NDT or NDT_OMP, tweak this parameter so you can obtain a good odometry estimation result.

  • other parameters All the configurable parameters are available in the launch file. Copy a template launch file (hdl_graph_slam_501.launch for indoor, hdl_graph_slam_400.launch for outdoor) and tweak parameters in the launch file to adapt it to your application.

License

This package is released under the BSD-2-Clause License.

Note that the cholmod solver in g2o is licensed under GPL. You may need to build g2o without cholmod dependency to avoid the GPL.

Related packages

Papers

Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, 2019 [link].

Contact

Kenji Koide, k.koide@aist.go.jp, https://staff.aist.go.jp/k.koide

Active Intelligent Systems Laboratory, Toyohashi University of Technology, Japan [URL]
Mobile Robotics Research Team, National Institute of Advanced Industrial Science and Technology (AIST), Japan [URL]