可视化按键对齐雷达-IMU：N/]/[/]切换运动对
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -165,9 +165,11 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.p
|
||||
- `2`：RTK运动A直接作为初值；
|
||||
- `3`：GICP测得的B；
|
||||
- `4`：最终外参预测的 `X^-1 A X`；
|
||||
- `Q/Esc`：退出。
|
||||
- `N` / `]`：下一运动对；
|
||||
- `P` / `[`：上一运动对；
|
||||
- `Q` / `Esc`：退出。
|
||||
|
||||
模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应查看多对，不能只挑视觉效果最好的一对。
|
||||
模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应用 `N`/`P` 多看几对，不能只挑视觉效果最好的一对。
|
||||
|
||||
## 7. data4与data4+data5结果对比
|
||||
|
||||
|
||||
+202
-77
@@ -1,5 +1,21 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Interactive 3D comparison of raw, RTK, GICP and hand-eye-predicted motion."""
|
||||
"""Interactive 3D comparison of raw, RTK, GICP and hand-eye-predicted motion.
|
||||
|
||||
Modes (keyboard), aligned with the LiDAR–IMU viewer:
|
||||
1 raw source (no transform)
|
||||
2 RTK prediction with X=I (B_pred = A)
|
||||
3 LiDAR registration B (reference)
|
||||
4 calibrated prediction B_pred = X^{-1} A X
|
||||
5 optional body-left RPY test (only if --left-rpy-deg is non-zero)
|
||||
N / ] next motion pair
|
||||
P / [ previous motion pair
|
||||
Q / Esc exit
|
||||
|
||||
Blue = target station i; orange = source station j after the selected transform.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
|
||||
@@ -7,7 +23,10 @@ import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from rigorous_calibration import (
|
||||
inverse_transform, load_stations, rotation_angle_deg, rpy_deg, transform_points,
|
||||
inverse_transform,
|
||||
load_stations,
|
||||
rotation_angle_deg,
|
||||
rpy_deg,
|
||||
)
|
||||
|
||||
|
||||
@@ -16,15 +35,29 @@ COLORS = {
|
||||
"source": [1.00, 0.35, 0.05],
|
||||
}
|
||||
|
||||
MODE_NAMES = (
|
||||
"1 raw",
|
||||
"2 RTK initial (X=I)",
|
||||
"3 GICP B",
|
||||
"4 calibrated X^-1 A X",
|
||||
)
|
||||
|
||||
|
||||
def cloud(o3d, points, color, voxel):
|
||||
item = o3d.geometry.PointCloud()
|
||||
item.points = o3d.utility.Vector3dVector(points)
|
||||
item = item.voxel_down_sample(voxel)
|
||||
if voxel > 0:
|
||||
item = item.voxel_down_sample(voxel)
|
||||
item.paint_uniform_color(color)
|
||||
return item
|
||||
|
||||
|
||||
def set_cloud_points(cloud_geom, points, color, voxel, o3d) -> None:
|
||||
tmp = cloud(o3d, points, color, voxel)
|
||||
cloud_geom.points = tmp.points
|
||||
cloud_geom.colors = tmp.colors
|
||||
|
||||
|
||||
def delta_components(reference, candidate):
|
||||
"""Components of reference^-1*candidate, plus coordinate-invariant norms."""
|
||||
delta = inverse_transform(reference) @ candidate
|
||||
@@ -50,15 +83,75 @@ def print_delta(name, reference, candidate):
|
||||
tx, ty, tz = item["translation_xyz_cm"]
|
||||
roll, pitch, yaw = item["rotation_rpy_deg_xyz"]
|
||||
print(
|
||||
f"{name}: B^-1*motion translation xyz = "
|
||||
f"[{tx:+.4f}, {ty:+.4f}, {tz:+.4f}] cm; "
|
||||
f"rpy xyz = [{roll:+.4f}, {pitch:+.4f}, {yaw:+.4f}] deg; "
|
||||
f"norm = {item['translation_norm_cm']:.4f} cm / "
|
||||
f"{item['rotation_angle_deg']:.6f} deg"
|
||||
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 transforms_for_pair(x, a_ij, b_gicp, left_rpy_deg):
|
||||
b_calibrated = inverse_transform(x) @ a_ij @ x
|
||||
transforms = {
|
||||
MODE_NAMES[0]: np.eye(4),
|
||||
MODE_NAMES[1]: a_ij.copy(),
|
||||
MODE_NAMES[2]: b_gicp.copy(),
|
||||
MODE_NAMES[3]: b_calibrated,
|
||||
}
|
||||
correction = np.asarray(left_rpy_deg, float)
|
||||
test_name = None
|
||||
if np.any(np.abs(correction) > 0.0):
|
||||
x_test = body_left_rpy(x, correction)
|
||||
test_name = f"5 test body-left RPY {correction.tolist()} deg"
|
||||
transforms[test_name] = inverse_transform(x_test) @ a_ij @ x_test
|
||||
return transforms, test_name
|
||||
|
||||
|
||||
def resolve_pair(stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy_deg):
|
||||
a_ij = np.asarray(pairs_a[pair_index], float)
|
||||
b_gicp = np.asarray(pairs_b[pair_index], float)
|
||||
i, j = np.asarray(pairs_meta[pair_index, :2], int)
|
||||
transforms, test_name = transforms_for_pair(x, a_ij, b_gicp, left_rpy_deg)
|
||||
label = (
|
||||
f"pair {pair_index + 1}/{len(pairs_a)} "
|
||||
f"station {i} <- {j} "
|
||||
f"rotB={rotation_angle_deg(b_gicp[:3, :3]):.2f} deg "
|
||||
f"|tB|={float(np.linalg.norm(b_gicp[:3, 3])):.3f} m"
|
||||
)
|
||||
return i, j, a_ij, b_gicp, transforms, test_name, label
|
||||
|
||||
|
||||
def print_pair_header(label, b_gicp, transforms, test_name, a_ij):
|
||||
print("-" * 72)
|
||||
print(label)
|
||||
print("blue=target i | orange=source j")
|
||||
mode_hint = "1-4"
|
||||
if test_name is not None:
|
||||
mode_hint = "1-5"
|
||||
print(f"{mode_hint}: overlay mode | N/]: next pair | P/[: prev pair | Q/Esc: exit")
|
||||
print(
|
||||
"IMPORTANT: delta xyz/rpy are components of B^-1*(X^-1*A*X), expressed "
|
||||
"in station-j LiDAR coordinates; screen-left/right depends on the 3D camera view."
|
||||
)
|
||||
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.")
|
||||
body_up = np.array([0.0, 0.0, 1.0])
|
||||
if np.linalg.norm(a_ij[:3, :3] @ body_up - body_up) < 1e-8:
|
||||
print(
|
||||
"observability: this A preserves the body Z axis, so body-left X.z "
|
||||
"translation is unobservable from this pair; use ground/external height constraints."
|
||||
)
|
||||
if test_name is not None:
|
||||
print_delta("mode5 minus mode3", b_gicp, transforms[test_name])
|
||||
|
||||
|
||||
def main():
|
||||
import open3d as o3d
|
||||
|
||||
@@ -66,10 +159,13 @@ def main():
|
||||
parser.add_argument("--frames", required=True)
|
||||
parser.add_argument("--pairs", required=True)
|
||||
parser.add_argument("--extrinsic", required=True)
|
||||
parser.add_argument("--pair-index", type=int, default=0)
|
||||
parser.add_argument("--pair-index", type=int, default=0, help="Starting motion-pair index")
|
||||
parser.add_argument("--voxel", type=float, default=0.10)
|
||||
parser.add_argument(
|
||||
"--left-rpy-deg", nargs=3, type=float, default=[0.0, 0.0, 0.0],
|
||||
"--left-rpy-deg",
|
||||
nargs=3,
|
||||
type=float,
|
||||
default=[0.0, 0.0, 0.0],
|
||||
metavar=("ROLL", "PITCH", "YAW"),
|
||||
help="optional body-frame left correction applied as DeltaR_body * X",
|
||||
)
|
||||
@@ -82,85 +178,114 @@ def main():
|
||||
f"frames contain {len(stations)} stations but pair file records "
|
||||
f"{len(data['station_times'])}"
|
||||
)
|
||||
if not 0 <= args.pair_index < len(data["A"]):
|
||||
raise IndexError(
|
||||
f"pair-index {args.pair_index} outside [0,{len(data['A']) - 1}]"
|
||||
)
|
||||
a_ij = np.asarray(data["A"][args.pair_index], float)
|
||||
b_gicp = np.asarray(data["B"][args.pair_index], float)
|
||||
i, j = np.asarray(data["meta"][args.pair_index, :2], int)
|
||||
pairs_a = np.asarray(data["A"], float)
|
||||
pairs_b = np.asarray(data["B"], float)
|
||||
pairs_meta = np.asarray(data["meta"])
|
||||
n_pairs = len(pairs_a)
|
||||
if not 0 <= args.pair_index < n_pairs:
|
||||
raise IndexError(f"pair-index {args.pair_index} outside [0,{n_pairs - 1}]")
|
||||
|
||||
with open(args.extrinsic, encoding="utf-8-sig") as stream:
|
||||
result = json.load(stream)
|
||||
x = np.asarray(result["matrix_4x4"], float)
|
||||
b_calibrated = inverse_transform(x) @ a_ij @ x
|
||||
left_rpy = np.asarray(args.left_rpy_deg, float)
|
||||
|
||||
transforms = {
|
||||
"1 raw": np.eye(4),
|
||||
"2 RTK initial (X0=I)": a_ij,
|
||||
"3 GICP B": b_gicp,
|
||||
"4 calibrated X^-1 A X": b_calibrated,
|
||||
}
|
||||
correction = np.asarray(args.left_rpy_deg, float)
|
||||
if np.any(np.abs(correction) > 0.0):
|
||||
x_test = body_left_rpy(x, correction)
|
||||
transforms[
|
||||
f"5 test body-left RPY {correction.tolist()} deg"
|
||||
] = inverse_transform(x_test) @ a_ij @ x_test
|
||||
|
||||
target = stations[i][3]
|
||||
source = stations[j][3]
|
||||
print(f"pair_index={args.pair_index}, station {i} <- {j}")
|
||||
print("blue = target station i; orange = source station j after selected transform")
|
||||
print("keys: 1 raw | 2 RTK initial | 3 GICP | 4 calibrated | 5 test correction | Q/Esc exit")
|
||||
print(
|
||||
"IMPORTANT: delta xyz/rpy are components of B^-1*(X^-1*A*X), expressed "
|
||||
"in station-j LiDAR coordinates; screen-left/right depends on the 3D camera view."
|
||||
pair_index = int(args.pair_index)
|
||||
i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair(
|
||||
stations, pairs_a, pairs_b, pairs_meta, pair_index, x, left_rpy
|
||||
)
|
||||
baseline = print_delta("mode 4 minus mode 3", b_gicp, b_calibrated)
|
||||
roll, pitch, yaw = np.abs(baseline["rotation_rpy_deg_xyz"])
|
||||
if max(roll, pitch) > max(0.10, 2.0 * yaw):
|
||||
print("diagnosis: roll/pitch components dominate yaw; do not prioritize yaw tuning for this pair.")
|
||||
tx, ty, tz = np.abs(baseline["translation_xyz_cm"])
|
||||
if tz > max(tx, ty):
|
||||
print("diagnosis: the largest translation component is relative Z, not lateral XY.")
|
||||
body_up = np.array([0.0, 0.0, 1.0])
|
||||
if np.linalg.norm(a_ij[:3, :3] @ body_up - body_up) < 1e-8:
|
||||
print(
|
||||
"observability: this A preserves the body Z axis, so body-left X.z "
|
||||
"translation is unobservable from this pair; use ground/external height constraints."
|
||||
)
|
||||
if "5 test body-left RPY " + str(correction.tolist()) + " deg" in transforms:
|
||||
print_delta("mode 5 minus mode 3", b_gicp, list(transforms.values())[-1])
|
||||
|
||||
viewer = o3d.visualization.VisualizerWithKeyCallback()
|
||||
viewer.create_window("Rigorous LiDAR registration inspection - 3D", 1400, 900)
|
||||
target_cloud = cloud(o3d, target, COLORS["target"], args.voxel)
|
||||
source_cloud = cloud(o3d, source, COLORS["source"], args.voxel)
|
||||
viewer.create_window("RTK–LiDAR registration inspection", 1400, 900)
|
||||
target_cloud = cloud(o3d, stations[i][3], COLORS["target"], args.voxel)
|
||||
source_cloud = cloud(o3d, stations[j][3], COLORS["source"], args.voxel)
|
||||
viewer.add_geometry(target_cloud)
|
||||
viewer.add_geometry(source_cloud)
|
||||
axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0)
|
||||
viewer.add_geometry(axes)
|
||||
current = np.eye(4)
|
||||
|
||||
def select(name):
|
||||
def callback(vis):
|
||||
nonlocal current
|
||||
desired = transforms[name]
|
||||
source_cloud.transform(desired @ inverse_transform(current))
|
||||
current = desired
|
||||
vis.update_geometry(source_cloud)
|
||||
if name == "3 GICP B":
|
||||
print(f"{name}: reference registration B; delta = 0")
|
||||
else:
|
||||
print_delta(name + " minus mode 3", b_gicp, desired)
|
||||
return False
|
||||
return callback
|
||||
|
||||
for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4"), ord("5")), transforms):
|
||||
viewer.register_key_callback(key, select(name))
|
||||
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,
|
||||
"a_ij": a_ij,
|
||||
"test_name": test_name,
|
||||
}
|
||||
|
||||
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:
|
||||
new_index = int(new_index) % n_pairs
|
||||
i, j, a_ij, b_gicp, transforms, test_name, label = resolve_pair(
|
||||
stations, pairs_a, pairs_b, pairs_meta, new_index, x, left_rpy
|
||||
)
|
||||
state["pair_index"] = new_index
|
||||
state["transforms"] = transforms
|
||||
state["b_gicp"] = b_gicp
|
||||
state["a_ij"] = a_ij
|
||||
state["test_name"] = test_name
|
||||
state["current"] = np.eye(4)
|
||||
set_cloud_points(target_cloud, stations[i][3], COLORS["target"], args.voxel, o3d)
|
||||
set_cloud_points(source_cloud, stations[j][3], COLORS["source"], args.voxel, o3d)
|
||||
vis.update_geometry(target_cloud)
|
||||
vis.update_geometry(source_cloud)
|
||||
# Keep current mode if still available (mode 5 may vanish when correction is zero).
|
||||
mode_name = state["mode_name"]
|
||||
if mode_name not in transforms:
|
||||
mode_name = MODE_NAMES[3]
|
||||
print_pair_header(label, b_gicp, transforms, test_name, a_ij)
|
||||
apply_mode(vis, mode_name, announce=True)
|
||||
|
||||
def make_mode_cb(mode_name: str):
|
||||
def callback(vis):
|
||||
if mode_name not in state["transforms"]:
|
||||
print(f"{mode_name}: unavailable (pass non-zero --left-rpy-deg for mode 5)")
|
||||
return False
|
||||
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, test_name, a_ij)
|
||||
for key, name in zip((ord("1"), ord("2"), ord("3"), ord("4")), MODE_NAMES):
|
||||
viewer.register_key_callback(key, make_mode_cb(name))
|
||||
|
||||
def mode5(vis):
|
||||
name = state["test_name"]
|
||||
if name is None or name not in state["transforms"]:
|
||||
print("5: unavailable (pass non-zero --left-rpy-deg for mode 5)")
|
||||
return False
|
||||
apply_mode(vis, name, announce=True)
|
||||
return False
|
||||
|
||||
viewer.register_key_callback(ord("5"), mode5)
|
||||
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()
|
||||
|
||||
|
||||
+3
-1
@@ -176,9 +176,11 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.p
|
||||
| `2` | 仅用 RTK 运动作初值 |
|
||||
| `3` | GICP 测得的 B |
|
||||
| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） |
|
||||
| `N` / `]` | 下一运动对 |
|
||||
| `P` / `[` | 上一运动对 |
|
||||
| `Q` / `Esc` | 退出 |
|
||||
|
||||
蓝 = 目标站 i，橙 = 源站 j。重点看模式 **3 与 4**：墙面、立柱、路缘、地面应基本重合。**多看几对**，不要只挑视觉最好的一对。
|
||||
蓝 = 目标站 i，橙 = 源站 j。重点看模式 **3 与 4**：墙面、立柱、路缘、地面应基本重合。用 `N`/`P` **多看几对**，不要只挑视觉最好的一对。
|
||||
|
||||
---
|
||||
|
||||
|
||||
Reference in New Issue
Block a user