diff --git a/data_only_viz/lidar_calib.py b/data_only_viz/lidar_calib.py index 647248f..ea0513e 100644 --- a/data_only_viz/lidar_calib.py +++ b/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 diff --git a/data_only_viz/scripts/calibrate_lidar.py b/data_only_viz/scripts/calibrate_lidar.py new file mode 100644 index 0000000..ce21c22 --- /dev/null +++ b/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()) diff --git a/data_only_viz/tests/test_lidar_calib.py b/data_only_viz/tests/test_lidar_calib.py index 2929e41..ba4a300 100644 --- a/data_only_viz/tests/test_lidar_calib.py +++ b/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)))