Show computed camera 3D position after calibration
- Debug image shows only court quadrilateral + camera XYZ coords - Remove all Hough line debug visualization noise - Simplify _detect_court_corners — returns corners + error only - Display camera positions in calibration card (CAM0/CAM1 X Y Z) - Clean up auto_calibrate flow Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
Ruslan Bakiev
178
jetson/main.py
178
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}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -331,13 +331,18 @@
|
||||
<div class="bottom-bar">
|
||||
<div class="bottom-card"><img id="cal-cam1" alt="Camera 1"></div>
|
||||
<div class="bottom-card"><img id="cal-cam0" alt="Camera 0"></div>
|
||||
<div class="cam-card">
|
||||
<div class="cam-card" style="width:auto;min-width:160px">
|
||||
<div class="cc-title">Calibration</div>
|
||||
<button class="btn-calibrate" id="btnCalibrate" onclick="doCalibrate()">Calibrate</button>
|
||||
<div class="calibrate-status" id="calStatus">
|
||||
<span id="calStatusText">Not calibrated</span>
|
||||
</div>
|
||||
<div id="calError" style="color:#ff4444;font-size:8px;word-break:break-all;display:none"></div>
|
||||
<div id="calPositions" style="display:none">
|
||||
<div class="cc-divider"></div>
|
||||
<div class="cc-item" id="calPos0" style="color:#4ecca3;font-size:8px"></div>
|
||||
<div class="cc-item" id="calPos1" style="color:#ff88cc;font-size:8px"></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="bottom-card" id="calDebug0" style="display:none"><img id="calDebugImg0" alt="CAM 0 debug"></div>
|
||||
<div class="bottom-card" id="calDebug1" style="display:none"><img id="calDebugImg1" alt="CAM 1 debug"></div>
|
||||
@@ -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); });
|
||||
|
||||
Reference in New Issue
Block a user