Rewrite court detection: find ALL court lines, not just 4 corners
- Detect white lines via color threshold + Canny + Hough - Group/merge nearby parallel segments into court lines - Classify as 'across' (baseline, kitchen) and 'along' (sidelines, service) - Match detected lines to known court geometry for solvePnP - Debug image shows: raw segments (gray), merged across (yellow), along (magenta) - Show line counts + matched points in calibration UI Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
Ruslan Bakiev
390
jetson/main.py
390
jetson/main.py
@@ -35,9 +35,12 @@ _args = None
|
||||
|
||||
|
||||
def auto_calibrate():
|
||||
"""One-click calibration: detect court rectangle from current frames,
|
||||
compute camera 3D position via solvePnP using known court dimensions.
|
||||
Returns debug images showing detected court quad + computed camera position.
|
||||
"""Detect all visible court lines, identify them, compute camera 3D position.
|
||||
|
||||
Each camera sees one half of the pickleball court from the net.
|
||||
We detect all white lines, classify them by angle and position,
|
||||
try to match them to known court lines, then use solvePnP.
|
||||
Returns debug images showing every detected line + camera position.
|
||||
"""
|
||||
results = {}
|
||||
|
||||
@@ -51,137 +54,318 @@ def auto_calibrate():
|
||||
side = 'left' if sensor_id == 0 else 'right'
|
||||
debug_frame = frame.copy()
|
||||
|
||||
# Detect court rectangle — find 4 corners of the court
|
||||
detection = _detect_court_corners(frame, side)
|
||||
corners_pixel = detection.get('corners') if detection else None
|
||||
# Detect all court lines
|
||||
detection = _detect_court_lines(frame)
|
||||
all_segments = detection['segments']
|
||||
grouped = detection['grouped']
|
||||
|
||||
# Draw ALL detected line segments on debug frame
|
||||
for seg in all_segments:
|
||||
x1, y1, x2, y2 = seg
|
||||
cv2.line(debug_frame, (x1, y1), (x2, y2), (80, 80, 80), 1)
|
||||
|
||||
# Draw grouped/merged lines with colors
|
||||
# "across" lines (roughly horizontal in image = baseline, kitchen, net)
|
||||
across_colors = [(0, 255, 255), (0, 200, 200), (0, 150, 150),
|
||||
(0, 100, 200), (0, 80, 160)]
|
||||
for i, line in enumerate(grouped.get('across', [])):
|
||||
x1, y1, x2, y2 = line['endpoints']
|
||||
color = across_colors[i % len(across_colors)]
|
||||
cv2.line(debug_frame, (x1, y1), (x2, y2), color, 3)
|
||||
cv2.putText(debug_frame, f"across#{i} ({line['count']}seg)",
|
||||
(x1, max(y1 - 8, 15)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.4, color, 1)
|
||||
|
||||
# "along" lines (roughly vertical in image = sidelines, center service)
|
||||
along_colors = [(255, 0, 255), (200, 0, 200), (150, 0, 150),
|
||||
(200, 0, 100), (160, 0, 80)]
|
||||
for i, line in enumerate(grouped.get('along', [])):
|
||||
x1, y1, x2, y2 = line['endpoints']
|
||||
color = along_colors[i % len(along_colors)]
|
||||
cv2.line(debug_frame, (x1, y1), (x2, y2), color, 3)
|
||||
cv2.putText(debug_frame, f"along#{i} ({line['count']}seg)",
|
||||
(x1 + 5, (y1 + y2) // 2),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.4, color, 1)
|
||||
|
||||
# Summary text
|
||||
n_across = len(grouped.get('across', []))
|
||||
n_along = len(grouped.get('along', []))
|
||||
cv2.putText(debug_frame,
|
||||
f"Lines: {len(all_segments)} raw, {n_across} across, {n_along} along",
|
||||
(10, h - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
|
||||
|
||||
# Try to match lines to known court geometry and calibrate
|
||||
match = _match_court_lines(grouped, side, w, h)
|
||||
|
||||
if match['points_2d'] is not None and len(match['points_2d']) >= 4:
|
||||
# We have enough correspondences — calibrate
|
||||
cal = CameraCalibrator()
|
||||
cal.calibrate(
|
||||
np.array(match['points_2d'], dtype=np.float32),
|
||||
np.array(match['points_3d'], dtype=np.float32),
|
||||
w, h
|
||||
)
|
||||
|
||||
cam_pos = (-cal.rotation_matrix.T @ cal.translation_vec).flatten()
|
||||
|
||||
# Draw matched points
|
||||
for i, (px, py) in enumerate(match['points_2d']):
|
||||
cv2.circle(debug_frame, (int(px), int(py)), 6, (0, 255, 0), -1)
|
||||
|
||||
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"Matched {len(match['points_2d'])} points, {side} half",
|
||||
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
||||
|
||||
# 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])
|
||||
results[str(sensor_id)] = {
|
||||
'ok': True,
|
||||
'camera_position': cam_pos.tolist(),
|
||||
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
||||
'lines_detected': {'across': n_across, 'along': n_along},
|
||||
'points_matched': len(match['points_2d']),
|
||||
}
|
||||
print(f"[CAM {sensor_id}] Calibrated! Camera at "
|
||||
f"({cam_pos[0]:.2f}, {cam_pos[1]:.2f}, {cam_pos[2]:.2f}), "
|
||||
f"{len(match['points_2d'])} correspondences")
|
||||
else:
|
||||
cv2.putText(debug_frame,
|
||||
f"Not enough correspondences: {match.get('error', 'unknown')}",
|
||||
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||||
|
||||
if corners_pixel is None:
|
||||
error_detail = detection.get('error', 'Unknown') if detection else 'No court detected'
|
||||
# Draw error on debug frame
|
||||
cv2.putText(debug_frame, f"FAILED: {error_detail}", (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}: {error_detail}',
|
||||
'error': f"CAM {sensor_id}: {match.get('error', 'Not enough line correspondences')}",
|
||||
'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'),
|
||||
'lines_detected': {'across': n_across, 'along': n_along},
|
||||
}
|
||||
continue
|
||||
|
||||
# Draw detected court quadrilateral on debug frame
|
||||
pts = corners_pixel.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)
|
||||
|
||||
# Get known 3D coordinates for this half-court
|
||||
corners_3d = get_half_court_3d_points(side)
|
||||
|
||||
# Calibrate — errors propagate
|
||||
cal = CameraCalibrator()
|
||||
cal.calibrate(
|
||||
np.array(corners_pixel, dtype=np.float32),
|
||||
corners_3d, w, h
|
||||
)
|
||||
|
||||
# Camera position in world coordinates
|
||||
cam_pos = (-cal.rotation_matrix.T @ cal.translation_vec).flatten()
|
||||
|
||||
# Draw camera position on debug frame
|
||||
cv2.putText(debug_frame,
|
||||
f"Camera: X={cam_pos[0]:.2f}m Y={cam_pos[1]:.2f}m Z={cam_pos[2]:.2f}m",
|
||||
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
||||
cv2.putText(debug_frame, f"Court: {side} half",
|
||||
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
||||
|
||||
_, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
|
||||
debug_b64 = base64.b64encode(jpeg.tobytes()).decode('ascii')
|
||||
|
||||
# Save calibration
|
||||
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
|
||||
|
||||
results[str(sensor_id)] = {
|
||||
'ok': True,
|
||||
'camera_position': cam_pos.tolist(),
|
||||
'debug_image': debug_b64,
|
||||
}
|
||||
print(f"[CAM {sensor_id}] Calibrated! Camera at "
|
||||
f"({cam_pos[0]:.2f}, {cam_pos[1]:.2f}, {cam_pos[2]:.2f})")
|
||||
|
||||
return results
|
||||
|
||||
|
||||
def _detect_court_corners(frame, side):
|
||||
"""Detect 4 corners of the court rectangle from camera frame.
|
||||
def _detect_court_lines(frame):
|
||||
"""Detect all white line segments in the frame.
|
||||
|
||||
Uses edge detection + Hough lines to find the court boundaries,
|
||||
then finds 4 intersection points forming the court quadrilateral.
|
||||
|
||||
Returns dict with:
|
||||
corners: 4x2 numpy array (TL, TR, BR, BL) or None
|
||||
error: description string if detection failed
|
||||
Returns:
|
||||
segments: list of [x1, y1, x2, y2] raw segments
|
||||
grouped: dict with 'across' and 'along' merged line groups
|
||||
"""
|
||||
# White line detection via color thresholding + edges
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
# White lines are bright — threshold to isolate them
|
||||
_, white_mask = cv2.threshold(gray, 180, 255, cv2.THRESH_BINARY)
|
||||
|
||||
# Also use edge detection
|
||||
blur = cv2.GaussianBlur(gray, (5, 5), 0)
|
||||
edges = cv2.Canny(blur, 50, 150)
|
||||
|
||||
lines = cv2.HoughLinesP(edges, 1, np.pi / 180, threshold=80,
|
||||
minLineLength=100, maxLineGap=20)
|
||||
# Combine: edges that are also near white regions
|
||||
dilated_white = cv2.dilate(white_mask, np.ones((5, 5), np.uint8))
|
||||
combined = cv2.bitwise_and(edges, dilated_white)
|
||||
|
||||
if lines is None or len(lines) < 4:
|
||||
n = 0 if lines is None else len(lines)
|
||||
return {'corners': None, 'error': f'Found {n} lines, need at least 4'}
|
||||
# Hough line detection on combined mask
|
||||
lines = cv2.HoughLinesP(combined, 1, np.pi / 180, threshold=50,
|
||||
minLineLength=60, maxLineGap=30)
|
||||
|
||||
# Classify into horizontal / vertical
|
||||
horizontals = []
|
||||
verticals = []
|
||||
if lines is None:
|
||||
return {'segments': [], 'grouped': {'across': [], 'along': []}}
|
||||
|
||||
for line in lines:
|
||||
x1, y1, x2, y2 = line[0]
|
||||
angle = abs(np.arctan2(y2 - y1, x2 - x1) * 180 / np.pi)
|
||||
if angle < 30 or angle > 150:
|
||||
horizontals.append(line[0])
|
||||
elif 60 < angle < 120:
|
||||
verticals.append(line[0])
|
||||
segments = [line[0].tolist() for line in lines]
|
||||
|
||||
if len(horizontals) < 2 or len(verticals) < 2:
|
||||
return {'corners': None,
|
||||
'error': f'{len(horizontals)} horiz + {len(verticals)} vert lines, need 2+ each'}
|
||||
# Classify by angle and group nearby parallel lines
|
||||
across_lines = [] # roughly horizontal (court width direction)
|
||||
along_lines = [] # roughly vertical (court length direction)
|
||||
|
||||
# Take outermost lines as court boundaries
|
||||
h_sorted = sorted(horizontals, key=lambda l: (l[1] + l[3]) / 2)
|
||||
v_sorted = sorted(verticals, key=lambda l: (l[0] + l[2]) / 2)
|
||||
for seg in segments:
|
||||
x1, y1, x2, y2 = seg
|
||||
angle = np.arctan2(y2 - y1, x2 - x1) * 180 / np.pi
|
||||
length = np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
|
||||
|
||||
top, bottom = h_sorted[0], h_sorted[-1]
|
||||
left, right = v_sorted[0], v_sorted[-1]
|
||||
if length < 40:
|
||||
continue
|
||||
|
||||
def intersect(l1, l2):
|
||||
x1, y1, x2, y2 = l1
|
||||
x3, y3, x4, y4 = l2
|
||||
abs_angle = abs(angle)
|
||||
if abs_angle < 40 or abs_angle > 140:
|
||||
across_lines.append(seg)
|
||||
elif 50 < abs_angle < 130:
|
||||
along_lines.append(seg)
|
||||
|
||||
def merge_nearby(segs, position_key, threshold=30):
|
||||
"""Merge line segments that are close together (same court line)."""
|
||||
if not segs:
|
||||
return []
|
||||
|
||||
# Sort by mid-position perpendicular to the line direction
|
||||
def get_pos(s):
|
||||
if position_key == 'y':
|
||||
return (s[1] + s[3]) / 2
|
||||
return (s[0] + s[2]) / 2
|
||||
|
||||
sorted_segs = sorted(segs, key=get_pos)
|
||||
groups = []
|
||||
current_group = [sorted_segs[0]]
|
||||
|
||||
for seg in sorted_segs[1:]:
|
||||
if abs(get_pos(seg) - get_pos(current_group[-1])) < threshold:
|
||||
current_group.append(seg)
|
||||
else:
|
||||
groups.append(current_group)
|
||||
current_group = [seg]
|
||||
groups.append(current_group)
|
||||
|
||||
# Merge each group into one representative line
|
||||
merged = []
|
||||
for group in groups:
|
||||
all_pts = []
|
||||
for s in group:
|
||||
all_pts.extend([(s[0], s[1]), (s[2], s[3])])
|
||||
all_pts = np.array(all_pts)
|
||||
|
||||
# Fit a line through all points
|
||||
if len(all_pts) >= 2:
|
||||
vx, vy, cx, cy = cv2.fitLine(all_pts, cv2.DIST_L2, 0, 0.01, 0.01).flatten()
|
||||
# Extend line across frame
|
||||
t_max = max(np.sqrt((p[0] - cx) ** 2 + (p[1] - cy) ** 2) for p in all_pts)
|
||||
x1, y1 = int(cx - vx * t_max), int(cy - vy * t_max)
|
||||
x2, y2 = int(cx + vx * t_max), int(cy + vy * t_max)
|
||||
mid_pos = get_pos(group[0])
|
||||
merged.append({
|
||||
'endpoints': [x1, y1, x2, y2],
|
||||
'mid_pos': mid_pos,
|
||||
'count': len(group),
|
||||
})
|
||||
|
||||
return merged
|
||||
|
||||
grouped = {
|
||||
'across': merge_nearby(across_lines, 'y'),
|
||||
'along': merge_nearby(along_lines, 'x'),
|
||||
}
|
||||
|
||||
return {'segments': segments, 'grouped': grouped}
|
||||
|
||||
|
||||
def _match_court_lines(grouped, side, frame_w, frame_h):
|
||||
"""Try to match detected line groups to known court lines.
|
||||
|
||||
For a camera at the net looking at one half:
|
||||
- 'across' lines map to: baseline, kitchen line (and possibly net line)
|
||||
- 'along' lines map to: left sideline, right sideline, center service line
|
||||
|
||||
Returns dict with points_2d, points_3d for solvePnP, or error.
|
||||
"""
|
||||
across = grouped.get('across', [])
|
||||
along = grouped.get('along', [])
|
||||
|
||||
if len(across) < 2 or len(along) < 2:
|
||||
return {
|
||||
'points_2d': None, 'points_3d': None,
|
||||
'error': f'{len(across)} across + {len(along)} along lines (need 2+ each)',
|
||||
}
|
||||
|
||||
# Sort across lines by vertical position (top of frame = far from camera = baseline)
|
||||
across_sorted = sorted(across, key=lambda l: l['mid_pos'])
|
||||
|
||||
# Sort along lines by horizontal position
|
||||
along_sorted = sorted(along, key=lambda l: l['mid_pos'])
|
||||
|
||||
# Known court geometry for this half
|
||||
# Court: 13.4m x 6.1m, half = 6.7m
|
||||
# Kitchen (NVZ) = 2.13m from net
|
||||
if side == 'left':
|
||||
# Camera at net (X=6.7) looking toward baseline (X=0)
|
||||
# "across" lines (perpendicular to camera view):
|
||||
# - baseline at X=0
|
||||
# - kitchen at X=6.7-2.13=4.57
|
||||
# "along" lines (parallel to camera view):
|
||||
# - near sideline at Y=0
|
||||
# - far sideline at Y=6.1
|
||||
# - center service at Y=3.05
|
||||
across_3d_x = [0, 4.57] # baseline, kitchen
|
||||
along_3d_y = [0, 6.1] # near sideline, far sideline
|
||||
center_service_y = 3.05
|
||||
else:
|
||||
# Camera at net (X=6.7) looking toward baseline (X=13.4)
|
||||
across_3d_x = [13.4, 6.7 + 2.13] # baseline (far), kitchen
|
||||
along_3d_y = [0, 6.1]
|
||||
center_service_y = 3.05
|
||||
|
||||
# Match: take outermost across lines as baseline (farthest) and kitchen (closest)
|
||||
# In image: top across line = far = baseline, bottom = near = kitchen
|
||||
baseline_px = across_sorted[0] # topmost = farthest = baseline
|
||||
kitchen_px = across_sorted[-1] # bottommost = nearest = kitchen
|
||||
|
||||
# Take outermost along lines as sidelines
|
||||
left_sideline_px = along_sorted[0]
|
||||
right_sideline_px = along_sorted[-1]
|
||||
|
||||
# Find intersection points as 2D-3D correspondences
|
||||
def line_intersect(l1_ep, l2_ep):
|
||||
x1, y1, x2, y2 = l1_ep
|
||||
x3, y3, x4, y4 = l2_ep
|
||||
denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
|
||||
if abs(denom) < 1e-6:
|
||||
return None
|
||||
t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / denom
|
||||
return [x1 + t * (x2 - x1), y1 + t * (y2 - y1)]
|
||||
|
||||
corners = [
|
||||
intersect(top, left),
|
||||
intersect(top, right),
|
||||
intersect(bottom, right),
|
||||
intersect(bottom, left),
|
||||
]
|
||||
points_2d = []
|
||||
points_3d = []
|
||||
|
||||
if any(c is None for c in corners):
|
||||
return {'corners': None, 'error': 'Court lines are parallel, cannot find intersections'}
|
||||
# Baseline x left sideline
|
||||
pt = line_intersect(baseline_px['endpoints'], left_sideline_px['endpoints'])
|
||||
if pt:
|
||||
points_2d.append(pt)
|
||||
points_3d.append([across_3d_x[0], along_3d_y[0], 0])
|
||||
|
||||
return {'corners': np.array(corners, dtype=np.float32), 'error': None}
|
||||
# Baseline x right sideline
|
||||
pt = line_intersect(baseline_px['endpoints'], right_sideline_px['endpoints'])
|
||||
if pt:
|
||||
points_2d.append(pt)
|
||||
points_3d.append([across_3d_x[0], along_3d_y[1], 0])
|
||||
|
||||
# Kitchen x right sideline
|
||||
pt = line_intersect(kitchen_px['endpoints'], right_sideline_px['endpoints'])
|
||||
if pt:
|
||||
points_2d.append(pt)
|
||||
points_3d.append([across_3d_x[1], along_3d_y[1], 0])
|
||||
|
||||
# Kitchen x left sideline
|
||||
pt = line_intersect(kitchen_px['endpoints'], left_sideline_px['endpoints'])
|
||||
if pt:
|
||||
points_2d.append(pt)
|
||||
points_3d.append([across_3d_x[1], along_3d_y[0], 0])
|
||||
|
||||
# If we have a center service line, add more correspondences
|
||||
if len(along_sorted) >= 3:
|
||||
center_px = along_sorted[len(along_sorted) // 2]
|
||||
pt = line_intersect(baseline_px['endpoints'], center_px['endpoints'])
|
||||
if pt:
|
||||
points_2d.append(pt)
|
||||
points_3d.append([across_3d_x[0], center_service_y, 0])
|
||||
pt = line_intersect(kitchen_px['endpoints'], center_px['endpoints'])
|
||||
if pt:
|
||||
points_2d.append(pt)
|
||||
points_3d.append([across_3d_x[1], center_service_y, 0])
|
||||
|
||||
if len(points_2d) < 4:
|
||||
return {
|
||||
'points_2d': None, 'points_3d': None,
|
||||
'error': f'Only {len(points_2d)} intersection points found (need >= 4)',
|
||||
}
|
||||
|
||||
return {'points_2d': points_2d, 'points_3d': points_3d, 'error': None}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -509,16 +509,23 @@ function doCalibrate() {
|
||||
errEl.style.display = 'none';
|
||||
updateCalibrationStatus();
|
||||
|
||||
// Show computed camera positions
|
||||
// Show computed camera positions and line stats
|
||||
var posEl = document.getElementById('calPositions');
|
||||
if (posEl && data.result) {
|
||||
posEl.style.display = 'block';
|
||||
for (var sid in data.result) {
|
||||
var r = data.result[sid];
|
||||
var el = document.getElementById('calPos' + sid);
|
||||
if (!el) continue;
|
||||
if (r.ok && r.camera_position) {
|
||||
var p = r.camera_position;
|
||||
var el = document.getElementById('calPos' + sid);
|
||||
if (el) el.textContent = 'CAM' + sid + ': X=' + p[0].toFixed(2) + ' Y=' + p[1].toFixed(2) + ' Z=' + p[2].toFixed(2) + 'm';
|
||||
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';
|
||||
el.style.color = '#ff4444';
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user