diff --git a/jetson/main.py b/jetson/main.py index 68b9d4c..a974818 100644 --- a/jetson/main.py +++ b/jetson/main.py @@ -35,13 +35,9 @@ _args = None def auto_calibrate(): - """One-click calibration: detect court lines from current frames, - compute camera pose, save to config. - - Each camera sees one half of the court from the net position. - Detects court lines via Hough transform, finds 4 corners, - then uses solvePnP to determine camera position. - Returns debug images with detected lines drawn on them. + """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. """ results = {} @@ -55,74 +51,55 @@ def auto_calibrate(): side = 'left' if sensor_id == 0 else 'right' debug_frame = frame.copy() - # Detect court lines — returns corners + debug info + # Detect court rectangle — find 4 corners of the court detection = _detect_court_corners(frame, side) - - # Draw all detected Hough lines on debug frame - if detection and detection.get('all_lines') is not None: - for line in detection['all_lines']: - x1, y1, x2, y2 = line[0] - cv2.line(debug_frame, (x1, y1), (x2, y2), (50, 50, 50), 1) - - # Draw classified lines - if detection and detection.get('horizontals'): - for line in detection['horizontals']: - x1, y1, x2, y2 = line - cv2.line(debug_frame, (x1, y1), (x2, y2), (0, 255, 255), 2) # yellow = horizontal - if detection and detection.get('verticals'): - for line in detection['verticals']: - x1, y1, x2, y2 = line - cv2.line(debug_frame, (x1, y1), (x2, y2), (255, 0, 255), 2) # magenta = vertical - - # Draw selected 4 lines (top/bottom/left/right) - if detection and detection.get('selected_lines'): - sel = detection['selected_lines'] - colors = {'top': (0, 255, 0), 'bottom': (0, 200, 0), - 'left': (255, 128, 0), 'right': (200, 100, 0)} - for name, line in sel.items(): - x1, y1, x2, y2 = line - cv2.line(debug_frame, (x1, y1), (x2, y2), colors.get(name, (255, 255, 255)), 3) - cv2.putText(debug_frame, name, (x1, y1 - 5), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, colors.get(name, (255, 255, 255)), 1) - - # Encode debug frame - _, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85]) - debug_b64 = base64.b64encode(jpeg.tobytes()).decode('ascii') - corners_pixel = detection.get('corners') if detection else None if corners_pixel is None: - error_detail = detection.get('error', 'Unknown') if detection else 'No lines detected at all' + 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}', - 'debug_image': debug_b64, + 'debug_image': base64.b64encode(jpeg.tobytes()).decode('ascii'), } continue - # Draw corners on debug frame - for i, corner in enumerate(corners_pixel): - pt = (int(corner[0]), int(corner[1])) - cv2.circle(debug_frame, pt, 8, (0, 0, 255), -1) - cv2.putText(debug_frame, f'C{i}', (pt[0] + 10, pt[1]), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) + # 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) - # Re-encode with corners - _, jpeg = cv2.imencode('.jpg', debug_frame, [cv2.IMWRITE_JPEG_QUALITY, 85]) - debug_b64 = base64.b64encode(jpeg.tobytes()).decode('ascii') - - # Get known 3D coordinates for this half + # Get known 3D coordinates for this half-court corners_3d = get_half_court_3d_points(side) - # Calibrate — no try/except, let errors propagate + # Calibrate — errors propagate cal = CameraCalibrator() cal.calibrate( np.array(corners_pixel, dtype=np.float32), - corners_3d, - w, h + corners_3d, w, h ) - # Save to config + # 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) @@ -130,120 +107,81 @@ def auto_calibrate(): state['calibrators'][sensor_id] = cal - # Get camera position for 3D scene - cam_pos = (-cal.rotation_matrix.T @ cal.translation_vec).flatten() - 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]:.1f}, {cam_pos[1]:.1f}, {cam_pos[2]:.1f})") + f"({cam_pos[0]:.2f}, {cam_pos[1]:.2f}, {cam_pos[2]:.2f})") return results def _detect_court_corners(frame, side): - """Detect court corners from frame using edge detection. + """Detect 4 corners of the court rectangle from camera 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 or None - all_lines: raw Hough lines - horizontals: classified horizontal lines - verticals: classified vertical lines - selected_lines: the 4 lines used (top/bottom/left/right) - error: description if detection failed + corners: 4x2 numpy array (TL, TR, BR, BL) or None + error: description string if detection failed """ gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) blur = cv2.GaussianBlur(gray, (5, 5), 0) edges = cv2.Canny(blur, 50, 150) - # Detect lines lines = cv2.HoughLinesP(edges, 1, np.pi / 180, threshold=80, minLineLength=100, maxLineGap=20) if lines is None or len(lines) < 4: n = 0 if lines is None else len(lines) - return { - 'corners': None, 'all_lines': lines, - 'horizontals': [], 'verticals': [], - 'selected_lines': {}, - 'error': f'Only {n} Hough lines found (need >= 4)', - } + return {'corners': None, 'error': f'Found {n} lines, need at least 4'} - # Classify lines into horizontal and vertical + # Classify into horizontal / vertical horizontals = [] verticals = [] 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]) if len(horizontals) < 2 or len(verticals) < 2: - return { - 'corners': None, 'all_lines': lines, - 'horizontals': [h.tolist() for h in horizontals], - 'verticals': [v.tolist() for v in verticals], - 'selected_lines': {}, - 'error': f'{len(horizontals)} horizontal, {len(verticals)} vertical lines (need >= 2 each)', - } + return {'corners': None, + 'error': f'{len(horizontals)} horiz + {len(verticals)} vert lines, need 2+ each'} - # Cluster lines by position to find the dominant ones - h_positions = sorted(horizontals, key=lambda l: (l[1] + l[3]) / 2) - v_positions = sorted(verticals, key=lambda l: (l[0] + l[2]) / 2) + # 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) - top_line = h_positions[0] - bottom_line = h_positions[-1] - left_line = v_positions[0] - right_line = v_positions[-1] + top, bottom = h_sorted[0], h_sorted[-1] + left, right = v_sorted[0], v_sorted[-1] - selected = { - 'top': top_line.tolist(), 'bottom': bottom_line.tolist(), - 'left': left_line.tolist(), 'right': right_line.tolist(), - } - - # Find intersections as corner points - def line_intersection(l1, l2): + def intersect(l1, l2): x1, y1, x2, y2 = l1 x3, y3, x4, y4 = l2 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 - ix = x1 + t * (x2 - x1) - iy = y1 + t * (y2 - y1) - return [ix, iy] + return [x1 + t * (x2 - x1), y1 + t * (y2 - y1)] corners = [ - line_intersection(top_line, left_line), # TL - line_intersection(top_line, right_line), # TR - line_intersection(bottom_line, right_line), # BR - line_intersection(bottom_line, left_line), # BL + intersect(top, left), + intersect(top, right), + intersect(bottom, right), + intersect(bottom, left), ] if any(c is None for c in corners): - return { - 'corners': None, 'all_lines': lines, - 'horizontals': [h.tolist() for h in horizontals], - 'verticals': [v.tolist() for v in verticals], - 'selected_lines': selected, - 'error': 'Lines are parallel — could not find all 4 corner intersections', - } + return {'corners': None, 'error': 'Court lines are parallel, cannot find intersections'} - return { - 'corners': np.array(corners, dtype=np.float32), - 'all_lines': lines, - 'horizontals': [h.tolist() for h in horizontals], - 'verticals': [v.tolist() for v in verticals], - 'selected_lines': selected, - 'error': None, - } + return {'corners': np.array(corners, dtype=np.float32), 'error': None} diff --git a/src/web/templates/index.html b/src/web/templates/index.html index 55e03eb..722e5bd 100644 --- a/src/web/templates/index.html +++ b/src/web/templates/index.html @@ -331,13 +331,18 @@
Camera 1
Camera 0
-
+
Calibration
Not calibrated
+
@@ -437,6 +442,20 @@ function doCalibrate() { errEl.style.display = 'none'; updateCalibrationStatus(); + // Show computed camera positions + var posEl = document.getElementById('calPositions'); + if (posEl && data.result) { + posEl.style.display = 'block'; + for (var sid in data.result) { + var r = data.result[sid]; + 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'; + } + } + } + fetch('/api/calibration/data') .then(function(r) { return r.json(); }) .then(function(camData) { addCamerasToScene(camData); });