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

支持 HI13/H32 主机 UTC 桥接对齐、多会话联合标定与 CAD 平移先验。

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-10 13:26:32 +08:00
co-authored by Cursor
parent 30f7e66db3
commit 2237be77a4
20 changed files with 1830 additions and 347 deletions
Download Patch File Download Diff File Expand all files Collapse all files
imu_lidar/joint_optimizer.py
+90 -18
View File
@@ -152,21 +152,21 @@ def _build_nav_rotations(
r_x: np.ndarray,
t_x: np.ndarray,
) -> list[np.ndarray]:
"""Chain IMU orientations in the first-keyframe nav frame using LiDAR+extrinsic."""
"""Chain IMU orientations; restart at session/gap boundaries (no cross-link)."""
del id_to_idx
rotations = [np.eye(3) for _ in keyframe_ids]
for k in range(len(keyframe_ids) - 1):
a = keyframe_ids[k]
b = keyframe_ids[k + 1]
pair = consecutive_pairs.get((a, b))
if pair is None:
rotations[k + 1] = rotations[k]
# Missing link or new session: start a fresh nav chain.
rotations[k + 1] = np.eye(3)
continue
t_b = np.zeros(3) if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float)
r_meas, _ = _lidar_to_imu_relative(r_x, t_x, pair.R_B, t_b)
rotations[k + 1] = orthonormalize_rotation(rotations[k] @ r_meas)
# Ensure list indexed by id_to_idx
del id_to_idx
return rotations
@@ -178,6 +178,9 @@ def _solve_phase_c_se3(
gravity_init: np.ndarray,
sigma_bg_rw: float = 1.0e-5,
sigma_ba_rw: float = 1.0e-3,
t_init: np.ndarray | None = None,
t_prior: np.ndarray | None = None,
t_prior_sigma_m: np.ndarray | float | None = None,
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, float, float, list[str]]:
"""Keyframe IMU factor optimization for full SE(3)."""
@@ -185,21 +188,36 @@ def _solve_phase_c_se3(
usable = [pair for pair in pairs if pair.t_B_m is not None and "delta_v" in pair.metadata]
if len(usable) < 3:
notes.append("phase-C skipped: need pairs with full preintegration metadata")
return r_x, np.zeros(3), gravity_init, gyro_bias0, np.zeros(3), 1e9, 1e9, notes
t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
return r_x, t0, gravity_init, gyro_bias0, np.zeros(3), 1e9, 1e9, notes
# Unique keyframes sorted by IMU time.
# Keyframes: group by session, sort each session by IMU time (no cross-session chain).
stamp: dict[int, float] = {}
kf_session: dict[int, str] = {}
for pair in usable:
stamp[pair.i] = float(pair.metadata.get("t_i_imu_s", pair.t_i_s))
stamp[pair.j] = float(pair.metadata.get("t_j_imu_s", pair.t_j_s))
keyframe_ids = sorted(stamp.keys(), key=lambda kid: stamp[kid])
kf_session[pair.i] = pair.session_id
kf_session[pair.j] = pair.session_id
session_ids = sorted(set(kf_session.values()))
keyframe_ids: list[int] = []
for sid in session_ids:
local = [kid for kid, sess in kf_session.items() if sess == sid]
local.sort(key=lambda kid: stamp[kid])
keyframe_ids.extend(local)
k_count = len(keyframe_ids)
id_to_idx = {kid: idx for idx, kid in enumerate(keyframe_ids)}
consecutive_pairs: dict[tuple[int, int], MotionPair] = {}
for pair in usable:
if kf_session.get(pair.i) != kf_session.get(pair.j):
continue
if id_to_idx[pair.j] == id_to_idx[pair.i] + 1:
consecutive_pairs[(pair.i, pair.j)] = pair
notes.append(
f"phase-C multi-session graph: sessions={len(session_ids)}, "
f"keyframes={k_count}, consecutive_links={len(consecutive_pairs)}"
)
g0 = np.asarray(gravity_init, dtype=float).reshape(3)
if np.linalg.norm(g0) < 1e-6:
@@ -214,6 +232,15 @@ def _solve_phase_c_se3(
n_b = 3 * k_count
dim = 3 + 3 + 2 + n_v + n_b + n_b
x0 = np.zeros(dim)
t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
x0[3:6] = t0
t_prior_vec = None if t_prior is None else np.asarray(t_prior, dtype=float).reshape(3)
if t_prior_sigma_m is None:
t_sigma = np.array([0.05, 0.05, 0.05], dtype=float)
else:
t_sigma = np.asarray(t_prior_sigma_m, dtype=float).reshape(-1)
if t_sigma.size == 1:
t_sigma = np.full(3, float(t_sigma[0]), dtype=float)
# velocities start at 0; biases at prior
for idx in range(k_count):
x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg0
@@ -269,18 +296,28 @@ def _solve_phase_c_se3(
w = np.sqrt(_pair_weight(pair))
out.append(w * (whiten @ err))
# Bias random-walk between consecutive keyframes.
# Bias random-walk between consecutive keyframes (same session only).
for k in range(k_count - 1):
dt = max(stamp[keyframe_ids[k + 1]] - stamp[keyframe_ids[k]], 1e-3)
a = keyframe_ids[k]
b = keyframe_ids[k + 1]
if kf_session.get(a) != kf_session.get(b):
continue
dt = max(stamp[b] - stamp[a], 1e-3)
scale_g = 1.0 / (max(sigma_bg_rw, 1e-8) * np.sqrt(dt))
scale_a = 1.0 / (max(sigma_ba_rw, 1e-8) * np.sqrt(dt))
out.append(scale_g * (bgs[k + 1] - bgs[k]))
out.append(scale_a * (bas[k + 1] - bas[k]))
# Weak priors: first-keyframe biases and translation magnitude.
out.append(50.0 * (bgs[0] - bg0))
out.append(20.0 * bas[0])
out.append(0.2 * t_opt) # soft |t| prior ~ meters
# Weak priors: first keyframe of each session + CAD/installation translation.
for sid in session_ids:
first = next(kid for kid in keyframe_ids if kf_session[kid] == sid)
idx0 = id_to_idx[first]
out.append(50.0 * (bgs[idx0] - bg0))
out.append(20.0 * bas[idx0])
if t_prior_vec is not None:
out.append((t_opt - t_prior_vec) / np.maximum(t_sigma, 1e-3))
else:
out.append(0.2 * t_opt) # soft |t|~0 prior when no CAD prior
return np.concatenate(out)
# Cap evaluations: Phase-C is high-dimensional; synthetic ICP already dominates runtime.
@@ -331,6 +368,9 @@ def solve_joint_extrinsic(
gravity_init_m_s2: np.ndarray | None = None,
bias_prior_sigma_rad_s: float = 0.02,
enable_phase_c: bool | None = None,
t_init_m: np.ndarray | None = None,
t_prior_m: np.ndarray | None = None,
t_prior_sigma_m: np.ndarray | float | None = None,
) -> JointExtrinsicResult:
"""Refine extrinsic using Phase-A whitened rotation factors, optional Phase-C SE(3)."""
@@ -344,6 +384,7 @@ def solve_joint_extrinsic(
r = orthonormalize_rotation(np.asarray(r_x, dtype=float))
bias0 = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
t_seed = None if t_init_m is None else np.asarray(t_init_m, dtype=float).reshape(3)
weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
whitens = [residual_whiten_matrix(_pair_cov(pair)) for pair in usable]
prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
@@ -389,7 +430,7 @@ def solve_joint_extrinsic(
rot_errs.append(np.degrees(np.linalg.norm(err)))
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
t = np.zeros(3)
t = np.zeros(3) if t_seed is None else t_seed.copy()
translation_accepted = False
trans_rms = 1e9
gravity_out: np.ndarray | None = None
@@ -400,6 +441,12 @@ def solve_joint_extrinsic(
else:
gravity_init = np.asarray(gravity_init_m_s2, dtype=float).reshape(3)
if t_prior_m is not None:
notes.append(
"using CAD/installation translation prior "
f"t={np.asarray(t_prior_m, dtype=float).reshape(3).tolist()}"
)
if (
enable_phase_c
and not force_rotation_only
@@ -412,12 +459,23 @@ def solve_joint_extrinsic(
r,
gyro_bias0=bias_out,
gravity_init=gravity_init,
t_init=t_seed if t_seed is not None else t_prior_m,
t_prior=t_prior_m,
t_prior_sigma_m=t_prior_sigma_m,
)
notes.extend(c_notes)
translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4)
if not translation_accepted:
notes.append("phase-C translation residual/gate failed; keeping translation at zero")
t = np.zeros(3)
# Prefer CAD prior over silent zero when motion SE3 is rejected.
if t_prior_m is not None:
t = np.asarray(t_prior_m, dtype=float).reshape(3)
translation_accepted = True
notes.append(
"phase-C translation residual/gate failed; keeping CAD translation prior"
)
else:
notes.append("phase-C translation residual/gate failed; keeping translation at zero")
t = np.zeros(3)
elif (
not force_rotation_only
and observability.translation_observable
@@ -437,9 +495,19 @@ def solve_joint_extrinsic(
pred = (pair.R_A - np.eye(3)) @ t_opt
meas = r_opt @ np.asarray(pair.t_B_m, dtype=float)
residuals.append(np.sqrt(weight) * (pred - meas))
if t_prior_m is not None:
sigma = np.asarray(t_prior_sigma_m if t_prior_sigma_m is not None else 0.05, dtype=float)
if sigma.size == 1:
sigma = np.full(3, float(sigma), dtype=float)
residuals.append((t_opt - np.asarray(t_prior_m, dtype=float).reshape(3)) / np.maximum(sigma, 1e-3))
return np.concatenate(residuals)
opt_t = least_squares(residual_se3, np.zeros(6), loss="huber", f_scale=0.05, max_nfev=200)
x_se3 = np.zeros(6)
if t_seed is not None:
x_se3[3:] = t_seed
elif t_prior_m is not None:
x_se3[3:] = np.asarray(t_prior_m, dtype=float).reshape(3)
opt_t = least_squares(residual_se3, x_se3, loss="huber", f_scale=0.05, max_nfev=200)
r = orthonormalize_rotation(so3_exp(opt_t.x[:3]) @ r)
t = opt_t.x[3:]
rot_errs = []
@@ -452,11 +520,15 @@ def solve_joint_extrinsic(
trans_errs.append(np.linalg.norm(pred - meas))
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs))))
trans_rms = float(np.sqrt(np.mean(np.square(trans_errs))))
translation_accepted = trans_rms < 0.5
translation_accepted = trans_rms < 0.5 or t_prior_m is not None
notes.append(f"legacy translation refine rms={trans_rms:.3f} m")
if not translation_accepted:
notes.append("translation residual too large; keeping translation at zero")
t = np.zeros(3)
elif not force_rotation_only and t_prior_m is not None:
t = np.asarray(t_prior_m, dtype=float).reshape(3)
translation_accepted = True
notes.append("SE3 motion solve gated off; using CAD translation prior with refined rotation")
else:
notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)")