PIPELINE OVERVIEW
This file implements a full offline 3D cartography pipeline that turns a folder of raw LiDAR frames (PCD files + optional JSON sidecars) into a single geo-referenced point cloud map saved as a LAS file.
The pipeline runs in five sequential stages, orchestrated by runPipeline():
- VALIDATION & LOADING (validateInputs, loadFrames) PCD files are discovered, sorted by embedded numeric ID, loaded into PCL clouds, and downsampled with a voxel grid filter. Matching JSON sidecar files supply per-frame metadata (frame_id, timestamp). Both the raw cloud (for the final merge) and the downsampled cloud (for registration) are retained in memory.
- GROUND LEVELLING + INITIAL ODOMETRY (computeInitialOdometry) a. Ground levelling: RANSAC plane fitting on the first frame detects the ground plane normal. A quaternion rotation aligning that normal to the world Z axis is stored as groundLevellingR0 and applied to the final merged cloud so the map is upright regardless of sensor tilt. b. Scan-to-Map GICP: frames are registered incrementally. Each new frame is aligned against a sliding local map window using FastVGICP (a voxelized, multi-threaded variant of Generalized ICP). Alignments that exceed RMSE or angular-distance thresholds are rejected and the previous pose is used as a fallback, preventing error propagation. The output is a vector of world-frame 4×4 pose matrices (one per frame).
- LOOP CLOSURE & ELCH GRAPH OPTIMIZATION (buildAndOptimizeElchGraph) A KD-tree on the pose translations finds frame pairs that are spatially close but temporally distant (loop closure candidates). Each candidate pair is verified with an additional GICP alignment; accepted pairs are fed into PCL's ELCH (Explicit Loop Closing Heuristic) optimizer, which distributes the accumulated drift correction across the entire pose graph. The correction for each frame is extracted from the ELCH graph via SVD. A consecutive-RMSE quality gate reverts the optimization if ELCH degraded alignment by more than 10% compared to the initial odometry.
- MAP MERGING & DYNAMIC OBJECT FILTERING (mergeAndFilter) Raw (non-downsampled) frames are transformed into world space using the optimized poses and merged. A voxel occupancy counter identifies dynamic objects: a voxel observed in fewer than minFrameObservations distinct frames is classified as transient (pedestrian, vehicle) and discarded. The surviving cloud is then voxel-downsampled to finalVoxelLeafSize and the ground-levelling rotation is applied.
- EXPORT (saveToXXX) The final point cloud is written. XYZ coordinates are stored as millimetre-precision integers (scale 0.001, offset 0.0), and intensity is preserved.
1.8.17