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