A ROS2 package for LiDAR-based pose graph optimization with loop closure detection, designed to work as a backend for FAST-LIO2 Mapping & Localization. This package generates globally consistent, drift-corrected maps with dynamic object removal using DUFOMap-style void mapping and PatchWork++ ground segmentation.
FAST-LIO (Odometry)
│
│ /key_frame (fast_lio::msg::Frame)
▼
┌─────────────────────────────────────────────────────┐
│ Pose Graph Optimization Node │
│ │
│ Thread 1: Loop Closure Detection (SOLiD) │
│ Thread 2: Loop Edge Calculation (Nano-GICP + DOP) │
│ Thread 3: Graph Optimization (GTSAM iSAM2) │
│ Thread 4: Map Visualization │
└─────────────────────────────────────────────────────┘
│
│ save_trajectory service
▼
┌─────────────────────────────────────────────────────┐
│ MapSaver (saveMapData.cpp/.hpp) │
│ Snapshot keyframes → generate maps off the │
│ live SLAM path, cancellable mid-generation │
└─────────────────────────────────────────────────────┘
│
▼
LioMap.pcd ← Raw FAST-LIO odometry map (no PGO correction)
OptimizedMap.pcd ← Pose-corrected full map
StaticMap.pcd ← Dynamic objects removed (ground + non-ground)
StaticNGMap.pcd ← Non-ground static map (cell-plane outliers removed)
StaticGroundMap.pcd ← Refined ground-only map
- Loop Closure Detection: SOLiD (Spherical Overlap-based Loop Detection) descriptor for robust place recognition
- Loop Edge Estimation: Nano-GICP for accurate point cloud registration with Hessian-based noise modelling
- DOP-based Loop Validation: Dilution of Precision (DOP) metric for rejecting geometrically degenerate loop closures
- Incremental Pose Graph Optimization: GTSAM iSAM2 with robust Cauchy noise model for loop constraints
- Two-Stage Ground Segmentation: PatchWork++ coarse pass followed by a finer re-segmentation pass for tighter ground/non-ground separation
- Dynamic Object Removal: UFOMap-based void mapping (ray casting +
seenFreequery) removes moving objects directly from the raw scans, before ground segmentation runs - Cell-Plane Ground Outlier Removal: Radius- and height-gated local ground reference planes filter out multipath/ghost-reflection outliers, robust to multi-story buildings
- Cancellable Map Saving: Long-running map generation runs against a thread-safe snapshot of the pose graph and can be cancelled mid-way without disturbing the live SLAM pipeline
- Designed for integration with FAST-LIO2 Mapping & Localization — a modified version of FAST-LIO2 extended with DOP-based scan matching confidence evaluation
Keyframe Callback (kf_callback)
├── Save scan to disk (Scans/<idx>.pcd)
├── Build SOLiD descriptor
├── Add odometry factor to GTSAM graph (mutex-protected: mKF)
└── Signal loop closure / optimization threads
Loop Closure Thread (process_lcd)
└── SOLiD descriptor matching → candidate pairs → solidLoopBuf
Edge Calculation Thread (process_edge)
└── Nano-GICP registration + DOP validation → verified loop edges (mutex-protected: mEdges)
Optimization Thread (process_optimization)
└── iSAM2 update → updatePoses()
Visualization Thread (process_viz)
└── Publish /PGO_map (downsampled global map)
Save Service (MapSaver::handleSave → save_trajectory)
├── Snapshot keyframe poses / times / covariances / loop edges (does not block live SLAM)
├── generateOdomMap() → LioMap.pcd
├── generateOptimizedMap() → OptimizedMap.pcd
└── generateStaticMap()
├── Phase 1: UFOMap void mapping (ray casting) over all raw scans
├── Phase 2: Raw-scan dynamic object removal via seenFree query (overwrites Scans/<idx>.pcd)
├── Phase 3: Two-stage PatchWork++ ground segmentation (coarse → fine) per frame
├── Phase 4: Local ground-reference cell-plane outlier removal (radius + height gated)
└── StaticMap.pcd, StaticNGMap.pcd, StaticGroundMap.pcd
Cancel Service (MapSaver::handleCancel → cancel_save_trajectory)
└── Sets a cancel flag checked between phases; aborts the in-progress save gracefully
| Library | Version | Purpose |
|---|---|---|
| GTSAM | ≥ 4.0 | Factor graph optimization (iSAM2) |
| PCL | ≥ 1.8 | Point cloud processing |
| Eigen3 | ≥ 3.3 | Linear algebra |
| Boost | — | system, timer, thread, serialization, chrono |
| OpenMP | — | Multi-core parallelization |
| liblz4-dev, liblzf-dev | — | Compression backends required by the bundled UFOMap |
| Library | Location | Purpose |
|---|---|---|
| UFOMap | thirdparty/ufomap (vendored, built via add_subdirectory) |
Octree-based void mapping / seenFree query used for dynamic object removal |
| Package | Purpose |
|---|---|
| fast_lio | LiDAR odometry & keyframe source |
| nano_gicp | Fast GICP for loop edge estimation |
| patchworkpp | Ground segmentation |
pcl_ros |
PCL–ROS2 bridge |
tf2, tf2_ros, tf2_geometry_msgs |
Transform handling |
std_srvs |
Trigger service used for save cancellation |
# Install from PPA (Ubuntu 22.04 / 24.04)
sudo add-apt-repository ppa:borglab/gtsam-release-4.1
sudo apt update
sudo apt install libgtsam-dev libgtsam-unstable-dev
sudo apt install liblz4-dev liblzf-dev
# Or build from source
git clone https://github.com/borglab/gtsam.git
cd gtsam && mkdir build && cd build
cmake .. -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF
make -j$(nproc) && sudo make installcd ~/your_ws/src
# FAST-LIO (modified version with DOP-based scan matching confidence evaluation)
# This is a custom fork extended by Kyu-Won Kim from the original FAST-LIO2
git clone https://github.com/Kimkyuwon/fast_lio2_mapping_and_localization.git --recursive fast_lio
# Nano-GICP
git clone https://github.com/vectr-ucla/direct_lidar_odometry.git
# PatchWork++
git clone https://github.com/url-kaist/patchwork-plusplus.git patchwork-plusplus-master
# This package (UFOMap is vendored under thirdparty/ufomap, no separate clone needed)
git clone https://github.com/Kimkyuwon/Pose_Graph_Optimization.git pose_graph_optimizationcd ~/your_ws
colcon build --symlink-install --packages-select pose_graph_optimization
source install/setup.bashRequired: FAST-LIO2 Mapping & Localization is required to run this package. This node receives keyframes from
fastlio_mappingvia the/key_frametopic and cannot operate standalone.
mapping.launch.py from the fast_lio package is configured to launch both fastlio_mapping and posegraphoptimization simultaneously. A single command starts both nodes together.
ros2 launch fast_lio mapping.launch.py config_file:=<your_lidar_config>.yamlInternal structure of mapping.launch.py:
fast_lio_node = Node(package='fast_lio', executable='fastlio_mapping')
pgo_node = Node(package='pose_graph_optimization', executable='posegraphoptimization')
# Both nodes share the same config YAML as parametersBoth nodes share the same config YAML file, so the config must include the posegraph.* parameters used by this node.
Once mapping is complete, call the save service:
ros2 service call /save_trajectory pose_graph_optimization/srv/SaveMap "{directory_name: 'MyMap'}"Map generation runs against a thread-safe snapshot of the pose graph, so live SLAM keeps running while the map is being saved. If needed, it can be cancelled mid-generation:
ros2 service call /cancel_save_trajectory std_srvs/srv/Trigger "{}"This generates the following files under <package_root>/MyMap/:
MyMap/
├── LioMap.pcd # Raw FAST-LIO odometry map (no PGO correction)
├── OptimizedMap.pcd # Full map with PGO-corrected poses
├── StaticMap.pcd # Map with dynamic objects removed (ground included)
├── StaticNGMap.pcd # Non-ground static map (cell-plane outliers removed)
├── StaticGroundMap.pcd # Refined ground-only map
├── optimized_poses.txt # TUM-format optimized trajectory
├── odom_poses.txt # TUM-format raw odometry trajectory
├── edges.txt # Pose graph edge list with covariances
└── Scans/ # Per-frame PCD scans (ground/nonground/cluster)
| File | Description |
|---|---|
LioMap.pcd |
Raw FAST-LIO odometry map, before pose graph optimization |
OptimizedMap.pcd |
All keyframe scans aggregated with PGO-corrected poses |
StaticMap.pcd |
OptimizedMap with dynamic objects (vehicles, pedestrians) removed |
StaticNGMap.pcd |
Non-ground static points, with cell-plane outliers removed |
StaticGroundMap.pcd |
Refined ground-only points (fine PatchWork++ pass output) |
Trajectory file format (TUM format):
timestamp tx ty tz qx qy qz qw
Edge file format:
from_idx to_idx tx ty tz roll pitch yaw cov0 cov1 cov2 cov3 cov4 cov5
SOLiD encodes each keyframe scan into a compact 3D histogram using (Range, Angle, Height) bins. Loop candidates are retrieved via KD-tree nearest-neighbor search on the descriptor space. The similarity score threshold is controlled by r_solid_thres.
For each loop candidate:
- Nano-GICP registers the current scan against the loop candidate.
- The Hessian matrix of the GICP solution is analysed — its inverse diagonal serves as the noise variance for the loop factor's information model.
- DOP ratio (
matching_dop / max(src_dop, tgt_dop)) filters out geometrically degenerate matches (e.g., long corridors).
- Prior factor: First keyframe anchored at origin with tight noise (1e-12).
- Odometry factors: Consecutive keyframe relative poses with covariance from FAST-LIO.
- Loop factors: Verified loop edges with Cauchy robust noise model.
- iSAM2 runs additional update iterations when a loop is closed to ensure convergence.
- Shared pose-graph state (
keyframePoses,keyframePosesUpdated, loop edge records) is protected by dedicated mutexes (mKF,mEdges) so that map saving can safely snapshot it without pausing the live SLAM threads.
generateStaticMap() runs four phases against a snapshot of all keyframe scans:
- UFOMap void mapping: every raw scan is ray-cast into a
SEEN_FREE | REFLECTIONUFOMap octree, building a global record of which voxels have been observed as free space from some other viewpoint. - Raw-scan dynamic removal: each frame's raw points are queried against that map — any point that another frame has seen through (
seenFree) is a moving object and is dropped, overwritingScans/<idx>.pcdbefore ground segmentation runs. - Two-stage PatchWork++ ground segmentation: a coarse pass separates ground/non-ground per frame, then a finer second pass re-segments the coarse ground for tighter separation.
- Local ground-reference cell-plane outlier removal: for each frame, a local reference ground map is built from nearby keyframes (within a radius and height gate around that frame's own pose), PCA planes are fit per grid cell, and non-ground points sitting too far below the local plane are removed as multipath/ghost-reflection outliers. The radius+height gating keeps floors of multi-story buildings from contaminating each other's ground reference.
Left: before dynamic object removal / Right: after dynamic object removal applied
This software is licensed under the GNU General Public License v2.0 (GPL-2.0), in accordance with the license of the primary dependency, FAST-LIO2 Mapping & Localization (GPL-2.0).
Other dependencies and their licenses:
| Package | License |
|---|---|
| FAST-LIO Localization and Mapping | GPL-2.0 |
| Nano-GICP | MIT |
| PatchWork++ | BSD 2-Clause |
| UFOMap | BSD |
| nanoflann | BSD |
| PCL, GTSAM, Eigen3 | BSD / BSD-like |
Non-commercial use notice: This software is primarily developed for academic and non-commercial research purposes. For commercial use, please contact the author.
See the GPL-2.0 license text for full terms and conditions. In summary, you are free to use, modify, and distribute this software, provided that any derivative work is also distributed under GPL-2.0 with source code made available.
Kyu-Won Kim (kimku1125@naver.com)