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

修正基线系标定默认：机械初值、地面ROI与航向偏移可配，并补充G90窗导出与契约测试

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-10 22:28:25 +08:00
co-authored by Cursor
parent 69bb44bccd
commit 46d2fa1d69
14 changed files with 932 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
code/finalize_direct_rtk_lidar.py
+68 -8
View File
@@ -34,7 +34,50 @@ def delta(a: np.ndarray, b: np.ndarray) -> dict:
}
def corrected(raw: dict, backend: str, reference_height: float) -> dict:
def coordinate_contract_audit(raw: dict) -> dict:
"""Compare the data-driven solution with the declared mechanical initial.
A near-180-degree disagreement is not auto-corrected: it normally means
that one physical forward-axis statement is reversed. Silently rotating
the point cloud would preserve residuals while changing the frame contract.
"""
path_text = raw.get("solver_initial_extrinsic")
if not path_text:
return {
"status": "mechanical_initial_not_available",
"requires_physical_axis_confirmation": False,
}
path = Path(path_text)
if not path.exists():
return {
"status": "mechanical_initial_file_missing",
"requires_physical_axis_confirmation": False,
"mechanical_initial_path": str(path),
}
initial_document = load(path)
initial = np.asarray(initial_document["matrix_4x4"], float)
solution = np.asarray(raw["matrix_4x4"], float)
comparison = delta(initial, solution)
near_180 = abs(comparison["rotation_deg"] - 180.0) <= 15.0
return {
"status": "near_180_degree_axis_conflict" if near_180 else "no_near_180_degree_axis_conflict",
"requires_physical_axis_confirmation": near_180,
"mechanical_initial_path": str(path.resolve()),
"solution_relative_to_mechanical_initial": comparison,
"note": (
"No automatic 180-degree point-cloud flip was applied. Confirm the Helios "
"aviation-connector side and the G90 vehicle-forward definition before deployment."
),
}
def corrected(raw: dict, backend: str, reference_height: float, heading_offset_deg: float) -> dict:
baseline_frame = abs(heading_offset_deg) <= 1e-12
x_axis = (
"horizontal projection of the rawHeading baseline direction reported by the receiver"
if baseline_frame else
"vehicle forward after applying the configured G90 heading offset"
)
return {
"schema_version": 1,
"success": bool(raw["success"]),
@@ -43,20 +86,24 @@ def corrected(raw: dict, backend: str, reference_height: float) -> dict:
"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",
"x_axis": x_axis,
"y_axis": "left",
"z_axis": "up",
"yaw_enu_deg": "90 - rawHeadingDeg",
"yaw_enu_deg": f"90 - (rawHeadingDeg + {heading_offset_deg:g})",
"frame_mode": "baseline_raw_heading" if baseline_frame else "vehicle_forward_heading_offset",
},
"LiDAR": "raw LiDAR sensor frame",
},
"backend": backend,
"measured_lidar_extrinsic_used_as_initial": False,
"body_heading_offset_used": False,
"measured_lidar_extrinsic_used_as_initial": bool(raw.get("measured_extrinsic_used_as_initial")),
"solver_initial_extrinsic": raw.get("solver_initial_extrinsic"),
"body_heading_offset_deg": heading_offset_deg,
"body_heading_offset_used": abs(heading_offset_deg) > 1e-12,
"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"],
"coordinate_contract_audit": coordinate_contract_audit(raw),
"matrix_4x4": raw["matrix_4x4"],
"quality": {
"stations": raw["estimation"]["stations"],
@@ -80,6 +127,7 @@ def main() -> None:
parser = argparse.ArgumentParser()
parser.add_argument("--result-root", type=Path, required=True)
parser.add_argument("--reference-height", type=float, required=True)
parser.add_argument("--heading-offset-deg", type=float, required=True)
args = parser.parse_args()
def solver_output(directory: str) -> Path:
@@ -94,16 +142,26 @@ def main() -> None:
}
docs = {}
for backend, path in paths.items():
document = corrected(load(path), backend, args.reference_height)
document = corrected(
load(path), backend, args.reference_height, args.heading_offset_deg
)
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"])
needs_axis_confirmation = bool(
final["coordinate_contract_audit"]["requires_physical_axis_confirmation"]
)
final["selection"] = {
"recommended": True,
"reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
"recommended": not needs_axis_confirmation,
"reason": (
"Physical axis confirmation is required because the data-driven solution differs "
"from the declared mechanical initial by approximately 180 degrees"
if needs_axis_confirmation else
"Uses only motion pairs accepted independently by both Open3D GICP and small_gicp"
),
"open3d_vs_small_gicp": delta(open_t, small_t),
}
@@ -117,6 +175,8 @@ def main() -> None:
"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"],
"coordinate_contract_status": final["coordinate_contract_audit"]["status"],
"recommended_for_deployment": final["selection"]["recommended"],
},
"backend_difference": delta(open_t, small_t),
}
code/rigorous_calibration.py
+108 -8
View File
@@ -80,6 +80,20 @@ def params_transform(params):
return make_transform(params[:3], so3_exp(params[3:]))
def transform_params(transform):
from scipy.spatial.transform import Rotation
transform = np.asarray(transform, float)
return np.r_[transform[:3, 3], Rotation.from_matrix(transform[:3, :3]).as_rotvec()]
def load_extrinsic_matrix(path):
document = json.loads(Path(path).read_text(encoding="utf-8-sig"))
transform = np.asarray(document["matrix_4x4"], dtype=float)
if transform.shape != (4, 4):
raise ValueError("initial extrinsic matrix_4x4 must be 4x4")
return transform
def inverse_transform(transform):
answer = np.eye(4)
answer[:3, :3] = transform[:3, :3].T
@@ -135,7 +149,8 @@ def load_npz_xyz(path, min_range=1.0, max_range=50.0):
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
time_key = "lidar_association_time_ns" if "lidar_association_time_ns" in data else "unix_time_ns"
timestamp = float(np.ravel(data[time_key])[0]) / 1e9
counter = int(np.ravel(data["frame_counter"])[0])
distance = raw[:, 0] * 0.001
azimuth = np.deg2rad(raw[:, 1])
@@ -179,6 +194,63 @@ def make_o3d_cloud(points, voxel):
return cloud.voxel_down_sample(voxel)
def make_global_features(points, voxel):
import open3d as o3d
cloud = make_o3d_cloud(points, voxel)
cloud.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(
radius=voxel * 2.5, max_nn=50
))
features = o3d.pipelines.registration.compute_fpfh_feature(
cloud,
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel * 5.0, max_nn=100),
)
return cloud, features
def global_lidar_initialization(target_features, source_features, args, pair_seed):
"""Estimate source-to-target motion from LiDAR geometry without RTK or an extrinsic."""
import open3d as o3d
registration = o3d.pipelines.registration
target_cloud, target_fpfh = target_features
source_cloud, source_fpfh = source_features
attempts = []
for attempt in range(args.global_ransac_attempts):
o3d.utility.random.seed(int(pair_seed + attempt))
answer = registration.registration_ransac_based_on_feature_matching(
source_cloud,
target_cloud,
source_fpfh,
target_fpfh,
True,
args.global_correspondence,
registration.TransformationEstimationPointToPoint(False),
4,
[
registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
registration.CorrespondenceCheckerBasedOnDistance(args.global_correspondence),
],
registration.RANSACConvergenceCriteria(
args.global_ransac_iterations, args.global_ransac_confidence
),
)
attempts.append({
"transform": np.asarray(answer.transformation, float),
"fitness": float(answer.fitness),
"inlier_rmse_m": float(answer.inlier_rmse),
})
best = max(attempts, key=lambda item: (item["fitness"], -item["inlier_rmse_m"]))
return {
"transform": best["transform"],
"method": "LiDAR-only FPFH RANSAC",
"fitness": best["fitness"],
"inlier_rmse_m": best["inlier_rmse_m"],
"attempts": [
{key: value for key, value in item.items() if key != "transform"}
for item in attempts
],
}
def align_open3d(target, source, initial, voxels, correspondences, iterations):
import open3d as o3d
registration = o3d.pipelines.registration
@@ -381,6 +453,8 @@ def cmd_pairs(args):
reference_poses = np.asarray(reference_poses)
split = [split_holdout(station[3], args.holdout_fraction, i)
for i, station in enumerate(stations)]
global_features = [make_global_features(points[0], args.global_voxel)
for points in split]
rng = np.random.default_rng(args.seed)
accepted_a, accepted_b, accepted_meta, reports = [], [], [], []
accepted_transforms = {}
@@ -389,9 +463,15 @@ def cmd_pairs(args):
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 args.max_reference_translation is not None and translation > args.max_reference_translation:
continue
if translation < args.min_translation and rotation < args.min_rotation:
continue
initial_b = a_ij.copy() # X0=I; no measured extrinsic.
global_initial = global_lidar_initialization(
global_features[i], global_features[j], args,
args.seed + i * 1009 + j * 9176,
)
initial_b = global_initial["transform"]
target_fit, target_holdout = split[i]
source_fit, source_holdout = split[j]
forward = align_backend(args.backend, target_fit, source_fit, initial_b, args)
@@ -447,7 +527,10 @@ def cmd_pairs(args):
"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)",
"initial_B_source": global_initial["method"],
"global_lidar_initialization": {
key: value for key, value in global_initial.items() if key != "transform"
},
"B_ij_4x4": forward["transform"].tolist(),
"backend": args.backend, "backend_converged": forward["converged"],
"backend_iterations": forward["iterations"],
@@ -482,7 +565,11 @@ def cmd_pairs(args):
"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,
"registration_initial_extrinsic": None,
"selection_is_X_independent": True,
"B_estimation_is_RTK_independent": True,
"candidate_pair_selection_uses_reference_motion": True,
"initialization_warning": None,
"stations": len(stations), "candidate_pairs": len(reports),
"accepted_pairs": len(accepted_a),
"parameters": vars(args),
@@ -585,9 +672,11 @@ def pair_metrics(a_array, b_array, x):
def solve_extrinsic(a_array, b_array, planes, args):
rng = np.random.default_rng(args.seed)
starts = [np.zeros(6)]
center = (transform_params(load_extrinsic_matrix(args.initial_extrinsic))
if args.initial_extrinsic else np.zeros(6))
starts = [center]
for _ in range(args.solver_multistart - 1):
starts.append(np.r_[
starts.append(center + np.r_[
rng.normal(0.0, args.start_translation_sigma, 3),
np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)),
])
@@ -647,7 +736,10 @@ def cmd_calibrate(args):
"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,
"measured_extrinsic_used_as_initial": bool(args.initial_extrinsic),
"solver_initial_extrinsic": (
str(Path(args.initial_extrinsic).resolve()) if args.initial_extrinsic else None
),
"translation_m": x[:3, 3].tolist(),
"rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]),
"quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(),
@@ -707,7 +799,8 @@ def build_parser():
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)
# Default ROI for ~2 m roof LiDAR (Z-up). Override for other mounting heights.
ground.add_argument("--z-min", type=float, default=-2.5); ground.add_argument("--z-max", type=float, default=-1.5)
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)
@@ -720,10 +813,16 @@ def build_parser():
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("--max-reference-translation", type=float)
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("--global-voxel", type=float, default=0.50)
pairs.add_argument("--global-correspondence", type=float, default=1.25)
pairs.add_argument("--global-ransac-attempts", type=int, default=3)
pairs.add_argument("--global-ransac-iterations", type=int, default=100000)
pairs.add_argument("--global-ransac-confidence", type=float, default=0.999)
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])
@@ -744,6 +843,7 @@ def build_parser():
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("--initial-extrinsic")
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)