diff --git a/jetson/main.py b/jetson/main.py index 2e1ca02..fbc330d 100644 --- a/jetson/main.py +++ b/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): diff --git a/src/calibration/camera_calibrator.py b/src/calibration/camera_calibrator.py index 0dc5984..f7e0c1e 100644 --- a/src/calibration/camera_calibrator.py +++ b/src/calibration/camera_calibrator.py @@ -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. diff --git a/src/web/templates/index.html b/src/web/templates/index.html index 871d6f4..9d67419 100644 --- a/src/web/templates/index.html +++ b/src/web/templates/index.html @@ -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'; } }