AutoNav Showcase (static, read-only)

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.

See these stages on real point clouds →