C · Algorithm laboratory
Pipeline, stage by stage
1 · Preprocessing
Drop non-finite points, keep ranges 0.5–50 m, then replace every occupied 0.15 m voxel by the centroid of its points. Voxel size trades registration accuracy for speed.
points per frame: raw 65536 → valid 65536 → range 56363 → voxel 31761 (median, MEASURED)
2 · ICP registration
Scan k is matched to scan k−1. Each iteration finds the nearest target point for every source point (k-d tree), keeps matches within 1.5 m, and solves for the rigid transform by SVD (Kabsch). It stops when the update is below 1e-4 m or after 30 iterations.
minimise Σ ‖qi − (R pi + t)‖² · variant p2plane: Σ ((R pi + t − qi)·ni)²
3 · Validity gate
An ICP result only counts if it passes four checks. Thresholds were fixed from sensor physics before comparing with ground truth, and rejections are counted by reason.
| inlier ratio ≥ | 0.30 |
|---|---|
| mean residual ≤ (m) | 0.25 |
| step translation ≤ (m) | 1.0 |
| step rotation ≤ (°) | 20 |
4 · Trajectory and frames
ICP returns T, which maps scan k into scan k−1. The sensor pose accumulates as Wk = Wk−1 · T. Using the inverse instead would mirror the trajectory; an earlier version did, and tests now compare against hand-derived poses.
5 · Evaluation
Poses are matched to ground truth by timestamp (within 20 ms). ATE is the position error after a rigid Umeyama alignment. RPE is the position error over fixed time or path-length windows.
The ground-truth body frame differs from the sensor frame (a ~141° yaw was observed), so rotation RPE is UNAVAILABLE and RPE uses world-frame positions after a global alignment. That removes the step-direction offset but not a sensor-to-body lever arm, so ATE and position RPE mix drift with extrinsic bias. See decision DEC-012 in the repository.
6 · Variants
- baseline: the algorithm above.
- optimized: same algorithm, faster implementation (packed-key voxel filter, cached tree, parallel queries).
- p2plane: point-to-plane ICP with a constant-velocity initial guess; preprocessing identical to baseline.