C++ implementations, derived variants, and compact baselines for localization papers

Many localization papers do not ship reusable reference implementations. This repository collects paper-oriented C++ implementations behind a unified API, with ROS 2 nodes and evaluation tools that are ready to run.

• Pure C++: ROS-independent core libraries for research, education, and embedded use
• ROS 2 Humble: Ready-to-run nodes via ros2 run
• Built-in benchmarking: Compare methods immediately after build
• Degeneracy-aware methods: Includes RELEAD and X-ICP for constrained environments

This repository mixes three levels of implementation scope:

• Paper reimplementation: intended to track the published method closely
• Derived variant: built from the paper idea, but adapted to this repository's shared components
• Compact baseline: a smaller approximation that keeps the interface and core intuition, but not the full paper pipeline

Methods already labeled Derived or Hybrid are intentionally adapted versions. Some paper-named entries still use compact or simplified internals today, especially NDT, KISS-ICP, and DLIO. Check each method README for current scope and deviations.

This repository now keeps a small stable benchmark core and a discardable experiment layer. The current search state is externalized here:

• docs/experiments.md: concrete variant comparisons
• docs/decisions.md: adoption and rejection reasons
• docs/interfaces.md: current minimum stable interface
• docs/paper_tracks.md: publication narratives ranked by current evidence
• docs/paper_roadmap.md: concrete path from benchmark state to a paper package
• docs/paper_assets.md: paper-ready tables and Pareto views exported from experiment aggregates
• docs/paper_captions.md: manuscript-facing caption snippets derived from the current aggregates

Concrete variants live under experiments/ and are meant to be compared, challenged, and replaced. The matrix runner tracks the generated problem set in experiments/results/index.json across Istanbul windows, HDL-400 reference windows, public ROS1 HDL-400 synthetic-time windows, MCD (Ouster) windows, and KITTI Raw (short and full-sequence windows). For heavy KITTI Raw full-sequence HDL Graph SLAM runs, the repository now keeps truthful exceptions instead of inventing pseudo-equivalent lightweight profiles: kitti_raw_0009_full stays default-only, and kitti_raw_0061_full stays skipped. docs/interfaces.md lists the current active selectors wired into pcd_dogfooding; the full integration surface is intentionally broader than any single manuscript-facing subset. CT-LIO and CLINS now both have public ROS1 HDL-400 synthetic-time comparisons, while GT-backed public CT-LIO remains blocked pending an independently curated trajectory CSV for that LiDAR+IMU stack. Those public ROS1 synthetic-time runs are useful comparative evidence, but they should be treated as a separate public-only benchmark, not as an exact native per-point-time reproduction of the reference/native-time HDL-400 setup.

cmake -B build -DCMAKE_BUILD_TYPE=Release && cmake --build build -j"$(nproc)"
bash evaluation/scripts/smoke_ci_fixture.sh

To run ctest, synthetic_benchmark, and the same smoke fixture in one go: bash evaluation/scripts/run_local_evaluation_suite.sh (see evaluation/README.md).

The fixture is a three-frame MCD slice committed under evaluation/fixtures/mcd_kth_smoke/ (~3MB); the same script runs in GitHub Actions after ctest. Full experiment docs need local dogfooding_results/ trees: python3 evaluation/scripts/refresh_study_docs.py (or see evaluation/scripts/SETUP_PUBLIC_BENCHMARK_WINDOWS.md).

python3 evaluation/scripts/run_experiment_matrix.py

Use --reuse-existing to regenerate docs and aggregates from stored summaries without rerunning every benchmark. Use --manifest experiments/<name>_matrix.json only for local iteration; a manifest-only run rewrites experiments/results/index.json to that subset, so commit paths should finish with python3 evaluation/scripts/refresh_study_docs.py (or a full run_experiment_matrix.py without --manifest). Use python3 evaluation/scripts/refresh_study_docs.py to refresh both experiment docs and publication docs together. Publication docs alone can be regenerated with python3 evaluation/scripts/generate_publication_docs.py. Paper-ready tables and Pareto figures can be regenerated with python3 evaluation/scripts/export_paper_assets.py.

The repository currently ships one real-data benchmark snapshot from the official Autoware Istanbul localization bag. GitHub Pages publishes the latest repository-stored report from docs/benchmarks/latest/results.json. The table below is the last full multi-method snapshot. Per-method profile adoption decisions are tracked separately in docs/decisions.md. The current snapshot uses a speed-oriented dogfooding profile with recent/local-map pruning. LiTAMIN2 additionally uses OpenMP-enabled voxel-map and cost accumulation in this repository, with the benchmark profile defaulting to half of the detected hardware threads. For GT-seeded scan-to-map methods, weak updates fall back to the seed pose instead of forcing a poor refinement.

• Topic: /localization/util/downsample/pointcloud
• Window: frames 10200-10307
• Sequence length: 108 frames, 302.1 m, 10.70 s
• Reference poses: reference_pose_full.csv
• Scan density: 940-1380 points per frame, 1128.7 average

