fast_csm

2D Fast Correlative Scan Matching for global localization in 2D maps.

Standalone C++17 port of Olson 2009 / Cartographer's FastCorrelativeScanMatcher2D, with optional Ceres sub-cell refinement and optional Open3D ICP polish. No protobuf, glog, ROS, or absl dependency on the library itself — Eigen is the only public dependency.

What it does

Given:

• a 2D map point cloud w_pts (world frame), and
• a 2D body-frame scan b_pts,

it returns the rigid transform w_T_b such that w_p = w_T_b · b_p, together with a matching score in [0, 1].

Pipeline:

1. Branch-and-bound over a multi-resolution occupancy-grid stack (Cartographer's algorithm, ported standalone). Globally searches the map and the full ±π orientation range — no prior needed.
2. Ceres sub-cell refinement (optional, build with FAST_CSM_USE_CERES=ON) — bicubic-interpolated probability + Huber loss.
3. Open3D ICP polish (optional, in the example layer only) — per-point correspondence search seeded with the Ceres-refined pose.

When to use it

Use case	fast_csm	Comment
Initial pose at startup, no prior	✅ Best fit	Branch-and-bound globally searches the map with no prior.
Re-localization after lost lock / kidnapped-robot	✅ Best fit	Same B&B path, ~100–300 ms on a typical indoor map.
Per-frame tracking at LiDAR rate	⚠️ Usable	OnlineLocalTracker is provided, but a point-to-plane matcher will produce tighter residuals on real LiDAR — probability-grid matching discards the per-direction information that surface normals retain.

Recommended usage: fast_csm for the initial pose (and for re-localization on lost lock), then hand the refined w_T_b off to your preferred local tracker — e.g. a point-to-plane ICP or a KISS-ICP-style frame-to-frame solver:

#include <fast_csm/registration.h>

// Frame 0: bootstrap — no prior needed.
auto r = fast_csm::GlobalRegistration(map_pts, first_scan_pts);
if (!r.ok || r.score < 0.5f) { /* low confidence; widen search or retry */ }

Eigen::Matrix4d w_T_b_init = r.w_T_b_refined;   // 4x4 SE(2)-in-SE(3)

// Frames 1+: feed w_T_b_init to your per-frame tracker.
local_tracker.SetInitialPose(w_T_b_init);
for (each new scan) { local_tracker.Track(scan); }

Convention

Uses the standard [to]_T_[from] notation:

w_p = w_T_b · b_p

i.e. w_T_b is the pose of body in world.

The flipped row-major grid layout from Cartographer is preserved internally — see include/fast_csm/types.h for the documented convention. The public matcher boundary always returns w_T_b in the sense above.

Build

From the project root:

mkdir -p build && cd build
cmake \
  -DFAST_CSM_USE_CERES=ON \
  -DFAST_CSM_BUILD_EXAMPLE=ON \
  -DFAST_CSM_BUILD_TESTS=ON \
  ..
make -j$(nproc)

All three options default to OFF so that consumers embedding fast_csm as a subdirectory only build the library. The library itself has no PCL, Open3D, or yaml-cpp dependency.

Build requirements

Target	Requires
fast_csm (library, default)	Eigen3, C++17 compiler, optionally OpenMP
FAST_CSM_USE_CERES=ON	+ Ceres Solver
FAST_CSM_BUILD_EXAMPLE=ON	+ Open3D, yaml-cpp
FAST_CSM_BUILD_TESTS=ON	+ Open3D, yaml-cpp

Use as a library

fast_csm is designed to drop into a parent CMake project without any install step. The exported target is fast_csm::fast_csm. Eigen is the only transitive dependency the consumer inherits (PUBLIC); spdlog is vendored and kept PRIVATE.

Option A — vendored / git submodule (add_subdirectory)

Drop the source tree under your project (e.g. third_party/fast_csm/) or add it as a submodule, then in your CMakeLists.txt:

# Optional: toggle features BEFORE add_subdirectory.  All default OFF, so if
# you don't need Ceres refinement you can omit these.
set(FAST_CSM_USE_CERES     ON CACHE BOOL "" FORCE)   # sub-cell refinement
# set(FAST_CSM_BUILD_EXAMPLE OFF CACHE BOOL "" FORCE)  # default OFF
# set(FAST_CSM_BUILD_TESTS   OFF CACHE BOOL "" FORCE)  # default OFF

add_subdirectory(third_party/fast_csm)

add_executable(my_app main.cpp)
target_link_libraries(my_app PRIVATE fast_csm::fast_csm)

Option B — remote fetch (FetchContent)

No copy-in, no submodule — let CMake grab it at configure time:

include(FetchContent)
FetchContent_Declare(
  fast_csm
  GIT_REPOSITORY https://github.com/Junbug331/fast_csm.git
  GIT_TAG        main            # or a specific commit/tag
)
# Propagate options before MakeAvailable (same as Option A).
set(FAST_CSM_USE_CERES ON CACHE BOOL "" FORCE)
FetchContent_MakeAvailable(fast_csm)

target_link_libraries(my_app PRIVATE fast_csm::fast_csm)

Requirements on the consuming project

You need…	Requirement on your project
The library itself	find_package(Eigen3 REQUIRED) is called by fast_csm
FAST_CSM_USE_CERES=ON	Ceres must be findable via find_package(Ceres REQUIRED)
FAST_CSM_BUILD_EXAMPLE=ON or _TESTS=ON	Open3D and yaml-cpp must be installed
C++ standard	C++17 or later (target sets cxx_std_17 PUBLIC)

A bare add_subdirectory with all options OFF pulls in only Eigen — no Open3D, no Ceres, no yaml-cpp leak in.

In your code

#include <fast_csm/registration.h>

std::vector<Eigen::Vector2d> w_pts = ...;     // map (world frame)
std::vector<Eigen::Vector2d> b_pts = ...;     // scan (body frame)

fast_csm::MatchOptions opts;
opts.resolution = 0.05;                       // meters per cell
opts.angular_step_size_override = 0.005;      // ~0.3°; 0 = auto

auto r = fast_csm::GlobalRegistration(w_pts, b_pts, opts);
if (r.ok) {
  Eigen::Matrix4d w_T_b = r.w_T_b;            // 4x4 SE(2)-in-SE(3)
  // r.w_T_b_refined  — populated when ceres_refine.enabled = true
  // r.score          — matching score [0, 1]
  // r.inlier_ratio   — fraction of scan agreeing with map
  // r.registration_time_s, r.refinement_time_s
}

An Eigen::Matrix<double, 2, Dynamic> overload (column-major 2×N) is also available for callers whose point sets are matrix-typed.

Library dependencies

Visibility	Required	Optional
PUBLIC	Eigen3	—
PRIVATE	(vendored spdlog)	OpenMP, Ceres (FAST_CSM_USE_CERES=ON)

Examples and tests additionally need:

• Open3D — PCD I/O, ICP polish, live viewer.
• yaml-cpp — config files, YAML output.

These are gated behind FAST_CSM_BUILD_EXAMPLE / FAST_CSM_BUILD_TESTS.

Examples

Built under build/ when FAST_CSM_BUILD_EXAMPLE=ON:

Binary	Source	Config
fast_csm_register	example/fast_csm_register.cpp	example/example_config.yaml
online_tracker_example	example/online_tracker_example.cpp	example/online_tracker_config.yaml
slam_dataset_runner	example/slam_dataset_runner.cpp	example/slam_dataset_runner_config.yaml

slam_dataset_runner iterates a dataset of timestamped body-frame scans, tracks them against a reference map with gyro-derived yaw deltas, runs Ceres + Open3D ICP polish per frame, and optionally shows a live Open3D viewer with the map, registered scan, robot axes, and trajectory — plus a per-frame FPS readout in the terminal.

Tests

test/test_registration.cpp runs 7 cases against PCDs under pcd_files/ (identity, translate(2,3), rot(45°), SE(2), partial overlap, noise σ=0.02 m, real data) and exits 0 iff all pass.

./build/test_registration

test/test_registration_api.cpp is a smoke test for both overloads of GlobalRegistration().

Note: pcd_files/ and data/ are gitignored (the real scans are sizeable and may be private to the author's dataset). To run the tests/examples, drop your own w_map.pcd + b_map.pcd + ground_truth_w_T_b.yaml into pcd_files/, or point the config paths at your own fixtures.

Tools

Python helpers under tools/ (no CMake — plain scripts):

Script	Purpose
visualize_registration.py	Overlay map and registered scan in matplotlib.
pointcloud_viewer.py	Top-down XY scatter of a PCD/PLY with an origin axis.
map_transformation_text.py	Inspect a saved 4×4 map transform alongside the clouds.
pose_to_matrix.py	Convert (x, y, yaw) to a 4×4 homogeneous matrix.

Dependencies: open3d, numpy, matplotlib, pyyaml.

File layout

fast_csm/
├── include/fast_csm/
│   ├── types.h                      # MatchOptions, MapLimits, CeresRefineOptions
│   ├── occupancy_grid.h             # rasterization + Gaussian blur
│   ├── precomputation_grid.h        # multi-res max-pool stack (Cartographer-style)
│   ├── scan_matcher.h               # B&B FastCorrelativeScanMatcher2D
│   ├── registration.h               # GlobalRegistration() one-call API
│   └── online_tracker.h             # OnlineLocalTracker (per-frame state class)
├── src/
│   ├── occupancy_grid.cpp
│   ├── precomputation_grid.cpp
│   ├── scan_matcher.cpp
│   ├── registration.cpp
│   ├── online_tracker.cpp
│   ├── ceres_refiner.{h,cpp}        # only built when FAST_CSM_USE_CERES=ON
│   └── spdlog/                      # vendored, header-only, PRIVATE to library
├── example/
│   ├── fast_csm_register.cpp        + example_config.yaml
│   ├── online_tracker_example.cpp   + online_tracker_config.yaml
│   └── slam_dataset_runner.cpp      + slam_dataset_runner_config.yaml
├── test/
│   ├── test_registration.cpp        # 7-case gating suite
│   └── test_registration_api.cpp    # overload smoke test
├── config/
│   └── test_config.yaml             # shared config for the test suite
├── tools/                           # Python helpers (visualize / inspect)
├── pcd_files/                       # gitignored; drop your own test fixtures here
├── data/                            # gitignored; datasets for slam_dataset_runner
└── CMakeLists.txt

Notes for porting to another project

• The library target is fast_csm::fast_csm (alias of fast_csm).
• C++17 is enforced via target_compile_features(fast_csm PUBLIC cxx_std_17) but is only set as a global default when fast_csm is the top-level project.
• All CMake options are namespaced (FAST_CSM_*) and default OFF.
• The vendored src/spdlog/ is linked as a PRIVATE interface library (fast_csm_spdlog); downstream code does not pick it up transitively.
• No PCL dependency anywhere.

License

No license file is currently included in the repository. If you plan to depend on this code, please contact the author before using it beyond personal evaluation.