crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(calibration): add data-driven ground alignment with debug and fast iteration flags

Browse Source
This commit is contained in:
crosstyan
2026-02-07 03:20:16 +00:00
parent afc8e9034d
commit 446c02d42a
10 changed files with 1221 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/calibrate_extrinsics.py
+232 -33
View File
@@ -7,7 +7,11 @@ import pyzed.sl as sl
from pathlib import Path
from typing import List, Dict, Any, Optional, Tuple
from aruco.marker_geometry import load_marker_geometry, validate_marker_geometry
from aruco.marker_geometry import (
load_marker_geometry,
validate_marker_geometry,
load_face_mapping,
)
from aruco.svo_sync import SVOReader
from aruco.detector import (
create_detector,
@@ -20,6 +24,37 @@ from aruco.pose_averaging import PoseAccumulator
from aruco.preview import draw_detected_markers, draw_pose_axes, show_preview
from aruco.depth_verify import verify_extrinsics_with_depth
from aruco.depth_refine import refine_extrinsics_with_depth
from aruco.alignment import (
get_face_normal_from_geometry,
detect_ground_face,
rotation_align_vectors,
apply_alignment_to_pose,
)
from loguru import logger
ARUCO_DICT_MAP = {
"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
"DICT_4X4_100": cv2.aruco.DICT_4X4_100,
"DICT_4X4_250": cv2.aruco.DICT_4X4_250,
"DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
"DICT_5X5_50": cv2.aruco.DICT_5X5_50,
"DICT_5X5_100": cv2.aruco.DICT_5X5_100,
"DICT_5X5_250": cv2.aruco.DICT_5X5_250,
"DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
"DICT_6X6_50": cv2.aruco.DICT_6X6_50,
"DICT_6X6_100": cv2.aruco.DICT_6X6_100,
"DICT_6X6_250": cv2.aruco.DICT_6X6_250,
"DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
"DICT_7X7_50": cv2.aruco.DICT_7X7_50,
"DICT_7X7_100": cv2.aruco.DICT_7X7_100,
"DICT_7X7_250": cv2.aruco.DICT_7X7_250,
"DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
"DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL,
"DICT_APRILTAG_16h5": cv2.aruco.DICT_APRILTAG_16h5,
"DICT_APRILTAG_25h9": cv2.aruco.DICT_APRILTAG_25h9,
"DICT_APRILTAG_36h10": cv2.aruco.DICT_APRILTAG_36h10,
"DICT_APRILTAG_36h11": cv2.aruco.DICT_APRILTAG_36h11,
}
def apply_depth_verify_refine_postprocess(
@@ -121,9 +156,9 @@ def apply_depth_verify_refine_postprocess(
}
improvement = verify_res.rmse - verify_res_post.rmse
results[str(serial)]["refine_depth"]["improvement_rmse"] = (
improvement
)
results[str(serial)]["refine_depth"][
"improvement_rmse"
] = improvement
click.echo(
f"Camera {serial} refined: RMSE={verify_res_post.rmse:.3f}m "
@@ -190,24 +225,73 @@ def apply_depth_verify_refine_postprocess(
@click.option(
"--report-csv", type=click.Path(), help="Optional path for per-frame CSV report."
)
@click.option(
"--auto-align/--no-auto-align",
default=False,
help="Automatically align ground plane.",
)
@click.option(
"--ground-face", type=str, help="Explicit face name for ground alignment."
)
@click.option(
"--ground-marker-id", type=int, help="Explicit marker ID to define ground face."
)
@click.option(
"--aruco-dictionary",
default="DICT_4X4_50",
type=click.Choice(list(ARUCO_DICT_MAP.keys())),
help="ArUco dictionary to use.",
)
@click.option(
"--min-markers",
default=1,
type=int,
help="Minimum markers required for pose estimation.",
)
@click.option(
"--debug/--no-debug",
default=False,
help="Enable verbose debug logging.",
)
@click.option(
"--max-samples",
default=None,
type=int,
help="Maximum number of samples to process before stopping.",
)
def main(
svo,
markers,
output,
sample_interval,
max_reproj_error,
preview,
validate_markers,
self_check,
verify_depth,
refine_depth,
depth_mode,
depth_confidence_threshold,
report_csv,
svo: tuple[str, ...],
markers: str,
output: str,
sample_interval: int,
max_reproj_error: float,
preview: bool,
validate_markers: bool,
self_check: bool,
verify_depth: bool,
refine_depth: bool,
depth_mode: str,
depth_confidence_threshold: int,
report_csv: str | None,
auto_align: bool,
ground_face: str | None,
ground_marker_id: int | None,
aruco_dictionary: str,
min_markers: int,
debug: bool,
max_samples: int | None,
):
"""
Calibrate camera extrinsics relative to a global coordinate system defined by ArUco markers.
"""
# Configure logging level
logger.remove()
logger.add(
lambda msg: click.echo(msg, nl=False),
level="DEBUG" if debug else "INFO",
format="{message}",
)
depth_mode_map = {
"NEURAL": sl.DEPTH_MODE.NEURAL,
"ULTRA": sl.DEPTH_MODE.ULTRA,
@@ -219,17 +303,30 @@ def main(
if not (verify_depth or refine_depth):
sl_depth_mode = sl.DEPTH_MODE.NONE
# 1. Load Marker Geometry
try:
marker_geometry = load_marker_geometry(markers)
if validate_markers:
validate_marker_geometry(marker_geometry)
click.echo(f"Loaded {len(marker_geometry)} markers from {markers}")
except Exception as e:
click.echo(f"Error loading markers: {e}", err=True)
raise SystemExit(1)
# Expand SVO paths (files or directories)
expanded_svo = []
for path_str in svo:
path = Path(path_str)
if path.is_dir():
click.echo(f"Searching for SVO files in {path}...")
found = sorted(
[
str(p)
for p in path.iterdir()
if p.is_file() and p.suffix.lower() in (".svo", ".svo2")
]
)
if found:
click.echo(f"Found {len(found)} SVO files in {path}")
expanded_svo.extend(found)
else:
click.echo(f"Warning: No .svo/.svo2 files found in {path}", err=True)
elif path.is_file():
expanded_svo.append(str(path))
else:
click.echo(f"Warning: Path not found: {path}", err=True)
if not svo:
if not expanded_svo:
if validate_markers:
click.echo("Marker validation successful. No SVOs provided, exiting.")
return
@@ -239,8 +336,27 @@ def main(
)
raise click.UsageError("Missing option '--svo' / '-s'.")
# 1. Load Marker Geometry
try:
marker_geometry = load_marker_geometry(markers)
if validate_markers:
validate_marker_geometry(marker_geometry)
click.echo(f"Loaded {len(marker_geometry)} markers from {markers}")
# Load face mapping if available
face_marker_map = load_face_mapping(markers)
if face_marker_map:
click.echo(f"Loaded face mapping for {len(face_marker_map)} faces.")
else:
click.echo("No face mapping found in parquet (missing 'name'/'ids').")
face_marker_map = None
except Exception as e:
click.echo(f"Error loading markers: {e}", err=True)
raise SystemExit(1)
# 2. Initialize SVO Reader
reader = SVOReader(svo, depth_mode=sl_depth_mode)
reader = SVOReader(expanded_svo, depth_mode=sl_depth_mode)
if not reader.cameras:
click.echo("No SVO files could be opened.", err=True)
return
@@ -278,7 +394,10 @@ def main(
# Store verification frames for post-process check
verification_frames = {}
detector = create_detector()
# Track all visible marker IDs for heuristic ground detection
all_visible_ids = set()
detector = create_detector(dictionary_id=ARUCO_DICT_MAP[aruco_dictionary])
frame_count = 0
sampled_count = 0
@@ -303,6 +422,14 @@ def main(
# Detect markers
corners, ids = detect_markers(frame.image, detector)
if ids is not None:
all_visible_ids.update(ids.flatten().tolist())
logger.debug(
f"Cam {serial}: Detected {len(ids)} markers: {ids.flatten()}"
)
else:
logger.debug(f"Cam {serial}: No markers detected")
if ids is None:
if preview:
preview_frames[serial] = frame.image
@@ -310,7 +437,7 @@ def main(
# Estimate pose (T_cam_from_world)
pose_res = estimate_pose_from_detections(
corners, ids, marker_geometry, K, min_markers=4
corners, ids, marker_geometry, K, min_markers=min_markers
)
if pose_res:
@@ -333,6 +460,14 @@ def main(
accumulators[serial].add_pose(
T_world_cam, reproj_err, frame_count
)
logger.debug(
f"Cam {serial}: Pose accepted. Reproj={reproj_err:.3f}, Markers={n_markers}"
)
else:
logger.debug(
f"Cam {serial}: Pose rejected. Reproj {reproj_err:.3f} > {max_reproj_error}"
)
if preview:
img = draw_detected_markers(
@@ -340,8 +475,13 @@ def main(
)
img = draw_pose_axes(img, rvec, tvec, K, length=0.2)
preview_frames[serial] = img
elif preview:
preview_frames[serial] = frame.image
else:
if ids is not None:
logger.debug(
f"Cam {serial}: Pose estimation failed (insufficient markers < {min_markers} or solver failure)"
)
elif preview:
preview_frames[serial] = frame.image
if preview and preview_frames:
key = show_preview(preview_frames)
@@ -349,12 +489,15 @@ def main(
break
sampled_count += 1
if max_samples is not None and sampled_count >= max_samples:
click.echo(f"\nReached max samples ({max_samples}). Stopping.")
break
frame_count += 1
if frame_count % 100 == 0:
counts = [len(acc.poses) for acc in accumulators.values()]
click.echo(
f"Frame {frame_count}, Detections: {dict(zip(serials, counts))}"
f"Frame {frame_count}, Accepted Poses: {dict(zip(serials, counts))}"
)
except KeyboardInterrupt:
@@ -398,6 +541,62 @@ def main(
report_csv,
)
# 5. Optional Ground Plane Alignment
if auto_align:
click.echo("\nPerforming ground plane alignment...")
target_face = ground_face
# Use loaded map or skip if None
if face_marker_map is None:
click.echo(
"Warning: No face mapping available (missing 'name'/'ids' in parquet). Skipping alignment.",
err=True,
)
# Skip alignment logic by ensuring loop below doesn't run and heuristic fails gracefully
mapping_to_use = {}
else:
mapping_to_use = face_marker_map
if not target_face and ground_marker_id is not None:
# Map marker ID to face
for face, ids in mapping_to_use.items():
if ground_marker_id in ids:
target_face = face
logger.info(
f"Mapped ground-marker-id {ground_marker_id} to face '{face}'"
)
break
ground_normal = None
if target_face:
ground_normal = get_face_normal_from_geometry(
target_face, marker_geometry, face_marker_map=face_marker_map
)
if ground_normal is not None:
logger.info(f"Using explicit ground face '{target_face}'")
else:
# Heuristic detection
heuristic_res = detect_ground_face(
all_visible_ids, marker_geometry, face_marker_map=face_marker_map
)
if heuristic_res:
target_face, ground_normal = heuristic_res
logger.info(f"Heuristically detected ground face '{target_face}'")
if ground_normal is not None:
R_align = rotation_align_vectors(ground_normal, np.array([0, 1, 0]))
logger.info(f"Computed alignment rotation for face '{target_face}'")
for serial, data in results.items():
T_mean = np.fromstring(data["pose"], sep=" ").reshape(4, 4)
T_aligned = apply_alignment_to_pose(T_mean, R_align)
data["pose"] = " ".join(f"{x:.6f}" for x in T_aligned.flatten())
logger.debug(f"Applied alignment to camera {serial}")
else:
click.echo(
"Warning: Could not determine ground normal. Skipping alignment."
)
# 6. Save to JSON
with open(output, "w") as f:
json.dump(results, f, indent=4, sort_keys=True)