./pcd_dogfooding <pcd_dir> <gt_csv> [max_frames] --methods <selector> --summary-json <path> [variant flags...] evaluates sequential PCD datasets against a shared trajectory CSV contract. The exact current selector list and manifest contract are generated in docs/interfaces.md. Method-specific profile flags now span the older paper-style families plus newer graph/LIO surfaces such as --hdl-graph-slam-*, --vgicp-slam-*, --suma-*, --balm2-*, --isc-loam-*, --loam-livox-*, --lio-sam-*, --lins-*, --fast-lio-slam-*, --point-lio-*, and --clins-*.

CT-LIO and CLINS expect imu.csv plus a dense raw LiDAR sequence. Sparse keyframe or submap sequences such as graph/000000xx/cloud.pcd are skipped automatically.

To extract a raw sequence from ROS 1 bags, use ./evaluation/scripts/extract_ros1_lidar_imu.py --pointcloud-bag corrected.bag --imu-bag record_slam.bag --output-dir dogfooding_results/raw_seq.

To build a KITTI Raw sync export (Velodyne *.bin + OXTS) into the sequential NNNNNNNN/cloud.pcd layout plus a trajectory CSV, run:

python3 evaluation/scripts/kitti_raw_to_benchmark.py \
  --drive-dir /path/to/2011_09_26_drive_XXXX_sync \
  --output-dir dogfooding_results/kitti_raw_XXXX \
  --gt-csv experiments/reference_data/kitti_raw_XXXX_gt.csv \
  --write-imu-csv   # optional: writes imu.csv for DLIO/CT-LIO (OXTS-derived, index-aligned stamps)

If you already have a dogfooding tree and only need imu.csv, use python3 evaluation/scripts/kitti_oxts_imu_for_dogfooding.py --drive-dir <sync> --pcd-dir <dogfooding_dir>.

To reproduce the repository-stored Istanbul run from a ROS 2 bag:

python3 -m pip install rosbags numpy matplotlib

python3 evaluation/scripts/extract_ros2_lidar_imu.py \
  --bag ../lidarloc_ws/data/official/autoware_istanbul/localization_rosbag \
  --pointcloud-topic /localization/util/downsample/pointcloud \
  --output-dir dogfooding_results/autoware_istanbul_open_108 \
  --start-frame 10200 \
  --max-frames 108

python3 evaluation/scripts/reference_pose_to_gt_csv.py \
  --input ../lidarloc_ws/data/official/autoware_istanbul/reference_pose_full.csv \
  --output dogfooding_results/autoware_istanbul_open_108_gt.csv

./build/evaluation/pcd_dogfooding \
  dogfooding_results/autoware_istanbul_open_108 \
  dogfooding_results/autoware_istanbul_open_108_gt.csv \
  --methods litamin2,gicp,ndt,kiss_icp,ct_lio,ct_icp

LiTAMIN2, GICP, and NDT currently use GT-seeded scan-to-map initialization inside pcd_dogfooding so that sequential PCD exports remain comparable. If a refinement step becomes weak or unstable, the current dogfooding profile falls back to that seeded pose instead of forcing the update. --litamin2-paper-profile switches LiTAMIN2 to a more paper-like setting centered on voxel_resolution=3.0. --litamin2-icp-only disables the covariance-shape term so the first KL-derived distance term can be benchmarked on its own. --litamin2-max-source-points caps the per-frame source cloud after voxel filtering, and --litamin2-num-threads overrides the OpenMP worker count. KISS-ICP and CT-ICP remain odometry-style methods in this tool, so their absolute ATE is reported after anchoring the estimated trajectory to the first GT pose. For long runs, methods can be filtered with ./pcd_dogfooding ... --methods gicp,ndt,kiss_icp. --ct-lio-estimate-bias is experimental and carries the previous-frame bias with a random-walk prior. --ct-lio-fixed-lag-window 4 enables a short history prior on velocity and bias. Current defaults are velocity_weight=0.0, gyro_bias_scale=0.25, accel_bias_scale=0.25, and history_decay=1.0. Lower history_decay biases the prior toward the most recent state. --ct-lio-fixed-lag-smoother re-optimizes begin/end pose + begin_velocity + bias inside the window with local point-to-plane and IMU residuals. --ct-lio-fixed-lag-outer-iterations controls correspondence relinearization passes in the smoother. The current default is 3 for accuracy; 1 is lighter but usually hurts long-run ATE. For public HDL-400 experiments, this repository now also supports a reference-trajectory CSV converted from pose_trace.csv via python3 evaluation/scripts/pose_trace_to_gt_csv.py. For additional HDL-400 windows, python3 evaluation/scripts/slice_trajectory_csv_by_frames.py can slice the shared reference CSV to the timestamp span covered by a newly extracted frame_timestamps.csv. The repository keeps those reference/native-time-style HDL-400 windows separate from the public ROS1 synthetic-time reconstructions under dogfooding_results/hdl_400_ros1_open_ct_lio_*_time_index. The latter are public and reproducible from the released ROS1 bag, but they use synthesized per-point time and therefore should not be described as exact native-time reproduction.

Method         ATE [m]     FPS
─────────────────────────────────
CT-ICP         0.124       0.1   << best accuracy
A-LOAM         2.059       4.6   << balanced baseline
X-ICP          16.634      5.9
LiTAMIN2       31.155      2.6

