112 lines
3.6 KiB
Python
112 lines
3.6 KiB
Python
#!/usr/bin/env python3
|
|
"""Build motion_pairs.json next to an existing summary without re-solving extrinsic.
|
|
|
|
Use this once for older calibration outputs that predate automatic pair caching.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import sys
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
if str(ROOT) not in sys.path:
|
|
sys.path.insert(0, str(ROOT))
|
|
|
|
from imu_lidar.imu_audit import audit_imu
|
|
from imu_lidar.imu_io import load_imu_samples
|
|
from imu_lidar.keyframes import build_keyframes
|
|
from imu_lidar.lidar_io import load_lidar_frames
|
|
from imu_lidar.motion_pairs import build_motion_pairs
|
|
from imu_lidar.motion_pairs_io import build_motion_pairs_payload, save_motion_pairs
|
|
|
|
|
|
def _load_summary_meta(summary_path: Path) -> tuple[float, np.ndarray, str]:
|
|
summary = json.loads(summary_path.read_text(encoding="utf-8"))
|
|
delta_t = float(summary.get("time_offset_s") or 0.0)
|
|
session = (summary.get("details") or {}).get("sessions", [{}])[0]
|
|
session_id = str(session.get("session_id") or summary_path.parent.name)
|
|
bias = np.asarray(
|
|
(session.get("imu_audit") or {}).get("gyro_bias_rad_s")
|
|
or (session.get("joint") or {}).get("gyro_bias_rad_s")
|
|
or [0.0, 0.0, 0.0],
|
|
dtype=float,
|
|
).reshape(3)
|
|
return delta_t, bias, session_id
|
|
|
|
|
|
def export_one(
|
|
*,
|
|
lidar: Path,
|
|
imu: Path,
|
|
summary: Path,
|
|
output: Path | None,
|
|
min_rotation_deg: float,
|
|
min_translation_m: float,
|
|
) -> Path:
|
|
delta_t, bias_from_summary, session_id = _load_summary_meta(summary)
|
|
imu_series = load_imu_samples(imu)
|
|
# Prefer freshly audited bias if summary bias is missing/zeros.
|
|
if float(np.linalg.norm(bias_from_summary)) < 1e-12:
|
|
bias = audit_imu(imu_series).gyro_bias_rad_s
|
|
else:
|
|
bias = bias_from_summary
|
|
|
|
frames = load_lidar_frames(lidar)
|
|
keyframes = build_keyframes(
|
|
frames,
|
|
min_translation_m=min_translation_m,
|
|
min_rotation_deg=min_rotation_deg,
|
|
)
|
|
pair_set = build_motion_pairs(
|
|
session_id=session_id,
|
|
keyframes=list(keyframes.frames),
|
|
keyframe_indices=keyframes.indices,
|
|
imu=imu_series,
|
|
delta_t_s=delta_t,
|
|
gyro_bias_rad_s=bias,
|
|
min_rotation_deg=min_rotation_deg,
|
|
min_translation_m=min_translation_m,
|
|
)
|
|
prepared = [
|
|
{
|
|
"session_id": session_id,
|
|
"time_offset_s": delta_t,
|
|
"gyro_bias_rad_s": np.asarray(bias, dtype=float).reshape(3),
|
|
"pairs": pair_set.pairs,
|
|
}
|
|
]
|
|
payload = build_motion_pairs_payload(prepared_sessions=prepared)
|
|
out = output or (summary.parent / "motion_pairs.json")
|
|
save_motion_pairs(out, payload)
|
|
print(f"wrote {out} ({len(pair_set.pairs)} pairs, session={session_id}, dt={delta_t:.6f})")
|
|
return out
|
|
|
|
|
|
def main() -> int:
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--lidar", type=Path, required=True)
|
|
parser.add_argument("--imu", type=Path, required=True)
|
|
parser.add_argument("--summary", type=Path, required=True)
|
|
parser.add_argument("--output", type=Path, default=None, help="Default: <summary_dir>/motion_pairs.json")
|
|
parser.add_argument("--min-pair-rotation-deg", type=float, default=2.0)
|
|
parser.add_argument("--min-pair-translation-m", type=float, default=0.3)
|
|
args = parser.parse_args()
|
|
export_one(
|
|
lidar=args.lidar,
|
|
imu=args.imu,
|
|
summary=args.summary,
|
|
output=args.output,
|
|
min_rotation_deg=args.min_pair_rotation_deg,
|
|
min_translation_m=args.min_pair_translation_m,
|
|
)
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|