feat(icp): Kabsch + calibration CLI scaffold

data_only_viz/lidar_calib.py

@@ -56,3 +56,28 @@ def load_extrinsic(path: Path | None = None) -> Extrinsic:
 def _path_from_env() -> Path:
     p = os.environ.get("ICP_LIDAR_EXTRINSIC")
     return Path(p) if p else DEFAULT_EXTRINSIC_PATH
+
+
+def kabsch_rigid(src: np.ndarray, tgt: np.ndarray) -> np.ndarray:
+    """Closed-form rigid alignment (Kabsch via SVD).
+
+    Returns a 4x4 transform T such that ``tgt ≈ (src @ R.T) + t``.
+    """
+    src = np.asarray(src, dtype=np.float64)
+    tgt = np.asarray(tgt, dtype=np.float64)
+    if src.shape != tgt.shape:
+        raise ValueError(f"shape mismatch: src={src.shape} tgt={tgt.shape}")
+    if src.shape[0] < 3 or src.shape[1] != 3:
+        raise ValueError("kabsch_rigid needs at least 3 paired 3D points")
+    src_c = src.mean(axis=0)
+    tgt_c = tgt.mean(axis=0)
+    H = (src - src_c).T @ (tgt - tgt_c)
+    U, _, Vt = np.linalg.svd(H)
+    d = np.linalg.det(Vt.T @ U.T)
+    D = np.diag([1.0, 1.0, np.sign(d)])
+    R = Vt.T @ D @ U.T
+    t = tgt_c - R @ src_c
+    T = np.eye(4)
+    T[:3, :3] = R
+    T[:3, 3] = t
+    return T

data_only_viz/scripts/calibrate_lidar.py

@@ -0,0 +1,91 @@
+"""Interactive one-shot extrinsic calibration between iPhone LiDAR and webcam.
+
+Usage:
+
+    cd data_only_viz
+    uv run --extra lidar python -m data_only_viz.scripts.calibrate_lidar \
+        --lidar-host 192.168.0.42 --lidar-port 5500 --webcam-index 0
+
+The script prompts the user to assume 4 stances (front, left, right, back),
+captures paired pelvis points (webcam: Multi-HMR vertex 5559; LiDAR: centroid
+of the largest mesh anchor), solves Kabsch, and writes the result to
+ICP_LIDAR_EXTRINSIC or the default path.
+
+Multi-HMR worker is launched in-process for this script (single-shot mode).
+"""
+from __future__ import annotations
+
+import argparse
+import datetime as dt
+import logging
+import sys
+import time
+
+import numpy as np
+
+from data_only_viz.lidar_calib import Extrinsic, kabsch_rigid, save_extrinsic
+from data_only_viz.lidar_receiver import LidarTCPReader
+
+_LOG = logging.getLogger("calibrate_lidar")
+_PELVIS_VERT_INDEX = 5559  # SMPL-X canonical pelvis vertex
+
+
+def _wait_for_lidar(reader: LidarTCPReader, timeout_s: float = 5.0):
+    deadline = time.monotonic() + timeout_s
+    while time.monotonic() < deadline:
+        latest = reader.latest()
+        if latest is not None and latest.points.shape[0] > 50:
+            return latest
+        time.sleep(0.05)
+    raise RuntimeError("LiDAR frame never arrived")
+
+
+def _capture_one_pair(reader: LidarTCPReader, get_smplx_pelvis_cam) -> tuple[np.ndarray, np.ndarray]:
+    input("Hold still, then press ENTER to capture...")
+    lidar = _wait_for_lidar(reader)
+    pelvis_cam = get_smplx_pelvis_cam()
+    pelvis_arkit = lidar.points.mean(axis=0)
+    _LOG.info("captured: cam=%s  arkit=%s", pelvis_cam, pelvis_arkit)
+    return pelvis_cam, pelvis_arkit
+
+
+def main(argv: list[str] | None = None) -> int:
+    p = argparse.ArgumentParser()
+    p.add_argument("--lidar-host", required=True)
+    p.add_argument("--lidar-port", type=int, default=5500)
+    p.add_argument("--webcam-index", type=int, default=0)
+    p.add_argument("--stances", type=int, default=4)
+    args = p.parse_args(argv)
+    logging.basicConfig(level=logging.INFO, format="%(asctime)s %(name)s %(levelname)s %(message)s")
+
+    reader = LidarTCPReader(host=args.lidar_host, port=args.lidar_port)
+    reader.start()
+
+    # NB: the actual Multi-HMR getter is wired in Task 9 when the main pipeline
+    # exposes a single-shot predictor. For now this script is the *scaffolding*
+    # — Task 9 plugs in `multi_hmr_worker.predict_once()`.
+    def _placeholder_pelvis_cam() -> np.ndarray:
+        raise SystemExit("calibrate_lidar requires Task 9 to be complete (predict_once API)")
+
+    pairs_cam, pairs_arkit = [], []
+    try:
+        for i in range(args.stances):
+            _LOG.info("stance %d/%d", i + 1, args.stances)
+            cam, arkit = _capture_one_pair(reader, _placeholder_pelvis_cam)
+            pairs_cam.append(cam)
+            pairs_arkit.append(arkit)
+    finally:
+        reader.stop()
+
+    T = kabsch_rigid(np.asarray(pairs_arkit), np.asarray(pairs_cam))
+    path = save_extrinsic(Extrinsic(
+        T_arkit_to_cam=T,
+        confidence=1.0,
+        captured_at_iso=dt.datetime.now(dt.timezone.utc).isoformat(),
+    ))
+    _LOG.info("extrinsic saved to %s", path)
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

data_only_viz/tests/test_lidar_calib.py

@@ -39,3 +39,38 @@ def test_load_extrinsic_missing_path_returns_identity(tmp_path: Path) -> None:
     e = load_extrinsic(tmp_path / "does-not-exist.json")
     np.testing.assert_allclose(e.T_arkit_to_cam, np.eye(4))
     assert e.confidence == 0.0
+
+
+def test_kabsch_recovers_known_rigid_transform() -> None:
+    from data_only_viz.lidar_calib import kabsch_rigid
+
+    rng = np.random.RandomState(7)
+    src = rng.randn(20, 3)
+    theta = np.deg2rad(30.0)
+    R = np.array([
+        [np.cos(theta), 0, np.sin(theta)],
+        [0, 1, 0],
+        [-np.sin(theta), 0, np.cos(theta)],
+    ])
+    t = np.array([0.1, -0.2, 0.5])
+    tgt = src @ R.T + t
+
+    T = kabsch_rigid(src, tgt)
+    R_est = T[:3, :3]
+    t_est = T[:3, 3]
+    np.testing.assert_allclose(R_est, R, atol=1e-6)
+    np.testing.assert_allclose(t_est, t, atol=1e-6)
+
+
+def test_kabsch_requires_at_least_three_pairs() -> None:
+    from data_only_viz.lidar_calib import kabsch_rigid
+
+    with pytest.raises(ValueError, match="at least 3"):
+        kabsch_rigid(np.zeros((2, 3)), np.zeros((2, 3)))
+
+
+def test_kabsch_rejects_mismatched_shapes() -> None:
+    from data_only_viz.lidar_calib import kabsch_rigid
+
+    with pytest.raises(ValueError, match="shape"):
+        kabsch_rigid(np.zeros((5, 3)), np.zeros((4, 3)))