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
- Session Metadata Export:
session_meta.yamlrecords the sensor/FoV configuration, session time span, and the scan-file contract (originalScans/<idx>.pcdvs. dynamic-object-removedScans/<idx>_remove.pcd) for downstream consumers such aslong_term_mapping - 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 original raw scans
│ ├── Phase 2: Raw-scan dynamic object removal via seenFree query → Scans/<idx>_remove.pcd
│ │ (Scans/<idx>.pcd itself is never overwritten — it always stays the original raw scan)
│ ├── Phase 3: Two-stage PatchWork++ ground segmentation (coarse → fine) per frame
│ │ → Scans/<idx>_ground.pcd, Scans/<idx>_nonground.pcd
│ ├── Phase 4: Local ground-reference cell-plane outlier removal (radius + height gated)
│ │ → refreshes Scans/<idx>_nonground.pcd / Scans/<idx>_remove.pcd
│ └── StaticMap.pcd, StaticNGMap.pcd, StaticGroundMap.pcd
└── optimized_poses.txt, edges.txt, session_meta.yaml written to the output session directory
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
├── session_meta.yaml # Sensor/FoV/session metadata + scan-file contract (see below)
└── Scans/ # Per-frame PCD scans
├── <idx>.pcd # Original raw scan, sensor-local frame — never overwritten
├── <idx>_remove.pcd # Dynamic-object-removed scan (ground + non-ground)
├── <idx>_ground.pcd # Refined ground-only points for this frame
└── <idx>_nonground.pcd # Non-ground points, cell-plane outliers removed
| 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
session_meta.yaml documents the session/sensor configuration and the scan-file contract so downstream consumers (e.g. long_term_mapping) don't have to guess it:
session:
label: "MyMap"
start_time: 1754460000.0 # first keyframe timestamp [s]
end_time: 1754463600.0 # last keyframe timestamp [s]
sensor:
frame_id: "lidar" # posegraph.sensor_frame_id
extrinsic_translation: [0.0, 0.0, 0.0] # hardcoded identity (extrinsic wiring is future work)
extrinsic_rotation_quat: [0.0, 0.0, 0.0, 1.0]
fov_up_deg: 22.5
fov_down_deg: -22.5
min_range: 1.0 # posegraph.min_r
max_range: 80.0 # posegraph.max_r
scans:
frame: "sensor_local"
deskewed: true
dynamic_removed: true # Scans/<idx>_remove.pcd exists; Scans/<idx>.pcd stays original
dor_voxel_size: 0.1 # posegraph.dor_voxel_size
dor_max_hits: 3 # posegraph.dufo_max_hits (currently metadata only, see note below)
count: 418
static_map:
dynamic_removed: true
voxel_size: 0.2Note:
dor_max_hitsis recorded for downstream reference but is not currently used as a removal threshold —generateStaticMap()'s raw-scan removal keeps a point whenevermap.seenFree(point)is false, with no hit-count gate.sensor.extrinsic_*is likewise a placeholder (identity) until sensor-extrinsic wiring is implemented.
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. The scan-file contract is fixed throughout: Scans/<idx>.pcd is the original raw scan and is never overwritten; every processed variant is written under its own suffix (_remove / _ground / _nonground), so DOR and ground segmentation can be re-run from scratch at any time.
- UFOMap void mapping (single pass): ray-casts every original
Scans/<idx>.pcd, transformed to world by that keyframe's optimized pose, into oneSEEN_FREE | REFLECTIONoctree covering the whole session. The voxel resolution isposegraph.dor_voxel_size(default0.1), independent ofposegraph.voxel_sizeused for map downsampling. - Raw-scan dynamic object removal: re-reads each original
Scans/<idx>.pcdand drops any point classifiedseenFreeby the void map from phase 1 — i.e. space some other frame observed as empty. The result is written toScans/<idx>_remove.pcd. This runs exactly once per session (no iterative refinement). - Two-stage PatchWork++ ground segmentation: reads
Scans/<idx>_remove.pcd; a coarse pass separates ground/non-ground per frame, then a finer second pass re-segments the coarse ground for tighter separation. WritesScans/<idx>_ground.pcd/Scans/<idx>_nonground.pcd. - 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. Refreshes
Scans/<idx>_nonground.pcd, recomposesScans/<idx>_remove.pcd(= refined non-ground + ground), and accumulatesStaticMap.pcd/StaticNGMap.pcd/StaticGroundMap.pcd.
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)