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

调整雷达到RTK标定分支为独立根目录结构

Browse Source
This commit is contained in:
lichun.qu
2026-07-24 08:42:16 +08:00
parent d2aae6177e
commit 6d87b6ba9c
132 changed files with 214 additions and 197832 deletions
Download Patch File Download Diff File Expand all files Collapse all files
code/README.md
+12
View File
@@ -0,0 +1,12 @@
# code目录
| 文件 | 职责 |
|---|---|
| `rigorous_calibration.py` | 核心CLI：读取静态点云/RTK位姿，Open3D或small_gicp求B，拟合地面，求解/验证AX=XB |
| `refine_pairs.py` | 不使用最终X，按留出点重叠率、RMSE、旋转共轭不变量和正反向一致性精筛运动对 |
| `cross_backend_filter.py` | 保留Open3D与small_gicp共同认可且变换接近的边；共识B数值取Open3D结果 |
| `finalize_direct_rtk_lidar.py` | 将三路求解结果封装为明确方向的`T_RTK_lidar`，选择consensus为最终结果 |
| `visualize_pair_3d.py` | 交互显示原始、RTK初值、GICP B和`X^-1AX`，并打印增量 |
| `compare_extrinsics.py` | 计算两套外参的SE(3)平移/旋转差异 |
核心约定：`A=T_Ri_Rj`、`B=T_Li_Lj`、`X=T_RTK_lidar`，满足`A X = X B`。点云配准以i为target、j为source，B将j帧点云变换到i帧。
code/compare_extrinsics.py
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Compare two T_body_lidar JSON files in parameter space and on SE(3)."""
"""Compare two homogeneous-extrinsic JSON files in parameter space and on SE(3)."""
import argparse
import json
code/finalize_consensus.py
-58
View File
@@ -1,58 +0,0 @@
#!/usr/bin/env python3
"""Publish the cross-backend-consensus result as the recommended deliverable."""
import argparse
import json
from pathlib import Path
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--consensus-extrinsic", required=True)
parser.add_argument("--consensus-check", required=True)
parser.add_argument("--open3d-extrinsic", required=True)
parser.add_argument("--small-extrinsic", required=True)
parser.add_argument("--output", required=True)
parser.add_argument("--summary", required=True)
args = parser.parse_args()
consensus = json.loads(Path(args.consensus_extrinsic).read_text(encoding="utf-8-sig"))
check = json.loads(Path(args.consensus_check).read_text(encoding="utf-8-sig"))
open3d = json.loads(Path(args.open3d_extrinsic).read_text(encoding="utf-8-sig"))
small = json.loads(Path(args.small_extrinsic).read_text(encoding="utf-8-sig"))
summary = {
"recommended_method": "Open3D B gated by Open3D-small_gicp cross-backend agreement",
"selection_is_X_independent": True,
"second_batch_role": "estimation (dense RTK)",
"first_batch_role": "auxiliary check only (sparse RTK)",
"consensus": {
"translation_m": consensus["translation_m"],
"rotation_rpy_deg_xyz": consensus["rotation_rpy_deg_xyz"],
"estimation": consensus["estimation"]["residuals"],
"bootstrap_std": consensus["bootstrap"]["std"],
"batch1_auxiliary": check["metrics"],
},
"separate_backend_results": {
"open3d_gicp": {
"translation_m": open3d["translation_m"],
"rotation_rpy_deg_xyz": open3d["rotation_rpy_deg_xyz"],
},
"small_gicp": {
"translation_m": small["translation_m"],
"rotation_rpy_deg_xyz": small["rotation_rpy_deg_xyz"],
},
},
"warning": "AX rotation RMS remains about one degree; this is not centimetre-grade absolute certification.",
}
published = dict(consensus)
published["selection"] = {
"method": summary["recommended_method"],
"selection_is_X_independent": True,
"consensus_pair_threshold": "Open3D-small_gicp B delta <= 0.05 m and <= 0.50 deg",
"warning": summary["warning"],
}
Path(args.output).write_text(json.dumps(published, ensure_ascii=False, indent=2), encoding="utf-8")
Path(args.summary).write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
print(json.dumps(summary, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()
code/finalize_direct_rtk_lidar.py
+128
View File
@@ -0,0 +1,128 @@
from __future__ import annotations
import argparse
import json
import math
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
def load(path: Path) -> dict:
return json.loads(path.read_text(encoding="utf-8-sig"))
def write(path: Path, document: dict) -> None:
path.parent.mkdir(parents=True, exist_ok=True)
path.write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
def inverse(t: np.ndarray) -> np.ndarray:
result = np.eye(4)
result[:3, :3] = t[:3, :3].T
result[:3, 3] = -result[:3, :3] @ t[:3, 3]
return result
def delta(a: np.ndarray, b: np.ndarray) -> dict:
d = inverse(a) @ b
return {
"translation_m": float(np.linalg.norm(d[:3, 3])),
"rotation_deg": float(np.linalg.norm(Rotation.from_matrix(d[:3, :3]).as_rotvec()) * 180.0 / math.pi),
"delta_matrix_4x4": d.tolist(),
}
def corrected(raw: dict, backend: str, reference_height: float) -> dict:
return {
"schema_version": 1,
"success": bool(raw["success"]),
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK navigation frame",
"equation": "A_RTK_ij X = X B_LiDAR_ij",
"frames": {
"RTK": {
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
"x_axis": "horizontal projection of the rawHeading baseline direction reported by the receiver",
"y_axis": "left",
"z_axis": "up",
"yaw_enu_deg": "90 - rawHeadingDeg",
},
"LiDAR": "raw LiDAR sensor frame",
},
"backend": backend,
"measured_lidar_extrinsic_used_as_initial": False,
"body_heading_offset_used": False,
"body_antenna_lever_xy_used": False,
"translation_m": raw["translation_m"],
"rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"],
"quaternion_xyzw": raw["quaternion_xyzw"],
"matrix_4x4": raw["matrix_4x4"],
"quality": {
"stations": raw["estimation"]["stations"],
"pairs": raw["estimation"]["pairs"],
"residuals": raw["estimation"]["residuals"],
"weighted_jacobian_condition_number": raw["weighted_jacobian_condition_number"],
"linearized_one_sigma": raw["linearized_one_sigma"],
"bootstrap": raw["bootstrap"],
},
"z_constraint": {
"observable_from_planar_AX_XB": False,
"method": "LiDAR ground planes plus externally supplied RTK reference-point height above ground",
"rtk_reference_height_above_ground_m": reference_height,
"warning": "z is conditional on the supplied RTK antenna height; it is not independently identified by planar Ackermann motion",
},
"important_limit": "AX residual and bootstrap quantify internal consistency, not independent centimetre-grade absolute certification",
}
def main() -> None:
parser = argparse.ArgumentParser()
parser.add_argument("--result-root", type=Path, required=True)
parser.add_argument("--reference-height", type=float, required=True)
args = parser.parse_args()
def solver_output(directory: str) -> Path:
raw = args.result_root / directory / "extrinsic_raw.json"
standard = args.result_root / directory / "extrinsic.json"
return raw if raw.exists() else standard
paths = {
"open3d_gicp": solver_output("open3d_gicp"),
"small_gicp": solver_output("small_gicp"),
"consensus": solver_output("consensus"),
}
docs = {}
for backend, path in paths.items():
document = corrected(load(path), backend, args.reference_height)
write(path.with_name("extrinsic_rtk_lidar.json"), document)
docs[backend] = document
open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float)
small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float)
final = dict(docs["consensus"])
final["selection"] = {
"recommended": True,
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
"open3d_vs_small_gicp": delta(open_t, small_t),
}
write(args.result_root / "final_T_RTK_lidar.json", final)
summary = {
"final": {
"translation_m": final["translation_m"],
"rotation_rpy_deg_xyz": final["rotation_rpy_deg_xyz"],
"pairs": final["quality"]["pairs"],
"translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"],
"rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"],
"condition_number": final["quality"]["weighted_jacobian_condition_number"],
},
"backend_difference": delta(open_t, small_t),
}
write(args.result_root / "summary.json", summary)
print(json.dumps(summary, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()
code/rigorous_calibration.py
+19 -17
View File
@@ -1,8 +1,9 @@
#!/usr/bin/env python3
"""Rigorous stationary LiDAR / dual-antenna RTK hand-eye calibration.
"""Rigorous stationary LiDAR / reference-trajectory hand-eye calibration.
Convention: T_A_B maps points from frame B into frame A.
X = T_body_lidar, A_ij = T_W_Bi^-1 T_W_Bj, B_ij = T_Li_Lj,
For this repository the reference frame is the RTK navigation frame.
X = T_RTK_lidar, A_ij = T_W_Ri^-1 T_W_Rj, B_ij = T_Li_Lj,
therefore A_ij X = X B_ij. Raw sensor-frame points_raw are used.
"""
from __future__ import annotations
@@ -367,17 +368,17 @@ def cmd_pairs(args):
stations = load_stations(
args.frames, args.min_range, args.max_range, args.z_min, args.z_max
)
body = read_poses(args.body)
reference = read_poses(args.reference_poses)
if len(stations) < args.min_stations:
raise ValueError(f"need at least {args.min_stations} stations, got {len(stations)}")
body_poses, body_dt = [], []
reference_poses, reference_dt = [], []
for timestamp, _, _, xyz in stations:
if len(xyz) < args.min_roi_points:
raise ValueError(f"station at {timestamp} has only {len(xyz)} ROI points")
pose, dt = nearest_pose(body, timestamp + args.time_offset)
body_poses.append(pose)
body_dt.append(dt)
body_poses = np.asarray(body_poses)
pose, dt = nearest_pose(reference, timestamp + args.time_offset)
reference_poses.append(pose)
reference_dt.append(dt)
reference_poses = np.asarray(reference_poses)
split = [split_holdout(station[3], args.holdout_fraction, i)
for i, station in enumerate(stations)]
rng = np.random.default_rng(args.seed)
@@ -385,7 +386,7 @@ def cmd_pairs(args):
accepted_transforms = {}
for i in range(len(stations)):
for j in range(i + args.min_gap, min(len(stations), i + args.max_gap + 1)):
a_ij = inverse_transform(body_poses[i]) @ body_poses[j]
a_ij = inverse_transform(reference_poses[i]) @ reference_poses[j]
translation = float(np.linalg.norm(a_ij[:2, 3]))
rotation = rotation_angle_deg(a_ij[:3, :3])
if translation < args.min_translation and rotation < args.min_rotation:
@@ -445,7 +446,7 @@ def cmd_pairs(args):
"lidar_time_i": stations[i][0], "lidar_time_j": stations[j][0],
"frame_counter_i": stations[i][1], "frame_counter_j": stations[j][1],
"rtk_translation_m": translation, "rtk_rotation_deg": rotation,
"nearest_rtk_dt_i_s": body_dt[i], "nearest_rtk_dt_j_s": body_dt[j],
"nearest_rtk_dt_i_s": reference_dt[i], "nearest_rtk_dt_j_s": reference_dt[j],
"initial_B_source": "X0=identity; B0=A (no measured extrinsic)",
"B_ij_4x4": forward["transform"].tolist(),
"backend": args.backend, "backend_converged": forward["converged"],
@@ -474,7 +475,7 @@ def cmd_pairs(args):
output, A=np.asarray(accepted_a), B=np.asarray(accepted_b),
meta=np.asarray(accepted_meta),
station_times=np.asarray([item[0] for item in stations]),
rtk_nearest_dt_s=np.asarray(body_dt), backend=np.asarray(args.backend),
rtk_nearest_dt_s=np.asarray(reference_dt), backend=np.asarray(args.backend),
)
quality = {
"schema_version": 2,
@@ -556,7 +557,7 @@ def calibration_residual(params, a_array, b_array, planes, args):
normal_body = x[:3, :3] @ plane[:3]
values.extend((np.cross(normal_body, body_up) / args.plane_normal_sigma).tolist())
body_distance = plane[3] - float(normal_body @ x[:3, 3])
values.append((body_distance - args.body_height) / args.plane_height_sigma)
values.append((body_distance - args.reference_height) / args.plane_height_sigma)
return np.asarray(values)
@@ -644,7 +645,7 @@ def cmd_calibrate(args):
"schema_version": 2,
"success": bool(best.success),
"message": best.message,
"convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
"convention": "T_reference_lidar maps raw LiDAR points into the supplied reference frame",
"equation": "A_ij X = X B_ij",
"measured_extrinsic_used_as_initial": False,
"translation_m": x[:3, 3].tolist(),
@@ -655,8 +656,8 @@ def cmd_calibrate(args):
"residuals": pair_metrics(a_array, b_array, x)},
"ground": {
"planes": len(planes),
"body_origin_height_above_ground_m": args.body_height,
"formula": "d_lidar - (R_X n_lidar)^T t_X - body_height",
"reference_origin_height_above_ground_m": args.reference_height,
"formula": "d_lidar - (R_X n_lidar)^T t_X - reference_height",
},
"linearized_one_sigma": {
"translation_m": sigma[:3].tolist(),
@@ -713,7 +714,8 @@ def build_parser():
pairs = commands.add_parser("pairs")
pairs.add_argument("--backend", choices=["open3d", "small_gicp"], required=True)
pairs.add_argument("--frames", required=True); pairs.add_argument("--body", required=True)
pairs.add_argument("--frames", required=True)
pairs.add_argument("--reference-poses", "--body", dest="reference_poses", required=True)
pairs.add_argument("--output", required=True); pairs.add_argument("--quality-json"); pairs.add_argument("--quality-csv")
pairs.add_argument("--time-offset", type=float, default=0.0)
pairs.add_argument("--min-stations", type=int, default=30); pairs.add_argument("--min-pairs", type=int, default=25)
@@ -746,7 +748,7 @@ def build_parser():
calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
calibrate.add_argument("--plane-height-sigma", type=float, default=0.03)
calibrate.add_argument("--body-height", type=float, default=0.2335)
calibrate.add_argument("--reference-height", "--body-height", dest="reference_height", type=float, default=0.8535)
calibrate.add_argument("--solver-multistart", type=int, default=12)
calibrate.add_argument("--start-translation-sigma", type=float, default=1.0)
calibrate.add_argument("--start-rotation-sigma", type=float, default=20.0)
code/scan_extrinsic_sensitivity.py
-311
View File
@@ -1,311 +0,0 @@
#!/usr/bin/env python3
"""Scan body-left RPY corrections locally and validate them over every B pair.
This command is diagnostic only. It never writes or replaces an extrinsic JSON.
"""
from __future__ import annotations
import argparse
import csv
import json
import math
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
from rigorous_calibration import (
inverse_transform, read_pairs, read_planes, rotation_angle_deg, rpy_deg,
)
def statistics(values):
values = np.asarray(values, float)
return {
"rms": float(np.sqrt(np.mean(values ** 2))),
"median": float(np.median(values)),
"p90": float(np.quantile(values, 0.90)),
"p95": float(np.quantile(values, 0.95)),
"max": float(np.max(values)),
}
def body_left_rpy(x, rpy_correction_deg):
correction = np.eye(4)
correction[:3, :3] = Rotation.from_euler(
"xyz", np.asarray(rpy_correction_deg, float), degrees=True
).as_matrix()
return correction @ x
def pair_delta(a_ij, b_ij, x):
predicted = inverse_transform(x) @ a_ij @ x
delta = inverse_transform(b_ij) @ predicted
translation = np.asarray(delta[:3, 3], float)
return {
"translation_xyz_m": translation.tolist(),
"translation_xyz_cm": (100.0 * translation).tolist(),
"translation_norm_m": float(np.linalg.norm(translation)),
"rotation_rpy_deg_xyz": rpy_deg(delta[:3, :3]),
"rotation_angle_deg": rotation_angle_deg(delta[:3, :3]),
}
def ground_metrics(planes, x, body_height):
if len(planes) == 0:
return None
up = np.array([0.0, 0.0, 1.0])
tilt_deg, height_m = [], []
for plane in planes:
normal_body = x[:3, :3] @ plane[:3]
normal_body /= np.linalg.norm(normal_body)
tilt_deg.append(math.degrees(math.atan2(
np.linalg.norm(np.cross(normal_body, up)),
float(np.clip(normal_body @ up, -1.0, 1.0)),
)))
height_m.append(
float(plane[3] - normal_body @ x[:3, 3] - body_height)
)
return {
"normal_tilt_deg": statistics(tilt_deg),
"height_residual_m": statistics(height_m),
}
def evaluate(label, correction, a_array, b_array, meta, x, pair_index,
translation_scale, rotation_scale, planes, body_height):
candidate_x = body_left_rpy(x, correction)
per_pair = []
translation, rotation, normalized = [], [], []
for index, (a_ij, b_ij, pair_meta) in enumerate(zip(a_array, b_array, meta)):
item = pair_delta(a_ij, b_ij, candidate_x)
item.update({
"pair_index": index,
"i": int(pair_meta[0]),
"j": int(pair_meta[1]),
})
t = item["translation_norm_m"]
r = item["rotation_angle_deg"]
translation.append(t)
rotation.append(r)
normalized.append(math.hypot(t / translation_scale, r / rotation_scale))
per_pair.append(item)
return {
"label": label,
"body_left_rpy_correction_deg_xyz": list(map(float, correction)),
"candidate_extrinsic": {
"translation_m": candidate_x[:3, 3].tolist(),
"rotation_rpy_deg_xyz": rpy_deg(candidate_x[:3, :3]),
},
"all_pairs": {
"count": len(per_pair),
"translation_m": statistics(translation),
"rotation_deg": statistics(rotation),
"normalized_pair_score": statistics(normalized),
"normalized_global_rms": float(np.sqrt(np.mean(np.asarray(normalized) ** 2))),
},
"selected_pair": per_pair[pair_index],
"ground": ground_metrics(planes, candidate_x, body_height),
"per_pair": per_pair,
}
def candidate_grid(pitch_values, roll_values, yaw_values):
answer = [("baseline", (0.0, 0.0, 0.0))]
for pitch in pitch_values:
answer.append((f"pitch_{pitch:+.3f}", (0.0, pitch, 0.0)))
for pitch in (0.0, *pitch_values):
for roll in roll_values:
answer.append((
f"pitch_{pitch:+.3f}_roll_{roll:+.3f}",
(roll, pitch, 0.0),
))
for yaw in yaw_values:
answer.append((f"yaw_{yaw:+.3f}_diagnostic", (0.0, 0.0, yaw)))
unique = []
seen = set()
for label, values in answer:
key = tuple(round(float(value), 12) for value in values)
if key not in seen:
seen.add(key)
unique.append((label, values))
return unique
def z_observability(a_array, x, test_shift_m):
shift = np.eye(4)
shift[2, 3] = test_shift_m
shifted_x = shift @ x
effects = []
for a_ij in a_array:
before = inverse_transform(x) @ a_ij @ x
after = inverse_transform(shifted_x) @ a_ij @ shifted_x
delta = inverse_transform(before) @ after
effects.append((
float(np.linalg.norm(delta[:3, 3])),
rotation_angle_deg(delta[:3, :3]),
))
effects = np.asarray(effects, float)
maximum = np.max(effects, axis=0)
return {
"body_left_z_test_shift_m": test_shift_m,
"max_predicted_motion_change_translation_m": float(maximum[0]),
"max_predicted_motion_change_rotation_deg": float(maximum[1]),
"numerically_unobservable": bool(maximum[0] < 1e-10 and maximum[1] < 1e-10),
"note": "AX pairs cannot determine X.z when every A rotation preserves body Z; use ground/external height constraints.",
}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--pairs", required=True)
parser.add_argument("--extrinsic", required=True)
parser.add_argument("--output", required=True)
parser.add_argument("--csv")
parser.add_argument("--ground-planes")
parser.add_argument("--pair-index", type=int, default=0)
parser.add_argument("--pitch-values", nargs="+", type=float, default=[0.1, 0.2, 0.3])
parser.add_argument("--roll-values", nargs="+", type=float, default=[-0.2, -0.1, 0.1])
parser.add_argument("--yaw-values", nargs="+", type=float, default=[-0.2, 0.2])
parser.add_argument("--translation-scale", type=float, default=0.05)
parser.add_argument("--rotation-scale", type=float, default=0.5)
parser.add_argument("--body-height", type=float, default=0.2335)
args = parser.parse_args()
a_array, b_array, meta, stations = read_pairs(args.pairs)
if not 0 <= args.pair_index < len(a_array):
raise IndexError(f"pair-index {args.pair_index} outside [0,{len(a_array)-1}]")
with Path(args.extrinsic).open(encoding="utf-8-sig") as stream:
x = np.asarray(json.load(stream)["matrix_4x4"], float)
planes = read_planes(args.ground_planes) if args.ground_planes else np.empty((0, 4))
candidates = [
evaluate(
label, correction, a_array, b_array, meta, x, args.pair_index,
args.translation_scale, args.rotation_scale, planes, args.body_height,
)
for label, correction in candidate_grid(
args.pitch_values, args.roll_values, args.yaw_values
)
]
baseline = candidates[0]
baseline_scores = np.asarray([
math.hypot(
item["translation_norm_m"] / args.translation_scale,
item["rotation_angle_deg"] / args.rotation_scale,
)
for item in baseline["per_pair"]
])
base_global = baseline["all_pairs"]["normalized_global_rms"]
for candidate in candidates:
scores = np.asarray([
math.hypot(
item["translation_norm_m"] / args.translation_scale,
item["rotation_angle_deg"] / args.rotation_scale,
)
for item in candidate["per_pair"]
])
delta = scores - baseline_scores
candidate["comparison_to_baseline"] = {
"normalized_global_rms_change": float(
candidate["all_pairs"]["normalized_global_rms"] - base_global
),
"improved_pairs": int(np.sum(delta < -1e-12)),
"worsened_pairs": int(np.sum(delta > 1e-12)),
"unchanged_pairs": int(np.sum(np.abs(delta) <= 1e-12)),
"median_per_pair_score_change": float(np.median(delta)),
"global_consistency_signal": bool(
candidate["all_pairs"]["normalized_global_rms"] < base_global
and np.sum(delta < -1e-12) > np.sum(delta > 1e-12)
),
}
ranking = sorted(
candidates,
key=lambda item: item["all_pairs"]["normalized_global_rms"],
)
report = {
"schema_version": 1,
"diagnostic_only": True,
"extrinsic_was_modified": False,
"equation": "delta_ij = B_ij^-1 * (X^-1 * A_ij * X)",
"correction_convention": "X_test = DeltaR_body * X; DeltaR uses fixed body xyz RPY axes",
"component_frame": "delta translation/RPY components are in station-j LiDAR coordinates, not screen axes",
"selection_rule": (
"Never accept a correction from selected_pair alone. Require improvement over all "
"refined pairs, directional consistency across pairs, acceptable ground constraints, "
"and independent visual review. This script never overwrites X."
),
"pairs_file": str(Path(args.pairs).resolve()),
"extrinsic_file": str(Path(args.extrinsic).resolve()),
"stations": stations,
"pairs": len(a_array),
"selected_pair_index": args.pair_index,
"selected_pair_stations": [int(meta[args.pair_index, 0]), int(meta[args.pair_index, 1])],
"normalization": {
"translation_scale_m": args.translation_scale,
"rotation_scale_deg": args.rotation_scale,
},
"z_observability": z_observability(a_array, x, 0.10),
"ranking_by_all_pair_normalized_rms": [
{
"rank": rank,
"label": item["label"],
"body_left_rpy_correction_deg_xyz": item["body_left_rpy_correction_deg_xyz"],
"normalized_global_rms": item["all_pairs"]["normalized_global_rms"],
**item["comparison_to_baseline"],
}
for rank, item in enumerate(ranking, 1)
],
"candidates": candidates,
}
output = Path(args.output)
output.parent.mkdir(parents=True, exist_ok=True)
output.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8")
csv_path = Path(args.csv) if args.csv else output.with_suffix(".csv")
with csv_path.open("w", encoding="utf-8", newline="") as stream:
fields = [
"label", "roll_correction_deg", "pitch_correction_deg", "yaw_correction_deg",
"selected_pair_translation_cm", "selected_pair_rotation_deg",
"all_pair_translation_rms_m", "all_pair_rotation_rms_deg",
"normalized_global_rms", "normalized_global_rms_change",
"improved_pairs", "worsened_pairs", "global_consistency_signal",
"ground_normal_tilt_rms_deg", "ground_height_rms_m",
]
writer = csv.DictWriter(stream, fieldnames=fields)
writer.writeheader()
for item in candidates:
correction = item["body_left_rpy_correction_deg_xyz"]
ground = item["ground"]
comparison = item["comparison_to_baseline"]
writer.writerow({
"label": item["label"],
"roll_correction_deg": correction[0],
"pitch_correction_deg": correction[1],
"yaw_correction_deg": correction[2],
"selected_pair_translation_cm": item["selected_pair"]["translation_norm_m"] * 100.0,
"selected_pair_rotation_deg": item["selected_pair"]["rotation_angle_deg"],
"all_pair_translation_rms_m": item["all_pairs"]["translation_m"]["rms"],
"all_pair_rotation_rms_deg": item["all_pairs"]["rotation_deg"]["rms"],
"normalized_global_rms": item["all_pairs"]["normalized_global_rms"],
"normalized_global_rms_change": comparison["normalized_global_rms_change"],
"improved_pairs": comparison["improved_pairs"],
"worsened_pairs": comparison["worsened_pairs"],
"global_consistency_signal": comparison["global_consistency_signal"],
"ground_normal_tilt_rms_deg": None if ground is None else ground["normal_tilt_deg"]["rms"],
"ground_height_rms_m": None if ground is None else ground["height_residual_m"]["rms"],
})
print(json.dumps({
"diagnostic_only": True,
"selected_pair": baseline["selected_pair"],
"z_observability": report["z_observability"],
"top_all_pair_candidates": report["ranking_by_all_pair_normalized_rms"][:8],
"output": str(output.resolve()),
"csv": str(csv_path.resolve()),
}, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()
code/summarize_results.py
-87
View File
@@ -1,87 +0,0 @@
#!/usr/bin/env python3
"""Build a concise backend comparison and select the recommended result."""
import argparse
import json
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--open3d", required=True)
parser.add_argument("--small", required=True)
parser.add_argument("--open3d-quality", required=True)
parser.add_argument("--small-quality", required=True)
parser.add_argument("--open3d-check", required=True)
parser.add_argument("--small-check", required=True)
parser.add_argument("--output", required=True)
parser.add_argument("--recommended-output", required=True)
args = parser.parse_args()
open_result = json.loads(Path(args.open3d).read_text(encoding="utf-8-sig"))
small_result = json.loads(Path(args.small).read_text(encoding="utf-8-sig"))
open_quality = json.loads(Path(args.open3d_quality).read_text(encoding="utf-8-sig"))
small_quality = json.loads(Path(args.small_quality).read_text(encoding="utf-8-sig"))
open_check = json.loads(Path(args.open3d_check).read_text(encoding="utf-8-sig"))
small_check = json.loads(Path(args.small_check).read_text(encoding="utf-8-sig"))
x_open = np.asarray(open_result["matrix_4x4"], float)
x_small = np.asarray(small_result["matrix_4x4"], float)
delta = np.linalg.inv(x_open) @ x_small
def compact(result, quality, check):
estimate = result["estimation"]["residuals"]
auxiliary = check["metrics"]
return {
"translation_m": result["translation_m"],
"rotation_rpy_deg_xyz": result["rotation_rpy_deg_xyz"],
"estimation_pairs": estimate["pairs"],
"estimation_translation_rms_m": estimate["translation_m"]["rms"],
"estimation_rotation_rms_deg": estimate["rotation_deg"]["rms"],
"bootstrap_std": result["bootstrap"]["std"],
"initial_B_loop_closure": quality["accepted_loop_closure"],
"batch1_auxiliary_pairs": auxiliary["pairs"],
"batch1_auxiliary_translation_rms_m": auxiliary["translation_m"]["rms"],
"batch1_auxiliary_rotation_rms_deg": auxiliary["rotation_deg"]["rms"],
}
summary = {
"recommended_backend": "open3d_gicp",
"selection_reason": (
"The two X estimates agree closely; Open3D has lower second-batch AX residual, "
"better B loop closure, and lower first-batch auxiliary residual."
),
"coordinate_convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
"measured_extrinsic_used_as_initial": False,
"second_batch_role": "estimation (dense RTK)",
"first_batch_role": "auxiliary check only (sparse RTK)",
"backend_difference": {
"translation_m": float(np.linalg.norm(delta[:3, 3])),
"rotation_deg": float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude())),
},
"open3d_gicp": compact(open_result, open_quality, open_check),
"small_gicp": compact(small_result, small_quality, small_check),
"important_limit": (
"Backend agreement is strong, but AX rotation RMS remains about one degree. "
"This is not a centimetre-grade absolute certification."
),
}
output = Path(args.output)
output.parent.mkdir(parents=True, exist_ok=True)
output.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
recommended = dict(open_result)
recommended["selection"] = {
"recommended_backend": "open3d_gicp",
"comparison_summary": str(output.name),
"backend_difference": summary["backend_difference"],
"warning": summary["important_limit"],
}
Path(args.recommended_output).write_text(
json.dumps(recommended, ensure_ascii=False, indent=2), encoding="utf-8"
)
print(json.dumps(summary, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()