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:
Ruslan Bakiev
535
jetson/main.py
535
jetson/main.py
@@ -22,7 +22,7 @@ from src.streaming.ring_buffer import FrameRingBuffer
|
||||
from src.physics.trajectory import TrajectoryModel
|
||||
from src.physics.event_detector import EventDetector
|
||||
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
|
||||
)
|
||||
from src.web.app import app, state
|
||||
@@ -35,14 +35,18 @@ _args = None
|
||||
|
||||
|
||||
def auto_calibrate():
|
||||
"""Detect the green court surface, find its boundary quadrilateral,
|
||||
map to known court dimensions, compute camera 3D position via solvePnP.
|
||||
"""Calibrate cameras by detecting court line intersections and matching
|
||||
them to the known pickleball court template.
|
||||
|
||||
Debug images show:
|
||||
- Green mask overlay (what the system sees as court)
|
||||
- Detected quadrilateral (court boundary)
|
||||
- White line segments found on court
|
||||
- Computed camera position
|
||||
Algorithm:
|
||||
1. Detect green court surface (search area)
|
||||
2. Find white line segments on the green surface
|
||||
3. Merge similar segments into distinct court lines
|
||||
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 = {}
|
||||
|
||||
@@ -56,190 +60,481 @@ def auto_calibrate():
|
||||
side = 'left' if sensor_id == 0 else 'right'
|
||||
debug_frame = frame.copy()
|
||||
|
||||
# Step 1: Detect green court surface
|
||||
court = _detect_court_surface(frame)
|
||||
# Step 1: Detect green court mask
|
||||
green_mask = _detect_green_mask(frame)
|
||||
green_overlay = np.zeros_like(debug_frame)
|
||||
green_overlay[green_mask > 0] = (0, 80, 0)
|
||||
debug_frame = cv2.addWeighted(debug_frame, 1.0, green_overlay, 0.3, 0)
|
||||
|
||||
# Draw green mask as semi-transparent overlay
|
||||
if court['green_mask'] is not None:
|
||||
green_overlay = np.zeros_like(debug_frame)
|
||||
green_overlay[court['green_mask'] > 0] = (0, 80, 0)
|
||||
debug_frame = cv2.addWeighted(debug_frame, 1.0, green_overlay, 0.3, 0)
|
||||
|
||||
if court['quad'] is None:
|
||||
cv2.putText(debug_frame, f"FAILED: {court['error']}", (10, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||||
green_pct = np.count_nonzero(green_mask) / (w * h) * 100
|
||||
if green_pct < 5:
|
||||
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])
|
||||
results[str(sensor_id)] = {
|
||||
'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'),
|
||||
}
|
||||
continue
|
||||
|
||||
quad = court['quad']
|
||||
|
||||
# Draw detected court quadrilateral
|
||||
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
|
||||
# Step 2: Detect white line segments on court
|
||||
white_segments = _detect_white_lines_on_court(frame, green_mask)
|
||||
for x1, y1, x2, y2 in white_segments:
|
||||
cv2.line(debug_frame, (x1, y1), (x2, y2), (255, 255, 0), 1)
|
||||
|
||||
# Step 3: Map quad corners to 3D court coordinates
|
||||
# Quad corners are ordered: TL, TR, BR, BL
|
||||
# For camera at net looking at one half-court:
|
||||
# TL = far-left, TR = far-right (baseline)
|
||||
# BL = near-left, BR = near-right (near net/camera)
|
||||
corners_3d = get_half_court_3d_points(side)
|
||||
# Step 3: Merge into distinct lines
|
||||
merged = _merge_line_segments(white_segments)
|
||||
for m in merged:
|
||||
p1, p2 = m['p1'], m['p2']
|
||||
cv2.line(debug_frame, (int(p1[0]), int(p1[1])),
|
||||
(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.calibrate(quad, corners_3d, w, h)
|
||||
|
||||
cal.calibrate(pts_2d, pts_3d, w, h)
|
||||
cam_pos = (-cal.rotation_matrix.T @ cal.translation_vec).flatten()
|
||||
|
||||
cv2.putText(debug_frame,
|
||||
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)
|
||||
cv2.putText(debug_frame,
|
||||
f"{side} half | {len(white_lines)} white line segments",
|
||||
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
||||
# 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
|
||||
|
||||
# Save
|
||||
cal_path = os.path.join(_args.calibration_dir,
|
||||
f'cam{sensor_id}_calibration.json')
|
||||
os.makedirs(os.path.dirname(cal_path), exist_ok=True)
|
||||
cal.save(cal_path)
|
||||
state['calibrators'][sensor_id] = cal
|
||||
|
||||
_, 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)
|
||||
|
||||
results[str(sensor_id)] = {
|
||||
'ok': True,
|
||||
'camera_position': cam_pos.tolist(),
|
||||
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
||||
'matched_lines_3d': matched_lines_3d,
|
||||
}
|
||||
print(f"[CAM {sensor_id}] Calibrated! Camera at "
|
||||
f"({cam_pos[0]:.2f}, {cam_pos[1]:.2f}, {cam_pos[2]:.2f})")
|
||||
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,
|
||||
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)
|
||||
cv2.putText(debug_frame,
|
||||
f"{side} half | {method_info}",
|
||||
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
||||
|
||||
cal_path = os.path.join(_args.calibration_dir,
|
||||
f'cam{sensor_id}_calibration.json')
|
||||
os.makedirs(os.path.dirname(cal_path), exist_ok=True)
|
||||
cal.save(cal_path)
|
||||
state['calibrators'][sensor_id] = cal
|
||||
|
||||
_, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
|
||||
|
||||
matched_lines_3d = _get_half_court_lines_3d(side)
|
||||
|
||||
results[str(sensor_id)] = {
|
||||
'ok': True,
|
||||
'camera_position': cam_pos.tolist(),
|
||||
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
||||
'matched_lines_3d': matched_lines_3d,
|
||||
'points_matched': n_intersections,
|
||||
}
|
||||
print(f"[CAM {sensor_id}] Calibrated! Camera at "
|
||||
f"({cam_pos[0]:.2f}, {cam_pos[1]:.2f}, {cam_pos[2]:.2f}) via {method_info}")
|
||||
|
||||
|
||||
def _get_half_court_lines_3d(side):
|
||||
"""Return 3D line segments for all lines of a half-court."""
|
||||
if side == 'left':
|
||||
bx, kx = 0, 4.57 # baseline, kitchen
|
||||
bx, kx = 0, 4.57
|
||||
else:
|
||||
bx, kx = 13.4, 8.83
|
||||
lines = [
|
||||
return [
|
||||
{'name': 'baseline', 'from': [bx, 0, 0], 'to': [bx, 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_far', 'from': [bx, 6.1, 0], 'to': [kx, 6.1, 0]},
|
||||
{'name': 'center_service', 'from': [bx, 3.05, 0], 'to': [kx, 3.05, 0]},
|
||||
]
|
||||
return lines
|
||||
|
||||
|
||||
def _detect_court_surface(frame):
|
||||
"""Detect the green court surface and find its boundary as a quadrilateral.
|
||||
|
||||
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
|
||||
"""
|
||||
def _detect_green_mask(frame):
|
||||
"""Detect green court surface pixels. Returns binary mask."""
|
||||
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
|
||||
|
||||
# Green court surface — relatively broad range to handle lighting
|
||||
lower_green = np.array([30, 30, 50])
|
||||
upper_green = np.array([90, 255, 220])
|
||||
green_mask = cv2.inRange(hsv, lower_green, upper_green)
|
||||
|
||||
# Clean up noise
|
||||
mask = cv2.inRange(hsv, lower_green, upper_green)
|
||||
kernel = np.ones((7, 7), np.uint8)
|
||||
green_mask = cv2.morphologyEx(green_mask, cv2.MORPH_CLOSE, kernel)
|
||||
green_mask = cv2.morphologyEx(green_mask, cv2.MORPH_OPEN, kernel)
|
||||
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, 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)
|
||||
|
||||
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)
|
||||
area = cv2.contourArea(largest)
|
||||
frame_area = frame.shape[0] * frame.shape[1]
|
||||
|
||||
if area < frame_area * 0.05:
|
||||
return {'quad': None, 'green_mask': green_mask,
|
||||
'error': f'Green area too small ({area / frame_area * 100:.0f}% of frame)'}
|
||||
return None
|
||||
|
||||
# Approximate contour with polygon
|
||||
epsilon = 0.02 * cv2.arcLength(largest, 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:
|
||||
quad = approx.reshape(4, 2).astype(np.float32)
|
||||
else:
|
||||
# Use minimum area rectangle as fallback
|
||||
rect = cv2.minAreaRect(largest)
|
||||
quad = cv2.boxPoints(rect).astype(np.float32)
|
||||
|
||||
# Order corners: top-left, top-right, bottom-right, bottom-left
|
||||
# Sort by y first to get top/bottom pairs, then by x within each pair
|
||||
# Order: TL, TR, BR, BL
|
||||
sorted_by_y = quad[np.argsort(quad[:, 1])]
|
||||
top_pair = sorted_by_y[:2]
|
||||
bottom_pair = sorted_by_y[2:]
|
||||
top_pair = top_pair[np.argsort(top_pair[:, 0])]
|
||||
bottom_pair = bottom_pair[np.argsort(bottom_pair[:, 0])]
|
||||
quad = np.array([top_pair[0], top_pair[1], bottom_pair[1], bottom_pair[0]], dtype=np.float32)
|
||||
top_pair = sorted_by_y[:2][np.argsort(sorted_by_y[:2, 0])]
|
||||
bottom_pair = sorted_by_y[2:][np.argsort(sorted_by_y[2:, 0])]
|
||||
return 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):
|
||||
"""Detect white lines within the green court area.
|
||||
|
||||
Returns list of line segments [x1, y1, x2, y2] that are on the court surface.
|
||||
"""
|
||||
"""Detect white line segments within the green court area.
|
||||
Returns list of [x1, y1, x2, y2] segments."""
|
||||
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)
|
||||
|
||||
# Only keep white pixels within/near the court
|
||||
dilated_court = cv2.dilate(green_mask, np.ones((15, 15), np.uint8))
|
||||
white_on_court = cv2.bitwise_and(white, dilated_court)
|
||||
|
||||
# Hough line detection
|
||||
lines = cv2.HoughLinesP(white_on_court, 1, np.pi / 180, threshold=40,
|
||||
minLineLength=50, maxLineGap=25)
|
||||
|
||||
if lines is None:
|
||||
return []
|
||||
|
||||
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):
|
||||
|
||||
@@ -64,10 +64,16 @@ class CameraCalibrator:
|
||||
|
||||
# Build ground plane homography (Z=0)
|
||||
ground_2d = court_corners_3d[:, :2].astype(np.float32)
|
||||
self.homography = cv2.getPerspectiveTransform(
|
||||
court_corners_pixel[:4].astype(np.float32),
|
||||
ground_2d[:4]
|
||||
)
|
||||
if len(court_corners_pixel) == 4:
|
||||
self.homography = cv2.getPerspectiveTransform(
|
||||
court_corners_pixel[:4].astype(np.float32),
|
||||
ground_2d[:4]
|
||||
)
|
||||
else:
|
||||
self.homography, _ = cv2.findHomography(
|
||||
court_corners_pixel.astype(np.float32),
|
||||
ground_2d
|
||||
)
|
||||
|
||||
self.calibrated = True
|
||||
return True
|
||||
@@ -158,6 +164,40 @@ class CameraCalibrator:
|
||||
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:
|
||||
"""Get 3D coordinates for one half of the court.
|
||||
|
||||
|
||||
@@ -540,12 +540,11 @@ function doCalibrate() {
|
||||
if (!el) continue;
|
||||
if (r.ok && 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'
|
||||
+ ' (' + (r.points_matched || 0) + 'pts, ' + (lines.across || 0) + 'a+' + (lines.along || 0) + 'l)';
|
||||
} else if (r.lines_detected) {
|
||||
var lines = r.lines_detected;
|
||||
el.textContent = 'CAM' + sid + ': ' + (lines.across || 0) + ' across + ' + (lines.along || 0) + ' along lines';
|
||||
+ ' (' + (r.points_matched || 0) + ' pts)';
|
||||
el.style.color = sid === '0' ? '#4ecca3' : '#ff88cc';
|
||||
} else {
|
||||
el.textContent = 'CAM' + sid + ': ' + (r.error || 'failed');
|
||||
el.style.color = '#ff4444';
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user