PRISM-Loc

A pluggable LiDAR localization stack for ROS 2 Humble — three backends over shared middleware-free estimator cores, one standard map → odom contract.

Overview  ·  Paper  ·  Install  ·  Quick start  ·  Evaluation  ·  Docs  ·  Roadmap

prism_loc localizes a mobile robot inside a prior map from a 2D LiDAR (LaserScan vs OccupancyGrid), a 3D LiDAR (PointCloud2 vs a .pcd map), or a 3D LiDAR fused with IMU and RTK-GNSS. Pick a backend; the output contract — map→odom TF, pose with covariance, /initialpose seeding — follows the nav2_amcl / hdl_localization / robot_localization conventions ROS users already know.

📢 News

• 2026-07 — v0.1 released: laser2d, ndt3d, and fusion3d backends, branch-and-bound global relocalization, and a systems-paper draft with a fully reproducible synthetic evaluation (docs/paper/main.pdf).

Overview

Backend	Sensors	Map	Estimator	State	Global relocalization
laser2d	2D LiDAR + wheel odometry (TF)	OccupancyGrid (map.yaml)	likelihood-field MCL, KLD-adaptive	planar (x, y, yaw)	✅ branch-and-bound (BBS), param or on-demand service
ndt3d	3D LiDAR + wheel odometry (TF)	.pcd point cloud	NDT-MCL (same particle-filter core)	planar (x, y, yaw)	— (/initialpose)
fusion3d	3D LiDAR + IMU + RTK-GNSS	.pcd point cloud	15-state error-state Kalman filter	full 6-DoF + IMU biases	auto-init from IMU attitude + first valid RTK fix

Why "PRISM" — Particle-filter · RTK · Inertial · Scan-matching: the four ingredients this stack fuses to localize against a prior map. The name also nods to the optics: a prism splits one beam into its components, just as prism_loc splits one estimator core into multiple sensor/observation paths — grounded in NDT-MCL (Saarinen et al., IROS 2013), which showed a 3D NDT map can serve as the measurement model inside the very same particle filter that 2D AMCL uses.

Design principle — all filter mathematics live in two pure C++17 + Eigen libraries (prism_loc_core, prism_loc_fusion) with no ROS and no PCL, unit-tested deterministically with gtest and a seedable RNG. rclcpp, tf2, and PCL appear only in the two thin node packages (prism_loc, prism_loc_fusion_ros). CI builds both paths: the bare-cmake cores and the full colcon workspace in a ros:humble container.

📄 Paper

The architecture, estimator cores, interface contract, and a reproducible synthetic evaluation are described in the systems-paper draft (LaTeX sources + PDF under docs/paper/):

PRISM-Loc: Three LiDAR Localization Backends Behind One ROS 2 Contract, with Middleware-Free Estimator Cores — PDF

If prism_loc is useful in your research, please cite it (see also CITATION.cff):

