feat(cli): add depth verify/refine outputs and tests
- Retrieve depth + confidence measures from SVOReader when depth enabled - Compute depth residual metrics and attach to output JSON - Optionally write per-corner residual CSV via --report-csv - Post-process refinement: optimize final pose and report pre/post metrics - Add unit tests for depth verification and refinement modules - Add basedpyright dev dependency for diagnostics
This commit is contained in:
@@ -4,7 +4,7 @@ import json
|
||||
import numpy as np
|
||||
import pyzed.sl as sl
|
||||
from pathlib import Path
|
||||
from typing import List, Dict, Any, Optional
|
||||
from typing import List, Dict, Any, Optional, Tuple
|
||||
|
||||
from aruco.marker_geometry import load_marker_geometry, validate_marker_geometry
|
||||
from aruco.svo_sync import SVOReader
|
||||
@@ -145,6 +145,9 @@ def main(
|
||||
for serial, cam in zip(serials, reader.cameras)
|
||||
}
|
||||
|
||||
# Store verification frames for post-process check
|
||||
verification_frames = {}
|
||||
|
||||
detector = create_detector()
|
||||
|
||||
frame_count = 0
|
||||
@@ -187,42 +190,20 @@ def main(
|
||||
# We want T_world_from_cam
|
||||
T_world_cam = invert_transform(T_cam_world)
|
||||
|
||||
if refine_depth and frame.depth_map is not None:
|
||||
marker_corners_world = {
|
||||
int(mid): marker_geometry[int(mid)]
|
||||
for mid in ids.flatten()
|
||||
if int(mid) in marker_geometry
|
||||
# Save latest valid frame for verification
|
||||
if (
|
||||
verify_depth or refine_depth
|
||||
) and frame.depth_map is not None:
|
||||
verification_frames[serial] = {
|
||||
"frame": frame,
|
||||
"ids": ids,
|
||||
"corners": corners,
|
||||
}
|
||||
if marker_corners_world:
|
||||
T_world_cam_refined, refine_stats = (
|
||||
refine_extrinsics_with_depth(
|
||||
T_world_cam,
|
||||
marker_corners_world,
|
||||
frame.depth_map,
|
||||
K,
|
||||
)
|
||||
)
|
||||
T_world_cam = T_world_cam_refined
|
||||
|
||||
accumulators[serial].add_pose(
|
||||
T_world_cam, reproj_err, frame_count
|
||||
)
|
||||
|
||||
if verify_depth and frame.depth_map is not None:
|
||||
marker_corners_world = {
|
||||
int(mid): marker_geometry[int(mid)]
|
||||
for mid in ids.flatten()
|
||||
if int(mid) in marker_geometry
|
||||
}
|
||||
if marker_corners_world:
|
||||
verify_extrinsics_with_depth(
|
||||
T_world_cam,
|
||||
marker_corners_world,
|
||||
frame.depth_map,
|
||||
K,
|
||||
confidence_thresh=depth_confidence_threshold,
|
||||
)
|
||||
|
||||
if preview:
|
||||
img = draw_detected_markers(
|
||||
frame.image.copy(), corners, ids
|
||||
@@ -275,12 +256,127 @@ def main(
|
||||
click.echo("No extrinsics computed.", err=True)
|
||||
return
|
||||
|
||||
# 4. Save to JSON
|
||||
# 4. Run Depth Verification if requested
|
||||
csv_rows: List[List[Any]] = []
|
||||
if verify_depth or refine_depth:
|
||||
click.echo("\nRunning depth verification/refinement on computed extrinsics...")
|
||||
for serial, acc in accumulators.items():
|
||||
if serial not in verification_frames or str(serial) not in results:
|
||||
continue
|
||||
|
||||
# Retrieve stored frame data
|
||||
vf = verification_frames[serial]
|
||||
frame = vf["frame"]
|
||||
ids = vf["ids"]
|
||||
|
||||
# Use the FINAL COMPUTED POSE for verification
|
||||
pose_str = results[str(serial)]["pose"]
|
||||
T_mean = np.fromstring(pose_str, sep=" ").reshape(4, 4)
|
||||
cam_matrix = camera_matrices[serial]
|
||||
|
||||
marker_corners_world = {
|
||||
int(mid): marker_geometry[int(mid)]
|
||||
for mid in ids.flatten()
|
||||
if int(mid) in marker_geometry
|
||||
}
|
||||
|
||||
if marker_corners_world and frame.depth_map is not None:
|
||||
verify_res = verify_extrinsics_with_depth(
|
||||
T_mean,
|
||||
marker_corners_world,
|
||||
frame.depth_map,
|
||||
cam_matrix,
|
||||
confidence_map=frame.confidence_map,
|
||||
confidence_thresh=depth_confidence_threshold,
|
||||
)
|
||||
|
||||
results[str(serial)]["depth_verify"] = {
|
||||
"rmse": verify_res.rmse,
|
||||
"mean_abs": verify_res.mean_abs,
|
||||
"median": verify_res.median,
|
||||
"depth_normalized_rmse": verify_res.depth_normalized_rmse,
|
||||
"n_valid": verify_res.n_valid,
|
||||
"n_total": verify_res.n_total,
|
||||
}
|
||||
|
||||
click.echo(
|
||||
f"Camera {serial} verification: RMSE={verify_res.rmse:.3f}m, "
|
||||
f"Valid={verify_res.n_valid}/{verify_res.n_total}"
|
||||
)
|
||||
|
||||
if refine_depth:
|
||||
if verify_res.n_valid < 4:
|
||||
click.echo(
|
||||
f"Camera {serial}: Not enough valid depth points for refinement ({verify_res.n_valid}). Skipping."
|
||||
)
|
||||
else:
|
||||
click.echo(
|
||||
f"Camera {serial}: Refining extrinsics with depth..."
|
||||
)
|
||||
T_refined, refine_stats = refine_extrinsics_with_depth(
|
||||
T_mean,
|
||||
marker_corners_world,
|
||||
frame.depth_map,
|
||||
cam_matrix,
|
||||
)
|
||||
|
||||
verify_res_post = verify_extrinsics_with_depth(
|
||||
T_refined,
|
||||
marker_corners_world,
|
||||
frame.depth_map,
|
||||
cam_matrix,
|
||||
confidence_map=frame.confidence_map,
|
||||
confidence_thresh=depth_confidence_threshold,
|
||||
)
|
||||
|
||||
pose_str_refined = " ".join(
|
||||
f"{x:.6f}" for x in T_refined.flatten()
|
||||
)
|
||||
results[str(serial)]["pose"] = pose_str_refined
|
||||
results[str(serial)]["refine_depth"] = refine_stats
|
||||
results[str(serial)]["depth_verify_post"] = {
|
||||
"rmse": verify_res_post.rmse,
|
||||
"mean_abs": verify_res_post.mean_abs,
|
||||
"median": verify_res_post.median,
|
||||
"depth_normalized_rmse": verify_res_post.depth_normalized_rmse,
|
||||
"n_valid": verify_res_post.n_valid,
|
||||
"n_total": verify_res_post.n_total,
|
||||
}
|
||||
|
||||
improvement = verify_res.rmse - verify_res_post.rmse
|
||||
results[str(serial)]["refine_depth"]["improvement_rmse"] = (
|
||||
improvement
|
||||
)
|
||||
|
||||
click.echo(
|
||||
f"Camera {serial} refined: RMSE={verify_res_post.rmse:.3f}m "
|
||||
f"(Improved by {improvement:.3f}m). "
|
||||
f"Delta Rot={refine_stats['delta_rotation_deg']:.2f}deg, "
|
||||
f"Trans={refine_stats['delta_translation_norm_m']:.3f}m"
|
||||
)
|
||||
|
||||
verify_res = verify_res_post
|
||||
|
||||
if report_csv:
|
||||
for mid, cidx, resid in verify_res.residuals:
|
||||
csv_rows.append([serial, mid, cidx, resid])
|
||||
|
||||
# 5. Save CSV Report
|
||||
if report_csv and csv_rows:
|
||||
import csv
|
||||
|
||||
with open(report_csv, "w", newline="") as f:
|
||||
writer = csv.writer(f)
|
||||
writer.writerow(["serial", "marker_id", "corner_idx", "residual"])
|
||||
writer.writerows(csv_rows)
|
||||
click.echo(f"Saved depth verification report to {report_csv}")
|
||||
|
||||
# 6. Save to JSON
|
||||
with open(output, "w") as f:
|
||||
json.dump(results, f, indent=4, sort_keys=True)
|
||||
click.echo(f"Saved extrinsics to {output}")
|
||||
|
||||
# 5. Optional Self-Check
|
||||
# 7. Optional Self-Check
|
||||
if self_check:
|
||||
# Verify reprojection error
|
||||
for serial, data in results.items():
|
||||
|
||||
Reference in New Issue
Block a user