File size: 4,995 Bytes
b9ff8f9 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 | """Recover distance from the box bottom row and score it against the truth.
This is the quantitative form of "do box bottoms sit at the ground contact
point". Looking at a contact sheet catches a box that is obviously wrong; it
does not catch a box that is twenty pixels low, which is a systematic depth bias
worth several metres at range.
It is also, deliberately, phase 2's ground-plane estimator run early: a pixel is
back-projected to a ray and intersected with the ground plane from
calib/<sensor_id>.yaml, using nothing but focal length, principal point, camera
height and the ground normal. Running it against gt_distance_m today says
whether the calibration handoff actually works before anything is built on it.
Stop signs are excluded by default: they are pole-mounted, so the bottom of the
box is not a ground contact and the estimator does not apply to them.
Usage:
python -m src.data.check_ground_plane
python -m src.data.check_ground_plane --min-visibility 0.7 --min-horizon-margin 20
"""
from __future__ import annotations
import argparse
from pathlib import Path
import numpy as np
import pandas as pd
from src.common import calib, paths, schema
BUCKETS = [(0, 10), (10, 20), (20, 30), (30, 45), (45, 1000)]
def horizon_row(calibration: calib.Calibration) -> float:
"""The image row whose back-projected ray runs parallel to the ground.
Distance estimates diverge as a box bottom approaches this row, so it is the
natural place to put a validity margin.
"""
normal = calibration.ground_normal_cam()
# Walk down the centre column until the ray turns parallel to the ground.
rows = np.linspace(0.0, float(calibration.height), 4001)
columns = np.full_like(rows, calibration.cx)
alignment = normal @ calibration.ray(columns, rows)
crossings = np.flatnonzero(np.sign(alignment[:-1]) != np.sign(alignment[1:]))
return float(rows[crossings[0]]) if len(crossings) else float(calibration.cy)
def estimate(calibration: calib.Calibration, u: np.ndarray, v: np.ndarray) -> np.ndarray:
"""Ray-plane intersection. Returns forward depth, matching gt_distance_m."""
normal = calibration.ground_normal_cam()
rays = calibration.ray(u, v)
scale = -calibration.cam_height_m / (normal @ rays)
return scale * rays[2]
def report(frame: pd.DataFrame, calibration: calib.Calibration,
min_visibility: float, min_margin: float) -> None:
objects = frame[frame["gt_distance_m"].notna() & (frame["truncation"] < 0.01)]
objects = objects[objects["class"].isin(schema.GROUND_CONTACT_CLASSES)]
if objects.empty:
print(" no ground-contact objects with distance")
return
u = ((objects["x1"] + objects["x2"]) / 2).to_numpy()
v = objects["y2"].to_numpy()
truth = objects["gt_distance_m"].to_numpy()
predicted = estimate(calibration, u, v)
margin = v - horizon_row(calibration)
visibility = objects["visibility"].to_numpy()
usable = (
np.isfinite(predicted) & (predicted > 0) & (margin >= min_margin)
& (np.isnan(visibility) | (visibility >= min_visibility))
)
print(f" horizon row {horizon_row(calibration):.1f} px, "
f"{usable.sum()}/{len(objects)} objects pass the validity gate")
print(f" {'range':>10} {'n':>5} {'m/px':>6} {'MAE':>8} {'bias':>8}")
for low, high in BUCKETS:
in_bucket = usable & (truth >= low) & (truth < high)
if in_bucket.sum() < 5:
continue
error = predicted[in_bucket] - truth[in_bucket]
sensitivity = np.median(truth[in_bucket] ** 2
/ (calibration.fy * calibration.cam_height_m))
label = f"{low}-{high} m" if high < 1000 else f"{low}+ m"
print(f" {label:>10} {in_bucket.sum():5d} {sensitivity:6.2f} "
f"{np.abs(error).mean():7.2f}m {np.median(error):+7.2f}m")
def main() -> None:
parser = argparse.ArgumentParser(description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument("--unified-root", type=Path, default=None)
parser.add_argument("--min-visibility", type=float, default=0.0)
parser.add_argument("--min-horizon-margin", type=float, default=0.0,
help="pixels a box bottom must sit below the horizon row")
args = parser.parse_args()
root = paths.unified_root(args.unified_root)
manifest = schema.objects_only(schema.read_manifest(root / "manifest.parquet"))
calibrations = calib.load_all(root)
for sensor_id, calibration in sorted(calibrations.items()):
rows = manifest[manifest["sensor_id"] == sensor_id]
print(f"\n{sensor_id} (cam_height {calibration.cam_height_m:.3f} m, "
f"fy {calibration.fy:.0f})")
if rows.empty:
print(" no objects")
continue
report(rows, calibration, args.min_visibility, args.min_horizon_margin)
if __name__ == "__main__":
main()
|