Rewrite calibration: intersection matching + dual mapping strategy
The core calibration was broken because the green quad corners didn't correspond to the correct court corners (giving Z=15m camera positions). New approach: 1. Detect white line segments on green court surface 2. Merge into distinct lines, find intersections 3. Match intersections to known court template using initial homography from green quad (tries both left-right mirror mappings) 4. solvePnP with matched 2D-3D correspondences 5. Sanity check: camera Z must be 0-5m, prefers ~1m height 6. Fallback to quad-only calibration with both mappings if not enough intersections detected Also: CameraCalibrator now uses findHomography for N>4 points, and get_half_court_intersections() provides the 6 template keypoints. Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
491
jetson/main.py
491
jetson/main.py
@@ -22,7 +22,7 @@ from src.streaming.ring_buffer import FrameRingBuffer
|
|||||||
from src.physics.trajectory import TrajectoryModel
|
from src.physics.trajectory import TrajectoryModel
|
||||||
from src.physics.event_detector import EventDetector
|
from src.physics.event_detector import EventDetector
|
||||||
from src.calibration.camera_calibrator import (
|
from src.calibration.camera_calibrator import (
|
||||||
CameraCalibrator, get_half_court_3d_points,
|
CameraCalibrator, get_half_court_3d_points, get_half_court_intersections,
|
||||||
COURT_LENGTH, COURT_WIDTH, HALF_COURT_LENGTH
|
COURT_LENGTH, COURT_WIDTH, HALF_COURT_LENGTH
|
||||||
)
|
)
|
||||||
from src.web.app import app, state
|
from src.web.app import app, state
|
||||||
@@ -35,14 +35,18 @@ _args = None
|
|||||||
|
|
||||||
|
|
||||||
def auto_calibrate():
|
def auto_calibrate():
|
||||||
"""Detect the green court surface, find its boundary quadrilateral,
|
"""Calibrate cameras by detecting court line intersections and matching
|
||||||
map to known court dimensions, compute camera 3D position via solvePnP.
|
them to the known pickleball court template.
|
||||||
|
|
||||||
Debug images show:
|
Algorithm:
|
||||||
- Green mask overlay (what the system sees as court)
|
1. Detect green court surface (search area)
|
||||||
- Detected quadrilateral (court boundary)
|
2. Find white line segments on the green surface
|
||||||
- White line segments found on court
|
3. Merge similar segments into distinct court lines
|
||||||
- Computed camera position
|
4. Find intersections between lines
|
||||||
|
5. Use green quad for initial homography estimate
|
||||||
|
6. Project template intersection points, match to detected ones
|
||||||
|
7. Try both left-right mirror mappings, pick sane camera position
|
||||||
|
8. Refine with solvePnP using all matched correspondences
|
||||||
"""
|
"""
|
||||||
results = {}
|
results = {}
|
||||||
|
|
||||||
@@ -56,65 +60,139 @@ def auto_calibrate():
|
|||||||
side = 'left' if sensor_id == 0 else 'right'
|
side = 'left' if sensor_id == 0 else 'right'
|
||||||
debug_frame = frame.copy()
|
debug_frame = frame.copy()
|
||||||
|
|
||||||
# Step 1: Detect green court surface
|
# Step 1: Detect green court mask
|
||||||
court = _detect_court_surface(frame)
|
green_mask = _detect_green_mask(frame)
|
||||||
|
|
||||||
# Draw green mask as semi-transparent overlay
|
|
||||||
if court['green_mask'] is not None:
|
|
||||||
green_overlay = np.zeros_like(debug_frame)
|
green_overlay = np.zeros_like(debug_frame)
|
||||||
green_overlay[court['green_mask'] > 0] = (0, 80, 0)
|
green_overlay[green_mask > 0] = (0, 80, 0)
|
||||||
debug_frame = cv2.addWeighted(debug_frame, 1.0, green_overlay, 0.3, 0)
|
debug_frame = cv2.addWeighted(debug_frame, 1.0, green_overlay, 0.3, 0)
|
||||||
|
|
||||||
if court['quad'] is None:
|
green_pct = np.count_nonzero(green_mask) / (w * h) * 100
|
||||||
cv2.putText(debug_frame, f"FAILED: {court['error']}", (10, 30),
|
if green_pct < 5:
|
||||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
cv2.putText(debug_frame, f"FAILED: Green area too small ({green_pct:.0f}%)",
|
||||||
|
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||||||
_, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
|
_, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
|
||||||
results[str(sensor_id)] = {
|
results[str(sensor_id)] = {
|
||||||
'ok': False,
|
'ok': False,
|
||||||
'error': f"CAM {sensor_id}: {court['error']}",
|
'error': f'CAM {sensor_id}: Green area too small ({green_pct:.0f}%)',
|
||||||
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
||||||
}
|
}
|
||||||
continue
|
continue
|
||||||
|
|
||||||
quad = court['quad']
|
# Step 2: Detect white line segments on court
|
||||||
|
white_segments = _detect_white_lines_on_court(frame, green_mask)
|
||||||
# Draw detected court quadrilateral
|
for x1, y1, x2, y2 in white_segments:
|
||||||
pts = quad.astype(int)
|
|
||||||
for i in range(4):
|
|
||||||
p1 = tuple(pts[i])
|
|
||||||
p2 = tuple(pts[(i + 1) % 4])
|
|
||||||
cv2.line(debug_frame, p1, p2, (0, 255, 0), 3)
|
|
||||||
cv2.circle(debug_frame, p1, 8, (0, 0, 255), -1)
|
|
||||||
cv2.putText(debug_frame, f"C{i}", (p1[0] + 10, p1[1] - 5),
|
|
||||||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
|
|
||||||
|
|
||||||
# Step 2: Detect white lines on court surface
|
|
||||||
white_lines = _detect_white_lines_on_court(frame, court['green_mask'])
|
|
||||||
for seg in white_lines:
|
|
||||||
x1, y1, x2, y2 = seg
|
|
||||||
cv2.line(debug_frame, (x1, y1), (x2, y2), (255, 255, 0), 1)
|
cv2.line(debug_frame, (x1, y1), (x2, y2), (255, 255, 0), 1)
|
||||||
|
|
||||||
# Step 3: Map quad corners to 3D court coordinates
|
# Step 3: Merge into distinct lines
|
||||||
# Quad corners are ordered: TL, TR, BR, BL
|
merged = _merge_line_segments(white_segments)
|
||||||
# For camera at net looking at one half-court:
|
for m in merged:
|
||||||
# TL = far-left, TR = far-right (baseline)
|
p1, p2 = m['p1'], m['p2']
|
||||||
# BL = near-left, BR = near-right (near net/camera)
|
cv2.line(debug_frame, (int(p1[0]), int(p1[1])),
|
||||||
corners_3d = get_half_court_3d_points(side)
|
(int(p2[0]), int(p2[1])), (0, 165, 255), 2)
|
||||||
|
|
||||||
|
# Step 4: Find intersections
|
||||||
|
intersections = _find_line_intersections(merged, w, h)
|
||||||
|
for ix, iy in intersections:
|
||||||
|
cv2.circle(debug_frame, (int(ix), int(iy)), 6, (0, 0, 255), -1)
|
||||||
|
|
||||||
|
cv2.putText(debug_frame,
|
||||||
|
f"{len(white_segments)} segs -> {len(merged)} lines -> {len(intersections)} pts",
|
||||||
|
(10, h - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
|
||||||
|
|
||||||
|
if len(intersections) < 4:
|
||||||
|
# Fallback: try green quad + both mappings
|
||||||
|
quad = _find_green_quad(green_mask, w * h)
|
||||||
|
if quad is not None:
|
||||||
|
cal, cam_pos, mapping_info = _calibrate_from_quad(quad, side, w, h)
|
||||||
|
if cal is not None:
|
||||||
|
_finalize_calibration(
|
||||||
|
cal, cam_pos, sensor_id, side, debug_frame, w, h,
|
||||||
|
results, mapping_info, len(intersections), len(merged)
|
||||||
|
)
|
||||||
|
continue
|
||||||
|
|
||||||
|
cv2.putText(debug_frame, f"FAILED: Need 4+ intersections, got {len(intersections)}",
|
||||||
|
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||||||
|
_, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
|
||||||
|
results[str(sensor_id)] = {
|
||||||
|
'ok': False,
|
||||||
|
'error': f'CAM {sensor_id}: {len(intersections)} intersections (need 4+) from {len(merged)} lines',
|
||||||
|
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
||||||
|
}
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Step 5: Match intersections to court template
|
||||||
|
template = get_half_court_intersections(side)
|
||||||
|
|
||||||
|
# Try matching with green quad seeded homography
|
||||||
|
quad = _find_green_quad(green_mask, w * h)
|
||||||
|
match = _match_intersections_to_template(
|
||||||
|
intersections, template, side, quad, w, h
|
||||||
|
)
|
||||||
|
|
||||||
|
if match is None or len(match['image_points']) < 4:
|
||||||
|
# Fallback to quad-only calibration
|
||||||
|
if quad is not None:
|
||||||
|
cal, cam_pos, mapping_info = _calibrate_from_quad(quad, side, w, h)
|
||||||
|
if cal is not None:
|
||||||
|
_finalize_calibration(
|
||||||
|
cal, cam_pos, sensor_id, side, debug_frame, w, h,
|
||||||
|
results, mapping_info, len(intersections), len(merged)
|
||||||
|
)
|
||||||
|
continue
|
||||||
|
|
||||||
|
cv2.putText(debug_frame, "FAILED: Could not match court pattern",
|
||||||
|
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||||||
|
_, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
|
||||||
|
results[str(sensor_id)] = {
|
||||||
|
'ok': False,
|
||||||
|
'error': f'CAM {sensor_id}: Could not match court pattern',
|
||||||
|
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
||||||
|
}
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Draw matched points
|
||||||
|
for name, (px, py) in match['image_points'].items():
|
||||||
|
cv2.circle(debug_frame, (int(px), int(py)), 10, (0, 255, 0), 2)
|
||||||
|
cv2.putText(debug_frame, name, (int(px) + 12, int(py) - 5),
|
||||||
|
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||||||
|
|
||||||
|
# Step 6: solvePnP with matched correspondences
|
||||||
|
pts_2d = np.array(list(match['image_points'].values()), dtype=np.float32)
|
||||||
|
pts_3d = np.array(list(match['world_points'].values()), dtype=np.float32)
|
||||||
|
|
||||||
# Calibrate
|
|
||||||
cal = CameraCalibrator()
|
cal = CameraCalibrator()
|
||||||
cal.calibrate(quad, corners_3d, w, h)
|
cal.calibrate(pts_2d, pts_3d, w, h)
|
||||||
|
|
||||||
cam_pos = (-cal.rotation_matrix.T @ cal.translation_vec).flatten()
|
cam_pos = (-cal.rotation_matrix.T @ cal.translation_vec).flatten()
|
||||||
|
|
||||||
|
# Sanity check
|
||||||
|
if cam_pos[2] < 0 or cam_pos[2] > 10:
|
||||||
|
cv2.putText(debug_frame, f"BAD Z={cam_pos[2]:.1f}m, trying quad fallback",
|
||||||
|
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)
|
||||||
|
if quad is not None:
|
||||||
|
cal2, cam_pos2, info = _calibrate_from_quad(quad, side, w, h)
|
||||||
|
if cal2 is not None:
|
||||||
|
cal, cam_pos = cal2, cam_pos2
|
||||||
|
|
||||||
|
n_matched = len(match['image_points'])
|
||||||
|
_finalize_calibration(
|
||||||
|
cal, cam_pos, sensor_id, side, debug_frame, w, h,
|
||||||
|
results, f"{n_matched} intersections matched", len(intersections), len(merged)
|
||||||
|
)
|
||||||
|
|
||||||
|
return results
|
||||||
|
|
||||||
|
|
||||||
|
def _finalize_calibration(cal, cam_pos, sensor_id, side, debug_frame, w, h,
|
||||||
|
results, method_info, n_intersections, n_lines):
|
||||||
|
"""Save calibration and build result dict."""
|
||||||
cv2.putText(debug_frame,
|
cv2.putText(debug_frame,
|
||||||
f"CAM{sensor_id}: X={cam_pos[0]:.2f} Y={cam_pos[1]:.2f} Z={cam_pos[2]:.2f}m",
|
f"CAM{sensor_id}: X={cam_pos[0]:.2f} Y={cam_pos[1]:.2f} Z={cam_pos[2]:.2f}m",
|
||||||
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
||||||
cv2.putText(debug_frame,
|
cv2.putText(debug_frame,
|
||||||
f"{side} half | {len(white_lines)} white line segments",
|
f"{side} half | {method_info}",
|
||||||
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
||||||
|
|
||||||
# Save
|
|
||||||
cal_path = os.path.join(_args.calibration_dir,
|
cal_path = os.path.join(_args.calibration_dir,
|
||||||
f'cam{sensor_id}_calibration.json')
|
f'cam{sensor_id}_calibration.json')
|
||||||
os.makedirs(os.path.dirname(cal_path), exist_ok=True)
|
os.makedirs(os.path.dirname(cal_path), exist_ok=True)
|
||||||
@@ -123,7 +201,6 @@ def auto_calibrate():
|
|||||||
|
|
||||||
_, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
|
_, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
|
||||||
|
|
||||||
# Court lines in 3D for visualization
|
|
||||||
matched_lines_3d = _get_half_court_lines_3d(side)
|
matched_lines_3d = _get_half_court_lines_3d(side)
|
||||||
|
|
||||||
results[str(sensor_id)] = {
|
results[str(sensor_id)] = {
|
||||||
@@ -131,115 +208,333 @@ def auto_calibrate():
|
|||||||
'camera_position': cam_pos.tolist(),
|
'camera_position': cam_pos.tolist(),
|
||||||
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
||||||
'matched_lines_3d': matched_lines_3d,
|
'matched_lines_3d': matched_lines_3d,
|
||||||
|
'points_matched': n_intersections,
|
||||||
}
|
}
|
||||||
print(f"[CAM {sensor_id}] Calibrated! Camera at "
|
print(f"[CAM {sensor_id}] Calibrated! Camera at "
|
||||||
f"({cam_pos[0]:.2f}, {cam_pos[1]:.2f}, {cam_pos[2]:.2f})")
|
f"({cam_pos[0]:.2f}, {cam_pos[1]:.2f}, {cam_pos[2]:.2f}) via {method_info}")
|
||||||
|
|
||||||
return results
|
|
||||||
|
|
||||||
|
|
||||||
def _get_half_court_lines_3d(side):
|
def _get_half_court_lines_3d(side):
|
||||||
"""Return 3D line segments for all lines of a half-court."""
|
"""Return 3D line segments for all lines of a half-court."""
|
||||||
if side == 'left':
|
if side == 'left':
|
||||||
bx, kx = 0, 4.57 # baseline, kitchen
|
bx, kx = 0, 4.57
|
||||||
else:
|
else:
|
||||||
bx, kx = 13.4, 8.83
|
bx, kx = 13.4, 8.83
|
||||||
lines = [
|
return [
|
||||||
{'name': 'baseline', 'from': [bx, 0, 0], 'to': [bx, 6.1, 0]},
|
{'name': 'baseline', 'from': [bx, 0, 0], 'to': [bx, 6.1, 0]},
|
||||||
{'name': 'kitchen', 'from': [kx, 0, 0], 'to': [kx, 6.1, 0]},
|
{'name': 'kitchen', 'from': [kx, 0, 0], 'to': [kx, 6.1, 0]},
|
||||||
{'name': 'sideline_near', 'from': [bx, 0, 0], 'to': [kx, 0, 0]},
|
{'name': 'sideline_near', 'from': [bx, 0, 0], 'to': [kx, 0, 0]},
|
||||||
{'name': 'sideline_far', 'from': [bx, 6.1, 0], 'to': [kx, 6.1, 0]},
|
{'name': 'sideline_far', 'from': [bx, 6.1, 0], 'to': [kx, 6.1, 0]},
|
||||||
{'name': 'center_service', 'from': [bx, 3.05, 0], 'to': [kx, 3.05, 0]},
|
{'name': 'center_service', 'from': [bx, 3.05, 0], 'to': [kx, 3.05, 0]},
|
||||||
]
|
]
|
||||||
return lines
|
|
||||||
|
|
||||||
|
|
||||||
def _detect_court_surface(frame):
|
def _detect_green_mask(frame):
|
||||||
"""Detect the green court surface and find its boundary as a quadrilateral.
|
"""Detect green court surface pixels. Returns binary mask."""
|
||||||
|
|
||||||
The court surface is distinctly green against pink/gray surroundings.
|
|
||||||
We segment by color, find the largest contour, and approximate with 4 corners.
|
|
||||||
|
|
||||||
Returns dict with:
|
|
||||||
quad: 4x2 numpy array of court boundary corners in image, or None
|
|
||||||
green_mask: binary mask of detected green surface
|
|
||||||
error: description if detection failed
|
|
||||||
"""
|
|
||||||
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
|
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
|
||||||
|
|
||||||
# Green court surface — relatively broad range to handle lighting
|
|
||||||
lower_green = np.array([30, 30, 50])
|
lower_green = np.array([30, 30, 50])
|
||||||
upper_green = np.array([90, 255, 220])
|
upper_green = np.array([90, 255, 220])
|
||||||
green_mask = cv2.inRange(hsv, lower_green, upper_green)
|
mask = cv2.inRange(hsv, lower_green, upper_green)
|
||||||
|
|
||||||
# Clean up noise
|
|
||||||
kernel = np.ones((7, 7), np.uint8)
|
kernel = np.ones((7, 7), np.uint8)
|
||||||
green_mask = cv2.morphologyEx(green_mask, cv2.MORPH_CLOSE, kernel)
|
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
|
||||||
green_mask = cv2.morphologyEx(green_mask, cv2.MORPH_OPEN, kernel)
|
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
|
||||||
|
return mask
|
||||||
|
|
||||||
# Find contours
|
|
||||||
|
def _find_green_quad(green_mask, frame_area):
|
||||||
|
"""Find the boundary quadrilateral of the green court surface.
|
||||||
|
Returns 4x2 float32 array ordered TL, TR, BR, BL, or None."""
|
||||||
contours, _ = cv2.findContours(green_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
contours, _ = cv2.findContours(green_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||||
|
|
||||||
if not contours:
|
if not contours:
|
||||||
return {'quad': None, 'green_mask': green_mask, 'error': 'No green surface detected'}
|
return None
|
||||||
|
|
||||||
# Take largest contour (should be the court)
|
|
||||||
largest = max(contours, key=cv2.contourArea)
|
largest = max(contours, key=cv2.contourArea)
|
||||||
area = cv2.contourArea(largest)
|
area = cv2.contourArea(largest)
|
||||||
frame_area = frame.shape[0] * frame.shape[1]
|
|
||||||
|
|
||||||
if area < frame_area * 0.05:
|
if area < frame_area * 0.05:
|
||||||
return {'quad': None, 'green_mask': green_mask,
|
return None
|
||||||
'error': f'Green area too small ({area / frame_area * 100:.0f}% of frame)'}
|
|
||||||
|
|
||||||
# Approximate contour with polygon
|
|
||||||
epsilon = 0.02 * cv2.arcLength(largest, True)
|
epsilon = 0.02 * cv2.arcLength(largest, True)
|
||||||
approx = cv2.approxPolyDP(largest, epsilon, True)
|
approx = cv2.approxPolyDP(largest, epsilon, True)
|
||||||
|
|
||||||
# We want 4 corners — if we got more, use convex hull + min area rect
|
|
||||||
if len(approx) == 4:
|
if len(approx) == 4:
|
||||||
quad = approx.reshape(4, 2).astype(np.float32)
|
quad = approx.reshape(4, 2).astype(np.float32)
|
||||||
else:
|
else:
|
||||||
# Use minimum area rectangle as fallback
|
|
||||||
rect = cv2.minAreaRect(largest)
|
rect = cv2.minAreaRect(largest)
|
||||||
quad = cv2.boxPoints(rect).astype(np.float32)
|
quad = cv2.boxPoints(rect).astype(np.float32)
|
||||||
|
|
||||||
# Order corners: top-left, top-right, bottom-right, bottom-left
|
# Order: TL, TR, BR, BL
|
||||||
# Sort by y first to get top/bottom pairs, then by x within each pair
|
|
||||||
sorted_by_y = quad[np.argsort(quad[:, 1])]
|
sorted_by_y = quad[np.argsort(quad[:, 1])]
|
||||||
top_pair = sorted_by_y[:2]
|
top_pair = sorted_by_y[:2][np.argsort(sorted_by_y[:2, 0])]
|
||||||
bottom_pair = sorted_by_y[2:]
|
bottom_pair = sorted_by_y[2:][np.argsort(sorted_by_y[2:, 0])]
|
||||||
top_pair = top_pair[np.argsort(top_pair[:, 0])]
|
return np.array([top_pair[0], top_pair[1], bottom_pair[1], bottom_pair[0]],
|
||||||
bottom_pair = bottom_pair[np.argsort(bottom_pair[:, 0])]
|
dtype=np.float32)
|
||||||
quad = np.array([top_pair[0], top_pair[1], bottom_pair[1], bottom_pair[0]], dtype=np.float32)
|
|
||||||
|
|
||||||
return {'quad': quad, 'green_mask': green_mask, 'error': None}
|
|
||||||
|
def _calibrate_from_quad(quad, side, w, h):
|
||||||
|
"""Try calibrating from green quad boundary by testing both left-right mappings.
|
||||||
|
|
||||||
|
The quad is ordered TL, TR, BR, BL in image coordinates.
|
||||||
|
Camera is at net looking at one half-court, so:
|
||||||
|
- Top of image = far from camera = baseline
|
||||||
|
- Bottom of image = near camera = kitchen/net area
|
||||||
|
|
||||||
|
We don't know which sideline is left vs right, so try both.
|
||||||
|
Returns (CameraCalibrator, cam_pos, info_string) or (None, None, None).
|
||||||
|
"""
|
||||||
|
corners_3d = get_half_court_3d_points(side)
|
||||||
|
# corners_3d order: [baseline_near, baseline_far, net_far, net_near]
|
||||||
|
# baseline_near = (bx, 0, 0), baseline_far = (bx, 6.1, 0)
|
||||||
|
# net_far = (nx, 6.1, 0), net_near = (nx, 0, 0)
|
||||||
|
|
||||||
|
# Mapping A: image-left = y=0 (near sideline), image-right = y=6.1 (far sideline)
|
||||||
|
# quad: TL→baseline_near, TR→baseline_far, BR→net_far, BL→net_near
|
||||||
|
mapping_a = corners_3d[[0, 1, 2, 3]]
|
||||||
|
|
||||||
|
# Mapping B: image-left = y=6.1 (far sideline), image-right = y=0 (near sideline)
|
||||||
|
# quad: TL→baseline_far, TR→baseline_near, BR→net_near, BL→net_far
|
||||||
|
mapping_b = corners_3d[[1, 0, 3, 2]]
|
||||||
|
|
||||||
|
best_cal = None
|
||||||
|
best_pos = None
|
||||||
|
best_label = None
|
||||||
|
|
||||||
|
for label, mapping in [('A', mapping_a), ('B', mapping_b)]:
|
||||||
|
cal = CameraCalibrator()
|
||||||
|
try:
|
||||||
|
cal.calibrate(quad, mapping, w, h)
|
||||||
|
except RuntimeError:
|
||||||
|
continue
|
||||||
|
|
||||||
|
cam_pos = (-cal.rotation_matrix.T @ cal.translation_vec).flatten()
|
||||||
|
|
||||||
|
# Camera should be: above ground (z>0), reasonable height (z<5m)
|
||||||
|
if 0 < cam_pos[2] < 5:
|
||||||
|
if best_cal is None or abs(cam_pos[2] - 1.0) < abs(best_pos[2] - 1.0):
|
||||||
|
best_cal = cal
|
||||||
|
best_pos = cam_pos
|
||||||
|
best_label = label
|
||||||
|
|
||||||
|
if best_cal is None:
|
||||||
|
return None, None, None
|
||||||
|
|
||||||
|
return best_cal, best_pos, f"quad mapping {best_label}"
|
||||||
|
|
||||||
|
|
||||||
def _detect_white_lines_on_court(frame, green_mask):
|
def _detect_white_lines_on_court(frame, green_mask):
|
||||||
"""Detect white lines within the green court area.
|
"""Detect white line segments within the green court area.
|
||||||
|
Returns list of [x1, y1, x2, y2] segments."""
|
||||||
Returns list of line segments [x1, y1, x2, y2] that are on the court surface.
|
|
||||||
"""
|
|
||||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||||
|
|
||||||
# White lines are bright pixels on the green surface
|
|
||||||
_, white = cv2.threshold(gray, 180, 255, cv2.THRESH_BINARY)
|
_, white = cv2.threshold(gray, 180, 255, cv2.THRESH_BINARY)
|
||||||
|
|
||||||
# Only keep white pixels within/near the court
|
# Only keep white pixels within/near the court
|
||||||
dilated_court = cv2.dilate(green_mask, np.ones((15, 15), np.uint8))
|
dilated_court = cv2.dilate(green_mask, np.ones((15, 15), np.uint8))
|
||||||
white_on_court = cv2.bitwise_and(white, dilated_court)
|
white_on_court = cv2.bitwise_and(white, dilated_court)
|
||||||
|
|
||||||
# Hough line detection
|
|
||||||
lines = cv2.HoughLinesP(white_on_court, 1, np.pi / 180, threshold=40,
|
lines = cv2.HoughLinesP(white_on_court, 1, np.pi / 180, threshold=40,
|
||||||
minLineLength=50, maxLineGap=25)
|
minLineLength=50, maxLineGap=25)
|
||||||
|
|
||||||
if lines is None:
|
if lines is None:
|
||||||
return []
|
return []
|
||||||
|
|
||||||
return [line[0].tolist() for line in lines]
|
return [line[0].tolist() for line in lines]
|
||||||
|
|
||||||
|
|
||||||
|
def _merge_line_segments(segments, angle_thresh=15, dist_thresh=30):
|
||||||
|
"""Merge nearby parallel line segments into distinct court lines.
|
||||||
|
|
||||||
|
Groups segments by angle and perpendicular distance, then represents
|
||||||
|
each group by the longest extent along the dominant direction.
|
||||||
|
|
||||||
|
Returns list of dicts with 'angle', 'p1', 'p2', 'weight'.
|
||||||
|
"""
|
||||||
|
if not segments:
|
||||||
|
return []
|
||||||
|
|
||||||
|
line_data = []
|
||||||
|
for x1, y1, x2, y2 in segments:
|
||||||
|
angle = np.arctan2(y2 - y1, x2 - x1) * 180 / np.pi
|
||||||
|
if angle < 0:
|
||||||
|
angle += 180
|
||||||
|
length = np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
|
||||||
|
mid = ((x1 + x2) / 2, (y1 + y2) / 2)
|
||||||
|
line_data.append({
|
||||||
|
'angle': angle, 'mid': mid,
|
||||||
|
'seg': (x1, y1, x2, y2), 'length': length,
|
||||||
|
})
|
||||||
|
|
||||||
|
used = [False] * len(line_data)
|
||||||
|
merged = []
|
||||||
|
|
||||||
|
for i in range(len(line_data)):
|
||||||
|
if used[i]:
|
||||||
|
continue
|
||||||
|
cluster = [line_data[i]]
|
||||||
|
used[i] = True
|
||||||
|
|
||||||
|
for j in range(i + 1, len(line_data)):
|
||||||
|
if used[j]:
|
||||||
|
continue
|
||||||
|
da = abs(line_data[i]['angle'] - line_data[j]['angle'])
|
||||||
|
if da > 90:
|
||||||
|
da = 180 - da
|
||||||
|
if da > angle_thresh:
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Perpendicular distance between midpoints
|
||||||
|
dx = line_data[j]['mid'][0] - line_data[i]['mid'][0]
|
||||||
|
dy = line_data[j]['mid'][1] - line_data[i]['mid'][1]
|
||||||
|
angle_rad = line_data[i]['angle'] * np.pi / 180
|
||||||
|
perp_dist = abs(dx * np.sin(angle_rad) - dy * np.cos(angle_rad))
|
||||||
|
|
||||||
|
if perp_dist < dist_thresh:
|
||||||
|
cluster.append(line_data[j])
|
||||||
|
used[j] = True
|
||||||
|
|
||||||
|
# Merge: use weighted average angle, find extent from all endpoints
|
||||||
|
total_weight = sum(l['length'] for l in cluster)
|
||||||
|
avg_angle = sum(l['angle'] * l['length'] for l in cluster) / total_weight
|
||||||
|
angle_rad = avg_angle * np.pi / 180
|
||||||
|
dx, dy = np.cos(angle_rad), np.sin(angle_rad)
|
||||||
|
|
||||||
|
all_pts = []
|
||||||
|
for l in cluster:
|
||||||
|
sx1, sy1, sx2, sy2 = l['seg']
|
||||||
|
all_pts.extend([(sx1, sy1), (sx2, sy2)])
|
||||||
|
|
||||||
|
projections = [p[0] * dx + p[1] * dy for p in all_pts]
|
||||||
|
min_idx = int(np.argmin(projections))
|
||||||
|
max_idx = int(np.argmax(projections))
|
||||||
|
|
||||||
|
merged.append({
|
||||||
|
'angle': avg_angle,
|
||||||
|
'p1': all_pts[min_idx],
|
||||||
|
'p2': all_pts[max_idx],
|
||||||
|
'weight': total_weight,
|
||||||
|
})
|
||||||
|
|
||||||
|
return merged
|
||||||
|
|
||||||
|
|
||||||
|
def _find_line_intersections(merged_lines, img_w, img_h, margin=50):
|
||||||
|
"""Find intersection points between merged lines.
|
||||||
|
Only keeps intersections where lines are sufficiently non-parallel
|
||||||
|
and the point is within (or near) the image bounds.
|
||||||
|
Clusters nearby intersections into single points.
|
||||||
|
"""
|
||||||
|
raw_intersections = []
|
||||||
|
|
||||||
|
for i in range(len(merged_lines)):
|
||||||
|
for j in range(i + 1, len(merged_lines)):
|
||||||
|
da = abs(merged_lines[i]['angle'] - merged_lines[j]['angle'])
|
||||||
|
if da > 90:
|
||||||
|
da = 180 - da
|
||||||
|
if da < 20: # too parallel
|
||||||
|
continue
|
||||||
|
|
||||||
|
x1, y1 = merged_lines[i]['p1']
|
||||||
|
x2, y2 = merged_lines[i]['p2']
|
||||||
|
x3, y3 = merged_lines[j]['p1']
|
||||||
|
x4, y4 = merged_lines[j]['p2']
|
||||||
|
|
||||||
|
denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
|
||||||
|
if abs(denom) < 1e-6:
|
||||||
|
continue
|
||||||
|
|
||||||
|
t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / denom
|
||||||
|
ix = x1 + t * (x2 - x1)
|
||||||
|
iy = y1 + t * (y2 - y1)
|
||||||
|
|
||||||
|
if -margin <= ix <= img_w + margin and -margin <= iy <= img_h + margin:
|
||||||
|
raw_intersections.append((ix, iy))
|
||||||
|
|
||||||
|
# Cluster nearby intersections (within 20px)
|
||||||
|
if not raw_intersections:
|
||||||
|
return []
|
||||||
|
|
||||||
|
pts = np.array(raw_intersections, dtype=np.float32)
|
||||||
|
used = [False] * len(pts)
|
||||||
|
clustered = []
|
||||||
|
|
||||||
|
for i in range(len(pts)):
|
||||||
|
if used[i]:
|
||||||
|
continue
|
||||||
|
cluster = [pts[i]]
|
||||||
|
used[i] = True
|
||||||
|
for j in range(i + 1, len(pts)):
|
||||||
|
if used[j]:
|
||||||
|
continue
|
||||||
|
if np.linalg.norm(pts[i] - pts[j]) < 20:
|
||||||
|
cluster.append(pts[j])
|
||||||
|
used[j] = True
|
||||||
|
centroid = np.mean(cluster, axis=0)
|
||||||
|
clustered.append((float(centroid[0]), float(centroid[1])))
|
||||||
|
|
||||||
|
return clustered
|
||||||
|
|
||||||
|
|
||||||
|
def _match_intersections_to_template(intersections, template, side, quad, w, h):
|
||||||
|
"""Match detected intersections to known court template points.
|
||||||
|
|
||||||
|
Strategy:
|
||||||
|
1. Use green quad to compute initial homography (world -> image)
|
||||||
|
2. Project all 6 template points into image
|
||||||
|
3. For each projected point, find nearest detected intersection
|
||||||
|
4. Try both left-right mirror mappings
|
||||||
|
5. Return the match with more inliers and sane reprojection
|
||||||
|
|
||||||
|
Returns dict with 'image_points' and 'world_points' dicts, or None.
|
||||||
|
"""
|
||||||
|
if quad is None or len(intersections) < 4:
|
||||||
|
return None
|
||||||
|
|
||||||
|
corners_3d = get_half_court_3d_points(side)
|
||||||
|
det_pts = np.array(intersections, dtype=np.float32)
|
||||||
|
template_names = list(template.keys())
|
||||||
|
template_world = np.array([template[n][:2] for n in template_names], dtype=np.float32)
|
||||||
|
|
||||||
|
best_match = None
|
||||||
|
best_count = 0
|
||||||
|
|
||||||
|
# Try both left-right mappings for the initial homography
|
||||||
|
for mapping_3d in [corners_3d[[0, 1, 2, 3]], corners_3d[[1, 0, 3, 2]]]:
|
||||||
|
# Homography: world 2D -> image
|
||||||
|
world_2d = mapping_3d[:, :2].astype(np.float32)
|
||||||
|
H, status = cv2.findHomography(world_2d, quad)
|
||||||
|
if H is None:
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Project template intersection points to image
|
||||||
|
projected = cv2.perspectiveTransform(
|
||||||
|
template_world.reshape(-1, 1, 2), H
|
||||||
|
).reshape(-1, 2)
|
||||||
|
|
||||||
|
# Match each projected point to nearest detected intersection
|
||||||
|
threshold = 50 # pixels
|
||||||
|
matched_img = {}
|
||||||
|
matched_world = {}
|
||||||
|
used_det = set()
|
||||||
|
|
||||||
|
for i, name in enumerate(template_names):
|
||||||
|
proj = projected[i]
|
||||||
|
dists = np.linalg.norm(det_pts - proj, axis=1)
|
||||||
|
order = np.argsort(dists)
|
||||||
|
for idx in order:
|
||||||
|
if dists[idx] > threshold:
|
||||||
|
break
|
||||||
|
if int(idx) not in used_det:
|
||||||
|
matched_img[name] = (float(det_pts[idx][0]), float(det_pts[idx][1]))
|
||||||
|
matched_world[name] = template[name].tolist()
|
||||||
|
used_det.add(int(idx))
|
||||||
|
break
|
||||||
|
|
||||||
|
if len(matched_img) > best_count:
|
||||||
|
best_count = len(matched_img)
|
||||||
|
best_match = {
|
||||||
|
'image_points': matched_img,
|
||||||
|
'world_points': matched_world,
|
||||||
|
}
|
||||||
|
|
||||||
|
return best_match if best_count >= 4 else None
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def _capture_var_snapshot(frame, event):
|
def _capture_var_snapshot(frame, event):
|
||||||
|
|||||||
@@ -64,10 +64,16 @@ class CameraCalibrator:
|
|||||||
|
|
||||||
# Build ground plane homography (Z=0)
|
# Build ground plane homography (Z=0)
|
||||||
ground_2d = court_corners_3d[:, :2].astype(np.float32)
|
ground_2d = court_corners_3d[:, :2].astype(np.float32)
|
||||||
|
if len(court_corners_pixel) == 4:
|
||||||
self.homography = cv2.getPerspectiveTransform(
|
self.homography = cv2.getPerspectiveTransform(
|
||||||
court_corners_pixel[:4].astype(np.float32),
|
court_corners_pixel[:4].astype(np.float32),
|
||||||
ground_2d[:4]
|
ground_2d[:4]
|
||||||
)
|
)
|
||||||
|
else:
|
||||||
|
self.homography, _ = cv2.findHomography(
|
||||||
|
court_corners_pixel.astype(np.float32),
|
||||||
|
ground_2d
|
||||||
|
)
|
||||||
|
|
||||||
self.calibrated = True
|
self.calibrated = True
|
||||||
return True
|
return True
|
||||||
@@ -158,6 +164,40 @@ class CameraCalibrator:
|
|||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
|
NVZ_DEPTH = 2.13 # non-volley zone depth from net
|
||||||
|
|
||||||
|
|
||||||
|
def get_half_court_intersections(side: str) -> Dict[str, np.ndarray]:
|
||||||
|
"""Get all line intersection points on a half-court.
|
||||||
|
|
||||||
|
These are where court lines cross — the key features for calibration.
|
||||||
|
Each half has 6 intersections: baseline x {near, mid, far} + kitchen x {near, mid, far}.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
side: 'left' (CAM 0, X: 0 to 6.7m) or 'right' (CAM 1, X: 6.7 to 13.4m)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
dict of name -> [x, y, z] 3D coordinates
|
||||||
|
"""
|
||||||
|
mid_y = COURT_WIDTH / 2 # 3.05
|
||||||
|
|
||||||
|
if side == 'left':
|
||||||
|
bx = 0.0 # baseline
|
||||||
|
kx = HALF_COURT_LENGTH - NVZ_DEPTH # kitchen = 4.57
|
||||||
|
else:
|
||||||
|
bx = COURT_LENGTH # baseline = 13.4
|
||||||
|
kx = HALF_COURT_LENGTH + NVZ_DEPTH # kitchen = 8.83
|
||||||
|
|
||||||
|
return {
|
||||||
|
'BL_near': np.array([bx, 0, 0], dtype=np.float32),
|
||||||
|
'BL_mid': np.array([bx, mid_y, 0], dtype=np.float32),
|
||||||
|
'BL_far': np.array([bx, COURT_WIDTH, 0], dtype=np.float32),
|
||||||
|
'K_near': np.array([kx, 0, 0], dtype=np.float32),
|
||||||
|
'K_mid': np.array([kx, mid_y, 0], dtype=np.float32),
|
||||||
|
'K_far': np.array([kx, COURT_WIDTH, 0], dtype=np.float32),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
def get_half_court_3d_points(side: str) -> np.ndarray:
|
def get_half_court_3d_points(side: str) -> np.ndarray:
|
||||||
"""Get 3D coordinates for one half of the court.
|
"""Get 3D coordinates for one half of the court.
|
||||||
|
|
||||||
|
|||||||
@@ -540,12 +540,11 @@ function doCalibrate() {
|
|||||||
if (!el) continue;
|
if (!el) continue;
|
||||||
if (r.ok && r.camera_position) {
|
if (r.ok && r.camera_position) {
|
||||||
var p = r.camera_position;
|
var p = r.camera_position;
|
||||||
var lines = r.lines_detected || {};
|
|
||||||
el.textContent = 'CAM' + sid + ': X=' + p[0].toFixed(2) + ' Y=' + p[1].toFixed(2) + ' Z=' + p[2].toFixed(2) + 'm'
|
el.textContent = 'CAM' + sid + ': X=' + p[0].toFixed(2) + ' Y=' + p[1].toFixed(2) + ' Z=' + p[2].toFixed(2) + 'm'
|
||||||
+ ' (' + (r.points_matched || 0) + 'pts, ' + (lines.across || 0) + 'a+' + (lines.along || 0) + 'l)';
|
+ ' (' + (r.points_matched || 0) + ' pts)';
|
||||||
} else if (r.lines_detected) {
|
el.style.color = sid === '0' ? '#4ecca3' : '#ff88cc';
|
||||||
var lines = r.lines_detected;
|
} else {
|
||||||
el.textContent = 'CAM' + sid + ': ' + (lines.across || 0) + ' across + ' + (lines.along || 0) + ' along lines';
|
el.textContent = 'CAM' + sid + ': ' + (r.error || 'failed');
|
||||||
el.style.color = '#ff4444';
|
el.style.color = '#ff4444';
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user