Reproduce with ./synthetic_benchmark. No external dataset is required.

# Dependencies (Ubuntu 22.04)
sudo apt install libeigen3-dev libpcl-dev libopencv-dev libceres-dev libgtest-dev

# Build and test
mkdir build && cd build
cmake .. && make -j$(nproc)
ctest

# Benchmark (no external data required)
./evaluation/synthetic_benchmark

cd ros2 && colcon build
source install/setup.bash

# Launch any algorithm node
ros2 run localization_zoo_ros litamin2_node
ros2 run localization_zoo_ros aloam_node
ros2 run localization_zoo_ros kiss_icp_node
ros2 run localization_zoo_ros ndt_node
ros2 run localization_zoo_ros ct_icp_node
ros2 run localization_zoo_ros gicp_node
ros2 run localization_zoo_ros dlo_node
ros2 run localization_zoo_ros ct_lio_node
ros2 run localization_zoo_ros fast_lio2_node
ros2 run localization_zoo_ros dlio_node
ros2 run localization_zoo_ros relead_node
ros2 run localization_zoo_ros xicp_node

# Play a rosbag
ros2 launch localization_zoo_ros play_rosbag.launch.py \
  bag_path:=/path/to/bag points_topic:=/velodyne_points

# Compare trajectories on any dataset such as KITTI, MulRan, nuScenes, or TUM
python3 evaluation/scripts/benchmark.py \
  --gt gt_poses.txt \
  --est LiTAMIN2:litamin2_poses.txt A-LOAM:aloam_poses.txt \
  --output_dir results/

localization_zoo/
├── common/                    # Shared Eigen/PCL utilities
├── papers/
│   ├── litamin2/              # KL-divergence ICP
│   ├── gicp/                  # Generalized ICP
│   ├── voxel_gicp/            # Voxelized GICP
│   ├── small_gicp/            # Compact lightweight GICP
│   ├── vgicp_slam/            # Voxel-GICP graph SLAM
│   ├── ndt/                   # Normal Distributions Transform
│   ├── kiss_icp/              # KISS-ICP
│   ├── scan_context/          # Loop closure / place recognition
│   ├── aloam/                 # Three-stage LOAM pipeline
│   ├── floam/                 # Fast LOAM-style lightweight pipeline
│   ├── isc_loam/              # Intensity-aware loop-closure LOAM
│   ├── loam_livox/            # Solid-state LiDAR-oriented LOAM
│   ├── lego_loam/             # Ground-aware LOAM for UGVs
│   ├── mulls/                 # Multi-metric scan-to-map
│   ├── balm2/                 # Local bundle-adjustment mapping
│   ├── suma/                  # Surfel-based dense LiDAR odometry
│   ├── dlo/                   # Direct LiDAR odometry
│   ├── hdl_graph_slam/        # NDT plus graph-based LiDAR SLAM
│   ├── ct_icp/                # Continuous-time ICP
│   ├── ct_lio/                # Continuous-time LiDAR-inertial odometry
│   ├── dlio/                  # Direct LiDAR-inertial odometry
│   ├── lins/                  # Iterated-filter LIO
│   ├── point_lio/             # Direct raw-point LiDAR-inertial odometry
│   ├── clins/                 # Continuous-time LiDAR-inertial pipeline
│   ├── lio_sam/               # Graph-based LiDAR-inertial SLAM
│   ├── lvi_sam/               # Graph-based visual-lidar-inertial SLAM
│   ├── vins_fusion/           # Compact visual-inertial odometry
│   ├── okvis/                 # Local-window visual-inertial odometry
│   ├── orb_slam3/             # Visual-inertial SLAM with loop closure
│   ├── fast_lio2/             # Direct LiDAR-inertial odometry
│   ├── fast_lio_slam/         # FAST-LIO2 with loop-closure graph SLAM
│   ├── relead/                # Degeneracy-aware constrained ESIKF
│   ├── xicp/                  # Observability-aware ICP
│   ├── ct_icp_relead/         # Hybrid method
│   └── imu_preintegration/    # IMU preintegration
├── evaluation/                # Benchmark and evaluation tools
├── ros2/                      # ROS 2 Humble wrappers
└── .github/workflows/ci.yml   # CI

Each directory under papers/*/ is self-contained with headers, sources, and tests. The core libraries are ROS-independent and can be used without ROS 2.

RELEAD and X-ICP can detect degenerate geometry such as tunnel-like environments:

=== Tunnel Environment ===
Has degeneracy: yes
Degenerate translation dirs: 1
  dir: [1, 0, 0]              # x direction (tunnel axis) is degenerate

=== Normal Environment ===
Has degeneracy: no             # walls and ground constrain all directions

ROS 2 nodes publish real-time status on /degeneracy with std_msgs/Bool.

mkdir -p papers/your_method/{include/your_method,src,test}
# 1. Write headers, sources, and tests
# 2. Add CMakeLists.txt
# 3. Add add_subdirectory to the top-level CMakeLists.txt
# 4. Run ctest and keep the full suite passing

MIT