lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修正安装Z离地先验，并改进旋转可视化模式4避免坏IMU位移误导。

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-11 13:28:28 +08:00
co-authored by Cursor
parent 03fcee7e32
commit 6b44a495fb
3 changed files with 298 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/visualize_pair_3d.py
+107 -15
View File
@@ -9,6 +9,8 @@ Modes (keyboard):
2 IMU prediction with X=I (B_pred = A)
3 LiDAR registration B (reference)
4 calibrated prediction B_pred = X^{-1} A X
(rotation_only runs default to R conjug + t_B so bad IMU Δp
does not dominate the overlay)
N / ] next motion pair
P / [ previous motion pair
Q / Esc exit
@@ -20,10 +22,16 @@ from __future__ import annotations
import argparse
import json
import sys
from pathlib import Path
from typing import Any
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.geometry import (
inverse_transform,
make_transform,
@@ -80,24 +88,45 @@ MODE_NAMES = (
)
def _load_extrinsic(summary_path: Path) -> tuple[np.ndarray, float, np.ndarray]:
def _load_extrinsic(summary_path: Path) -> tuple[np.ndarray, float, np.ndarray, dict[str, Any]]:
summary = json.loads(summary_path.read_text(encoding="utf-8"))
t_block = summary.get("T_IMU_lidar")
meta: dict[str, Any] = {
"rotation_only": False,
"translation_accepted": False,
"status": str(summary.get("status") or ""),
}
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)
return np.asarray(matrix, dtype=float), 0.0, np.zeros(3), meta
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]
joint = session.get("joint") or {}
bias = np.asarray(
(session.get("imu_audit") or {}).get("gyro_bias_rad_s")
or (session.get("joint") or {}).get("gyro_bias_rad_s")
or joint.get("gyro_bias_rad_s")
or [0.0, 0.0, 0.0],
dtype=float,
).reshape(3)
return t_mat, dt, bias
status = str(summary.get("status") or "")
translation_accepted = bool(
joint.get("translation_accepted")
or (summary.get("details") or {}).get("joint", {}).get("translation_accepted")
)
rotation_only = ("rotation_only" in status) or (
not translation_accepted and float(np.linalg.norm(t_mat[:3, 3])) < 1e-9
)
meta.update(
{
"rotation_only": rotation_only,
"translation_accepted": translation_accepted,
"status": status,
}
)
return t_mat, dt, bias, meta
def _delta_components(reference: np.ndarray, candidate: np.ndarray) -> dict:
@@ -196,16 +225,42 @@ def _pair_from_indices(
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]:
def _transforms_for_pair(
x: np.ndarray,
a_ij: np.ndarray,
b_gicp: np.ndarray,
*,
mode4_translation: str = "imu",
) -> dict[str, np.ndarray]:
"""Build overlay transforms.
``mode4_translation``:
- ``imu``: full SE3 conjug ``X^{-1} A X`` (needs trustworthy IMU Δp)
- ``gicp``: rotation conjug only; translation taken from LiDAR B
(correct check for rotation_only calibrations)
"""
calibrated = inverse_transform(x) @ a_ij @ x
if mode4_translation == "gicp":
calibrated = make_transform(b_gicp[:3, 3], calibrated[:3, :3])
elif mode4_translation != "imu":
raise ValueError(f"unknown mode4_translation={mode4_translation!r}")
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,
MODE_NAMES[3]: calibrated,
}
def _resolve_pair(frames, pairs, pair_index: int, x: np.ndarray):
def _resolve_pair(
frames,
pairs,
pair_index: int,
x: np.ndarray,
*,
mode4_translation: str = "imu",
):
pair = pairs[pair_index]
frame_i = frames[pair.i]
frame_j = frames[pair.j]
@@ -217,7 +272,9 @@ def _resolve_pair(frames, pairs, pair_index: int, x: np.ndarray):
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)
transforms = _transforms_for_pair(
x, a_ij, b_gicp, mode4_translation=mode4_translation
)
label = (
f"pair {pair_index + 1}/{len(pairs)} "
f"frames {pair.i} <- {pair.j} "
@@ -282,12 +339,15 @@ def _run_gui(
start_index: int,
voxel: float,
fixed_single_pair: tuple | None,
mode4_translation: str = "imu",
) -> 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)
transforms = _transforms_for_pair(
x, a_ij, b_gicp, mode4_translation=mode4_translation
)
label = f"fixed frames (no pair switching)"
pair_index = 0
n_pairs = 1
@@ -297,7 +357,7 @@ def _run_gui(
n_pairs = len(pairs)
use_list = True
frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
frames, pairs, pair_index, x
frames, pairs, pair_index, x, mode4_translation=mode4_translation
)
viewer = o3d.visualization.VisualizerWithKeyCallback()
@@ -338,7 +398,7 @@ def _run_gui(
return
new_index = int(new_index) % n_pairs
frame_i, frame_j, _a, b_gicp, transforms, label = _resolve_pair(
frames, pairs, new_index, x
frames, pairs, new_index, x, mode4_translation=mode4_translation
)
state["pair_index"] = new_index
state["transforms"] = transforms
@@ -429,9 +489,22 @@ def main(argv: list[str] | None = None) -> int:
action="store_true",
help="Skip Open3D window (use with --save-png)",
)
parser.add_argument(
"--mode4-translation",
choices=("auto", "gicp", "imu"),
default="auto",
help=(
"Mode-4 translation source: gicp=R conjug + t_B (rotation check); "
"imu=full X^-1 A X; auto=gicp for rotation_only summaries"
),
)
args = parser.parse_args(argv)
x, delta_t_s, gyro_bias = _load_extrinsic(args.summary)
x, delta_t_s, gyro_bias, extr_meta = _load_extrinsic(args.summary)
if args.mode4_translation == "auto":
mode4_translation = "gicp" if extr_meta.get("rotation_only") else "imu"
else:
mode4_translation = args.mode4_translation
cache_path = args.motion_pairs or resolve_motion_pairs_path(args.summary)
use_cache = (not args.rebuild_pairs) and cache_path is not None and args.frame_i is None
@@ -456,7 +529,7 @@ def main(argv: list[str] | None = None) -> int:
frames = _LazyFrameStore(args.lidar)
pairs = tuple(pair_list)
frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
frames, pairs, args.pair_index, x
frames, pairs, args.pair_index, x, mode4_translation=mode4_translation
)
print(f"loaded {len(pairs)} cached pairs from {cache_path}")
else:
@@ -479,7 +552,9 @@ def main(argv: list[str] | None = None) -> int:
delta_t_s=delta_t_s,
gyro_bias=gyro_bias,
)
transforms = _transforms_for_pair(x, a_ij, b_gicp)
transforms = _transforms_for_pair(
x, a_ij, b_gicp, mode4_translation=mode4_translation
)
label = f"frames {args.frame_i} <- {args.frame_j}"
fixed_single_pair = (frame_i, frame_j, a_ij, b_gicp)
pairs = ()
@@ -493,10 +568,26 @@ def main(argv: list[str] | None = None) -> int:
)
pairs = pair_set.pairs
frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair(
frames, pairs, args.pair_index, x
frames, pairs, args.pair_index, x, mode4_translation=mode4_translation
)
print(f"rebuilt {len(pairs)} pairs from {len(keyframes.indices)} keyframes")
print(
f"mode4 translation={mode4_translation} "
f"(status={extr_meta.get('status') or 'n/a'}, "
f"rotation_only={bool(extr_meta.get('rotation_only'))})"
)
if mode4_translation == "gicp":
print(
"note: mode4 uses R conjug + t_B; IMU Δp is ignored "
"(typical for rotation_only — raw Δp often has large Z drift)."
)
if mode4_translation == "imu":
print(
"note: mode4 uses full X^-1 A X. If clouds stack vertically, "
"IMU Δp is likely bad; retry with --mode4-translation gicp."
)
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)
@@ -520,6 +611,7 @@ def main(argv: list[str] | None = None) -> int:
start_index=args.pair_index,
voxel=args.voxel,
fixed_single_pair=fixed_single_pair,
mode4_translation=mode4_translation,
)
return 0