添加 LiDAR-IMU 外参标定流水线与说明文档
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -0,0 +1 @@
|
||||
# Tools package for local scripts and tests.
|
||||
@@ -0,0 +1,75 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Compare two S2 offline summary.json runs (real artifacts only)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.geometry import rpy_deg_xyz, so3_log
|
||||
|
||||
|
||||
def dig(path: Path):
|
||||
data = json.loads(path.read_text(encoding="utf-8"))
|
||||
session = data["details"]["sessions"][0]
|
||||
return data, session, session.get("handeye", {}), session.get("time_offset", {})
|
||||
|
||||
|
||||
def main() -> int:
|
||||
old_path = Path(sys.argv[1])
|
||||
new_path = Path(sys.argv[2])
|
||||
old, so, heo, too = dig(old_path)
|
||||
new, sn, hen, ton = dig(new_path)
|
||||
|
||||
print("==== COMPARISON (real summary.json artifacts) ====")
|
||||
print(f"{'metric':28s} {'run_a':28s} {'run_b':28s}")
|
||||
rows = [
|
||||
("status", old.get("status"), new.get("status")),
|
||||
("stage", so.get("stage"), sn.get("stage")),
|
||||
("delta_t_s", f"{too.get('delta_t_s'):.6f}", f"{ton.get('delta_t_s'):.6f}"),
|
||||
(
|
||||
"corr/mag_peak",
|
||||
f"{too.get('correlation_peak'):.6f}",
|
||||
f"{ton.get('correlation_peak'):.6f}",
|
||||
),
|
||||
("keyframes", so.get("keyframes"), sn.get("keyframes")),
|
||||
("handeye_pairs", heo.get("pair_count"), hen.get("pair_count")),
|
||||
("handeye_ok", heo.get("ok"), hen.get("ok")),
|
||||
("rms_deg", f"{heo.get('residual_rms_deg'):.4f}", f"{hen.get('residual_rms_deg'):.4f}"),
|
||||
(
|
||||
"median_deg",
|
||||
f"{heo.get('residual_median_deg'):.4f}",
|
||||
f"{hen.get('residual_median_deg'):.4f}",
|
||||
),
|
||||
]
|
||||
for key, a, b in rows:
|
||||
print(f"{key:28s} {str(a):28s} {str(b):28s}")
|
||||
|
||||
print("run_a pair_notes:", so.get("pair_notes"))
|
||||
print("run_b pair_notes:", sn.get("pair_notes"))
|
||||
print("run_a time notes:", too.get("notes"))
|
||||
print("run_b time notes:", ton.get("notes"))
|
||||
print("run_a handeye notes:", heo.get("notes"))
|
||||
print("run_b handeye notes:", hen.get("notes"))
|
||||
|
||||
r_old = np.asarray(heo["R_IMU_lidar"], dtype=float)
|
||||
r_new = np.asarray(hen["R_IMU_lidar"], dtype=float)
|
||||
print("R relative change deg:", float(np.degrees(np.linalg.norm(so3_log(r_old.T @ r_new)))))
|
||||
print("RPY run_a deg:", rpy_deg_xyz(r_old))
|
||||
print("RPY run_b deg:", rpy_deg_xyz(r_new))
|
||||
print("delta rms (b-a):", hen.get("residual_rms_deg") - heo.get("residual_rms_deg"))
|
||||
print(
|
||||
"delta median (b-a):",
|
||||
hen.get("residual_median_deg") - heo.get("residual_median_deg"),
|
||||
)
|
||||
print("artifacts:")
|
||||
print(" run_a:", old_path)
|
||||
print(" run_b:", new_path)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Generate a tiny synthetic session for V1 smoke tests."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.contracts import ImuSeries, LidarFrame
|
||||
from imu_lidar.geometry import so3_exp
|
||||
from imu_lidar.imu_io import save_imu_csv
|
||||
from imu_lidar.lidar_io import save_lidar_session
|
||||
|
||||
|
||||
def _wall_cloud(rng: np.random.Generator, n: int = 800) -> np.ndarray:
|
||||
yz = rng.uniform([-5, -1], [5, 3], size=(n // 3, 2))
|
||||
wall_x = np.column_stack([np.full(n // 3, 8.0), yz[:, 0], yz[:, 1]])
|
||||
xz = rng.uniform([-5, -1], [5, 3], size=(n // 3, 2))
|
||||
wall_y = np.column_stack([xz[:, 0], np.full(n // 3, 6.0), xz[:, 1]])
|
||||
xy = rng.uniform([-5, -5], [5, 5], size=(n - 2 * (n // 3), 2))
|
||||
ground = np.column_stack([xy[:, 0], xy[:, 1], np.full(xy.shape[0], -1.0)])
|
||||
return np.vstack([wall_x, wall_y, ground])
|
||||
|
||||
|
||||
def generate_synthetic_session(
|
||||
output_root: Path,
|
||||
*,
|
||||
delta_t_s: float = 0.17,
|
||||
yaw_extrinsic_deg: float = 25.0,
|
||||
seed: int = 0,
|
||||
) -> dict[str, float]:
|
||||
"""Write IMU CSV + LiDAR frames with known extrinsic rotation and time offset."""
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
output_root = Path(output_root)
|
||||
output_root.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
r_x = so3_exp(np.deg2rad(np.array([2.0, -1.5, yaw_extrinsic_deg])))
|
||||
map_points = _wall_cloud(rng)
|
||||
|
||||
lidar_hz = 10.0
|
||||
duration = 8.0
|
||||
lidar_times = np.arange(0.0, duration, 1.0 / lidar_hz)
|
||||
# Non-yaw excitation is required for unique SO(3) hand-eye observability.
|
||||
yaw = 0.5 * np.sin(0.8 * lidar_times) + 0.12 * lidar_times
|
||||
pitch = 0.18 * np.sin(1.3 * lidar_times + 0.4)
|
||||
roll = 0.12 * np.sin(1.7 * lidar_times + 1.0)
|
||||
yaw_rate = np.gradient(yaw, lidar_times)
|
||||
pitch_rate = np.gradient(pitch, lidar_times)
|
||||
roll_rate = np.gradient(roll, lidar_times)
|
||||
|
||||
frames: list[LidarFrame] = []
|
||||
for index, (t, yaw_i, pitch_i, roll_i) in enumerate(zip(lidar_times, yaw, pitch, roll)):
|
||||
r_wl = so3_exp(np.array([roll_i, pitch_i, yaw_i]))
|
||||
t_wl = np.array([0.4 * t, 0.05 * np.sin(0.5 * t), 0.0])
|
||||
points = (map_points - t_wl) @ r_wl
|
||||
points = points + rng.normal(0.0, 0.01, size=points.shape)
|
||||
frames.append(
|
||||
LidarFrame(
|
||||
frame_id=str(index),
|
||||
t_start_s=float(t),
|
||||
t_end_s=float(t + 0.08),
|
||||
points_xyz=points.astype(float),
|
||||
)
|
||||
)
|
||||
save_lidar_session(output_root / "lidar", frames)
|
||||
|
||||
imu_hz = 100.0
|
||||
t_lidar_grid = np.arange(0.0, duration, 1.0 / imu_hz)
|
||||
omega_lidar = np.column_stack(
|
||||
[
|
||||
np.interp(t_lidar_grid, lidar_times, roll_rate),
|
||||
np.interp(t_lidar_grid, lidar_times, pitch_rate),
|
||||
np.interp(t_lidar_grid, lidar_times, yaw_rate),
|
||||
]
|
||||
)
|
||||
omega_imu = omega_lidar @ r_x.T
|
||||
|
||||
g_world = np.array([0.0, 0.0, 9.80665])
|
||||
acc_rows = []
|
||||
for yaw_i, pitch_i, roll_i in zip(
|
||||
np.interp(t_lidar_grid, lidar_times, yaw),
|
||||
np.interp(t_lidar_grid, lidar_times, pitch),
|
||||
np.interp(t_lidar_grid, lidar_times, roll),
|
||||
):
|
||||
r_wl = so3_exp(np.array([roll_i, pitch_i, yaw_i]))
|
||||
g_in_lidar = r_wl.T @ g_world
|
||||
acc_rows.append(r_x @ g_in_lidar)
|
||||
acc = np.asarray(acc_rows, dtype=float)
|
||||
|
||||
static_t = np.arange(-1.0, 0.0, 1.0 / imu_hz)
|
||||
static_gyro = np.zeros((static_t.size, 3))
|
||||
static_acc = np.tile(r_x @ g_world, (static_t.size, 1))
|
||||
|
||||
t_imu = np.concatenate([static_t + delta_t_s, t_lidar_grid + delta_t_s])
|
||||
gyro = np.vstack([static_gyro, omega_imu]) + rng.normal(0.0, 0.001, size=(t_imu.size, 3))
|
||||
acc_all = np.vstack([static_acc, acc]) + rng.normal(0.0, 0.01, size=(t_imu.size, 3))
|
||||
imu = ImuSeries(t_s=t_imu, gyro_rad_s=gyro, acc_m_s2=acc_all)
|
||||
save_imu_csv(output_root / "imu.csv", imu)
|
||||
|
||||
return {
|
||||
"delta_t_s": float(delta_t_s),
|
||||
"yaw_extrinsic_deg": float(yaw_extrinsic_deg),
|
||||
}
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import json
|
||||
|
||||
out = Path("examples/synthetic_session")
|
||||
meta = generate_synthetic_session(out)
|
||||
(out / "meta.json").write_text(json.dumps(meta, indent=2), encoding="utf-8")
|
||||
print(f"wrote {out}")
|
||||
print(meta)
|
||||
@@ -0,0 +1,23 @@
|
||||
# 一键复现合成标定（Windows）
|
||||
# 用法：在仓库根目录执行
|
||||
# powershell -File tools\reproduce_synthetic.ps1
|
||||
# powershell -File tools\reproduce_synthetic.ps1 -SkipPytest
|
||||
# powershell -File tools\reproduce_synthetic.ps1 -Mode full_se3
|
||||
|
||||
param(
|
||||
[ValidateSet("rotation_only", "full_se3")]
|
||||
[string]$Mode = "rotation_only",
|
||||
[switch]$SkipPytest
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Root = Split-Path -Parent $PSScriptRoot
|
||||
Set-Location $Root
|
||||
|
||||
$args = @("tools\reproduce_synthetic.py", "--mode", $Mode)
|
||||
if ($SkipPytest) {
|
||||
$args += "--skip-pytest"
|
||||
}
|
||||
|
||||
python @args
|
||||
exit $LASTEXITCODE
|
||||
@@ -0,0 +1,109 @@
|
||||
"""One-click synthetic reproduce: generate → calibrate → report → pytest."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def _repo_root() -> Path:
|
||||
return Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def _run(cmd: list[str], cwd: Path) -> None:
|
||||
print("+", " ".join(cmd), flush=True)
|
||||
completed = subprocess.run(cmd, cwd=str(cwd), check=False)
|
||||
if completed.returncode != 0:
|
||||
raise SystemExit(completed.returncode)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Generate synthetic LiDAR–IMU data, run calibration, show report, run tests."
|
||||
)
|
||||
parser.add_argument(
|
||||
"--mode",
|
||||
choices=["rotation_only", "full_se3"],
|
||||
default="rotation_only",
|
||||
)
|
||||
parser.add_argument("--skip-pytest", action="store_true")
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
root = _repo_root()
|
||||
session = root / "examples" / "synthetic_session"
|
||||
imu = session / "imu.csv"
|
||||
lidar = session / "lidar"
|
||||
calib_out = session / "out"
|
||||
config = root / "config" / "vehicle_installation.template.yaml"
|
||||
|
||||
print("=== 1/4 generate synthetic session ===", flush=True)
|
||||
_run([sys.executable, str(root / "tools" / "generate_synthetic_session.py")], cwd=root)
|
||||
|
||||
print("=== 2/4 run calibration ===", flush=True)
|
||||
_run(
|
||||
[
|
||||
sys.executable,
|
||||
"-m",
|
||||
"imu_lidar.cli",
|
||||
"run",
|
||||
"--vehicle-config",
|
||||
str(config),
|
||||
"--imu",
|
||||
str(imu),
|
||||
"--lidar",
|
||||
str(lidar),
|
||||
"--output",
|
||||
str(calib_out),
|
||||
"--mode",
|
||||
args.mode,
|
||||
"--time-offset-search-s",
|
||||
"0.5",
|
||||
"--min-pair-rotation-deg",
|
||||
"2.0",
|
||||
"--min-pair-translation-m",
|
||||
"0.05",
|
||||
"--max-iterations",
|
||||
"1",
|
||||
],
|
||||
cwd=root,
|
||||
)
|
||||
|
||||
print("=== 3/4 show report ===", flush=True)
|
||||
_run(
|
||||
[
|
||||
sys.executable,
|
||||
str(root / "tools" / "show_calibration_report.py"),
|
||||
"--summary",
|
||||
str(calib_out / "summary.json"),
|
||||
"--truth-meta",
|
||||
str(session / "meta.json"),
|
||||
"--plot",
|
||||
str(calib_out / "report_preview.png"),
|
||||
],
|
||||
cwd=root,
|
||||
)
|
||||
|
||||
if args.skip_pytest:
|
||||
print("=== 4/4 pytest skipped ===", flush=True)
|
||||
else:
|
||||
print("=== 4/4 pytest ===", flush=True)
|
||||
_run([sys.executable, "-m", "pytest", "-q"], cwd=root)
|
||||
|
||||
print("\nDone.")
|
||||
print(" INPUT")
|
||||
print(f" IMU CSV : {imu}")
|
||||
print(f" LiDAR dir : {lidar}")
|
||||
print(f" vehicle YAML: {config}")
|
||||
print(" OUTPUT")
|
||||
print(f" directory : {calib_out}")
|
||||
print(f" T : {calib_out / 'T_IMU_lidar.json'}")
|
||||
print(f" δt : {calib_out / 'time_offset.json'}")
|
||||
print(f" summary : {calib_out / 'summary.json'}")
|
||||
print(f" preview PNG : {calib_out / 'report_preview.png'}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,133 @@
|
||||
"""Pretty-print calibration summary.json and optionally write a small preview figure."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def _load(path: Path) -> dict:
|
||||
return json.loads(path.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _session0(summary: dict) -> dict:
|
||||
sessions = summary.get("details", {}).get("sessions", [])
|
||||
return sessions[0] if sessions else {}
|
||||
|
||||
|
||||
def print_report(summary: dict, truth: dict | None = None) -> None:
|
||||
print("---------- calibration report ----------")
|
||||
print(f"status : {summary.get('status')}")
|
||||
print(f"message: {summary.get('message')}")
|
||||
dt = summary.get("time_offset_s")
|
||||
if dt is not None:
|
||||
print(f"δt : {dt:.6f} s (t_imu = t_lidar + δt)")
|
||||
if truth and "delta_t_s" in truth:
|
||||
print(f" truth={truth['delta_t_s']:.6f} s err={dt - truth['delta_t_s']:+.6f} s")
|
||||
|
||||
t_block = summary.get("T_IMU_lidar")
|
||||
if isinstance(t_block, dict):
|
||||
rpy = t_block.get("rpy_deg_xyz")
|
||||
trans = t_block.get("translation_m")
|
||||
if rpy is not None:
|
||||
print(f"RPY xyz: [{rpy[0]:.3f}, {rpy[1]:.3f}, {rpy[2]:.3f}] deg")
|
||||
if truth and "yaw_extrinsic_deg" in truth:
|
||||
print(f" yaw truth≈{truth['yaw_extrinsic_deg']:.3f} deg")
|
||||
if trans is not None:
|
||||
print(f"t : [{trans[0]:.4f}, {trans[1]:.4f}, {trans[2]:.4f}] m")
|
||||
|
||||
session = _session0(summary)
|
||||
handeye = session.get("handeye") or {}
|
||||
joint = session.get("joint") or {}
|
||||
if handeye:
|
||||
print(
|
||||
"handeye: "
|
||||
f"pairs={handeye.get('pair_count')} "
|
||||
f"rms={handeye.get('residual_rms_deg')}° "
|
||||
f"median={handeye.get('residual_median_deg')}° "
|
||||
f"ok={handeye.get('ok')}"
|
||||
)
|
||||
if joint:
|
||||
obs = joint.get("observability") or {}
|
||||
print(
|
||||
"joint : "
|
||||
f"rot_rms={joint.get('residual_rms_rot_deg')}° "
|
||||
f"trans_rms={joint.get('residual_rms_trans_m')} m "
|
||||
f"trans_accepted={joint.get('translation_accepted')}"
|
||||
)
|
||||
print(
|
||||
"observ : "
|
||||
f"rotation={obs.get('rotation_observable')} "
|
||||
f"translation={obs.get('translation_observable')} "
|
||||
f"cond_R={obs.get('condition_rotation')}"
|
||||
)
|
||||
if joint.get("gravity_m_s2") is not None:
|
||||
print(f"gravity: {joint.get('gravity_m_s2')}")
|
||||
if joint.get("gyro_bias_rad_s") is not None:
|
||||
print(f"b_g : {joint.get('gyro_bias_rad_s')}")
|
||||
print("----------------------------------------")
|
||||
|
||||
|
||||
def maybe_plot(summary: dict, plot_path: Path, truth: dict | None = None) -> None:
|
||||
try:
|
||||
import matplotlib.pyplot as plt
|
||||
except ImportError:
|
||||
print("(matplotlib not installed; skip plot)")
|
||||
return
|
||||
|
||||
session = _session0(summary)
|
||||
handeye = session.get("handeye") or {}
|
||||
joint = session.get("joint") or {}
|
||||
labels = []
|
||||
values = []
|
||||
if handeye.get("residual_rms_deg") is not None:
|
||||
labels.append("handeye\nRMS (°)")
|
||||
values.append(float(handeye["residual_rms_deg"]))
|
||||
if joint.get("residual_rms_rot_deg") is not None:
|
||||
labels.append("joint rot\nRMS (°)")
|
||||
values.append(float(joint["residual_rms_rot_deg"]))
|
||||
if joint.get("residual_rms_trans_m") is not None:
|
||||
labels.append("joint trans\nRMS (m)")
|
||||
values.append(float(joint["residual_rms_trans_m"]))
|
||||
dt = summary.get("time_offset_s")
|
||||
if dt is not None:
|
||||
labels.append("|δt| (s)")
|
||||
values.append(abs(float(dt)))
|
||||
if truth and "delta_t_s" in truth:
|
||||
labels.append("|δt err| (s)")
|
||||
values.append(abs(float(dt) - float(truth["delta_t_s"])))
|
||||
|
||||
if not labels:
|
||||
print("(no numeric fields to plot)")
|
||||
return
|
||||
|
||||
fig, ax = plt.subplots(figsize=(7.5, 3.8))
|
||||
ax.bar(labels, values, color="#3b6ea5")
|
||||
ax.set_title(f"Calibration preview — {summary.get('status')}")
|
||||
ax.set_ylabel("value")
|
||||
ax.grid(axis="y", alpha=0.3)
|
||||
fig.tight_layout()
|
||||
plot_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.savefig(plot_path, dpi=120)
|
||||
plt.close(fig)
|
||||
print(f"wrote plot: {plot_path}")
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description="Show LiDAR–IMU calibration summary")
|
||||
parser.add_argument("--summary", type=Path, required=True, help="Path to summary.json")
|
||||
parser.add_argument("--truth-meta", type=Path, default=None, help="Optional synthetic meta.json")
|
||||
parser.add_argument("--plot", type=Path, default=None, help="Optional PNG path for a bar chart")
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
summary = _load(args.summary)
|
||||
truth = _load(args.truth_meta) if args.truth_meta and args.truth_meta.exists() else None
|
||||
print_report(summary, truth)
|
||||
if args.plot is not None:
|
||||
maybe_plot(summary, args.plot, truth)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,27 @@
|
||||
param(
|
||||
[Parameter(Mandatory = $true)][string]$Lidar,
|
||||
[Parameter(Mandatory = $true)][string]$Imu,
|
||||
[Parameter(Mandatory = $true)][string]$Summary,
|
||||
[int]$PairIndex = 0,
|
||||
[double]$Voxel = 0.12,
|
||||
[string]$SavePng = ""
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$Root = Split-Path -Parent $PSScriptRoot
|
||||
Set-Location $Root
|
||||
|
||||
$args = @(
|
||||
"tools\visualize_pair_3d.py",
|
||||
"--lidar", $Lidar,
|
||||
"--imu", $Imu,
|
||||
"--summary", $Summary,
|
||||
"--pair-index", "$PairIndex",
|
||||
"--voxel", "$Voxel"
|
||||
)
|
||||
if ($SavePng -ne "") {
|
||||
$args += @("--save-png", $SavePng)
|
||||
}
|
||||
|
||||
python @args
|
||||
if ($LASTEXITCODE -ne 0) { throw "visualize_pair_3d failed: $LASTEXITCODE" }
|
||||
@@ -0,0 +1,452 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Interactive 3D inspection of LiDAR–IMU motion-pair registration.
|
||||
|
||||
Similar to the RTK–LiDAR ``visualize_pair_3d`` viewer, but A comes from IMU
|
||||
preintegration and X is ``T_IMU_lidar``.
|
||||
|
||||
Modes (keyboard):
|
||||
1 raw source (no transform)
|
||||
2 IMU prediction with X=I (B_pred = A)
|
||||
3 LiDAR registration B (reference)
|
||||
4 calibrated prediction B_pred = X^{-1} A X
|
||||
N / ] next motion pair
|
||||
P / [ previous motion pair
|
||||
Q / Esc exit
|
||||
|
||||
Blue = target keyframe i; orange = source keyframe j after the selected transform.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.geometry import (
|
||||
inverse_transform,
|
||||
make_transform,
|
||||
rotation_angle_deg,
|
||||
rpy_deg_xyz,
|
||||
transform_points,
|
||||
)
|
||||
from imu_lidar.imu_io import load_imu_samples
|
||||
from imu_lidar.imu_preintegration import preintegrate_imu
|
||||
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.registration import register_lidar_pair
|
||||
from imu_lidar.time_offset import lidar_time_to_imu_time
|
||||
|
||||
|
||||
COLORS = {
|
||||
"target": [0.10, 0.65, 1.00],
|
||||
"source": [1.00, 0.35, 0.05],
|
||||
}
|
||||
|
||||
MODE_NAMES = (
|
||||
"1 raw",
|
||||
"2 IMU (X=I)",
|
||||
"3 LiDAR B",
|
||||
"4 calibrated X^-1 A X",
|
||||
)
|
||||
|
||||
|
||||
def _load_extrinsic(summary_path: Path) -> tuple[np.ndarray, float, np.ndarray]:
|
||||
summary = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
t_block = summary.get("T_IMU_lidar")
|
||||
if t_block is None:
|
||||
matrix = summary.get("matrix")
|
||||
if matrix is not None:
|
||||
return np.asarray(matrix, dtype=float), 0.0, np.zeros(3)
|
||||
raise ValueError(f"no T_IMU_lidar in {summary_path}")
|
||||
t_mat = np.asarray(t_block["matrix"], dtype=float)
|
||||
dt = float(summary.get("time_offset_s") or 0.0)
|
||||
session = (summary.get("details") or {}).get("sessions", [{}])[0]
|
||||
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 t_mat, dt, bias
|
||||
|
||||
|
||||
def _delta_components(reference: np.ndarray, candidate: np.ndarray) -> dict:
|
||||
delta = inverse_transform(reference) @ candidate
|
||||
translation = np.asarray(delta[:3, 3], dtype=float)
|
||||
return {
|
||||
"translation_xyz_cm": (translation * 100.0).tolist(),
|
||||
"translation_norm_cm": float(np.linalg.norm(translation) * 100.0),
|
||||
"rotation_rpy_deg_xyz": rpy_deg_xyz(delta[:3, :3]).tolist(),
|
||||
"rotation_angle_deg": rotation_angle_deg(delta[:3, :3]),
|
||||
}
|
||||
|
||||
|
||||
def _print_delta(name: str, reference: np.ndarray, candidate: np.ndarray) -> dict:
|
||||
item = _delta_components(reference, candidate)
|
||||
tx, ty, tz = item["translation_xyz_cm"]
|
||||
roll, pitch, yaw = item["rotation_rpy_deg_xyz"]
|
||||
print(
|
||||
f"{name}: B^-1*motion "
|
||||
f"t_xyz=[{tx:+.3f}, {ty:+.3f}, {tz:+.3f}] cm "
|
||||
f"rpy=[{roll:+.3f}, {pitch:+.3f}, {yaw:+.3f}] deg "
|
||||
f"|t|={item['translation_norm_cm']:.3f} cm "
|
||||
f"|R|={item['rotation_angle_deg']:.4f} deg"
|
||||
)
|
||||
return item
|
||||
|
||||
|
||||
def _cloud(o3d, points: np.ndarray, color, voxel: float):
|
||||
item = o3d.geometry.PointCloud()
|
||||
item.points = o3d.utility.Vector3dVector(points)
|
||||
if voxel > 0:
|
||||
item = item.voxel_down_sample(voxel)
|
||||
item.paint_uniform_color(color)
|
||||
return item
|
||||
|
||||
|
||||
def _set_cloud_points(cloud, points: np.ndarray, color, voxel: float, o3d) -> None:
|
||||
tmp = _cloud(o3d, points, color, voxel)
|
||||
cloud.points = tmp.points
|
||||
cloud.colors = tmp.colors
|
||||
|
||||
|
||||
def _build_pair_list(
|
||||
*,
|
||||
lidar_dir: Path,
|
||||
imu_path: Path,
|
||||
delta_t_s: float,
|
||||
gyro_bias: np.ndarray,
|
||||
min_rotation_deg: float,
|
||||
min_translation_m: float,
|
||||
):
|
||||
frames = load_lidar_frames(lidar_dir)
|
||||
imu = load_imu_samples(imu_path)
|
||||
keyframes = build_keyframes(
|
||||
frames,
|
||||
min_translation_m=min_translation_m,
|
||||
min_rotation_deg=min_rotation_deg,
|
||||
)
|
||||
pair_set = build_motion_pairs(
|
||||
session_id="viz",
|
||||
keyframes=list(keyframes.frames),
|
||||
keyframe_indices=keyframes.indices,
|
||||
imu=imu,
|
||||
delta_t_s=delta_t_s,
|
||||
gyro_bias_rad_s=gyro_bias,
|
||||
min_rotation_deg=min_rotation_deg,
|
||||
min_translation_m=min_translation_m,
|
||||
)
|
||||
return frames, imu, keyframes, pair_set
|
||||
|
||||
|
||||
def _pair_from_indices(
|
||||
frames,
|
||||
imu,
|
||||
*,
|
||||
i: int,
|
||||
j: int,
|
||||
delta_t_s: float,
|
||||
gyro_bias: np.ndarray,
|
||||
):
|
||||
frame_i = frames[i]
|
||||
frame_j = frames[j]
|
||||
reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
|
||||
t_i = lidar_time_to_imu_time(frame_i.t_mid_s, delta_t_s)
|
||||
t_j = lidar_time_to_imu_time(frame_j.t_mid_s, delta_t_s)
|
||||
preint = preintegrate_imu(
|
||||
imu.t_s,
|
||||
imu.gyro_rad_s,
|
||||
imu.acc_m_s2,
|
||||
t_i,
|
||||
t_j,
|
||||
gyro_bias,
|
||||
np.zeros(3),
|
||||
)
|
||||
a = make_transform(preint.delta_p, preint.delta_R)
|
||||
return frame_i, frame_j, a, reg.transform
|
||||
|
||||
|
||||
def _transforms_for_pair(x: np.ndarray, a_ij: np.ndarray, b_gicp: np.ndarray) -> dict[str, np.ndarray]:
|
||||
return {
|
||||
MODE_NAMES[0]: np.eye(4),
|
||||
MODE_NAMES[1]: a_ij.copy(),
|
||||
MODE_NAMES[2]: b_gicp.copy(),
|
||||
MODE_NAMES[3]: inverse_transform(x) @ a_ij @ x,
|
||||
}
|
||||
|
||||
|
||||
def _resolve_pair(frames, pairs, pair_index: int, x: np.ndarray):
|
||||
pair = pairs[pair_index]
|
||||
frame_i = frames[pair.i]
|
||||
frame_j = frames[pair.j]
|
||||
a_ij = make_transform(
|
||||
pair.t_A_m if pair.t_A_m is not None else np.zeros(3),
|
||||
pair.R_A,
|
||||
)
|
||||
b_gicp = make_transform(
|
||||
pair.t_B_m if pair.t_B_m is not None else np.zeros(3),
|
||||
pair.R_B,
|
||||
)
|
||||
transforms = _transforms_for_pair(x, a_ij, b_gicp)
|
||||
label = (
|
||||
f"pair {pair_index + 1}/{len(pairs)} "
|
||||
f"frames {pair.i} <- {pair.j} "
|
||||
f"rotB={rotation_angle_deg(pair.R_B):.2f} deg "
|
||||
f"|tB|={0.0 if pair.t_B_m is None else float(np.linalg.norm(pair.t_B_m)):.3f} m"
|
||||
)
|
||||
return frame_i, frame_j, a_ij, b_gicp, transforms, label
|
||||
|
||||
|
||||
def _print_pair_header(label: str, b_gicp: np.ndarray, transforms: dict[str, np.ndarray]) -> None:
|
||||
print("-" * 72)
|
||||
print(label)
|
||||
print("blue=target i | orange=source j")
|
||||
print("1-4: overlay mode | N/]: next pair | P/[: prev pair | Q/Esc: exit")
|
||||
baseline = _print_delta("mode4 minus mode3", b_gicp, transforms[MODE_NAMES[3]])
|
||||
roll, pitch, yaw = np.abs(baseline["rotation_rpy_deg_xyz"])
|
||||
if max(roll, pitch) > max(0.10, 2.0 * yaw):
|
||||
print("note: roll/pitch dominate yaw on this pair.")
|
||||
tx, ty, tz = np.abs(baseline["translation_xyz_cm"])
|
||||
if tz > max(tx, ty):
|
||||
print("note: largest translation component is Z for this pair.")
|
||||
|
||||
|
||||
def _save_topdown_png(
|
||||
path: Path,
|
||||
target: np.ndarray,
|
||||
source: np.ndarray,
|
||||
transforms: dict[str, np.ndarray],
|
||||
) -> None:
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
def downsample(points: np.ndarray) -> np.ndarray:
|
||||
if points.shape[0] <= 8000:
|
||||
return points
|
||||
idx = np.linspace(0, points.shape[0] - 1, 8000).astype(int)
|
||||
return points[idx]
|
||||
|
||||
names = list(transforms.keys())
|
||||
fig, axes = plt.subplots(2, 2, figsize=(10, 10), sharex=True, sharey=True)
|
||||
tgt = downsample(target)
|
||||
for ax, name in zip(axes.ravel(), names):
|
||||
src = downsample(transform_points(source, transforms[name]))
|
||||
ax.scatter(tgt[:, 0], tgt[:, 1], s=1, c="tab:blue", alpha=0.35, label="target i")
|
||||
ax.scatter(src[:, 0], src[:, 1], s=1, c="tab:orange", alpha=0.35, label="source j")
|
||||
ax.set_title(name)
|
||||
ax.set_aspect("equal", adjustable="box")
|
||||
ax.grid(alpha=0.3)
|
||||
axes[0, 0].legend(loc="upper right", markerscale=4)
|
||||
fig.suptitle("LiDAR–IMU pair overlay (XY top-down)")
|
||||
fig.tight_layout()
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.savefig(path, dpi=140)
|
||||
plt.close(fig)
|
||||
print(f"wrote {path}")
|
||||
|
||||
|
||||
def _run_gui(
|
||||
*,
|
||||
frames,
|
||||
pairs,
|
||||
x: np.ndarray,
|
||||
start_index: int,
|
||||
voxel: float,
|
||||
fixed_single_pair: tuple | None,
|
||||
) -> None:
|
||||
import open3d as o3d
|
||||
|
||||
if fixed_single_pair is not None:
|
||||
frame_i, frame_j, a_ij, b_gicp = fixed_single_pair
|
||||
transforms = _transforms_for_pair(x, a_ij, b_gicp)
|
||||
label = f"fixed frames (no pair switching)"
|
||||
pair_index = 0
|
||||
n_pairs = 1
|
||||
use_list = False
|
||||
else:
|
||||
pair_index = int(np.clip(start_index, 0, len(pairs) - 1))
|
||||
n_pairs = len(pairs)
|
||||
use_list = True
|
||||
frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
|
||||
frames, pairs, pair_index, x
|
||||
)
|
||||
|
||||
viewer = o3d.visualization.VisualizerWithKeyCallback()
|
||||
viewer.create_window("LiDAR–IMU registration inspection", 1400, 900)
|
||||
target_cloud = _cloud(o3d, frame_i.points_xyz, COLORS["target"], voxel)
|
||||
source_cloud = _cloud(o3d, frame_j.points_xyz, COLORS["source"], voxel)
|
||||
viewer.add_geometry(target_cloud)
|
||||
viewer.add_geometry(source_cloud)
|
||||
viewer.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0))
|
||||
viewer.get_render_option().background_color = np.array([0.02, 0.02, 0.02])
|
||||
viewer.get_render_option().point_size = 2.0
|
||||
|
||||
state = {
|
||||
"pair_index": pair_index,
|
||||
"mode_name": MODE_NAMES[3],
|
||||
"current": np.eye(4),
|
||||
"transforms": transforms,
|
||||
"b_gicp": b_gicp,
|
||||
"frame_i": frame_i,
|
||||
"frame_j": frame_j,
|
||||
}
|
||||
|
||||
def apply_mode(vis, mode_name: str, *, announce: bool = True) -> None:
|
||||
desired = state["transforms"][mode_name]
|
||||
source_cloud.transform(desired @ inverse_transform(state["current"]))
|
||||
state["current"] = desired
|
||||
state["mode_name"] = mode_name
|
||||
vis.update_geometry(source_cloud)
|
||||
if announce:
|
||||
if mode_name == MODE_NAMES[2]:
|
||||
print(f"{mode_name}: registration reference; delta = 0")
|
||||
else:
|
||||
_print_delta(mode_name + " minus mode3", state["b_gicp"], desired)
|
||||
|
||||
def load_pair(vis, new_index: int) -> None:
|
||||
if not use_list:
|
||||
print("pair switching disabled in --frame-i/--frame-j mode")
|
||||
return
|
||||
new_index = int(new_index) % n_pairs
|
||||
frame_i, frame_j, _a, b_gicp, transforms, label = _resolve_pair(
|
||||
frames, pairs, new_index, x
|
||||
)
|
||||
state["pair_index"] = new_index
|
||||
state["transforms"] = transforms
|
||||
state["b_gicp"] = b_gicp
|
||||
state["frame_i"] = frame_i
|
||||
state["frame_j"] = frame_j
|
||||
state["current"] = np.eye(4)
|
||||
_set_cloud_points(target_cloud, frame_i.points_xyz, COLORS["target"], voxel, o3d)
|
||||
_set_cloud_points(source_cloud, frame_j.points_xyz, COLORS["source"], voxel, o3d)
|
||||
vis.update_geometry(target_cloud)
|
||||
vis.update_geometry(source_cloud)
|
||||
_print_pair_header(label, b_gicp, transforms)
|
||||
apply_mode(vis, state["mode_name"], announce=True)
|
||||
|
||||
def make_mode_cb(mode_name: str):
|
||||
def callback(vis):
|
||||
apply_mode(vis, mode_name, announce=True)
|
||||
return False
|
||||
|
||||
return callback
|
||||
|
||||
def next_pair(vis):
|
||||
load_pair(vis, state["pair_index"] + 1)
|
||||
return False
|
||||
|
||||
def prev_pair(vis):
|
||||
load_pair(vis, state["pair_index"] - 1)
|
||||
return False
|
||||
|
||||
_print_pair_header(label, b_gicp, transforms)
|
||||
for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4")), MODE_NAMES):
|
||||
viewer.register_key_callback(key, make_mode_cb(name))
|
||||
for key in (ord("N"), ord("n"), ord("]")):
|
||||
viewer.register_key_callback(key, next_pair)
|
||||
for key in (ord("P"), ord("p"), ord("[")):
|
||||
viewer.register_key_callback(key, prev_pair)
|
||||
|
||||
apply_mode(viewer, MODE_NAMES[3], announce=False)
|
||||
viewer.run()
|
||||
viewer.destroy_window()
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--lidar", required=True, type=Path, help="LiDAR session directory")
|
||||
parser.add_argument("--imu", required=True, type=Path, help="IMU CSV")
|
||||
parser.add_argument(
|
||||
"--summary",
|
||||
required=True,
|
||||
type=Path,
|
||||
help="summary.json (or T_IMU_lidar.json) from a calibration run",
|
||||
)
|
||||
parser.add_argument("--pair-index", type=int, default=0, help="Starting motion-pair index")
|
||||
parser.add_argument("--frame-i", type=int, default=None, help="Optional explicit frame index i")
|
||||
parser.add_argument("--frame-j", type=int, default=None, help="Optional explicit frame index j")
|
||||
parser.add_argument("--voxel", type=float, default=0.12)
|
||||
parser.add_argument("--min-pair-rotation-deg", type=float, default=3.0)
|
||||
parser.add_argument("--min-pair-translation-m", type=float, default=0.3)
|
||||
parser.add_argument(
|
||||
"--save-png",
|
||||
type=Path,
|
||||
default=None,
|
||||
help="Write a 2x2 XY top-down comparison PNG for the starting pair",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-gui",
|
||||
action="store_true",
|
||||
help="Skip Open3D window (use with --save-png)",
|
||||
)
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
x, delta_t_s, gyro_bias = _load_extrinsic(args.summary)
|
||||
frames, imu, keyframes, pair_set = _build_pair_list(
|
||||
lidar_dir=args.lidar,
|
||||
imu_path=args.imu,
|
||||
delta_t_s=delta_t_s,
|
||||
gyro_bias=gyro_bias,
|
||||
min_rotation_deg=args.min_pair_rotation_deg,
|
||||
min_translation_m=args.min_pair_translation_m,
|
||||
)
|
||||
|
||||
fixed_single_pair = None
|
||||
if args.frame_i is not None and args.frame_j is not None:
|
||||
frame_i, frame_j, a_ij, b_gicp = _pair_from_indices(
|
||||
frames,
|
||||
imu,
|
||||
i=args.frame_i,
|
||||
j=args.frame_j,
|
||||
delta_t_s=delta_t_s,
|
||||
gyro_bias=gyro_bias,
|
||||
)
|
||||
transforms = _transforms_for_pair(x, a_ij, b_gicp)
|
||||
label = f"frames {args.frame_i} <- {args.frame_j}"
|
||||
fixed_single_pair = (frame_i, frame_j, a_ij, b_gicp)
|
||||
pairs = ()
|
||||
else:
|
||||
if not pair_set.pairs:
|
||||
raise SystemExit("no motion pairs rebuilt; loosen min-pair thresholds or check data")
|
||||
if not 0 <= args.pair_index < len(pair_set.pairs):
|
||||
raise SystemExit(
|
||||
f"pair-index {args.pair_index} outside [0, {len(pair_set.pairs) - 1}] "
|
||||
f"({len(pair_set.pairs)} pairs available)"
|
||||
)
|
||||
pairs = pair_set.pairs
|
||||
frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
|
||||
frames, pairs, args.pair_index, x
|
||||
)
|
||||
print(f"rebuilt {len(pairs)} pairs from {len(keyframes.indices)} keyframes")
|
||||
|
||||
if args.save_png is not None:
|
||||
_print_pair_header(label, b_gicp, transforms)
|
||||
_save_topdown_png(args.save_png, frame_i.points_xyz, frame_j.points_xyz, transforms)
|
||||
|
||||
if args.no_gui:
|
||||
return 0
|
||||
|
||||
try:
|
||||
import open3d # noqa: F401
|
||||
except ImportError as exc:
|
||||
raise SystemExit(
|
||||
"Open3D is required for interactive view. "
|
||||
"Install with: python -m pip install -e \".[open3d]\" "
|
||||
"or use --no-gui --save-png out.png"
|
||||
) from exc
|
||||
|
||||
_run_gui(
|
||||
frames=frames,
|
||||
pairs=pairs,
|
||||
x=x,
|
||||
start_index=args.pair_index,
|
||||
voxel=args.voxel,
|
||||
fixed_single_pair=fixed_single_pair,
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user