新增雷达到RTK直接手眼标定流程
This commit is contained in:
lichun.qu
@@ -0,0 +1,771 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user