@unpublished{kang2026prismloc,
  author = {Kang, Jung Mo},
  title  = {{PRISM-Loc}: Three {LiDAR} Localization Backends Behind One
            {ROS}~2 Contract, with Middleware-Free Estimator Cores},
  year   = {2026},
  note   = {Draft manuscript, available at
            \url{https://github.com/kjungmo/prism_loc} under docs/paper/},
}

⚙️ Prerequisites

Core-only path (no ROS) — builds/tests the two estimator libraries by themselves. This is exactly what CI installs in .github/workflows/ci.yml:

sudo apt-get update
sudo apt-get install -y --no-install-recommends cmake g++ libeigen3-dev libgtest-dev

Full ROS path — needed for the launch files / RViz / TF:

1. Install ROS 2 Humble (ros-humble-desktop — includes rviz2).
2. From the workspace root (with this repo checked out at <ws>/src/prism_loc), resolve the ROS package dependencies declared in each package.xml (pulls in nav2_map_server, nav2_lifecycle_manager, rviz2, PCL, ...):

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

🚀 Quick start

# 1. Estimator cores — build & test with the plain system toolchain (no ROS):
cmake -S prism_loc_core   -B build/core   -DPRISM_LOC_CORE_BUILD_TESTS=ON
cmake -S prism_loc_fusion -B build/fusion -DPRISM_LOC_FUSION_BUILD_TESTS=ON
cmake --build build/core   -j && ( cd build/core   && ctest --output-on-failure )
cmake --build build/fusion -j && ( cd build/fusion && ctest --output-on-failure )

# 2. Full ROS 2 Humble build (workspace):
#    place this repo at <ws>/src/prism_loc, then:
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install
colcon test --packages-select prism_loc_core prism_loc prism_loc_fusion prism_loc_fusion_ros

# 3. Run — 2D LiDAR (MCL):
ros2 launch prism_loc laser2d.launch.py map:=/path/to/map.yaml
#    3D LiDAR (NDT-MCL):
ros2 launch prism_loc ndt3d.launch.py map_pcd_path:=/path/to/map.pcd
#    3D LiDAR + IMU + RTK-GNSS (ESKF fusion):
ros2 launch prism_loc_fusion_ros fusion3d.launch.py map_pcd_path:=/path/to/map.pcd
#    laser2d/ndt3d: click "2D Pose Estimate" in RViz to seed; fusion3d
#    auto-initializes once it has IMU attitude + a valid RTK fix
#    (or use /initialpose).

All three launch files accept use_sim_time (default false). Leave it false against a real robot; set it true only when replaying a bag / running in simulation, where pose/TF are stamped from a /clock topic instead of the wall clock:

ros2 launch prism_loc laser2d.launch.py map:=/path/to/map.yaml use_sim_time:=true

Verify it's working

After launching, check the node came up and is actually publishing pose/TF:

ros2 topic echo /prism_loc/pose --once
ros2 run tf2_ros tf2_echo map odom

(fusion3d runs as node prism_loc_fusion, so its pose topic is /prism_loc_fusion/pose instead.)

Expected startup log lines (RCLCPP_INFO, from prism_loc/src/localization_node.cpp and prism_loc_fusion_ros/src/fusion_localization_node.cpp):

• laser2d: "prism_loc up: backend=laser2d" then, once the map arrives, "laser2d: map received (%dx%d)" (plus "laser2d: BBS global-localization matcher ready" if try_global_localization:=true)
• ndt3d: "ndt3d: NDT map built (%zu voxels)" then "prism_loc up: backend=ndt3d"
• fusion3d: "fusion3d: NDT map loaded (%zu pts)" then "prism_loc_fusion (fusion3d) up", then, once IMU+position priors land, "fusion3d: initialized at (%.2f, %.2f, %.2f)"

If any of these don't show up, or the pose/TF commands above hang or print nothing, see TROUBLESHOOTING.md.

📊 Evaluation (synthetic)

The paper ships a fully reproducible synthetic evaluation: three closed-loop experiments compile the actual estimator cores directly with g++ (no ROS, no CMake), drive them through end-to-end localization scenarios with fixed seeds, and write the CSVs committed under docs/paper/data/. Every number and figure below is recomputable from those drivers (experiments/); a committed guard script (docs/paper/check_numbers.py) asserts the quoted statistics still trace to the CSVs. This is a correctness and behavior demonstration on synthetic data — not a field-accuracy benchmark; evaluation on public datasets is on the roadmap.

Global relocalization (BBS) — 100 kidnapped-robot queries: 97 % success, median error 0.037 m / 0.48°, zero false accepts at the score threshold, median full-map latency 37.65 ms.	ESKF under aiding dropout — position RMSE 0.016 m → 0.125 m → 0.009 m before/during/after a 10 s IMU-only blackout; the filter's 3σ bound stays above the true error throughout.

MCL tracking — 65 s closed loop, three seeds: steady-state mean position error 0.105 m, mean yaw error 1.8°; KLD resampling shrinks the particle set 2000 → 500 (4× fewer likelihood evaluations) after convergence.

🗺️ Building a map

laser2d consumes a standard Nav2 2D map (map.yaml + .pgm). Build one with slam_toolbox while driving/teleoperating the robot through the space, then save it with nav2_map_server's saver once the map looks complete:

ros2 run nav2_map_server map_saver_cli -f /path/to/map
#   -> writes /path/to/map.yaml + /path/to/map.pgm; point laser2d.launch.py's
#      map:= argument at the .yaml.

ndt3d / fusion3d consume a single prior .pcd point cloud (map_pcd_path). prism_loc itself is localization-only — it does not ship a mapping node — so build the map with a separate 3D SLAM/registration pipeline, then hand the result to prism_loc:

1. Drive the robot through the space once, recording the raw cloud (and TF) to a bag: ros2 bag record -o mapping_bag /points /tf /tf_static.
2. Post-process the bag with a 3D SLAM/scan-registration pipeline (e.g. a LiDAR-inertial odometry package or PCL registration) to produce one globally-consistent point cloud.
3. Save that merged, downsampled cloud as a single named .pcd file (e.g. pcl::io::savePCDFileBinary("/path/to/map.pcd", merged_cloud)).
4. Point map_pcd_path:=/path/to/map.pcd at it for ndt3d.launch.py / fusion3d.launch.py.

🔌 Interface

Backend	Package	Input	Output
laser2d	prism_loc	/scan, /map, /initialpose, TF odom→base	/tf map→odom, ~/pose, ~/particle_cloud
ndt3d	prism_loc	/points, map.pcd, /initialpose, TF odom→base	same
fusion3d	prism_loc_fusion_ros	/points, /imu, /gnss (NavSatFix), map.pcd, /initialpose	/tf map→odom, ~/pose, ~/odometry

📚 Documentation

Document	Contents
docs/paper/main.pdf	Systems paper: architecture, estimator cores, interface contract, synthetic evaluation
PARAMS.md	Every laser2d/ndt3d/fusion3d parameter, its default, and its meaning
TROUBLESHOOTING.md	Symptom → cause → fix for every silent failure mode
SPEC.md / SPEC_fusion.md	Full design specifications
experiments/README.md	How to rebuild and rerun the synthetic evaluation

🧭 Roadmap

v0.1 ships 2D likelihood-field MCL and 3D NDT-MCL (planar x, y, yaw), plus a 3D LiDAR + IMU + RTK-GNSS error-state Kalman fusion (fusion3d) with full 6-DoF state and IMU-bias estimation. The laser2d backend self-initializes via branch-and-bound (BBS) global localization — recovering pose from a single scan with no /initialpose (param try_global_localization, on-demand service ~/global_localization). Planned:

• Quantitative field evaluation (ATE/RPE) on public datasets
• 3D global localization for ndt3d / fusion3d
• Tight LiDAR–IMU time-offset estimation
• Sample map + bag pair for a known-good first run

🙏 Acknowledgements

prism_loc claims no new estimator theory — it stands on well-known work and conventions: MCL / AMCL (Dellaert, Fox, Thrun et al.; nav2_amcl), NDT & NDT-MCL (Biber & Straßer; Magnusson; Saarinen et al.), branch-and-bound scan matching (Olson; Hess et al., Cartographer), error-state Kalman filtering (Solà), and the ROS interface patterns of robot_localization and hdl_localization. See the paper's Related Work section for the full positioning.

💛 Sponsor

If prism_loc saves you time, consider sponsoring. Sponsorship funds maintenance, new features, and faster issue response. Backers will be acknowledged here — thank you.

License

Apache-2.0.