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/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)