#!/usr/bin/env python3 """Rigorous stationary LiDAR / reference-trajectory hand-eye calibration. Convention: T_A_B maps points from frame B into frame A. 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 import argparse import csv import json import math import time from dataclasses import dataclass from pathlib import Path import numpy as np from scipy.optimize import least_squares from scipy.spatial import cKDTree def skew(v): x, y, z = v return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]]) def so3_exp(v): angle = float(np.linalg.norm(v)) if angle < 1e-12: return np.eye(3) + skew(v) k = skew(np.asarray(v, float) / angle) return np.eye(3) + math.sin(angle) * k + (1.0 - math.cos(angle)) * k @ k def so3_log(rotation): cosine = float(np.clip((np.trace(rotation) - 1.0) / 2.0, -1.0, 1.0)) angle = math.acos(cosine) vee = np.array([ rotation[2, 1] - rotation[1, 2], rotation[0, 2] - rotation[2, 0], rotation[1, 0] - rotation[0, 1], ]) if angle < 1e-9: return vee / 2.0 if abs(math.pi - angle) < 1e-5: values, vectors = np.linalg.eigh((rotation + np.eye(3)) / 2.0) return vectors[:, int(np.argmax(values))] * angle return vee * angle / (2.0 * math.sin(angle)) def quat_to_rotation(q): x, y, z, w = np.asarray(q, float) / np.linalg.norm(q) return np.array([ [1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)], [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)], [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)], ]) def rotation_to_quat(rotation): from scipy.spatial.transform import Rotation return Rotation.from_matrix(rotation).as_quat() def rpy_deg(rotation): from scipy.spatial.transform import Rotation return Rotation.from_matrix(rotation).as_euler("xyz", degrees=True).tolist() def make_transform(translation, rotation): transform = np.eye(4) transform[:3, :3] = rotation transform[:3, 3] = translation return transform def params_transform(params): return make_transform(params[:3], so3_exp(params[3:])) def inverse_transform(transform): answer = np.eye(4) answer[:3, :3] = transform[:3, :3].T answer[:3, 3] = -answer[:3, :3] @ transform[:3, 3] return answer def transform_points(points, transform): return points @ transform[:3, :3].T + transform[:3, 3] def rotation_angle_deg(rotation): return math.degrees(np.linalg.norm(so3_log(rotation))) @dataclass class PoseSeries: time: np.ndarray transforms: np.ndarray def read_poses(path): timestamps, transforms = [], [] with Path(path).open(encoding="utf-8-sig", newline="") as stream: reader = csv.DictReader(stream) required = ("time", "x", "y", "z", "qx", "qy", "qz", "qw") missing = [key for key in required if key not in (reader.fieldnames or [])] if missing: raise ValueError(f"{path}: missing pose fields {missing}") for row in reader: timestamps.append(float(row["time"])) translation = np.array([float(row[k]) for k in ("x", "y", "z")]) quaternion = np.array([float(row[k]) for k in ("qx", "qy", "qz", "qw")]) transforms.append(make_transform(translation, quat_to_rotation(quaternion))) order = np.argsort(timestamps) return PoseSeries(np.asarray(timestamps)[order], np.asarray(transforms)[order]) def nearest_pose(series, timestamp): index = int(np.argmin(np.abs(series.time - timestamp))) return series.transforms[index], float(abs(series.time[index] - timestamp)) def npz_files(root): files = sorted(Path(root).rglob("*.npz")) if not files: raise FileNotFoundError(f"no NPZ files under {root}") return files def load_npz_xyz(path, min_range=1.0, max_range=50.0): with np.load(path, allow_pickle=False) as data: if "points_raw" not in data: raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden") raw = np.asarray(data["points_raw"], dtype=np.float64) timestamp = float(np.ravel(data["unix_time_ns"])[0]) / 1e9 counter = int(np.ravel(data["frame_counter"])[0]) distance = raw[:, 0] * 0.001 azimuth = np.deg2rad(raw[:, 1]) altitude = np.deg2rad(raw[:, 2]) valid = ( np.isfinite(distance + azimuth + altitude) & (distance >= min_range) & (distance <= max_range) ) distance, azimuth, altitude = distance[valid], azimuth[valid], altitude[valid] xyz = np.column_stack(( distance * np.cos(altitude) * np.cos(azimuth), distance * np.cos(altitude) * np.sin(azimuth), distance * np.sin(altitude), )) return timestamp, counter, xyz def load_stations(root, min_range, max_range, z_min=None, z_max=None): stations = [] for path in npz_files(root): timestamp, counter, xyz = load_npz_xyz(path, min_range, max_range) if z_min is not None: xyz = xyz[(xyz[:, 2] >= z_min) & (xyz[:, 2] <= z_max)] stations.append((timestamp, counter, path, xyz)) stations.sort(key=lambda item: item[0]) return stations def split_holdout(points, fraction, phase): stride = max(int(round(1.0 / fraction)), 2) index = np.arange(len(points)) holdout = ((index + phase) % stride) == 0 return points[~holdout], points[holdout] def make_o3d_cloud(points, voxel): import open3d as o3d cloud = o3d.geometry.PointCloud() cloud.points = o3d.utility.Vector3dVector(np.asarray(points, float)) return cloud.voxel_down_sample(voxel) def align_open3d(target, source, initial, voxels, correspondences, iterations): import open3d as o3d registration = o3d.pipelines.registration estimate = registration.TransformationEstimationForGeneralizedICP() criteria = registration.ICPConvergenceCriteria(max_iteration=iterations) transform, stages = np.asarray(initial, float), [] final_target = final_source = final_answer = None started = time.perf_counter() for voxel, correspondence in zip(voxels, correspondences): target_cloud = make_o3d_cloud(target, voxel) source_cloud = make_o3d_cloud(source, voxel) answer = registration.registration_generalized_icp( source_cloud, target_cloud, correspondence, transform, estimate, criteria ) transform = np.asarray(answer.transformation, float) stages.append({ "voxel_m": voxel, "max_correspondence_m": correspondence, "fitness": float(answer.fitness), "inlier_rmse_m": float(answer.inlier_rmse), "target_points": len(target_cloud.points), "source_points": len(source_cloud.points), }) final_target, final_source, final_answer = target_cloud, source_cloud, answer information = registration.get_information_matrix_from_point_clouds( final_source, final_target, correspondences[-1], transform ) inliers = int(round(float(final_answer.fitness) * len(final_source.points))) return { "transform": transform, "hessian": np.asarray(information, float), "converged": None, "iterations": None, "num_inliers": inliers, "objective": float(final_answer.inlier_rmse ** 2 * max(inliers, 1)), "elapsed_sec": time.perf_counter() - started, "stages": stages, } def align_small_gicp(target, source, initial, voxels, correspondences, iterations, threads): import small_gicp transform, stages, result = np.asarray(initial, float), [], None started = time.perf_counter() for voxel, correspondence in zip(voxels, correspondences): result = small_gicp.align( np.ascontiguousarray(target), np.ascontiguousarray(source), transform, registration_type="GICP", downsampling_resolution=voxel, max_correspondence_distance=correspondence, num_threads=threads, max_iterations=iterations, rotation_epsilon=math.radians(0.005), translation_epsilon=0.0005, verbose=False, ) transform = np.asarray(result.T_target_source, float) stages.append({ "voxel_m": voxel, "max_correspondence_m": correspondence, "converged": bool(result.converged), "iterations": int(result.iterations), "num_inliers": int(result.num_inliers), "objective": float(result.error), }) return { "transform": transform, "hessian": np.asarray(result.H, float), "converged": bool(result.converged), "iterations": int(result.iterations), "num_inliers": int(result.num_inliers), "objective": float(result.error), "elapsed_sec": time.perf_counter() - started, "stages": stages, } def align_backend(backend, target, source, initial, args): if backend == "open3d": return align_open3d( target, source, initial, args.voxels, args.correspondences, args.iterations ) return align_small_gicp( target, source, initial, args.voxels, args.correspondences, args.iterations, args.threads ) def symmetric_heldout_metrics(target_fit, target_holdout, source_fit, source_holdout, transform, threshold): transformed_source_fit = transform_points(source_fit, transform) transformed_source_holdout = transform_points(source_holdout, transform) forward = cKDTree(target_fit).query(transformed_source_holdout, workers=-1)[0] reverse = cKDTree(transformed_source_fit).query(target_holdout, workers=-1)[0] distances = np.concatenate((forward, reverse)) inliers = distances[distances <= threshold] return { "evaluated": int(len(distances)), "inliers": int(len(inliers)), "inlier_ratio": float(len(inliers) / max(len(distances), 1)), "inlier_rmse_m": float(np.sqrt(np.mean(inliers**2))) if len(inliers) else None, "median_m": float(np.median(distances)), "p90_m": float(np.quantile(distances, 0.90)), "p95_m": float(np.quantile(distances, 0.95)), } def hessian_metrics(hessian, characteristic_length=10.0): hessian = 0.5 * (np.asarray(hessian, float) + np.asarray(hessian, float).T) scale = np.diag([1.0 / characteristic_length] * 3 + [1.0] * 3) scaled = scale.T @ hessian @ scale values, vectors = np.linalg.eigh(scaled) largest = max(float(np.max(np.abs(values))), np.finfo(float).eps) positive = values[values > largest * 1e-9] condition = float(positive[-1] / positive[0]) if len(positive) else float("inf") return { "native_order": ["rx_rad", "ry_rad", "rz_rad", "tx_m", "ty_m", "tz_m"], "scaled_eigenvalues": values.tolist(), "effective_rank": int(len(positive)), "scaled_condition_number": condition, "weakest_scaled_direction": vectors[:, int(np.argmin(values))].tolist(), } def transform_difference(reference, candidate): delta = inverse_transform(reference) @ candidate return { "translation_m": float(np.linalg.norm(delta[:3, 3])), "rotation_deg": rotation_angle_deg(delta[:3, :3]), } def loop_metrics(transforms): loops = [] for (i, j), b_ij in transforms.items(): for (j2, k), b_jk in transforms.items(): if j2 != j or (i, k) not in transforms: continue loops.append(transform_difference(transforms[(i, k)], b_ij @ b_jk)) if not loops: return {"count": 0} translation = np.array([item["translation_m"] for item in loops]) rotation = np.array([item["rotation_deg"] for item in loops]) return { "count": len(loops), "translation_rms_m": float(np.sqrt(np.mean(translation**2))), "translation_p95_m": float(np.quantile(translation, 0.95)), "rotation_rms_deg": float(np.sqrt(np.mean(rotation**2))), "rotation_p95_deg": float(np.quantile(rotation, 0.95)), } def cmd_ground(args): stations = load_stations(args.frames, args.min_range, args.max_range) rows = [] for timestamp, counter, _, xyz in stations: roi = xyz[(xyz[:, 2] >= args.z_min) & (xyz[:, 2] <= args.z_max)] if len(roi) < args.min_inliers: continue cloud = make_o3d_cloud(roi, args.voxel) plane, indexes = cloud.segment_plane( args.distance_threshold, 3, args.ransac_iterations ) normal = np.asarray(plane[:3], float) norm = np.linalg.norm(normal) normal, distance = normal / norm, float(plane[3] / norm) if distance < 0: normal, distance = -normal, -distance points = np.asarray(cloud.points)[indexes] rms = float(np.sqrt(np.mean((points @ normal + distance) ** 2))) if len(indexes) >= args.min_inliers and rms <= args.max_rms: rows.append([timestamp, *normal, distance, len(indexes), rms, counter]) output = Path(args.output) output.parent.mkdir(parents=True, exist_ok=True) with output.open("w", encoding="utf-8", newline="") as stream: writer = csv.writer(stream) writer.writerow(["time", "nx", "ny", "nz", "d", "inliers", "rms_m", "frame_counter"]) writer.writerows(rows) print(json.dumps({"planes": len(rows), "output": str(output.resolve())}, indent=2)) def cmd_pairs(args): if len(args.voxels) != len(args.correspondences): raise ValueError("--voxels and --correspondences must have equal lengths") stations = load_stations( args.frames, args.min_range, args.max_range, args.z_min, args.z_max ) 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)}") 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(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) accepted_a, accepted_b, accepted_meta, reports = [], [], [], [] 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(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: continue initial_b = a_ij.copy() # X0=I; no measured extrinsic. target_fit, target_holdout = split[i] source_fit, source_holdout = split[j] forward = align_backend(args.backend, target_fit, source_fit, initial_b, args) heldout = symmetric_heldout_metrics( target_fit, target_holdout, source_fit, source_holdout, forward["transform"], args.evaluation_distance ) hessian = hessian_metrics(forward["hessian"]) reverse_answer = align_backend( args.backend, source_fit, target_fit, inverse_transform(initial_b), args ) reverse = transform_difference( forward["transform"], inverse_transform(reverse_answer["transform"]) ) multistart = [] for _ in range(args.multistart): perturb = np.r_[ rng.normal(0.0, args.multistart_translation_sigma, 3), np.deg2rad(rng.normal(0.0, args.multistart_rotation_sigma, 3)), ] candidate = align_backend( args.backend, target_fit, source_fit, params_transform(perturb) @ initial_b, args ) multistart.append(transform_difference(forward["transform"], candidate["transform"])) stable = [ item["translation_m"] <= args.multistart_translation_tolerance and item["rotation_deg"] <= args.multistart_rotation_tolerance for item in multistart ] success_rate = float(np.mean(stable)) if stable else 1.0 reasons = [] if forward["converged"] is False: reasons.append("backend_not_converged") if heldout["inlier_ratio"] < args.min_inlier_ratio: reasons.append("heldout_inlier_ratio") if heldout["inlier_rmse_m"] is None or heldout["inlier_rmse_m"] > args.max_inlier_rmse: reasons.append("heldout_inlier_rmse") if hessian["effective_rank"] < 6: reasons.append("hessian_rank") if hessian["scaled_condition_number"] > args.max_hessian_condition: reasons.append("hessian_condition") if reverse["translation_m"] > args.reverse_translation_tolerance: reasons.append("forward_reverse_translation") if reverse["rotation_deg"] > args.reverse_rotation_tolerance: reasons.append("forward_reverse_rotation") if success_rate < args.min_multistart_success: reasons.append("multistart_instability") accepted = not reasons report = { "i": i, "j": j, "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": 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"], "backend_iterations": forward["iterations"], "backend_num_inliers": forward["num_inliers"], "backend_objective": forward["objective"], "backend_elapsed_sec": forward["elapsed_sec"], "multiscale_stages": forward["stages"], "heldout_symmetric": heldout, "hessian": hessian, "forward_reverse": reverse, "multistart": {"runs": len(multistart), "success_rate": success_rate, "deltas": multistart}, "accepted": accepted, "rejection_reasons": reasons, } reports.append(report) print(f"{args.backend} {i:02d}->{j:02d} rmse={heldout['inlier_rmse_m']} " f"ratio={heldout['inlier_ratio']:.3f} accepted={accepted}") if accepted: accepted_a.append(a_ij) accepted_b.append(forward["transform"]) accepted_meta.append([i, j, stations[i][0], stations[j][0]]) accepted_transforms[(i, j)] = forward["transform"] output = Path(args.output) output.parent.mkdir(parents=True, exist_ok=True) np.savez_compressed( 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(reference_dt), backend=np.asarray(args.backend), ) quality = { "schema_version": 2, "backend": args.backend, "transform_convention": "B_ij=T_Li_Lj maps station j points into station i", "raw_point_field": "points_raw", "measured_extrinsic_used_as_initial": False, "stations": len(stations), "candidate_pairs": len(reports), "accepted_pairs": len(accepted_a), "parameters": vars(args), "accepted_loop_closure": loop_metrics(accepted_transforms), "pairs": reports, } quality["parameters"].pop("func", None) quality_path = Path(args.quality_json or output.with_suffix(".quality.json")) quality_path.write_text(json.dumps(quality, ensure_ascii=False, indent=2), encoding="utf-8") csv_path = Path(args.quality_csv or output.with_suffix(".quality.csv")) with csv_path.open("w", encoding="utf-8", newline="") as stream: fields = ["i", "j", "rtk_translation_m", "rtk_rotation_deg", "heldout_inlier_ratio", "heldout_inlier_rmse_m", "hessian_rank", "hessian_condition", "reverse_translation_m", "reverse_rotation_deg", "multistart_success_rate", "accepted", "rejection_reasons"] writer = csv.DictWriter(stream, fieldnames=fields) writer.writeheader() for item in reports: writer.writerow({ "i": item["i"], "j": item["j"], "rtk_translation_m": item["rtk_translation_m"], "rtk_rotation_deg": item["rtk_rotation_deg"], "heldout_inlier_ratio": item["heldout_symmetric"]["inlier_ratio"], "heldout_inlier_rmse_m": item["heldout_symmetric"]["inlier_rmse_m"], "hessian_rank": item["hessian"]["effective_rank"], "hessian_condition": item["hessian"]["scaled_condition_number"], "reverse_translation_m": item["forward_reverse"]["translation_m"], "reverse_rotation_deg": item["forward_reverse"]["rotation_deg"], "multistart_success_rate": item["multistart"]["success_rate"], "accepted": item["accepted"], "rejection_reasons": ";".join(item["rejection_reasons"]), }) if len(accepted_a) < args.min_pairs: raise RuntimeError(f"only {len(accepted_a)} accepted pairs; need {args.min_pairs}") print(json.dumps({ "backend": args.backend, "stations": len(stations), "candidate_pairs": len(reports), "accepted_pairs": len(accepted_a), "output": str(output.resolve()), "quality_json": str(quality_path.resolve()), "loop": quality["accepted_loop_closure"], }, indent=2)) def read_planes(path): planes = [] with Path(path).open(encoding="utf-8-sig", newline="") as stream: for row in csv.DictReader(stream): normal = np.array([float(row[k]) for k in ("nx", "ny", "nz")]) norm = np.linalg.norm(normal) normal, distance = normal / norm, float(row["d"]) / norm if distance < 0: normal, distance = -normal, -distance planes.append([*normal, distance]) return np.asarray(planes) def read_pairs(path): with np.load(path, allow_pickle=False) as data: return (np.asarray(data["A"], float), np.asarray(data["B"], float), np.asarray(data["meta"], float), len(data["station_times"])) def calibration_residual(params, a_array, b_array, planes, args): x = params_transform(params) values = [] for a_ij, b_ij in zip(a_array, b_array): error = inverse_transform(a_ij @ x) @ x @ b_ij values.extend((error[:3, 3] / args.translation_sigma).tolist()) values.extend((so3_log(error[:3, :3]) / math.radians(args.rotation_sigma)).tolist()) body_up = np.array([0.0, 0.0, 1.0]) for plane in planes: 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.reference_height) / args.plane_height_sigma) return np.asarray(values) def pair_metrics(a_array, b_array, x): translation, rotation, rows = [], [], [] for index, (a_ij, b_ij) in enumerate(zip(a_array, b_array)): predicted = inverse_transform(x) @ a_ij @ x delta = inverse_transform(b_ij) @ predicted t = float(np.linalg.norm(delta[:3, 3])) r = rotation_angle_deg(delta[:3, :3]) translation.append(t); rotation.append(r) rows.append({"pair_index": index, "translation_m": t, "rotation_deg": r}) translation, rotation = np.asarray(translation), np.asarray(rotation) def stats(values): 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)), } return {"pairs": len(rows), "translation_m": stats(translation), "rotation_deg": stats(rotation), "per_pair": rows} def solve_extrinsic(a_array, b_array, planes, args): rng = np.random.default_rng(args.seed) starts = [np.zeros(6)] for _ in range(args.solver_multistart - 1): starts.append(np.r_[ rng.normal(0.0, args.start_translation_sigma, 3), np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)), ]) candidates = [] lower = np.r_[[-5.0] * 3, [-math.pi] * 3] upper = np.r_[[5.0] * 3, [math.pi] * 3] for start in starts: answer = least_squares( calibration_residual, np.clip(start, lower, upper), args=(a_array, b_array, planes, args), bounds=(lower, upper), loss="huber", f_scale=1.5, x_scale="jac", max_nfev=args.max_nfev, ) candidates.append(answer) best = min(candidates, key=lambda item: item.cost) return best, candidates def cmd_calibrate(args): a_array, b_array, meta, stations = read_pairs(args.pairs) planes = read_planes(args.ground_planes) best, candidates = solve_extrinsic(a_array, b_array, planes, args) x = params_transform(best.x) residual = calibration_residual(best.x, a_array, b_array, planes, args) absolute = np.abs(residual) weights = np.ones_like(residual) weights[absolute > 1.5] = 1.5 / absolute[absolute > 1.5] weighted_jacobian = best.jac * np.sqrt(weights)[:, None] singular = np.linalg.svd(weighted_jacobian, compute_uv=False) condition = float(singular[0] / max(singular[-1], 1e-15)) dof = max(len(residual) - 6, 1) covariance = np.linalg.pinv(weighted_jacobian.T @ weighted_jacobian) * float( np.sum(weights * residual**2) / dof ) sigma = np.sqrt(np.maximum(np.diag(covariance), 0.0)) candidate_summary = [] for item in candidates: candidate_x = params_transform(item.x) candidate_summary.append({ "cost": float(item.cost), "success": bool(item.success), **transform_difference(x, candidate_x), }) bootstrap = [] rng = np.random.default_rng(args.seed + 1) for _ in range(args.bootstrap): indexes = rng.integers(0, len(a_array), len(a_array)) answer = least_squares( calibration_residual, best.x, args=(a_array[indexes], b_array[indexes], planes, args), loss="huber", f_scale=1.5, x_scale="jac", max_nfev=args.max_nfev, ) bootstrap.append(np.r_[answer.x[:3], rpy_deg(so3_exp(answer.x[3:]))]) bootstrap = np.asarray(bootstrap) result = { "schema_version": 2, "success": bool(best.success), "message": best.message, "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(), "rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]), "quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(), "matrix_4x4": x.tolist(), "estimation": {"stations": stations, "pairs": len(a_array), "residuals": pair_metrics(a_array, b_array, x)}, "ground": { "planes": len(planes), "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(), "rotation_deg": np.rad2deg(sigma[3:]).tolist(), "warning": "conditional local estimate; bootstrap is the primary stability check", }, "weighted_jacobian_condition_number": condition, "solver_multistart": { "runs": len(candidates), "candidates_relative_to_best": candidate_summary, }, "bootstrap": { "runs": len(bootstrap), "order": ["x_m", "y_m", "z_m", "roll_deg", "pitch_deg", "yaw_deg"], "std": np.std(bootstrap, axis=0, ddof=1).tolist() if len(bootstrap) > 1 else None, "p025": np.quantile(bootstrap, 0.025, axis=0).tolist() if len(bootstrap) else None, "p975": np.quantile(bootstrap, 0.975, axis=0).tolist() if len(bootstrap) else None, }, } output = Path(args.output) output.parent.mkdir(parents=True, exist_ok=True) output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8") print(json.dumps(result, ensure_ascii=False, indent=2)) def cmd_validate(args): result = json.loads(Path(args.extrinsic).read_text(encoding="utf-8-sig")) x = np.asarray(result["matrix_4x4"], float) a_array, b_array, meta, stations = read_pairs(args.pairs) metrics = pair_metrics(a_array, b_array, x) for row, pair_meta in zip(metrics["per_pair"], meta): row.update({"i": int(pair_meta[0]), "j": int(pair_meta[1])}) report = { "role": "auxiliary check only; first-batch RTK is sparse", "blind_with_respect_to_X": True, "note": "No AX residual was used to select these pairs", "stations": stations, "metrics": metrics, } 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") print(json.dumps(report, ensure_ascii=False, indent=2)) def build_parser(): parser = argparse.ArgumentParser(description=__doc__) commands = parser.add_subparsers(dest="command", required=True) ground = commands.add_parser("ground") ground.add_argument("--frames", required=True); ground.add_argument("--output", required=True) ground.add_argument("--min-range", type=float, default=1.0); ground.add_argument("--max-range", type=float, default=30.0) ground.add_argument("--z-min", type=float, default=-1.4); ground.add_argument("--z-max", type=float, default=-0.4) ground.add_argument("--voxel", type=float, default=0.08); ground.add_argument("--distance-threshold", type=float, default=0.025) ground.add_argument("--ransac-iterations", type=int, default=500); ground.add_argument("--min-inliers", type=int, default=500) ground.add_argument("--max-rms", type=float, default=0.025); ground.set_defaults(func=cmd_ground) pairs = commands.add_parser("pairs") pairs.add_argument("--backend", choices=["open3d", "small_gicp"], 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) pairs.add_argument("--min-gap", type=int, default=1); pairs.add_argument("--max-gap", type=int, default=5) pairs.add_argument("--min-translation", type=float, default=0.5); pairs.add_argument("--min-rotation", type=float, default=3.0) pairs.add_argument("--min-range", type=float, default=2.0); pairs.add_argument("--max-range", type=float, default=50.0) pairs.add_argument("--z-min", type=float, default=-0.60); pairs.add_argument("--z-max", type=float, default=5.0) pairs.add_argument("--min-roi-points", type=int, default=1000) pairs.add_argument("--holdout-fraction", type=float, default=0.20) pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08]) pairs.add_argument("--correspondences", nargs="+", type=float, default=[1.20, 0.50, 0.25]) pairs.add_argument("--iterations", type=int, default=60); pairs.add_argument("--threads", type=int, default=8) pairs.add_argument("--evaluation-distance", type=float, default=0.25) pairs.add_argument("--min-inlier-ratio", type=float, default=0.35); pairs.add_argument("--max-inlier-rmse", type=float, default=0.16) pairs.add_argument("--max-hessian-condition", type=float, default=1e8) pairs.add_argument("--reverse-translation-tolerance", type=float, default=0.08) pairs.add_argument("--reverse-rotation-tolerance", type=float, default=0.50) pairs.add_argument("--multistart", type=int, default=2) pairs.add_argument("--multistart-translation-sigma", type=float, default=0.30) pairs.add_argument("--multistart-rotation-sigma", type=float, default=3.0) pairs.add_argument("--multistart-translation-tolerance", type=float, default=0.08) pairs.add_argument("--multistart-rotation-tolerance", type=float, default=0.50) pairs.add_argument("--min-multistart-success", type=float, default=0.50) pairs.add_argument("--seed", type=int, default=20260721); pairs.set_defaults(func=cmd_pairs) calibrate = commands.add_parser("calibrate") calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True) calibrate.add_argument("--output", required=True) calibrate.add_argument("--translation-sigma", type=float, default=0.05) 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("--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) calibrate.add_argument("--bootstrap", type=int, default=100) calibrate.add_argument("--max-nfev", type=int, default=1000) calibrate.add_argument("--seed", type=int, default=20260721); calibrate.set_defaults(func=cmd_calibrate) validate = commands.add_parser("validate") validate.add_argument("--pairs", required=True); validate.add_argument("--extrinsic", required=True) validate.add_argument("--output", required=True); validate.set_defaults(func=cmd_validate) return parser def main(): args = build_parser().parse_args() args.func(args) if __name__ == "__main__": main()