Callibration Board 0
This commit is contained in:
chk
@@ -253,19 +253,22 @@
|
||||
<div class="sections">
|
||||
|
||||
<div class="section full">
|
||||
<h2>Board <span class="status-badge open">offen</span></h2>
|
||||
<div class="placeholder-note">
|
||||
Ziel: Extrinsische Position des Marker-Boards im Kamera-Koordinatensystem bestimmen
|
||||
(Rotations- und Translationsvektor via <code>solvePnP</code>).<br><br>
|
||||
Geplante Aktionen: Kamerabild mit erkannten Markern anzeigen · Pose berechnen ·
|
||||
Kalibrierungsdatei speichern.<br><br>
|
||||
<em>Aktionen werden ergänzt sobald das Konzept feststeht.</em>
|
||||
<h2>Board – ArUco & Kamera-Pose</h2>
|
||||
<div class="info-grid" style="margin-top: 14px;">
|
||||
<span class="info-label">Ablauf</span>
|
||||
<span class="info-value" style="font-family: inherit; font-size: 13px; color: var(--muted);">
|
||||
Foto aufnehmen → ArUco erkennen → Kamera-Pose schätzen
|
||||
</span>
|
||||
<span class="info-label">Schritte</span>
|
||||
<span class="info-value" style="font-family: inherit; font-size: 13px; color: var(--muted);">
|
||||
1_detect_aruco_observations → 2_estimate_camera_from_observations
|
||||
</span>
|
||||
<span class="info-label">Letzter Run</span>
|
||||
<span class="info-value" id="board-last-run">–</span>
|
||||
</div>
|
||||
<div class="controls" style="margin-top: 14px;">
|
||||
<button disabled>Foto aufnehmen</button>
|
||||
<button disabled>Marker erkennen</button>
|
||||
<button disabled>Pose berechnen</button>
|
||||
<button disabled>Board-Pose speichern</button>
|
||||
<div class="controls" style="margin-top: 16px;">
|
||||
<button id="btn-board-run">Board erkennen</button>
|
||||
<button disabled title="Folgt später">Ergebnis anzeigen</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
@@ -543,6 +546,78 @@
|
||||
btn.disabled = false;
|
||||
}
|
||||
});
|
||||
|
||||
// ── Board ──────────────────────────────────────────────────────────────────
|
||||
|
||||
const logBoard = document.getElementById('log-board');
|
||||
|
||||
function logB(msg) {
|
||||
const ts = new Date().toLocaleTimeString('de-CH');
|
||||
logBoard.value += `[${ts}] ${msg}\n`;
|
||||
logBoard.scrollTop = logBoard.scrollHeight;
|
||||
}
|
||||
|
||||
// SSE-Stream lesen (gleiche Logik wie compute)
|
||||
async function readSseStream(response, logFn, onDone) {
|
||||
const reader = response.body.getReader();
|
||||
const decoder = new TextDecoder();
|
||||
let buffer = '';
|
||||
while (true) {
|
||||
const { done, value } = await reader.read();
|
||||
if (done) break;
|
||||
buffer += decoder.decode(value, { stream: true });
|
||||
const parts = buffer.split('\n\n');
|
||||
buffer = parts.pop();
|
||||
for (const part of parts) {
|
||||
for (const line of part.split('\n')) {
|
||||
if (!line.startsWith('data: ')) continue;
|
||||
try {
|
||||
const evt = JSON.parse(line.slice(6));
|
||||
if (evt.type === 'log') {
|
||||
if (evt.text !== '') logFn(evt.text);
|
||||
} else if (evt.type === 'done') {
|
||||
onDone(evt);
|
||||
}
|
||||
} catch { /* ignore */ }
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
document.getElementById('btn-board-run').addEventListener('click', async () => {
|
||||
logB('Board-Erkennung wird gestartet …');
|
||||
const btn = document.getElementById('btn-board-run');
|
||||
btn.disabled = true;
|
||||
|
||||
try {
|
||||
const response = await fetch('/api/board/run', { method: 'POST' });
|
||||
|
||||
if (!response.ok) {
|
||||
const raw = await response.text().catch(() => '');
|
||||
let msg;
|
||||
try { msg = JSON.parse(raw).error || raw; }
|
||||
catch { msg = raw.slice(0, 300) || `HTTP ${response.status}`; }
|
||||
logB(`❌ HTTP ${response.status}: ${msg}`);
|
||||
return;
|
||||
}
|
||||
|
||||
await readSseStream(response, logB, (evt) => {
|
||||
if (evt.exitCode === 0) {
|
||||
logB('✅ Board-Run abgeschlossen.');
|
||||
if (evt.runDir) {
|
||||
document.getElementById('board-last-run').textContent = evt.runDir;
|
||||
}
|
||||
} else {
|
||||
logB(`❌ Beendet mit Exit-Code ${evt.exitCode}`);
|
||||
}
|
||||
});
|
||||
|
||||
} catch (err) {
|
||||
logB(`❌ Fehler: ${err}`);
|
||||
} finally {
|
||||
btn.disabled = false;
|
||||
}
|
||||
});
|
||||
</script>
|
||||
|
||||
</body>
|
||||
|
||||
636
scripts/1_detect_aruco_observations.py
Normal file
636
scripts/1_detect_aruco_observations.py
Normal file
@@ -0,0 +1,636 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import hashlib
|
||||
import time
|
||||
import uuid
|
||||
from typing import Dict, Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
# Utilities
|
||||
# ------------------------------------------------------------
|
||||
|
||||
|
||||
def resolve_path(path):
|
||||
path = os.path.expanduser(path)
|
||||
|
||||
# Absoluter Pfad → direkt verwenden
|
||||
if os.path.isabs(path):
|
||||
return path
|
||||
|
||||
# Relativer Pfad → absolut machen (auf Basis aktuellem cwd)
|
||||
return os.path.abspath(path)
|
||||
|
||||
def load_intrinsics_npz(npz_path: str):
|
||||
data = np.load(npz_path)
|
||||
|
||||
for k in ('camera_matrix', 'mtx', 'K'):
|
||||
if k in data:
|
||||
K = data[k].astype(np.float32)
|
||||
break
|
||||
else:
|
||||
raise KeyError('Camera matrix not found in npz')
|
||||
|
||||
for k in ('dist_coeffs', 'dist', 'D'):
|
||||
if k in data:
|
||||
D = data[k].astype(np.float32).reshape(-1, 1)
|
||||
break
|
||||
else:
|
||||
D = np.zeros((5, 1), dtype=np.float32)
|
||||
|
||||
return K, D
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def load_robot_vision_config(robot_json_path: str):
|
||||
|
||||
|
||||
|
||||
robot_json_path = resolve_path(robot_json_path)
|
||||
with open(robot_json_path, 'r', encoding='utf-8') as f:
|
||||
robot = json.load(f)
|
||||
|
||||
vision_config = robot.get('vision_config', {})
|
||||
|
||||
marker_type = vision_config.get('MarkerType', 'DICT_4X4_250')
|
||||
marker_size = float(vision_config.get('MarkerSize', 0.025))
|
||||
|
||||
return {
|
||||
'MarkerType': marker_type,
|
||||
'MarkerSize': marker_size
|
||||
}
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def get_aruco_detector(dict_name: str):
|
||||
|
||||
mapping = {
|
||||
'DICT_4X4_250': cv2.aruco.DICT_4X4_250,
|
||||
'DICT_5X5_100': cv2.aruco.DICT_5X5_100,
|
||||
'DICT_6X6_250': cv2.aruco.DICT_6X6_250,
|
||||
'DICT_ARUCO_ORIGINAL': cv2.aruco.DICT_ARUCO_ORIGINAL,
|
||||
}
|
||||
|
||||
dict_id = mapping.get(dict_name, cv2.aruco.DICT_4X4_250)
|
||||
|
||||
dictionary = cv2.aruco.getPredefinedDictionary(dict_id)
|
||||
|
||||
try:
|
||||
params = cv2.aruco.DetectorParameters()
|
||||
except Exception:
|
||||
params = cv2.aruco.DetectorParameters_create()
|
||||
|
||||
try:
|
||||
detector = cv2.aruco.ArucoDetector(dictionary, params)
|
||||
return detector, None
|
||||
|
||||
except Exception:
|
||||
return None, (dictionary, params)
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def detect_markers(image, detector_tuple):
|
||||
|
||||
detector, fallback = detector_tuple
|
||||
|
||||
if detector is not None:
|
||||
|
||||
corners, ids, rejected = detector.detectMarkers(image)
|
||||
|
||||
else:
|
||||
|
||||
dictionary, params = fallback
|
||||
|
||||
corners, ids, rejected = cv2.aruco.detectMarkers(
|
||||
image,
|
||||
dictionary,
|
||||
parameters=params
|
||||
)
|
||||
|
||||
return corners, ids, rejected
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def hash_file(path):
|
||||
|
||||
sha = hashlib.sha256()
|
||||
|
||||
with open(path, 'rb') as f:
|
||||
|
||||
while True:
|
||||
|
||||
chunk = f.read(1024 * 1024)
|
||||
|
||||
if not chunk:
|
||||
break
|
||||
|
||||
sha.update(chunk)
|
||||
|
||||
return sha.hexdigest()
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def polygon_mask(shape, polygon):
|
||||
|
||||
mask = np.zeros(shape, dtype=np.uint8)
|
||||
|
||||
cv2.fillConvexPoly(
|
||||
mask,
|
||||
polygon.astype(np.int32),
|
||||
255
|
||||
)
|
||||
|
||||
return mask
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def shrink_polygon(points, scale=0.80):
|
||||
|
||||
center = np.mean(points, axis=0)
|
||||
|
||||
shrunk = center + (points - center) * scale
|
||||
|
||||
return shrunk.astype(np.float32)
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def compute_sharpness(gray_image, polygon):
|
||||
|
||||
shrunk = shrink_polygon(polygon, scale=0.80)
|
||||
|
||||
mask = polygon_mask(gray_image.shape, shrunk)
|
||||
|
||||
pixels = gray_image[mask == 255]
|
||||
|
||||
if pixels.size == 0:
|
||||
return 0.0
|
||||
|
||||
temp = np.zeros_like(gray_image)
|
||||
temp[mask == 255] = gray_image[mask == 255]
|
||||
|
||||
lap = cv2.Laplacian(temp, cv2.CV_64F)
|
||||
|
||||
values = lap[mask == 255]
|
||||
|
||||
if values.size == 0:
|
||||
return 0.0
|
||||
|
||||
return float(values.var())
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def compute_contrast(gray_image, polygon):
|
||||
|
||||
shrunk = shrink_polygon(polygon, scale=0.80)
|
||||
|
||||
mask = polygon_mask(gray_image.shape, shrunk)
|
||||
|
||||
pixels = gray_image[mask == 255]
|
||||
|
||||
if pixels.size == 0:
|
||||
|
||||
return {
|
||||
'p05': 0.0,
|
||||
'p95': 0.0,
|
||||
'dynamic_range': 0.0,
|
||||
'mean_gray': 0.0,
|
||||
'std_gray': 0.0
|
||||
}
|
||||
|
||||
p05 = float(np.percentile(pixels, 5))
|
||||
p95 = float(np.percentile(pixels, 95))
|
||||
|
||||
return {
|
||||
'p05': p05,
|
||||
'p95': p95,
|
||||
'dynamic_range': float(p95 - p05),
|
||||
'mean_gray': float(np.mean(pixels)),
|
||||
'std_gray': float(np.std(pixels))
|
||||
}
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def compute_edge_ratio(corners):
|
||||
|
||||
edge_lengths = []
|
||||
|
||||
for k in range(4):
|
||||
|
||||
p1 = corners[k]
|
||||
p2 = corners[(k + 1) % 4]
|
||||
|
||||
edge_lengths.append(
|
||||
float(np.linalg.norm(p1 - p2))
|
||||
)
|
||||
|
||||
edge_ratio = (
|
||||
max(edge_lengths) /
|
||||
max(1e-6, min(edge_lengths))
|
||||
)
|
||||
|
||||
return edge_ratio, edge_lengths
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def compute_geometry_metrics(center, corners, width, height):
|
||||
|
||||
image_center = np.array(
|
||||
[width / 2.0, height / 2.0],
|
||||
dtype=np.float32
|
||||
)
|
||||
|
||||
dist_center = np.linalg.norm(center - image_center)
|
||||
|
||||
max_dist = np.linalg.norm(image_center)
|
||||
|
||||
distance_center_norm = float(
|
||||
dist_center / max(1e-6, max_dist)
|
||||
)
|
||||
|
||||
min_x = np.min(corners[:, 0])
|
||||
max_x = np.max(corners[:, 0])
|
||||
|
||||
min_y = np.min(corners[:, 1])
|
||||
max_y = np.max(corners[:, 1])
|
||||
|
||||
border_distance_px = float(min(
|
||||
min_x,
|
||||
min_y,
|
||||
width - max_x,
|
||||
height - max_y
|
||||
))
|
||||
|
||||
return {
|
||||
'distance_to_center_norm': distance_center_norm,
|
||||
'distance_to_border_px': border_distance_px
|
||||
}
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def compute_confidence(
|
||||
area_px,
|
||||
sharpness,
|
||||
edge_ratio,
|
||||
dynamic_range,
|
||||
border_distance_px
|
||||
):
|
||||
|
||||
score = 1.0
|
||||
|
||||
# area
|
||||
score *= min(1.0, area_px / 1500.0)
|
||||
|
||||
# sharpness
|
||||
score *= min(1.0, sharpness / 120.0)
|
||||
|
||||
# edge distortion
|
||||
score *= 1.0 / max(1.0, edge_ratio)
|
||||
|
||||
# contrast
|
||||
score *= min(1.0, dynamic_range / 80.0)
|
||||
|
||||
# border distance
|
||||
score *= min(1.0, max(0.0, border_distance_px) / 50.0)
|
||||
|
||||
score = max(0.0, min(1.0, score))
|
||||
|
||||
return float(score)
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
def main():
|
||||
|
||||
parser = argparse.ArgumentParser()
|
||||
|
||||
parser.add_argument(
|
||||
'-i',
|
||||
'--image',
|
||||
required=True
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'-npz',
|
||||
'--intrinsics',
|
||||
required=True
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'-robot',
|
||||
'--robot',
|
||||
required=True
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'-cameraId',
|
||||
'--cameraId',
|
||||
required=True,
|
||||
type=str
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'-outDir',
|
||||
'--outDir',
|
||||
required=True
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'--saveDebugImage',
|
||||
action='store_true',
|
||||
help='Speichert ein Debug-JPG mit eingezeichneten Marker-Rahmen'
|
||||
)
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
out_dir = resolve_path(args.outDir)
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Load robot vision config
|
||||
# --------------------------------------------------------
|
||||
|
||||
vision_config = load_robot_vision_config(args.robot)
|
||||
|
||||
marker_type = vision_config['MarkerType']
|
||||
marker_size = vision_config['MarkerSize']
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Load image
|
||||
# --------------------------------------------------------
|
||||
|
||||
|
||||
image_path = resolve_path(args.image)
|
||||
image = cv2.imread(image_path)
|
||||
|
||||
|
||||
if image is None:
|
||||
raise RuntimeError(f'Cannot read image: {args.image}')
|
||||
|
||||
gray = cv2.cvtColor(
|
||||
image,
|
||||
cv2.COLOR_BGR2GRAY
|
||||
)
|
||||
|
||||
height, width = gray.shape[:2]
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Intrinsics
|
||||
# --------------------------------------------------------
|
||||
|
||||
|
||||
intrinsics_path = resolve_path(args.intrinsics)
|
||||
K, D = load_intrinsics_npz(intrinsics_path)
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Detection
|
||||
# --------------------------------------------------------
|
||||
|
||||
detector_tuple = get_aruco_detector(marker_type)
|
||||
|
||||
corners_list, ids, rejected = detect_markers(
|
||||
gray,
|
||||
detector_tuple
|
||||
)
|
||||
|
||||
# ids_raw: original numpy array für drawDetectedMarkers
|
||||
ids_raw = ids
|
||||
|
||||
detections = []
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Valid detections
|
||||
# --------------------------------------------------------
|
||||
|
||||
if ids is not None:
|
||||
|
||||
ids = ids.flatten().tolist()
|
||||
|
||||
for i, marker_id in enumerate(ids):
|
||||
|
||||
corners = corners_list[i].reshape((4, 2)).astype(np.float32)
|
||||
|
||||
center = corners.mean(axis=0)
|
||||
|
||||
area_px = float(
|
||||
cv2.contourArea(corners)
|
||||
)
|
||||
|
||||
perimeter_px = float(
|
||||
cv2.arcLength(corners, True)
|
||||
)
|
||||
|
||||
edge_ratio, edge_lengths = compute_edge_ratio(corners)
|
||||
|
||||
sharpness = compute_sharpness(
|
||||
gray,
|
||||
corners
|
||||
)
|
||||
|
||||
contrast = compute_contrast(
|
||||
gray,
|
||||
corners
|
||||
)
|
||||
|
||||
geometry = compute_geometry_metrics(
|
||||
center,
|
||||
corners,
|
||||
width,
|
||||
height
|
||||
)
|
||||
|
||||
confidence = compute_confidence(
|
||||
area_px=area_px,
|
||||
sharpness=sharpness,
|
||||
edge_ratio=edge_ratio,
|
||||
dynamic_range=contrast['dynamic_range'],
|
||||
border_distance_px=geometry['distance_to_border_px']
|
||||
)
|
||||
|
||||
detection = {
|
||||
|
||||
'observation_id': str(uuid.uuid4()),
|
||||
|
||||
'type': 'aruco',
|
||||
|
||||
'marker_id': int(marker_id),
|
||||
|
||||
'marker_size_m': marker_size,
|
||||
|
||||
'image_points_px': corners.tolist(),
|
||||
|
||||
'center_px': center.tolist(),
|
||||
|
||||
'quality': {
|
||||
|
||||
'area_px': area_px,
|
||||
|
||||
'perimeter_px': perimeter_px,
|
||||
|
||||
'sharpness': {
|
||||
'laplacian_var': sharpness
|
||||
},
|
||||
|
||||
'contrast': contrast,
|
||||
|
||||
'geometry': geometry,
|
||||
|
||||
'edge_ratio': edge_ratio,
|
||||
|
||||
'edge_lengths_px': edge_lengths
|
||||
},
|
||||
|
||||
'confidence': confidence
|
||||
}
|
||||
|
||||
detections.append(detection)
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Rejected candidates
|
||||
# --------------------------------------------------------
|
||||
|
||||
rejected_candidates = []
|
||||
|
||||
if rejected is not None:
|
||||
|
||||
for candidate in rejected:
|
||||
|
||||
pts = candidate.reshape((-1, 2)).astype(np.float32)
|
||||
|
||||
center = pts.mean(axis=0)
|
||||
|
||||
area_px = float(
|
||||
cv2.contourArea(pts)
|
||||
)
|
||||
|
||||
rejected_candidates.append({
|
||||
|
||||
'image_points_px': pts.tolist(),
|
||||
|
||||
'center_px': center.tolist(),
|
||||
|
||||
'area_px': area_px
|
||||
})
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Final output
|
||||
# --------------------------------------------------------
|
||||
|
||||
output = {
|
||||
|
||||
'schema_version': '1.0',
|
||||
|
||||
'created_utc': time.strftime(
|
||||
'%Y-%m-%dT%H:%M:%SZ',
|
||||
time.gmtime()
|
||||
),
|
||||
|
||||
'vision_config': {
|
||||
'MarkerType': marker_type,
|
||||
'MarkerSize': marker_size
|
||||
},
|
||||
|
||||
'camera': {
|
||||
|
||||
'camera_id': args.cameraId,
|
||||
|
||||
'intrinsics_file': os.path.abspath(args.intrinsics),
|
||||
|
||||
'camera_matrix': K.tolist(),
|
||||
|
||||
'distortion_coefficients': D.reshape(-1).tolist()
|
||||
},
|
||||
|
||||
'image': {
|
||||
|
||||
'image_file': os.path.abspath(args.image),
|
||||
|
||||
'image_sha256': hash_file(args.image),
|
||||
|
||||
'width_px': int(width),
|
||||
|
||||
'height_px': int(height)
|
||||
},
|
||||
|
||||
'aruco': {
|
||||
|
||||
'dictionary': marker_type,
|
||||
|
||||
'num_detected_markers': len(detections),
|
||||
|
||||
'num_rejected_candidates': len(rejected_candidates)
|
||||
},
|
||||
|
||||
'detections': detections,
|
||||
|
||||
'rejected_candidates': rejected_candidates
|
||||
}
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Output path
|
||||
# --------------------------------------------------------
|
||||
|
||||
input_filename = os.path.basename(args.image)
|
||||
|
||||
input_base = os.path.splitext(input_filename)[0]
|
||||
|
||||
out_json = os.path.join(
|
||||
out_dir,
|
||||
f'{input_base}_aruco_detection.json'
|
||||
)
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Save JSON
|
||||
# --------------------------------------------------------
|
||||
|
||||
with open(out_json, 'w', encoding='utf-8') as f:
|
||||
|
||||
json.dump(
|
||||
output,
|
||||
f,
|
||||
indent=2
|
||||
)
|
||||
|
||||
print(f'Saved: {out_json}')
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Debug-Bild mit Marker-Rahmen
|
||||
# --------------------------------------------------------
|
||||
|
||||
if args.saveDebugImage:
|
||||
|
||||
debug_img = image.copy()
|
||||
|
||||
if corners_list and ids_raw is not None:
|
||||
cv2.aruco.drawDetectedMarkers(debug_img, corners_list, ids_raw)
|
||||
|
||||
debug_path = os.path.join(
|
||||
out_dir,
|
||||
f'{input_base}_debug.jpg'
|
||||
)
|
||||
|
||||
cv2.imwrite(debug_path, debug_img)
|
||||
print(f'Saved debug: {debug_path}')
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
834
scripts/2_estimate_camera_from_observations.py
Normal file
834
scripts/2_estimate_camera_from_observations.py
Normal file
@@ -0,0 +1,834 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
2_estimate_camera_from_observations.py
|
||||
|
||||
Estimate a single camera pose from ArUco observations stored in
|
||||
*_aruco_detection.json, using marker world positions from robot.json.
|
||||
|
||||
This follows the same mathematical idea as readTwoImages.py:
|
||||
1) use detected marker observations,
|
||||
2) get an initial pose from a rigid transform,
|
||||
3) refine with Levenberg-Marquardt on normalized reprojection residuals.
|
||||
|
||||
Difference to readTwoImages.py:
|
||||
- No image processing here.
|
||||
- Input is the observation JSON created by 1_detect_aruco_observations.py.
|
||||
- Output is xxx_camera_pose.json.
|
||||
- Unknown marker reconstruction is intentionally omitted.
|
||||
|
||||
Assumptions:
|
||||
- robot.json contains a marker list for the board/world frame.
|
||||
- At minimum, marker positions are present for the reference markers.
|
||||
- The detection JSON contains camera intrinsics and marker corners.
|
||||
|
||||
Typical usage:
|
||||
python3 2_estimate_camera_from_observations.py \
|
||||
-i frame_0001_aruco_detection.json \
|
||||
-robot robot.json \
|
||||
-outDir results/
|
||||
|
||||
Output:
|
||||
frame_0001_camera_pose.json
|
||||
|
||||
Notes on uncertainty:
|
||||
- The script computes an approximate 6x6 covariance for the pose parameters
|
||||
[rvec_x, rvec_y, rvec_z, t_x, t_y, t_z].
|
||||
- It also propagates that covariance to camera center uncertainty in world
|
||||
coordinates and to approximate roll/pitch/yaw uncertainty.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from typing import Any, Dict, List, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Path / JSON helpers
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def resolve_path(path: str) -> str:
|
||||
path = os.path.expanduser(path)
|
||||
if os.path.isabs(path):
|
||||
return path
|
||||
return os.path.abspath(path)
|
||||
|
||||
|
||||
def load_json(path: str) -> Dict[str, Any]:
|
||||
with open(resolve_path(path), "r", encoding="utf-8") as f:
|
||||
return json.load(f)
|
||||
|
||||
|
||||
def save_json(path: str, data: Dict[str, Any]) -> None:
|
||||
with open(resolve_path(path), "w", encoding="utf-8") as f:
|
||||
json.dump(data, f, indent=2)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Intrinsics
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def load_intrinsics_from_detection(detection: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
Primary source: the embedded camera intrinsics in the detection JSON.
|
||||
"""
|
||||
camera = detection.get("camera", {})
|
||||
K = camera.get("camera_matrix", None)
|
||||
D = camera.get("distortion_coefficients", None)
|
||||
|
||||
if K is None:
|
||||
raise KeyError("camera_matrix missing in detection JSON.")
|
||||
if D is None:
|
||||
D = [0, 0, 0, 0, 0]
|
||||
|
||||
K = np.array(K, dtype=np.float32).reshape(3, 3)
|
||||
D = np.array(D, dtype=np.float32).reshape(-1, 1)
|
||||
return K, D
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Robot JSON parsing
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def _rotation_matrix_from_any(rotation: Any) -> np.ndarray:
|
||||
"""
|
||||
Best-effort parser for marker rotation.
|
||||
|
||||
Supported inputs:
|
||||
- 3x3 matrix as nested list
|
||||
- flat 9 list
|
||||
- dict with keys:
|
||||
* rotation_matrix / matrix
|
||||
* rvec / rodriques / rodrigues
|
||||
* euler_deg / rpy_deg / roll_pitch_yaw_deg
|
||||
* euler_rad / rpy_rad / roll_pitch_yaw_rad
|
||||
* quaternion / quat (best-effort, expects [x,y,z,w] unless specified)
|
||||
- None => identity
|
||||
|
||||
The pose estimator below only needs marker positions, but we keep
|
||||
this parser for completeness and future extension.
|
||||
"""
|
||||
if rotation is None:
|
||||
return np.eye(3, dtype=np.float32)
|
||||
|
||||
# Direct matrix
|
||||
if isinstance(rotation, (list, tuple, np.ndarray)):
|
||||
arr = np.array(rotation, dtype=np.float32)
|
||||
if arr.shape == (3, 3):
|
||||
return arr
|
||||
if arr.size == 9:
|
||||
return arr.reshape(3, 3).astype(np.float32)
|
||||
if arr.size == 3:
|
||||
# Treat as Rodrigues vector
|
||||
R, _ = cv2.Rodrigues(arr.reshape(3, 1))
|
||||
return R.astype(np.float32)
|
||||
return np.eye(3, dtype=np.float32)
|
||||
|
||||
if isinstance(rotation, dict):
|
||||
for key in ("rotation_matrix", "matrix"):
|
||||
if key in rotation:
|
||||
return _rotation_matrix_from_any(rotation[key])
|
||||
|
||||
for key in ("rvec", "rodrigues", "rodriques"):
|
||||
if key in rotation:
|
||||
v = np.array(rotation[key], dtype=np.float32).reshape(3, 1)
|
||||
R, _ = cv2.Rodrigues(v)
|
||||
return R.astype(np.float32)
|
||||
|
||||
def euler_to_R(roll: float, pitch: float, yaw: float, degrees: bool = True) -> np.ndarray:
|
||||
if degrees:
|
||||
roll = np.deg2rad(roll)
|
||||
pitch = np.deg2rad(pitch)
|
||||
yaw = np.deg2rad(yaw)
|
||||
cr, sr = np.cos(roll), np.sin(roll)
|
||||
cp, sp = np.cos(pitch), np.sin(pitch)
|
||||
cy, sy = np.cos(yaw), np.sin(yaw)
|
||||
|
||||
Rx = np.array([[1, 0, 0],
|
||||
[0, cr, -sr],
|
||||
[0, sr, cr]], dtype=np.float32)
|
||||
Ry = np.array([[cp, 0, sp],
|
||||
[0, 1, 0],
|
||||
[-sp, 0, cp]], dtype=np.float32)
|
||||
Rz = np.array([[cy, -sy, 0],
|
||||
[sy, cy, 0],
|
||||
[0, 0, 1]], dtype=np.float32)
|
||||
# ZYX convention
|
||||
return (Rz @ Ry @ Rx).astype(np.float32)
|
||||
|
||||
for key in ("euler_deg", "rpy_deg", "roll_pitch_yaw_deg"):
|
||||
if key in rotation:
|
||||
vals = np.array(rotation[key], dtype=np.float32).reshape(-1)
|
||||
if vals.size == 3:
|
||||
return euler_to_R(float(vals[0]), float(vals[1]), float(vals[2]), degrees=True)
|
||||
|
||||
for key in ("euler_rad", "rpy_rad", "roll_pitch_yaw_rad"):
|
||||
if key in rotation:
|
||||
vals = np.array(rotation[key], dtype=np.float32).reshape(-1)
|
||||
if vals.size == 3:
|
||||
return euler_to_R(float(vals[0]), float(vals[1]), float(vals[2]), degrees=False)
|
||||
|
||||
for key in ("quaternion", "quat"):
|
||||
if key in rotation:
|
||||
q = np.array(rotation[key], dtype=np.float32).reshape(-1)
|
||||
if q.size == 4:
|
||||
# Best-effort: try [x,y,z,w]
|
||||
x, y, z, w = [float(v) for v in q]
|
||||
R = np.array([
|
||||
[1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
|
||||
[2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w],
|
||||
[2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y]
|
||||
], dtype=np.float32)
|
||||
return R
|
||||
|
||||
return np.eye(3, dtype=np.float32)
|
||||
|
||||
|
||||
def get_marker_rotation(marker: Dict[str, Any]) -> np.ndarray:
|
||||
"""
|
||||
Flexible rotation extraction. Falls back to identity if absent.
|
||||
"""
|
||||
for key in ("rotation", "rotation_matrix", "matrix", "pose_rotation", "orientation"):
|
||||
if key in marker:
|
||||
return _rotation_matrix_from_any(marker[key])
|
||||
|
||||
# Also allow flat pose-style fields
|
||||
if "rvec" in marker or "rodrigues" in marker:
|
||||
return _rotation_matrix_from_any({"rvec": marker.get("rvec", marker.get("rodrigues"))})
|
||||
if "euler_deg" in marker:
|
||||
return _rotation_matrix_from_any({"euler_deg": marker["euler_deg"]})
|
||||
if "rpy_deg" in marker:
|
||||
return _rotation_matrix_from_any({"rpy_deg": marker["rpy_deg"]})
|
||||
if "quaternion" in marker:
|
||||
return _rotation_matrix_from_any({"quaternion": marker["quaternion"]})
|
||||
|
||||
return np.eye(3, dtype=np.float32)
|
||||
|
||||
|
||||
def load_marker_lookup(robot_json_path: str) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Supports the new format:
|
||||
robot_data["links"]["Board"]["markers"]
|
||||
|
||||
Fallback:
|
||||
robot_data["Marker"]
|
||||
"""
|
||||
robot_json_path = resolve_path(robot_json_path)
|
||||
with open(robot_json_path, "r", encoding="utf-8") as f:
|
||||
robot_data = json.load(f)
|
||||
|
||||
length_units = str(robot_data.get("units", {}).get("length", "")).strip().lower()
|
||||
length_scale = 1.0
|
||||
if length_units in ("mm", "millimeter", "millimeters"):
|
||||
length_scale = 1.0 / 1000.0
|
||||
elif length_units in ("cm", "centimeter", "centimeters"):
|
||||
length_scale = 1.0 / 100.0
|
||||
|
||||
marker_lookup: Dict[int, Dict[str, Any]] = {}
|
||||
|
||||
links = robot_data.get("links", {})
|
||||
board = links.get("Board")
|
||||
|
||||
markers = None
|
||||
if board and "markers" in board:
|
||||
markers = board["markers"]
|
||||
|
||||
if not markers:
|
||||
markers = robot_data.get("Marker", [])
|
||||
|
||||
for marker in markers:
|
||||
marker_id = int(marker.get("id", -1))
|
||||
if marker_id < 0:
|
||||
continue
|
||||
|
||||
if "position" not in marker:
|
||||
continue
|
||||
|
||||
pos = marker.get("position")
|
||||
if pos is None:
|
||||
continue
|
||||
|
||||
if len(pos) != 3:
|
||||
continue
|
||||
|
||||
rotation = get_marker_rotation(marker)
|
||||
|
||||
marker_lookup[marker_id] = {
|
||||
"position": np.array(pos, dtype=np.float32) * np.float32(length_scale),
|
||||
"rotation": rotation,
|
||||
"on": marker.get("on", "unknown"),
|
||||
}
|
||||
|
||||
return marker_lookup
|
||||
|
||||
|
||||
def load_robot_marker_size(robot_json_path: str) -> Optional[float]:
|
||||
"""
|
||||
Best-effort marker size reader from robot.json.
|
||||
Returns meters if found, otherwise None.
|
||||
"""
|
||||
robot_json_path = resolve_path(robot_json_path)
|
||||
with open(robot_json_path, "r", encoding="utf-8") as f:
|
||||
robot_data = json.load(f)
|
||||
|
||||
vision_config = robot_data.get("vision_config", {})
|
||||
size = vision_config.get("MarkerSize", None)
|
||||
if size is None:
|
||||
return None
|
||||
try:
|
||||
return float(size)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Geometry / pose helpers
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def marker_local_corners(marker_size_m: float) -> np.ndarray:
|
||||
half = marker_size_m / 2.0
|
||||
# Same corner order as the readTwoImages.py example
|
||||
return np.array([
|
||||
[-half, half, 0.0],
|
||||
[ half, half, 0.0],
|
||||
[ half, -half, 0.0],
|
||||
[-half, -half, 0.0],
|
||||
], dtype=np.float32)
|
||||
|
||||
|
||||
def rigid_transform_no_scale(A: np.ndarray, B: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
Find R, t such that B ≈ R A + t.
|
||||
A, B: Nx3
|
||||
"""
|
||||
assert A.shape == B.shape and A.shape[1] == 3, "A and B must be Nx3"
|
||||
N = A.shape[0]
|
||||
if N < 2:
|
||||
raise ValueError("Need at least 2 points; 3+ recommended.")
|
||||
|
||||
centroid_A = A.mean(axis=0)
|
||||
centroid_B = B.mean(axis=0)
|
||||
|
||||
AA = A - centroid_A
|
||||
BB = B - centroid_B
|
||||
|
||||
H = AA.T @ BB
|
||||
U, S, Vt = np.linalg.svd(H)
|
||||
R = Vt.T @ U.T
|
||||
|
||||
if np.linalg.det(R) < 0:
|
||||
Vt[-1, :] *= -1
|
||||
R = Vt.T @ U.T
|
||||
|
||||
t = centroid_B - R @ centroid_A
|
||||
return R.astype(np.float32), t.astype(np.float32)
|
||||
|
||||
|
||||
def undistort_to_normalized(points_px: np.ndarray, K: np.ndarray, D: np.ndarray) -> np.ndarray:
|
||||
pts = points_px.reshape(-1, 1, 2).astype(np.float32)
|
||||
und = cv2.undistortPoints(pts, K, D, P=None)
|
||||
return und.reshape(-1, 2).astype(np.float32)
|
||||
|
||||
|
||||
def rvec_to_R(rvec: np.ndarray) -> np.ndarray:
|
||||
R, _ = cv2.Rodrigues(rvec.reshape(3, 1))
|
||||
return R.astype(np.float32)
|
||||
|
||||
|
||||
def R_to_euler_zyx(R: np.ndarray) -> Tuple[float, float, float]:
|
||||
"""
|
||||
Return roll, pitch, yaw in degrees using ZYX convention.
|
||||
"""
|
||||
yaw = float(np.degrees(np.arctan2(R[1, 0], R[0, 0])))
|
||||
sp = np.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2)
|
||||
pitch = float(np.degrees(np.arctan2(-R[2, 0], sp)))
|
||||
roll = float(np.degrees(np.arctan2(R[2, 1], R[2, 2])))
|
||||
return roll, pitch, yaw
|
||||
|
||||
|
||||
def theta_to_camera_pose(theta: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""
|
||||
theta = [omega_x, omega_y, omega_z, t_x, t_y, t_z]
|
||||
Returns:
|
||||
R_wc, t_wc, camera_center_world
|
||||
"""
|
||||
omega = theta[0:3]
|
||||
t_wc = theta[3:6].reshape(3, 1).astype(np.float32)
|
||||
R_wc, _ = cv2.Rodrigues(omega.reshape(3, 1))
|
||||
R_wc = R_wc.astype(np.float32)
|
||||
R_cw = R_wc.T
|
||||
camera_center_world = (-R_cw @ t_wc).reshape(3)
|
||||
return R_wc, t_wc.reshape(3), camera_center_world
|
||||
|
||||
|
||||
def build_projection_matrix(K: np.ndarray, R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
return K @ np.hstack([R, t.reshape(3, 1)])
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# LM on normalized residuals (same style as readTwoImages.py)
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def pack_params(omega: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
return np.hstack([omega.reshape(3), t.reshape(3)]).astype(np.float64)
|
||||
|
||||
|
||||
def unpack_params(theta: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
omega = theta[0:3]
|
||||
t = theta[3:6]
|
||||
return omega, t
|
||||
|
||||
|
||||
def residuals_centers_normalized(theta: np.ndarray,
|
||||
X_world: np.ndarray,
|
||||
obs_norm: np.ndarray) -> np.ndarray:
|
||||
"""
|
||||
Residuals in normalized coordinates:
|
||||
obs_norm - project(R X_world + t)
|
||||
"""
|
||||
omega, t = unpack_params(theta)
|
||||
R_wc = cv2.Rodrigues(omega.reshape(3, 1))[0].astype(np.float64)
|
||||
X_cam = (R_wc @ X_world.T + t.reshape(3, 1)).T
|
||||
uv = X_cam[:, :2] / X_cam[:, 2:3]
|
||||
r = (obs_norm - uv).reshape(-1)
|
||||
return r
|
||||
|
||||
|
||||
def numerical_jacobian(f, theta: np.ndarray, eps: float, *args) -> Tuple[np.ndarray, np.ndarray]:
|
||||
r0 = f(theta, *args)
|
||||
m = r0.size
|
||||
n = theta.size
|
||||
J = np.zeros((m, n), dtype=np.float64)
|
||||
for k in range(n):
|
||||
th = theta.copy()
|
||||
th[k] += eps
|
||||
rk = f(th, *args)
|
||||
J[:, k] = (rk - r0) / eps
|
||||
return J, r0
|
||||
|
||||
|
||||
def lm_solve(theta0: np.ndarray,
|
||||
X_world: np.ndarray,
|
||||
obs_norm: np.ndarray,
|
||||
max_iter: int = 60,
|
||||
eps_jac: float = 1e-6,
|
||||
lambda_init: float = 1e-3) -> Tuple[np.ndarray, Dict[str, List[float]]]:
|
||||
lam = lambda_init
|
||||
theta = theta0.copy().astype(np.float64)
|
||||
history = {"iters": [], "rms": [], "lambda": []}
|
||||
|
||||
for it in range(max_iter):
|
||||
J, r = numerical_jacobian(residuals_centers_normalized, theta, eps_jac, X_world, obs_norm)
|
||||
rms = float(np.sqrt(np.mean(r * r))) if r.size else 0.0
|
||||
history["iters"].append(it)
|
||||
history["rms"].append(rms)
|
||||
history["lambda"].append(lam)
|
||||
|
||||
JTJ = J.T @ J
|
||||
g = J.T @ r
|
||||
H = JTJ + lam * np.eye(JTJ.shape[0], dtype=np.float64)
|
||||
|
||||
try:
|
||||
delta = -np.linalg.solve(H, g)
|
||||
except np.linalg.LinAlgError:
|
||||
delta, *_ = np.linalg.lstsq(H, -g, rcond=None)
|
||||
|
||||
theta_trial = theta + delta
|
||||
r_trial = residuals_centers_normalized(theta_trial, X_world, obs_norm)
|
||||
rms_trial = float(np.sqrt(np.mean(r_trial * r_trial))) if r_trial.size else rms
|
||||
|
||||
if rms_trial < rms:
|
||||
theta = theta_trial
|
||||
lam *= 0.5
|
||||
else:
|
||||
lam *= 2.0
|
||||
|
||||
if np.linalg.norm(delta) < 1e-10:
|
||||
break
|
||||
if abs(rms - rms_trial) < 1e-12:
|
||||
break
|
||||
|
||||
return theta, history
|
||||
|
||||
|
||||
def pose_covariance(theta: np.ndarray,
|
||||
X_world: np.ndarray,
|
||||
obs_norm: np.ndarray,
|
||||
eps_jac: float = 1e-6) -> Tuple[np.ndarray, float, np.ndarray]:
|
||||
"""
|
||||
Returns:
|
||||
cov_theta_6x6, sigma2, residual_vector
|
||||
"""
|
||||
J, r = numerical_jacobian(residuals_centers_normalized, theta, eps_jac, X_world, obs_norm)
|
||||
m = r.size
|
||||
n = theta.size
|
||||
dof = max(1, m - n)
|
||||
sigma2 = float((r @ r) / dof)
|
||||
|
||||
JTJ = J.T @ J
|
||||
cov = sigma2 * np.linalg.pinv(JTJ)
|
||||
return cov.astype(np.float64), sigma2, r
|
||||
|
||||
|
||||
def propagate_covariance(theta: np.ndarray,
|
||||
cov_theta: np.ndarray) -> Dict[str, Any]:
|
||||
"""
|
||||
Propagate pose covariance to camera center and Euler angle uncertainties.
|
||||
"""
|
||||
def camera_center_fn(th: np.ndarray) -> np.ndarray:
|
||||
_, _, c = theta_to_camera_pose(th)
|
||||
return c.astype(np.float64)
|
||||
|
||||
def euler_fn(th: np.ndarray) -> np.ndarray:
|
||||
R_wc, _, _ = theta_to_camera_pose(th)
|
||||
return np.array(R_to_euler_zyx(R_wc), dtype=np.float64) # deg
|
||||
|
||||
Jc, _ = numerical_jacobian(lambda th, *_: camera_center_fn(th), theta, 1e-6)
|
||||
cov_center = Jc @ cov_theta @ Jc.T
|
||||
|
||||
Je, _ = numerical_jacobian(lambda th, *_: euler_fn(th), theta, 1e-6)
|
||||
cov_euler = Je @ cov_theta @ Je.T
|
||||
|
||||
center_std_m = np.sqrt(np.maximum(0.0, np.diag(cov_center)))
|
||||
euler_std_deg = np.sqrt(np.maximum(0.0, np.diag(cov_euler)))
|
||||
|
||||
# Parameter std directly from covariance
|
||||
param_std = np.sqrt(np.maximum(0.0, np.diag(cov_theta)))
|
||||
rvec_std_deg = np.degrees(param_std[0:3])
|
||||
tvec_std_m = param_std[3:6]
|
||||
|
||||
return {
|
||||
"pose_covariance_6x6": cov_theta.tolist(),
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [float(x) for x in rvec_std_deg],
|
||||
"tvec_std_m": [float(x) for x in tvec_std_m],
|
||||
},
|
||||
"camera_center_std_m": [float(x) for x in center_std_m],
|
||||
"camera_center_std_mm": [float(x * 1000.0) for x in center_std_m],
|
||||
"orientation_std_deg": {
|
||||
"roll": float(euler_std_deg[0]),
|
||||
"pitch": float(euler_std_deg[1]),
|
||||
"yaw": float(euler_std_deg[2]),
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Marker processing
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def build_object_corners_from_world_position(position_m: np.ndarray,
|
||||
marker_size_m: float) -> np.ndarray:
|
||||
"""
|
||||
Marker corners in world coordinates, assuming the marker frame is aligned
|
||||
with the world frame and only translated to 'position_m'.
|
||||
|
||||
This is the direct analogue of readTwoImages.py using marker center positions.
|
||||
"""
|
||||
h = marker_size_m / 2.0
|
||||
local = np.array([
|
||||
[-h, h, 0.0],
|
||||
[ h, h, 0.0],
|
||||
[ h, -h, 0.0],
|
||||
[-h, -h, 0.0],
|
||||
], dtype=np.float32)
|
||||
return local + position_m.reshape(1, 3)
|
||||
|
||||
|
||||
def solve_single_marker_pose(corners_px: np.ndarray,
|
||||
K: np.ndarray,
|
||||
D: np.ndarray,
|
||||
marker_size_m: float) -> Optional[Tuple[np.ndarray, np.ndarray]]:
|
||||
obj = marker_local_corners(marker_size_m)
|
||||
success, rvec, tvec = cv2.solvePnP(
|
||||
obj,
|
||||
corners_px.astype(np.float32),
|
||||
K,
|
||||
D,
|
||||
flags=cv2.SOLVEPNP_IPPE_SQUARE
|
||||
)
|
||||
if not success:
|
||||
success, rvec, tvec = cv2.solvePnP(
|
||||
obj,
|
||||
corners_px.astype(np.float32),
|
||||
K,
|
||||
D,
|
||||
flags=cv2.SOLVEPNP_ITERATIVE
|
||||
)
|
||||
if not success:
|
||||
return None
|
||||
return rvec.reshape(3), tvec.reshape(3)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Main
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Estimate camera pose from ArUco observation JSON")
|
||||
parser.add_argument("-i", "--input", required=True, help="*_aruco_detection.json")
|
||||
parser.add_argument("-robot", "--robot", required=True, help="robot.json with board markers")
|
||||
parser.add_argument("-outDir", "--outDir", default=None, help="Optional output directory")
|
||||
parser.add_argument("--minConfidence", type=float, default=0.0,
|
||||
help="Skip detections below this confidence")
|
||||
parser.add_argument("--minCommonMarkers", type=int, default=3,
|
||||
help="Minimum number of world markers required")
|
||||
parser.add_argument("--maxRmsPx", type=float, default=None,
|
||||
help="Optional soft warning threshold for final reprojection RMS in pixels")
|
||||
parser.add_argument("--epsJac", type=float, default=1e-6, help="Finite-difference epsilon")
|
||||
args = parser.parse_args()
|
||||
|
||||
detection_path = resolve_path(args.input)
|
||||
robot_path = resolve_path(args.robot)
|
||||
|
||||
detection = load_json(detection_path)
|
||||
marker_lookup = load_marker_lookup(robot_path)
|
||||
|
||||
K, D = load_intrinsics_from_detection(detection)
|
||||
|
||||
robot_marker_size = load_robot_marker_size(robot_path)
|
||||
det_marker_size = detection.get("vision_config", {}).get("MarkerSize", None)
|
||||
if det_marker_size is not None:
|
||||
marker_size_m = float(det_marker_size)
|
||||
elif robot_marker_size is not None:
|
||||
marker_size_m = float(robot_marker_size)
|
||||
else:
|
||||
marker_size_m = 0.025
|
||||
|
||||
detections = detection.get("detections", [])
|
||||
if not isinstance(detections, list):
|
||||
raise TypeError("detection['detections'] must be a list")
|
||||
|
||||
used_ids: List[int] = []
|
||||
used_world_positions: List[np.ndarray] = []
|
||||
used_obs_centers_px: List[np.ndarray] = []
|
||||
used_obs_centers_norm: List[np.ndarray] = []
|
||||
used_marker_cam_centers: List[np.ndarray] = []
|
||||
used_marker_meta: List[Dict[str, Any]] = []
|
||||
|
||||
sanity_notes: List[str] = []
|
||||
|
||||
for det in detections:
|
||||
if det.get("type", "aruco") != "aruco":
|
||||
continue
|
||||
|
||||
marker_id = int(det.get("marker_id", -1))
|
||||
if marker_id < 0:
|
||||
continue
|
||||
|
||||
if marker_id not in marker_lookup:
|
||||
continue
|
||||
|
||||
confidence = float(det.get("confidence", 1.0))
|
||||
if confidence < args.minConfidence:
|
||||
continue
|
||||
|
||||
corners = det.get("image_points_px", None)
|
||||
if corners is None:
|
||||
continue
|
||||
|
||||
corners_px = np.array(corners, dtype=np.float32).reshape(4, 2)
|
||||
center_from_corners = corners_px.mean(axis=0)
|
||||
|
||||
center_px = np.array(det.get("center_px", center_from_corners), dtype=np.float32).reshape(2)
|
||||
center_delta = float(np.linalg.norm(center_from_corners - center_px))
|
||||
if center_delta > 0.75:
|
||||
sanity_notes.append(
|
||||
f"marker {marker_id}: center_px differs from corner-mean by {center_delta:.2f}px"
|
||||
)
|
||||
|
||||
pnp = solve_single_marker_pose(corners_px, K, D, marker_size_m)
|
||||
if pnp is None:
|
||||
continue
|
||||
|
||||
rvec_m, tvec_m = pnp
|
||||
world_pos = marker_lookup[marker_id]["position"].astype(np.float32)
|
||||
|
||||
used_ids.append(marker_id)
|
||||
used_world_positions.append(world_pos)
|
||||
used_obs_centers_px.append(center_from_corners.astype(np.float32))
|
||||
used_obs_centers_norm.append(undistort_to_normalized(center_from_corners.reshape(1, 2), K, D)[0])
|
||||
used_marker_cam_centers.append(tvec_m.astype(np.float32))
|
||||
used_marker_meta.append({
|
||||
"marker_id": marker_id,
|
||||
"confidence": confidence,
|
||||
"center_px": [float(center_from_corners[0]), float(center_from_corners[1])],
|
||||
"marker_size_m": marker_size_m,
|
||||
})
|
||||
|
||||
# Unique / deduplicate by marker_id while preserving order
|
||||
dedup: Dict[int, int] = {}
|
||||
uniq_ids: List[int] = []
|
||||
uniq_world_positions: List[np.ndarray] = []
|
||||
uniq_obs_px: List[np.ndarray] = []
|
||||
uniq_obs_norm: List[np.ndarray] = []
|
||||
uniq_cam_centers: List[np.ndarray] = []
|
||||
uniq_meta: List[Dict[str, Any]] = []
|
||||
|
||||
for idx, mid in enumerate(used_ids):
|
||||
if mid in dedup:
|
||||
continue
|
||||
dedup[mid] = idx
|
||||
uniq_ids.append(mid)
|
||||
uniq_world_positions.append(used_world_positions[idx])
|
||||
uniq_obs_px.append(used_obs_centers_px[idx])
|
||||
uniq_obs_norm.append(used_obs_centers_norm[idx])
|
||||
uniq_cam_centers.append(used_marker_cam_centers[idx])
|
||||
uniq_meta.append(used_marker_meta[idx])
|
||||
|
||||
if len(uniq_ids) < args.minCommonMarkers:
|
||||
raise RuntimeError(
|
||||
f"Need at least {args.minCommonMarkers} common markers; found {len(uniq_ids)}: {uniq_ids}"
|
||||
)
|
||||
|
||||
X_world = np.stack(uniq_world_positions, axis=0).astype(np.float64)
|
||||
obs_px = np.stack(uniq_obs_px, axis=0).astype(np.float64)
|
||||
obs_norm = np.stack(uniq_obs_norm, axis=0).astype(np.float64)
|
||||
marker_cam_centers = np.stack(uniq_cam_centers, axis=0).astype(np.float64)
|
||||
|
||||
# Initial pose from rigid transform of per-marker camera-frame centers to world positions
|
||||
# B ≈ R A + t -> world = R * camera + t
|
||||
R_cw_init, t_cw_init = rigid_transform_no_scale(marker_cam_centers, X_world)
|
||||
R_wc_init = R_cw_init.T
|
||||
t_wc_init = -(R_wc_init @ t_cw_init).reshape(3)
|
||||
|
||||
omega_init = cv2.Rodrigues(R_wc_init)[0].reshape(3)
|
||||
theta0 = pack_params(omega_init, t_wc_init)
|
||||
|
||||
theta_opt, hist = lm_solve(
|
||||
theta0=theta0,
|
||||
X_world=X_world,
|
||||
obs_norm=obs_norm,
|
||||
max_iter=60,
|
||||
eps_jac=args.epsJac,
|
||||
lambda_init=1e-3,
|
||||
)
|
||||
|
||||
R_wc, t_wc, camera_center_world = theta_to_camera_pose(theta_opt)
|
||||
|
||||
cov_theta, sigma2, residual_vec = pose_covariance(
|
||||
theta_opt, X_world, obs_norm, eps_jac=args.epsJac
|
||||
)
|
||||
propagated = propagate_covariance(theta_opt, cov_theta)
|
||||
|
||||
# Exact pixel-space reprojection statistics
|
||||
proj_pts, _ = cv2.projectPoints(
|
||||
X_world.reshape(-1, 1, 3).astype(np.float32),
|
||||
theta_opt[0:3].reshape(3, 1).astype(np.float32),
|
||||
theta_opt[3:6].reshape(3, 1).astype(np.float32),
|
||||
K,
|
||||
D,
|
||||
)
|
||||
proj_pts = proj_pts.reshape(-1, 2)
|
||||
reproj_err_px = np.linalg.norm(proj_pts - obs_px, axis=1)
|
||||
rms_px = float(np.sqrt(np.mean(reproj_err_px ** 2))) if reproj_err_px.size else 0.0
|
||||
median_px = float(np.median(reproj_err_px)) if reproj_err_px.size else 0.0
|
||||
max_px = float(np.max(reproj_err_px)) if reproj_err_px.size else 0.0
|
||||
|
||||
if args.maxRmsPx is not None and rms_px > args.maxRmsPx:
|
||||
print(f"[WARN] Final reprojection RMS is {rms_px:.3f}px (threshold {args.maxRmsPx:.3f}px).")
|
||||
|
||||
# Convert outputs
|
||||
roll, pitch, yaw = R_to_euler_zyx(R_wc)
|
||||
position_mm = (camera_center_world * 1000.0).astype(float).tolist()
|
||||
|
||||
# Reproject each used marker center for QA
|
||||
per_marker_results = []
|
||||
proj_pts_exact, _ = cv2.projectPoints(
|
||||
X_world.reshape(-1, 1, 3).astype(np.float32),
|
||||
theta_opt[0:3].reshape(3, 1).astype(np.float32),
|
||||
theta_opt[3:6].reshape(3, 1).astype(np.float32),
|
||||
K,
|
||||
D,
|
||||
)
|
||||
proj_pts_exact = proj_pts_exact.reshape(-1, 2)
|
||||
|
||||
for idx, mid in enumerate(uniq_ids):
|
||||
x = proj_pts_exact[idx]
|
||||
err = float(np.linalg.norm(x - obs_px[idx]))
|
||||
per_marker_results.append({
|
||||
"marker_id": int(mid),
|
||||
"observed_center_px": [float(obs_px[idx, 0]), float(obs_px[idx, 1])],
|
||||
"projected_center_px": [float(x[0]), float(x[1])],
|
||||
"reprojection_error_px": err,
|
||||
"confidence": float(uniq_meta[idx]["confidence"]),
|
||||
})
|
||||
|
||||
# Output directory
|
||||
in_base = os.path.splitext(os.path.basename(detection_path))[0]
|
||||
out_name = in_base.replace("_aruco_detection", "_camera_pose") + ".json"
|
||||
|
||||
if args.outDir is not None:
|
||||
out_dir = resolve_path(args.outDir)
|
||||
else:
|
||||
out_dir = os.path.dirname(detection_path) or "."
|
||||
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
out_json = os.path.join(out_dir, out_name)
|
||||
|
||||
output = {
|
||||
"schema_version": "1.0",
|
||||
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"source": {
|
||||
"detection_json": detection_path,
|
||||
"robot_json": robot_path,
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": detection.get("camera", {}).get("camera_id", "unknown"),
|
||||
"camera_matrix": K.tolist(),
|
||||
"distortion_coefficients": D.reshape(-1).tolist(),
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": float(marker_size_m),
|
||||
"num_used_markers": int(len(uniq_ids)),
|
||||
"used_marker_ids": [int(x) for x in uniq_ids],
|
||||
"history": hist,
|
||||
"residual_rms_px": float(rms_px),
|
||||
"residual_median_px": float(median_px),
|
||||
"residual_max_px": float(max_px),
|
||||
"sigma2_normalized": float(sigma2),
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": R_wc.tolist(),
|
||||
"translation_m": [float(x) for x in t_wc.tolist()],
|
||||
"rvec_rad": [float(x) for x in theta_opt[0:3].tolist()],
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [float(x) for x in camera_center_world.tolist()],
|
||||
"position_mm": [float(x) for x in position_mm],
|
||||
"orientation_deg": {
|
||||
"roll": float(roll),
|
||||
"pitch": float(pitch),
|
||||
"yaw": float(yaw),
|
||||
},
|
||||
},
|
||||
"uncertainty": propagated,
|
||||
},
|
||||
"observations": {
|
||||
"markers": per_marker_results,
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": sanity_notes,
|
||||
},
|
||||
}
|
||||
|
||||
save_json(out_json, output)
|
||||
print(f"[INFO] Saved camera pose JSON: {out_json}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except Exception as exc:
|
||||
print(f"[ERROR] {exc}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
1499
scripts/3_multiview_bundle_adjustment_v4.py
Normal file
1499
scripts/3_multiview_bundle_adjustment_v4.py
Normal file
File diff suppressed because it is too large
Load Diff
271
scripts/3_multiview_bundle_adjustment_v5_board_anchor_patch.py
Normal file
271
scripts/3_multiview_bundle_adjustment_v5_board_anchor_patch.py
Normal file
@@ -0,0 +1,271 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
3_board_anchor_patch.py
|
||||
-----------------------
|
||||
DROP-IN PATCH for 3_multiview_bundle_adjustment_v4.py
|
||||
|
||||
What it adds
|
||||
------------
|
||||
Board markers have KNOWN world positions (they are fixed to the physical board,
|
||||
which defines the coordinate frame). Letting the optimizer move them freely
|
||||
makes them drift, which degrades all arm-marker estimates.
|
||||
|
||||
This patch adds a PositionAnchorConstraint that pins each observed Board
|
||||
marker to its robot.json world position during bundle adjustment.
|
||||
|
||||
How to integrate
|
||||
----------------
|
||||
Copy the three sections labelled [PATCH – copy into v4] into the v4 file:
|
||||
|
||||
1. The PositionAnchorConstraint dataclass → after the JointAxisConstraint dataclass
|
||||
2. Update the Constraint type alias → replace the existing line
|
||||
3. The load_board_anchors() function → anywhere before main()
|
||||
4. The change to bundle_adjustment_residuals → add one branch inside the for-loop
|
||||
5. The three lines in main() → just before "STEP 4"
|
||||
|
||||
Quick-test without editing v4
|
||||
------------------------------
|
||||
You can also run this file standalone — it imports from v4 and patches it at
|
||||
runtime. Then call main_anchored() instead of main() for the patched run.
|
||||
|
||||
python 3_board_anchor_patch.py -det ... -pose ... -robot ... -outDir ...
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
# [PATCH section 1 – copy into v4 after JointAxisConstraint]
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
from dataclasses import dataclass
|
||||
import numpy as np
|
||||
from typing import Dict, Any, List, Optional, Tuple
|
||||
|
||||
|
||||
@dataclass
|
||||
class PositionAnchorConstraint:
|
||||
"""
|
||||
Pins a single marker to a known world-space position.
|
||||
Used to anchor Board markers whose world positions are read from robot.json.
|
||||
"""
|
||||
marker_id: int
|
||||
target_world_m: np.ndarray # shape (3,), in metres
|
||||
weight: float = 100.0
|
||||
source: str = "board_anchor"
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
# [PATCH section 2 – replace the Constraint type alias in v4]
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
# OLD: Constraint = MarkerDistanceConstraint | JointAxisConstraint
|
||||
# NEW:
|
||||
# from 3_board_anchor_patch import PositionAnchorConstraint
|
||||
# Constraint = MarkerDistanceConstraint | JointAxisConstraint | PositionAnchorConstraint
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
# [PATCH section 3 – new function, copy anywhere before main()]
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
|
||||
def load_board_anchors(
|
||||
robot_data: Dict[str, Any],
|
||||
length_scale: float,
|
||||
weight: float = 100.0,
|
||||
) -> List[PositionAnchorConstraint]:
|
||||
"""
|
||||
Build PositionAnchorConstraints for every marker on the 'Board' link.
|
||||
|
||||
Board is the root link; its marker positions are in the Board (world) frame.
|
||||
length_scale = 1/1000 when robot.json uses mm (the standard).
|
||||
|
||||
Parameters
|
||||
----------
|
||||
robot_data : loaded robot.json
|
||||
length_scale : float (1/1000 to convert mm → m)
|
||||
weight : constraint weight (100 ≈ 1 mm anchor tolerance at λ=100)
|
||||
"""
|
||||
links = robot_data.get("links", {}) or {}
|
||||
board = links.get("Board", {}) or {}
|
||||
anchors: List[PositionAnchorConstraint] = []
|
||||
|
||||
for m in board.get("markers", []):
|
||||
mid = int(m.get("id", -1))
|
||||
pos = m.get("position", None)
|
||||
if mid < 0 or pos is None or len(pos) != 3:
|
||||
continue
|
||||
world_m = np.array(pos, dtype=np.float64) * float(length_scale)
|
||||
anchors.append(
|
||||
PositionAnchorConstraint(
|
||||
marker_id=mid,
|
||||
target_world_m=world_m,
|
||||
weight=weight,
|
||||
source="board_anchor",
|
||||
)
|
||||
)
|
||||
|
||||
return anchors
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
# [PATCH section 4 – add this branch inside bundle_adjustment_residuals,
|
||||
# in the "for constraint in constraints:" loop, after the JointAxisConstraint branch]
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
|
||||
def _anchor_residuals(constraint: PositionAnchorConstraint,
|
||||
marker_dict: Dict[int, np.ndarray],
|
||||
lambda_constraint: float) -> List[float]:
|
||||
"""
|
||||
Returns 3 residual components w·λ·(X_observed - X_target) for each axis.
|
||||
|
||||
Paste the body of this function into bundle_adjustment_residuals like this:
|
||||
|
||||
elif isinstance(constraint, PositionAnchorConstraint):
|
||||
if constraint.marker_id in marker_dict:
|
||||
diff = marker_dict[constraint.marker_id] - constraint.target_world_m
|
||||
for d in diff:
|
||||
residuals.append(float(d) * constraint.weight * lambda_constraint)
|
||||
"""
|
||||
residuals: List[float] = []
|
||||
if constraint.marker_id in marker_dict:
|
||||
diff = marker_dict[constraint.marker_id] - constraint.target_world_m
|
||||
for d in diff:
|
||||
residuals.append(float(d) * constraint.weight * lambda_constraint)
|
||||
return residuals
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
# [PATCH section 5 – add these lines in main(), just BEFORE
|
||||
# "print('\n[STEP 4] Bundle adjustment with constraints...')"]
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
#
|
||||
# board_anchors = load_board_anchors(robot_data, length_scale, weight=100.0)
|
||||
# constraints += board_anchors
|
||||
# print(f"[INFO] Board anchors added: {len(board_anchors)}")
|
||||
#
|
||||
# ══════════════════════════════════════════════════════════════
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# STANDALONE RUNTIME PATCH (alternative to editing v4)
|
||||
# Run this file directly and it patches v4 in memory.
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _monkey_patch_v4():
|
||||
"""
|
||||
Import the v4 module under an alias and patch it so Board markers are
|
||||
anchored without touching the original file.
|
||||
"""
|
||||
import importlib.util, sys, types, pathlib
|
||||
|
||||
v4_path = pathlib.Path(__file__).parent / "3_multiview_bundle_adjustment_v4.py"
|
||||
if not v4_path.exists():
|
||||
raise FileNotFoundError(
|
||||
f"Expected v4 at {v4_path}.\n"
|
||||
"Place this patch file alongside 3_multiview_bundle_adjustment_v4.py."
|
||||
)
|
||||
|
||||
spec = importlib.util.spec_from_file_location("ba_v4", v4_path)
|
||||
mod = importlib.util.module_from_spec(spec)
|
||||
sys.modules["ba_v4"] = mod # must be registered before exec_module for @dataclass
|
||||
spec.loader.exec_module(mod)
|
||||
|
||||
# Inject PositionAnchorConstraint into the module namespace
|
||||
mod.PositionAnchorConstraint = PositionAnchorConstraint
|
||||
|
||||
# Patch bundle_adjustment_residuals ─────────────────────────
|
||||
_orig_residuals = mod.bundle_adjustment_residuals
|
||||
|
||||
def _patched_residuals(marker_positions_flat, marker_ids, marker_observations,
|
||||
cameras, constraints, obs_weights, lambda_constraint=100.0):
|
||||
r = _orig_residuals(marker_positions_flat, marker_ids, marker_observations,
|
||||
cameras, constraints, obs_weights, lambda_constraint)
|
||||
# Rebuild marker_dict (same as inside the original function)
|
||||
marker_dict = {
|
||||
mid: marker_positions_flat[i*3:(i+1)*3]
|
||||
for i, mid in enumerate(marker_ids)
|
||||
}
|
||||
extra = []
|
||||
for c in constraints:
|
||||
if isinstance(c, PositionAnchorConstraint):
|
||||
extra.extend(_anchor_residuals(c, marker_dict, lambda_constraint))
|
||||
if extra:
|
||||
r = np.concatenate([r, np.array(extra, dtype=np.float64)])
|
||||
return r
|
||||
|
||||
mod.bundle_adjustment_residuals = _patched_residuals
|
||||
|
||||
# Patch main to inject board anchors ────────────────────────
|
||||
_orig_main = mod.main
|
||||
|
||||
def _patched_main():
|
||||
import sys as _sys
|
||||
# We hijack the module-level function calls by patching compile_constraints
|
||||
_orig_compile = mod.compile_constraints
|
||||
_orig_initial = mod.initial_triangulation
|
||||
|
||||
# robot_data / length_scale are captured here so the initial-triangulation
|
||||
# wrapper below can pin Board markers to their true world positions.
|
||||
# In v4.main(), compile_constraints() runs BEFORE initial_triangulation(),
|
||||
# so this dict is populated by the time the triangulation wrapper fires.
|
||||
captured: Dict[str, Any] = {}
|
||||
|
||||
def _compile_with_anchors(robot_data, length_scale, cfg):
|
||||
captured["robot_data"] = robot_data
|
||||
captured["length_scale"] = length_scale
|
||||
|
||||
marker_to_link, link_markers, constraints, issues, marker_meta = \
|
||||
_orig_compile(robot_data, length_scale, cfg)
|
||||
|
||||
anchors = load_board_anchors(robot_data, length_scale, weight=100.0)
|
||||
print(f"[PATCH] Board anchors added: {len(anchors)}")
|
||||
constraints = constraints + anchors
|
||||
return marker_to_link, link_markers, constraints, issues, marker_meta
|
||||
|
||||
def _initial_with_board(marker_observations, cameras):
|
||||
"""
|
||||
Run the normal multi-view triangulation, then overwrite every
|
||||
OBSERVED Board marker with its exact robot.json world position.
|
||||
Board markers define the world frame, so they must not be left at
|
||||
their (z-noisy) triangulated values — not even in the initial JSON.
|
||||
"""
|
||||
tri = _orig_initial(marker_observations, cameras)
|
||||
|
||||
robot_data = captured.get("robot_data")
|
||||
length_scale = captured.get("length_scale", 1.0)
|
||||
if robot_data is None:
|
||||
return tri
|
||||
|
||||
board = (robot_data.get("links", {}) or {}).get("Board", {}) or {}
|
||||
pinned = 0
|
||||
for m in board.get("markers", []):
|
||||
mid = int(m.get("id", -1))
|
||||
pos = m.get("position", None)
|
||||
if mid < 0 or pos is None or len(pos) != 3:
|
||||
continue
|
||||
# Only pin markers that were actually observed/triangulated,
|
||||
# so we don't introduce markers that no camera ever saw.
|
||||
if mid in tri:
|
||||
tri[mid] = np.array(pos, dtype=np.float64) * float(length_scale)
|
||||
pinned += 1
|
||||
print(f"[PATCH] Board markers pinned to robot.json in initial triangulation: {pinned}")
|
||||
return tri
|
||||
|
||||
mod.compile_constraints = _compile_with_anchors
|
||||
mod.initial_triangulation = _initial_with_board
|
||||
_orig_main()
|
||||
mod.compile_constraints = _orig_compile # restore
|
||||
mod.initial_triangulation = _orig_initial # restore
|
||||
|
||||
mod.main = _patched_main
|
||||
return mod
|
||||
|
||||
|
||||
def main():
|
||||
"""Run the patched v4 pipeline with Board marker anchoring."""
|
||||
import sys
|
||||
mod = _monkey_patch_v4()
|
||||
sys.argv[0] = "3_multiview_bundle_adjustment_v4.py" # cosmetic
|
||||
mod.main()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
189
scripts/3b_corner_marker_poses.py
Normal file
189
scripts/3b_corner_marker_poses.py
Normal file
@@ -0,0 +1,189 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
3b_corner_marker_poses.py
|
||||
=========================
|
||||
Produktiver Pipeline-Schritt: leitet aus den 4 ArUco-Ecken jedes Markers eine
|
||||
volle Marker-Pose ab (Position + gemessene Normale), statt nur den Center zu
|
||||
triangulieren.
|
||||
|
||||
Validiert in benchmark/stage0_corner_normals.py: die aus triangulierten Ecken
|
||||
abgeleitete Normale ist ~1 deg genau (Median), auch fuer Finger-Marker.
|
||||
|
||||
Input:
|
||||
--evalDir Ordner mit render_*_aruco_detection.json + _camera_pose.json
|
||||
--robot robot.json (fuer marker_id -> link Zuordnung)
|
||||
Output:
|
||||
<evalDir>/aruco_marker_poses.json (pro Marker: position, gemessene normal,
|
||||
4 triangulierte Ecken, #Kameras, Kantenlaenge)
|
||||
|
||||
Das Format ist kompatibel mit robot_viewer.html (marker_id, position_m/mm, normal)
|
||||
und mit 9_evaluateMarker.py (position_m), erweitert um die gemessene Orientierung.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import time
|
||||
from typing import Dict, List, Tuple
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Loading
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def load_cameras(eval_dir: str) -> Dict[str, dict]:
|
||||
cams: Dict[str, dict] = {}
|
||||
for det_path in glob.glob(os.path.join(eval_dir, "*_aruco_detection.json")):
|
||||
base = os.path.basename(det_path)
|
||||
m = re.match(r"render_([A-Za-z0-9]+)_aruco_detection\.json", base)
|
||||
if not m:
|
||||
continue
|
||||
cam_id = m.group(1)
|
||||
pose_path = os.path.join(eval_dir, f"render_{cam_id}_camera_pose.json")
|
||||
if not os.path.exists(pose_path):
|
||||
print(f"[WARN] no pose for camera {cam_id}, skipping")
|
||||
continue
|
||||
det = json.load(open(det_path, "r", encoding="utf-8"))
|
||||
pose = json.load(open(pose_path, "r", encoding="utf-8"))
|
||||
K = np.array(det["camera"]["camera_matrix"], dtype=float).reshape(3, 3)
|
||||
D = np.array(det["camera"]["distortion_coefficients"], dtype=float).reshape(-1, 1)
|
||||
w2c = pose["camera_pose"]["world_to_camera"]
|
||||
R = np.array(w2c["rotation_matrix"], dtype=float).reshape(3, 3)
|
||||
t = np.array(w2c["translation_m"], dtype=float).reshape(3)
|
||||
markers: Dict[int, np.ndarray] = {}
|
||||
for d in det.get("detections", []):
|
||||
pts = d.get("image_points_px")
|
||||
if pts is not None:
|
||||
markers[int(d["marker_id"])] = np.array(pts, dtype=float).reshape(4, 2)
|
||||
cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers)
|
||||
return cams
|
||||
|
||||
|
||||
def load_marker_links(robot_path: str) -> Dict[int, str]:
|
||||
robot = json.load(open(robot_path, "r", encoding="utf-8"))
|
||||
out: Dict[int, str] = {}
|
||||
for link_name, link in (robot.get("links", {}) or {}).items():
|
||||
for mk in link.get("markers", []) or []:
|
||||
mid = int(mk.get("id", -1))
|
||||
if mid >= 0:
|
||||
out[mid] = link_name
|
||||
return out
|
||||
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Geometry (validated in stage0)
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def triangulate_multiview(observations) -> np.ndarray:
|
||||
A = []
|
||||
for K, D, R, t, uv in observations:
|
||||
und = cv2.undistortPoints(np.array([[uv]], dtype=np.float32), K, D).reshape(2)
|
||||
x, y = float(und[0]), float(und[1])
|
||||
P = np.hstack([R, t.reshape(3, 1)])
|
||||
A.append(x * P[2] - P[0])
|
||||
A.append(y * P[2] - P[1])
|
||||
_, _, Vt = np.linalg.svd(np.asarray(A, dtype=float))
|
||||
X = Vt[-1]
|
||||
return np.array([np.nan] * 3) if abs(X[3]) < 1e-12 else X[:3] / X[3]
|
||||
|
||||
|
||||
def corner_plane_normal(corners3d: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
center = corners3d.mean(axis=0)
|
||||
_, _, Vt = np.linalg.svd(corners3d - center)
|
||||
n = Vt[-1]
|
||||
# ArUco corners clockwise from the front: outward (camera-facing) normal,
|
||||
# matching the Blender/robot.json convention, points opposite cross(e01,e02).
|
||||
cross = np.cross(corners3d[1] - corners3d[0], corners3d[2] - corners3d[0])
|
||||
if np.dot(n, cross) > 0:
|
||||
n = -n
|
||||
nn = np.linalg.norm(n)
|
||||
return (n / nn if nn > 1e-12 else n), center
|
||||
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Main
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def main() -> None:
|
||||
ap = argparse.ArgumentParser(description="Derive marker poses (position + measured normal) from ArUco corners")
|
||||
ap.add_argument("--evalDir", required=True, help="folder with detection + camera_pose JSONs")
|
||||
ap.add_argument("--robot", required=True, help="robot.json (for marker->link)")
|
||||
ap.add_argument("--minCams", type=int, default=2, help="min cameras to triangulate a marker")
|
||||
ap.add_argument("--out", default=None, help="output path (default <evalDir>/aruco_marker_poses.json)")
|
||||
args = ap.parse_args()
|
||||
|
||||
cams = load_cameras(args.evalDir)
|
||||
if len(cams) < 2:
|
||||
print("[ERROR] need >=2 cameras")
|
||||
return
|
||||
links = load_marker_links(args.robot)
|
||||
print(f"[INFO] Cameras: {sorted(cams.keys())} | marker-link entries: {len(links)}")
|
||||
|
||||
marker_cams: Dict[int, List[str]] = {}
|
||||
for cid, cam in cams.items():
|
||||
for mid in cam["markers"]:
|
||||
marker_cams.setdefault(mid, []).append(cid)
|
||||
|
||||
markers_out = []
|
||||
for mid, cam_ids in sorted(marker_cams.items()):
|
||||
if len(cam_ids) < args.minCams:
|
||||
continue
|
||||
corners3d, ok = [], True
|
||||
for ci in range(4):
|
||||
obs = [(cams[c]["K"], cams[c]["D"], cams[c]["R"], cams[c]["t"], cams[c]["markers"][mid][ci])
|
||||
for c in cam_ids]
|
||||
X = triangulate_multiview(obs)
|
||||
if not np.all(np.isfinite(X)):
|
||||
ok = False
|
||||
break
|
||||
corners3d.append(X)
|
||||
if not ok:
|
||||
continue
|
||||
corners3d = np.array(corners3d)
|
||||
normal, center = corner_plane_normal(corners3d)
|
||||
edge_mm = float(np.mean([np.linalg.norm(corners3d[(i + 1) % 4] - corners3d[i]) for i in range(4)]) * 1000.0)
|
||||
|
||||
markers_out.append({
|
||||
"marker_id": int(mid),
|
||||
"link": links.get(mid, "unknown"),
|
||||
"position_m": [float(v) for v in center],
|
||||
"position_mm": [float(v * 1000.0) for v in center],
|
||||
"normal": [float(v) for v in normal],
|
||||
"corners_m": [[float(v) for v in c] for c in corners3d],
|
||||
"num_cameras": len(cam_ids),
|
||||
"edge_length_mm": edge_mm,
|
||||
})
|
||||
|
||||
# camera poses in world (for viewer frusta): centre C = -R^T t, view axis = R[2]
|
||||
cameras_out = []
|
||||
for cid in sorted(cams.keys()):
|
||||
cam = cams[cid]
|
||||
C = -cam["R"].T @ cam["t"]
|
||||
cameras_out.append({
|
||||
"camera_id": cid,
|
||||
"position_m": [float(v) for v in C],
|
||||
"position_mm": [float(v * 1000.0) for v in C],
|
||||
"direction": [float(v) for v in cam["R"][2]],
|
||||
})
|
||||
|
||||
out_path = args.out or os.path.join(args.evalDir, "aruco_marker_poses.json")
|
||||
output = {
|
||||
"schema_version": "1.1",
|
||||
"stage": "corner_marker_poses",
|
||||
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"summary": {"num_cameras": len(cams), "num_markers": len(markers_out)},
|
||||
"cameras": cameras_out,
|
||||
"markers": markers_out,
|
||||
}
|
||||
json.dump(output, open(out_path, "w", encoding="utf-8"), indent=2)
|
||||
print(f"[INFO] {len(markers_out)} marker poses -> {out_path}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
83
scripts/A0_60Arucos_25mm_Seet223.json
Normal file
83
scripts/A0_60Arucos_25mm_Seet223.json
Normal file
@@ -0,0 +1,83 @@
|
||||
{
|
||||
"page_format": "A0",
|
||||
"orientation": "portrait",
|
||||
"page_size_mm": {
|
||||
"width": 841.0,
|
||||
"height": 1189.0
|
||||
},
|
||||
"seed": 223,
|
||||
"num_arucos": 60,
|
||||
"aruco_size_mm": 25.0,
|
||||
"aruco_dictionary": "DICT_4X4_250",
|
||||
"aruco_start_id": 46,
|
||||
"page_border_margin_mm": 50.0,
|
||||
"forbidden_rectangle_mm": {
|
||||
"x": 318.5,
|
||||
"y": 94.5,
|
||||
"w": 204.0,
|
||||
"h": 1000.0
|
||||
},
|
||||
"forbidden_rectangle_margin_mm": 30.0,
|
||||
"placements": [
|
||||
{"id": 46, "position": [536.71, 185.44, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 47, "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 48, "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 49, "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 50, "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 51, "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 52, "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 53, "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 54, "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 55, "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 56, "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 57, "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 58, "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 59, "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 60, "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 61, "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 62, "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 63, "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 64, "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 65, "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 66, "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 67, "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 68, "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 69, "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 70, "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 71, "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 72, "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 73, "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 74, "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 75, "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 76, "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 77, "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 78, "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 79, "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 80, "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 81, "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 82, "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 83, "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 84, "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 85, "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 86, "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 87, "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 88, "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 89, "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 90, "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 91, "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 92, "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 93, "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 94, "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 95, "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 96, "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 97, "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 98, "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 99, "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 100, "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 101, "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 102, "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 103, "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 104, "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 105, "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90}
|
||||
]
|
||||
}
|
||||
74
scripts/A0_60Arucos_25mm_Seet223.pdf
Normal file
74
scripts/A0_60Arucos_25mm_Seet223.pdf
Normal file
File diff suppressed because one or more lines are too long
433
scripts/robot_1781069752019.json
Normal file
433
scripts/robot_1781069752019.json
Normal file
@@ -0,0 +1,433 @@
|
||||
{
|
||||
"coordinateSystem": {"handedness": "right", "x": "right", "y": "backward", "z": "up"},
|
||||
"units": {"length": "mm", "rotation": "degree"},
|
||||
"vision_config": {"MarkerType": "DICT_4X4_250", "MarkerSize": 0.025},
|
||||
"renderingInfo": {
|
||||
"width": 1280,
|
||||
"height": 720,
|
||||
"renderDefaults": {"width": 1280, "height": 720, "dofFStop": 11},
|
||||
"cameraPosition__1": [-10, -800, 500],
|
||||
"cameraPosition__2": [-500, 300, 1200],
|
||||
"cameraPosition__3": [-200, -900, 200],
|
||||
"cameraPosition__4": [1200, 200, 300],
|
||||
"cameraPosition_a": [-300, -800, 500],
|
||||
"cameraPosition": [-200, 200, 1400],
|
||||
"cameraPosition_c": [600, -500, 600],
|
||||
"cameraTarget": [200, -200, 180],
|
||||
"cameraUpVector": [0, 0, 1],
|
||||
"lightPosition": [-500, -500, 500],
|
||||
"lightTarget": [0, 0, 0],
|
||||
"lightUpVector": [0, 0, 1],
|
||||
"metric": "mm",
|
||||
"showSkeleton": true,
|
||||
"showMarkers": true,
|
||||
"backgroundColor": [0.7, 0.85, 1.0],
|
||||
"backgroundStrength": 0.2,
|
||||
"sunEnergy": 0.35,
|
||||
"areaEnergy": 120,
|
||||
"exposure": -1.5,
|
||||
"lensDirt": true,
|
||||
"lensDirtStrength": 0.08,
|
||||
"dofEnabled": true,
|
||||
"dofFStop": 11.0,
|
||||
"arucoDust": true,
|
||||
"arucoDustStrength": 1.6,
|
||||
"markerOffsetMaxMm": 4.0,
|
||||
"markerOffsetSeed": 0,
|
||||
"markerRotationMaxDeg": 3,
|
||||
"motionBlur": true,
|
||||
"motionBlurMaxPx": 5.5,
|
||||
"focalErrorPct": 0.5,
|
||||
"principalErrorPx": 3.0,
|
||||
"residualDistortion": [0.02, -0.01],
|
||||
"localizedBlur": false,
|
||||
"localizedBlurStrength": 0.15,
|
||||
"vignette": true,
|
||||
"vignetteStrength": 0.08,
|
||||
"sensorNoise": true,
|
||||
"sensorNoiseStrength": 0.01,
|
||||
"lensDistortion": true,
|
||||
"lensDistortionStrength": 0.002,
|
||||
"materials": {
|
||||
"wood": {"baseColor": [0.72, 0.52, 0.33], "roughness": 0.85, "metallic": 0.0},
|
||||
"plaWhite": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.45, "metallic": 0.0},
|
||||
"steel": {"baseColor": [0.72, 0.72, 0.75], "roughness": 0.25, "metallic": 1.0},
|
||||
"powderCoatBlue": {"baseColor": [0.15, 0.25, 0.7], "roughness": 0.55, "metallic": 0.0},
|
||||
"defaultPlastic": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.4, "metallic": 0.0},
|
||||
"skeletonRed": {"baseColor": [0.85, 0.2, 0.2], "roughness": 0.35, "metallic": 0.0},
|
||||
"markerBlack": {"baseColor": [0.04, 0.04, 0.04], "roughness": 0.8, "metallic": 0.0}
|
||||
},
|
||||
"skeletonDefaults": {"radius": 4, "color": [0.85, 0.2, 0.2]},
|
||||
"markerDefaults": {"size": 25, "thickness": 1, "color": [0.04, 0.04, 0.04]},
|
||||
"defaultPosition": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3}
|
||||
},
|
||||
"defaultPosition__": {"x": 10, "y": 4, "z": 20, "a": 10, "b": 2, "c": 9, "e": 1},
|
||||
"defaultPosition": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
|
||||
"recognized": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null},
|
||||
"constraint_rules": {
|
||||
"rigid_distance": {"enabled": true, "mode": "mst", "weight": 1.0},
|
||||
"joint_axis_projection": {"enabled": true, "max_pairs": 2, "weight": 0.35},
|
||||
"chain_axis_projection": {"enabled": false, "max_depth": 3, "max_pairs": 2, "weight": 0.15},
|
||||
"axis_alignment_threshold": 0.95
|
||||
},
|
||||
"observation_weighting": {"enabled": true, "distance_weight": true, "marker_size_weight": true, "view_angle_weight": true},
|
||||
"multiview_calculation": {
|
||||
"combine_mode": "mean",
|
||||
"size_ref_px": 50.0,
|
||||
"border_ref_px": 120.0,
|
||||
"center_ref_norm": 0.01,
|
||||
"sharpness_ref": 2500.0,
|
||||
"homography_ref": 0.18,
|
||||
"size_factor": 0.3,
|
||||
"aspect_factor": 0.3,
|
||||
"border_factor": 0.01,
|
||||
"center_factor": 0.01,
|
||||
"sharpness_factor": 0.5,
|
||||
"homography_factor": 0.2,
|
||||
"normal_visibility_factor": 0.01,
|
||||
"spin_factor": 0.3,
|
||||
"weight_floor": 0.3
|
||||
},
|
||||
"pose_estimation": {
|
||||
"method": "hybrid",
|
||||
"marker_observation": "corner_pose",
|
||||
"use_normals": true,
|
||||
"normal_weight": 100.0,
|
||||
"robust_loss": "huber",
|
||||
"huber_delta_mm": 8.0,
|
||||
"max_iterations": 200,
|
||||
"min_cameras_per_marker": 2,
|
||||
"finger_block_joints": ["b", "c", "e"],
|
||||
"per_link_method": {}
|
||||
},
|
||||
"robot_test_poses": {
|
||||
"4": {"x": 70, "y": 50, "z": -70, "a": 120, "b": 50, "c": 30, "e": 20},
|
||||
"5": {"x": 180, "y": 86, "z": -120, "a": -60, "b": 22, "c": 91, "e": 10},
|
||||
"6": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3},
|
||||
"7": {"x": 30, "y": -2, "z": 95, "a": 20, "b": 23, "c": 9, "e": 9},
|
||||
"8": {"x": 50, "y": -2, "z": 95, "a": 20, "b": 60, "c": 9, "e": 3},
|
||||
"9": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
|
||||
"9a": {
|
||||
"x": 60,
|
||||
"y": -2,
|
||||
"z": 95,
|
||||
"a": 200,
|
||||
"b": 60,
|
||||
"c": 9,
|
||||
"e": 8,
|
||||
"rendering": {"width": 1440, "height": 1080, "dofFStop": 11}
|
||||
},
|
||||
"9b": {
|
||||
"x": 60,
|
||||
"y": -2,
|
||||
"z": 95,
|
||||
"a": 200,
|
||||
"b": 60,
|
||||
"c": 9,
|
||||
"e": 8,
|
||||
"rendering": {"width": 4896, "height": 3264, "dofFStop": 5.6}
|
||||
},
|
||||
"10": {"x": 120, "y": 60, "z": -110, "a": 20, "b": 30, "c": 180, "e": 4},
|
||||
"11": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
|
||||
"12": {"x": 50, "y": 0, "z": 178, "a": 210, "b": 80, "c": 90, "e": 6}
|
||||
},
|
||||
"test_camera_positions": {
|
||||
"a": [-300, -800, 800],
|
||||
"b": [300, -900, 1200],
|
||||
"c": [300, -900, 400],
|
||||
"d": [700, -800, 400],
|
||||
"e": [1200, -900, 400],
|
||||
"f": [500, -300, 1400],
|
||||
"g": [-200, 200, 1400]
|
||||
},
|
||||
"test_camera_targets": {
|
||||
"a": [210, -100, 180],
|
||||
"b": [310, -80, 180],
|
||||
"c": [210, -100, 150],
|
||||
"d": [210, -100, 150],
|
||||
"e": [210, -100, 50],
|
||||
"f": [200, -200, 180],
|
||||
"g": [200, -200, 180]
|
||||
},
|
||||
"movements": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null},
|
||||
"state_pose_params": {
|
||||
"numbers_of_Elements_to_consider_start": 3,
|
||||
"numbers_of_Elements_to_consider_final": 5,
|
||||
"solver_in_between_geometrical": false,
|
||||
"solver_after_geometrical": false,
|
||||
"geometric_passes_per_stage": 2,
|
||||
"revolute_search_coarse_deg": 5.0,
|
||||
"revolute_search_fine_deg": 1.0,
|
||||
"root_pose_min_markers": 3,
|
||||
"use_marker_normals_flip_tiebreak": true,
|
||||
"normal_flip_weight": 0.05
|
||||
},
|
||||
"links": {
|
||||
"Board": {
|
||||
"parent": null,
|
||||
"size": [1000, 200, 25],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"skeleton": {"from": [0, 0, 16], "to": [1000, 0, 16], "radius": 4, "color": [0.85, 0.2, 0.2]},
|
||||
"markers": [
|
||||
{
|
||||
"id": 46,
|
||||
"set": "A0",
|
||||
"position": [536.71, 185.44, -27.3],
|
||||
"normal": [0, 0, 1],
|
||||
"spin": 90,
|
||||
"info": "is placed on a white paper, A0_60Arucos_25mm_Seet223.pdf, with the following marker placements:"
|
||||
},
|
||||
{"id": 47, "set": "A0", "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 48, "set": "A0", "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 49, "set": "A0", "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 50, "set": "A0", "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 51, "set": "A0", "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 52, "set": "A0", "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 53, "set": "A0", "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 54, "set": "A0", "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 55, "set": "A0", "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 56, "set": "A0", "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 57, "set": "A0", "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 58, "set": "A0", "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 59, "set": "A0", "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 60, "set": "A0", "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 61, "set": "A0", "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 62, "set": "A0", "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 63, "set": "A0", "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 64, "set": "A0", "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 65, "set": "A0", "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 66, "set": "A0", "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 67, "set": "A0", "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 68, "set": "A0", "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 69, "set": "A0", "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 70, "set": "A0", "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 71, "set": "A0", "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 72, "set": "A0", "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 73, "set": "A0", "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 74, "set": "A0", "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 75, "set": "A0", "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 76, "set": "A0", "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 77, "set": "A0", "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 78, "set": "A0", "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 79, "set": "A0", "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 80, "set": "A0", "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 81, "set": "A0", "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 82, "set": "A0", "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 83, "set": "A0", "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 84, "set": "A0", "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 85, "set": "A0", "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 86, "set": "A0", "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 87, "set": "A0", "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 88, "set": "A0", "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 89, "set": "A0", "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 90, "set": "A0", "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 91, "set": "A0", "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 92, "set": "A0", "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 93, "set": "A0", "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 94, "set": "A0", "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 95, "set": "A0", "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 96, "set": "A0", "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 97, "set": "A0", "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 98, "set": "A0", "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 99, "set": "A0", "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 100, "set": "A0", "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 101, "set": "A0", "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 102, "set": "A0", "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 103, "set": "A0", "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 104, "set": "A0", "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 105, "set": "A0", "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Board.stl",
|
||||
"originOfModel": [0, 0, 0],
|
||||
"rotationOfModelDegree": [0, 0, -90],
|
||||
"material": "wood"
|
||||
},
|
||||
{
|
||||
"stlFile": "surfaces/BoardRail.stl",
|
||||
"originOfModel": [0, 0, 0],
|
||||
"rotationOfModelDegree": [0, 0, -90],
|
||||
"material": "steel"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Base": {
|
||||
"parent": "Board",
|
||||
"size": [150, 200, 150],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [0, 0, 16],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "x"
|
||||
},
|
||||
"skeleton": {"from": [0, 108, 45], "to": [110, 108, 45], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Base.stl",
|
||||
"originOfModel": [-30, 0, -35],
|
||||
"rotationOfModelDegree": [0, 0, 0],
|
||||
"material": "plaWhite"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Arm1": {
|
||||
"parent": "Base",
|
||||
"size": [70, 250, 70],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint1",
|
||||
"type": "revolute",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [110, 108, 45],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "y"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.2, 0.2, 0.9]},
|
||||
"markers": [ ],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Holm.stl",
|
||||
"originOfModel__": [-25, 29, -28.5],
|
||||
"originOfModel": [-29, 25, 28.5],
|
||||
"rotationOfModelDegree__": [0, 0, 0],
|
||||
"rotationOfModelDegree": [180, 0, -90],
|
||||
"material": "powderCoatBlue"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Ellbow": {
|
||||
"parent": "Arm1",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint2",
|
||||
"type": "revolute",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [0, -250, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "z"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [90, 0, 0], "radius": 4, "color": [0.9, 0.2, 0.2]},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Ellebogen.stl",
|
||||
"originOfModel": [90, 0, 0],
|
||||
"rotationOfModelDegree": [0, -90, -90],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
],
|
||||
"markers": [
|
||||
]
|
||||
},
|
||||
"Arm2": {
|
||||
"parent": "Ellbow",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint3",
|
||||
"type": "revolute",
|
||||
"axis": [0, -1, 0],
|
||||
"origin": [90, 0, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "a"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.95, 0.85, 0.2]},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Unterarm.stl",
|
||||
"originOfModel": [0, -250, 0],
|
||||
"rotationOfModelDegree": [180, 0, -90],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
],
|
||||
"markers": [
|
||||
]
|
||||
},
|
||||
"Hand": {
|
||||
"parent": "Arm2",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint4",
|
||||
"type": "revolute",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [0, -250, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "b"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -35, 0], "radius": 4, "color": [0.95, 0.55, 0.15]}
|
||||
},
|
||||
"Palm": {
|
||||
"parent": "Hand",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint3",
|
||||
"type": "revolute",
|
||||
"axis": [0, -1, 0],
|
||||
"origin": [0, 0, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "c"
|
||||
},
|
||||
"skeleton": {"from": [-50, -35, 0], "to": [50, -35, 0], "radius": 7, "color": [0.95, 0.2, 0.2]}
|
||||
},
|
||||
"FingerA": {
|
||||
"parent": "Palm",
|
||||
"size": [80, 60, 20],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [4, -35, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "e"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [24, 0, -9.1],
|
||||
"rotationOfModelDegree": [90, -90, 0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
},
|
||||
"FingerB": {
|
||||
"parent": "Palm",
|
||||
"size": [80, 60, 20],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [-4, -35, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "e"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [-24, 0, 9.1],
|
||||
"rotationOfModelDegree": [90, 90, 0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
214
server/server.js
214
server/server.js
@@ -260,7 +260,14 @@ async function capturePhotos(sessionName) {
|
||||
|
||||
const savedFiles = [];
|
||||
for (const camId of cameraIds) {
|
||||
const response = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
|
||||
let response;
|
||||
// Bei 503 (Kamera kurz busy nach Hires-Grab) einmal nach 2 s neu versuchen
|
||||
for (let attempt = 1; attempt <= 2; attempt++) {
|
||||
response = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
|
||||
if (response.status !== 503) break;
|
||||
if (attempt < 2) await new Promise(r => setTimeout(r, 2000));
|
||||
}
|
||||
if (!response.ok) throw new Error(`getSnapshot(${camId}): HTTP ${response.status}`);
|
||||
const buffer = Buffer.from(await response.arrayBuffer());
|
||||
const filename = `${camId}_${setNr}.jpg`;
|
||||
await fsPromises.writeFile(path.join(sessionDir, filename), buffer);
|
||||
@@ -371,44 +378,15 @@ app.post('/api/calibration/compute', async (req, res) => {
|
||||
send({ type: 'log', text: `▶ Script: ${calibScriptPath}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
// -u = unbuffered (Python gibt jede Zeile sofort aus)
|
||||
const proc = spawn(PYTHON_BIN, [
|
||||
'-u',
|
||||
const exitCode = await runScript([
|
||||
calibScriptPath,
|
||||
'--camera', camera,
|
||||
'--input-dir', sessionDir,
|
||||
'--output-dir', sessionDir,
|
||||
]);
|
||||
], send);
|
||||
|
||||
let stdoutBuf = '';
|
||||
proc.stdout.on('data', (chunk) => {
|
||||
stdoutBuf += chunk.toString();
|
||||
const lines = stdoutBuf.split('\n');
|
||||
stdoutBuf = lines.pop();
|
||||
for (const line of lines) send({ type: 'log', text: line });
|
||||
});
|
||||
|
||||
let stderrBuf = '';
|
||||
proc.stderr.on('data', (chunk) => {
|
||||
stderrBuf += chunk.toString();
|
||||
const lines = stderrBuf.split('\n');
|
||||
stderrBuf = lines.pop();
|
||||
for (const line of lines) send({ type: 'log', text: `[stderr] ${line}` });
|
||||
});
|
||||
|
||||
proc.on('error', (err) => {
|
||||
console.error('calibration/compute spawn error:', err);
|
||||
send({ type: 'log', text: `Fehler beim Starten: ${err.message}` });
|
||||
send({ type: 'done', exitCode: -1 });
|
||||
if (!res.writableEnded) res.end();
|
||||
});
|
||||
|
||||
proc.on('close', (code) => {
|
||||
if (stdoutBuf) send({ type: 'log', text: stdoutBuf });
|
||||
if (stderrBuf) send({ type: 'log', text: `[stderr] ${stderrBuf}` });
|
||||
send({ type: 'done', exitCode: code ?? -1 });
|
||||
if (!res.writableEnded) res.end();
|
||||
});
|
||||
send({ type: 'done', exitCode });
|
||||
if (!res.writableEnded) res.end();
|
||||
|
||||
} catch (err) {
|
||||
// Fehler VOR flushHeaders → normaler JSON-Fehler
|
||||
@@ -426,6 +404,174 @@ app.post('/api/calibration/compute', async (req, res) => {
|
||||
}
|
||||
});
|
||||
|
||||
// ── Board-Erkennung ───────────────────────────────────────────────────────────
|
||||
|
||||
const boardDataDir = path.join(__dirname, '..', 'data', 'board');
|
||||
const ROBOT_JSON = process.env.ROBOT_JSON
|
||||
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
|
||||
const SCRIPT_1 = path.join(__dirname, '..', 'scripts', '1_detect_aruco_observations.py');
|
||||
const SCRIPT_2 = path.join(__dirname, '..', 'scripts', '2_estimate_camera_from_observations.py');
|
||||
|
||||
/**
|
||||
* Führt ein Python-Script aus und leitet stdout/stderr zeilenweise an `send` weiter.
|
||||
* Gibt den Exit-Code zurück (Promise<number>).
|
||||
*/
|
||||
function runScript(args, send) {
|
||||
return new Promise((resolve) => {
|
||||
const proc = spawn(PYTHON_BIN, ['-u', ...args]);
|
||||
|
||||
let outBuf = '';
|
||||
proc.stdout.on('data', chunk => {
|
||||
outBuf += chunk.toString();
|
||||
const lines = outBuf.split('\n');
|
||||
outBuf = lines.pop();
|
||||
for (const line of lines) send({ type: 'log', text: line });
|
||||
});
|
||||
|
||||
let errBuf = '';
|
||||
proc.stderr.on('data', chunk => {
|
||||
errBuf += chunk.toString();
|
||||
const lines = errBuf.split('\n');
|
||||
errBuf = lines.pop();
|
||||
for (const line of lines) send({ type: 'log', text: `[stderr] ${line}` });
|
||||
});
|
||||
|
||||
proc.on('error', err => {
|
||||
send({ type: 'log', text: `Fehler beim Starten: ${err.message}` });
|
||||
resolve(-1);
|
||||
});
|
||||
|
||||
proc.on('close', code => {
|
||||
if (outBuf) send({ type: 'log', text: outBuf });
|
||||
if (errBuf) send({ type: 'log', text: `[stderr] ${errBuf}` });
|
||||
resolve(code ?? -1);
|
||||
});
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* POST /api/board/run
|
||||
* 1. Erstellt data/board/{timestamp}/
|
||||
* 2. Holt Snapshot jeder Kamera
|
||||
* 3. Für jede Kamera: Script 1 (ArUco-Erkennung) → Script 2 (Kamera-Pose)
|
||||
* SSE-Stream während der Ausführung.
|
||||
*/
|
||||
app.post('/api/board/run', async (req, res) => {
|
||||
try {
|
||||
res.setHeader('Content-Type', 'text/event-stream');
|
||||
res.setHeader('Cache-Control', 'no-cache');
|
||||
res.setHeader('Connection', 'keep-alive');
|
||||
res.flushHeaders();
|
||||
|
||||
const send = (obj) => {
|
||||
if (!res.writableEnded) res.write(`data: ${JSON.stringify(obj)}\n\n`);
|
||||
};
|
||||
|
||||
// 1. Temp-Verzeichnis
|
||||
const ts = makeTimestamp();
|
||||
const runDir = path.join(boardDataDir, ts);
|
||||
await fsPromises.mkdir(runDir, { recursive: true });
|
||||
send({ type: 'log', text: `▶ Board-Run: ${ts}` });
|
||||
send({ type: 'log', text: `▶ Ordner: ${runDir}` });
|
||||
send({ type: 'log', text: `▶ Robot-JSON: ${ROBOT_JSON}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
// 2. Kameras ermitteln
|
||||
if (!WEBCAM_URL) throw new Error('WEBCAM_URL nicht konfiguriert');
|
||||
const camData = await new WebcamClient(WEBCAM_URL).getCameras();
|
||||
const cameraIds = (camData.cameras ?? []).map(c => c.id);
|
||||
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
|
||||
|
||||
// 3. Aktuelle Kalibrierungs-Session für NPZ-Dateien
|
||||
const calibSession = await findLatestCalibSession();
|
||||
if (!calibSession) throw new Error('Keine Kalibrierungs-Session. Bitte zuerst Camera NPZ kalibrieren.');
|
||||
send({ type: 'log', text: `▶ NPZ-Session: ${calibSession}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
// 4. Pro Kamera: Foto → Script 1 → Script 2
|
||||
for (const camId of cameraIds) {
|
||||
send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` });
|
||||
|
||||
// Snapshot
|
||||
send({ type: 'log', text: 'Foto aufnehmen …' });
|
||||
let snapResp;
|
||||
for (let attempt = 1; attempt <= 2; attempt++) {
|
||||
snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
|
||||
if (snapResp.status !== 503) break;
|
||||
if (attempt < 2) await new Promise(r => setTimeout(r, 2000));
|
||||
}
|
||||
if (!snapResp.ok) {
|
||||
send({ type: 'log', text: `⚠ HTTP ${snapResp.status} – Kamera übersprungen` });
|
||||
continue;
|
||||
}
|
||||
const imgPath = path.join(runDir, `${camId}.jpg`);
|
||||
await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer()));
|
||||
send({ type: 'log', text: `✅ Foto: ${camId}.jpg` });
|
||||
|
||||
// NPZ prüfen
|
||||
const npzPath = path.join(calibDataDir, calibSession, `${camId}_calibration.npz`);
|
||||
try { await fsPromises.access(npzPath); }
|
||||
catch {
|
||||
send({ type: 'log', text: `⚠ Keine NPZ (${camId}_calibration.npz) – übersprungen` });
|
||||
continue;
|
||||
}
|
||||
|
||||
// Script 1 – ArUco-Erkennung
|
||||
send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' });
|
||||
const exit1 = await runScript([
|
||||
SCRIPT_1,
|
||||
'-i', imgPath,
|
||||
'-npz', npzPath,
|
||||
'-robot', ROBOT_JSON,
|
||||
'-cameraId', camId,
|
||||
'-outDir', runDir,
|
||||
'--saveDebugImage',
|
||||
], send);
|
||||
if (exit1 !== 0) {
|
||||
send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` });
|
||||
continue;
|
||||
}
|
||||
|
||||
// Script 2 – Kamera-Pose schätzen
|
||||
const detJson = path.join(runDir, `${camId}_aruco_detection.json`);
|
||||
try { await fsPromises.access(detJson); }
|
||||
catch {
|
||||
send({ type: 'log', text: '⚠ Detection-JSON fehlt – Script 2 übersprungen' });
|
||||
continue;
|
||||
}
|
||||
|
||||
send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' });
|
||||
const exit2 = await runScript([
|
||||
SCRIPT_2,
|
||||
'-i', detJson,
|
||||
'-robot', ROBOT_JSON,
|
||||
'-outDir', runDir,
|
||||
], send);
|
||||
if (exit2 !== 0) {
|
||||
send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` });
|
||||
}
|
||||
|
||||
send({ type: 'log', text: '' });
|
||||
}
|
||||
|
||||
send({ type: 'log', text: `✅ Board-Run abgeschlossen: ${ts}` });
|
||||
send({ type: 'done', exitCode: 0, runDir: ts });
|
||||
if (!res.writableEnded) res.end();
|
||||
|
||||
} catch (err) {
|
||||
console.error('board/run error:', err);
|
||||
if (!res.headersSent) {
|
||||
res.status(500).json({ error: String(err) });
|
||||
} else {
|
||||
try {
|
||||
res.write(`data: ${JSON.stringify({ type: 'log', text: `❌ ${err.message}` })}\n\n`);
|
||||
res.write(`data: ${JSON.stringify({ type: 'done', exitCode: -1 })}\n\n`);
|
||||
res.end();
|
||||
} catch {}
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
/**
|
||||
* POST /api/calibration/upload-npz
|
||||
* Liest {camera}_calibration.npz aus der aktuellen Session und
|
||||
|
||||
Reference in New Issue
Block a user