ChK/appRobotVideoControls
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Phase 1 abgeschlossen: Positionen werden erkannt.

Browse Source 
Positionen aus den Merkern heraus erkennbar. Viele Bilder gleichzeitig verarbeitbar.
This commit is contained in:
chk
2026-05-25 22:16:11 +02:00
parent f37097ea96
commit 5a7176920a
11 changed files with 3005 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

504
programs/2_estimate_camera_pose_from_aruco_json.py
View File

@@ -1,27 +1,26 @@
#!/usr/bin/env python3
"""
estimate_camera_pose_from_aruco_json.py
2_estimate_camera_pose_from_aruco_json.py
Berechnet die Kameraposition im Maschinen-/Board-Koordinatensystem
aus:
1. einer ArUco-Detections-JSON
2. robots.json mit bekannten Marker-Positionen
Das Script verwendet ausschließlich bekannte Marker
und bestimmt daraus die Kamera-Extrinsics mittels solvePnP.
Ergebnis:
- Kamera-Position im Weltkoordinatensystem
- Kamera-Orientierung (Roll/Pitch/Yaw)
- optionale Reprojektion zur Qualitätskontrolle
NEU:
- Marker-Orientierungen unterstützt
- Default: Board-Marker zeigen nach +Z
- Qualitätsbewertung erweitert
- Speichert ALLE erkannten Marker
- Speichert auch fehlgeschlagene Lösungen
- Bewertet Kamerageometrie
- Bewertet Markerabdeckung
- Bewertet Sichtwinkel
- Bewertet Markeranzahl
- Speichert vollständige Rohdaten
Benötigt:
pip install opencv-python numpy
Beispiel:
python 2_estimate_camera_pose_from_aruco_json.py \
--detections detection.json \
--robots robots.json
"""
import argparse
@@ -37,31 +36,45 @@ import numpy as np
# Hilfsfunktionen
# ============================================================
def normalize(v):
n = np.linalg.norm(v)
if n < 1e-9:
return v
return v / n
def rotation_matrix_from_axes(x_axis, y_axis, z_axis):
R = np.column_stack([
normalize(x_axis),
normalize(y_axis),
normalize(z_axis)
])
return R.astype(np.float32)
def rvec_tvec_to_camera_pose(rvec, tvec):
"""
OpenCV liefert:
OpenCV:
X_cam = R * X_world + t
Gesucht:
Kamera-Pose im Weltkoordinatensystem
=> R_wc = R^T
=> C = -R^T * t
Kamera im Weltkoordinatensystem:
C = -R^T * t
"""
R_cw, _ = cv2.Rodrigues(rvec)
R_wc = R_cw.T
cam_pos = -R_wc @ tvec
return R_wc, cam_pos.reshape(3)
def rotation_matrix_to_euler_zyx(R):
"""
Euler ZYX:
yaw(Z), pitch(Y), roll(X)
"""
yaw = math.degrees(math.atan2(R[1, 0], R[0, 0]))
@@ -74,14 +87,42 @@ def rotation_matrix_to_euler_zyx(R):
return roll, pitch, yaw
def build_marker_lookup(robot_data):
"""
Liest nur Marker mit ABSOLUTER Position.
# ============================================================
# Marker-Orientierung
# ============================================================
Unterstützt:
"position" -> absolute Weltposition [m]
"relPos" -> wird aktuell ignoriert
"""
def get_marker_rotation(marker):
# Explizite Rotation vorhanden?
if "rotation_matrix" in marker:
return np.array(
marker["rotation_matrix"],
dtype=np.float32
)
# Default:
# Marker zeigt nach +Z
#
# x = rechts
# y = oben
# z = aus Board heraus (+Z)
x_axis = np.array([1, 0, 0], dtype=np.float32)
y_axis = np.array([0, 1, 0], dtype=np.float32)
z_axis = np.array([0, 0, 1], dtype=np.float32)
return rotation_matrix_from_axes(
x_axis,
y_axis,
z_axis
)
# ============================================================
# Marker Lookup
# ============================================================
def build_marker_lookup(robot_data):
marker_lookup = {}
@@ -89,11 +130,10 @@ def build_marker_lookup(robot_data):
marker_id = int(marker.get("id", -1))
# negative IDs ignorieren
if marker_id < 0:
continue
# Nur absolute Weltpositionen verwenden
# Nur absolute Marker verwenden
if "position" not in marker:
continue
@@ -105,29 +145,32 @@ def build_marker_lookup(robot_data):
if len(pos) != 3:
continue
marker_lookup[marker_id] = np.array(
pos,
dtype=np.float32
)
rotation = get_marker_rotation(marker)
marker_lookup[marker_id] = {
"position": np.array(
pos,
dtype=np.float32
),
"rotation": rotation,
"on": marker.get("on", "unknown")
}
return marker_lookup
def build_marker_object_points(marker_center_world, marker_size_m):
"""
Baut die 3D-Eckpunkte eines Markers auf.
Wichtig:
Die Corner-Reihenfolge MUSS zur OpenCV-ArUco-Reihenfolge passen.
# ============================================================
# Marker-Eckpunkte
# ============================================================
Reihenfolge:
top-left
top-right
bottom-right
bottom-left
"""
def build_marker_object_points(
marker_center_world,
marker_rotation,
marker_size_m):
half = marker_size_m / 2.0
# OpenCV Corner Reihenfolge
local = np.array([
[-half, half, 0.0],
[ half, half, 0.0],
@@ -135,7 +178,113 @@ def build_marker_object_points(marker_center_world, marker_size_m):
[-half, -half, 0.0],
], dtype=np.float32)
return local + marker_center_world.reshape(1, 3)
rotated = (marker_rotation @ local.T).T
return rotated + marker_center_world.reshape(1, 3)
# ============================================================
# Qualitätsmetriken
# ============================================================
def compute_marker_spread(points_3d):
if len(points_3d) < 2:
return 0.0
mins = np.min(points_3d, axis=0)
maxs = np.max(points_3d, axis=0)
diag = np.linalg.norm(maxs - mins)
return float(diag)
def compute_viewing_angles(
camera_position,
marker_lookup,
used_markers):
results = []
for marker_id in used_markers:
marker = marker_lookup[marker_id]
pos = marker["position"]
R = marker["rotation"]
normal = R[:, 2]
to_camera = normalize(camera_position - pos)
dot = np.clip(
np.dot(normal, to_camera),
-1.0,
1.0
)
angle = math.degrees(math.acos(dot))
results.append(angle)
if len(results) == 0:
return {
"mean": None,
"max": None
}
return {
"mean": float(np.mean(results)),
"max": float(np.max(results))
}
def compute_pose_quality(
rms,
max_err,
num_markers,
marker_spread,
viewing_angle_mean):
score = 100.0
# Reprojection Error
score -= rms * 8.0
# Max Error
score -= max_err * 2.0
# Wenige Marker
if num_markers < 2:
score -= 50
elif num_markers < 4:
score -= 25
elif num_markers < 6:
score -= 10
# Schlechte räumliche Verteilung
if marker_spread < 0.10:
score -= 30
elif marker_spread < 0.25:
score -= 15
# Schlechter Blickwinkel
if viewing_angle_mean is not None:
if viewing_angle_mean > 70:
score -= 25
elif viewing_angle_mean > 50:
score -= 10
score = max(0.0, min(100.0, score))
return float(score)
# ============================================================
@@ -148,28 +297,24 @@ def main():
parser.add_argument(
"--detections",
required=True,
help="ArUco detection JSON"
required=True
)
parser.add_argument(
"--robots",
required=True,
help="robots.json"
required=True
)
parser.add_argument(
"--min-confidence",
type=float,
default=0.5,
help="Minimale Marker-Confidence"
default=0.5
)
parser.add_argument(
"--max-reprojection-error",
type=float,
default=3.0,
help="Maximal erlaubter Reprojektionsfehler in Pixel"
default=3.0
)
args = parser.parse_args()
@@ -185,7 +330,7 @@ def main():
robot_data = json.load(f)
# ============================================================
# Kamera-Parameter
# Kamera
# ============================================================
K = np.array(
@@ -199,19 +344,20 @@ def main():
).reshape(-1, 1)
# ============================================================
# Bekannte Marker
# Marker laden
# ============================================================
known_markers = build_marker_lookup(robot_data)
# ============================================================
# 2D/3D Punktlisten aufbauen
# Punktlisten
# ============================================================
object_points = []
image_points = []
used_markers = []
rejected_markers = []
detections = detection_data["detections"]
@@ -219,20 +365,41 @@ def main():
marker_id = int(det["marker_id"])
confidence = float(det.get("confidence", 1.0))
confidence = float(
det.get("confidence", 1.0)
)
reason = None
if confidence < args.min_confidence:
reason = "low_confidence"
elif marker_id not in known_markers:
reason = "unknown_marker"
if reason is not None:
rejected_markers.append({
"marker_id": marker_id,
"reason": reason,
"confidence": confidence
})
continue
if marker_id not in known_markers:
continue
marker_info = known_markers[marker_id]
marker_center_world = known_markers[marker_id]
marker_center_world = marker_info["position"]
marker_size = float(det["marker_size_m"])
marker_rotation = marker_info["rotation"]
marker_size = float(
det["marker_size_m"]
)
obj_pts = build_marker_object_points(
marker_center_world,
marker_rotation,
marker_size
)
@@ -246,25 +413,60 @@ def main():
used_markers.append(marker_id)
# ============================================================
# Ausgabe vorbereiten
# ============================================================
out = {
"success": False,
"camera_pose": None,
"quality": {},
"used_markers": [],
"rejected_markers": rejected_markers,
"all_detected_markers": [
int(d["marker_id"])
for d in detections
]
}
# ============================================================
# Zu wenige Marker
# ============================================================
if len(object_points) == 0:
raise RuntimeError(
"Keine bekannten Marker gefunden."
out["quality"] = {
"error": "no_known_markers",
"num_detected_markers": len(detections),
"num_used_markers": 0
}
out_file = Path(args.detections).with_suffix(
".camera_pose.json"
)
object_points = np.concatenate(object_points, axis=0)
image_points = np.concatenate(image_points, axis=0)
with open(out_file, "w", encoding="utf-8") as f:
json.dump(out, f, indent=2)
print()
print("==================================================")
print("Bekannte Marker verwendet:")
print(sorted(set(used_markers)))
print("==================================================")
print()
print("Keine bekannten Marker gefunden.")
print(out_file)
return
# ============================================================
# solvePnP
# ============================================================
object_points = np.concatenate(
object_points,
axis=0
)
image_points = np.concatenate(
image_points,
axis=0
)
success, rvec, tvec = cv2.solvePnP(
object_points,
image_points,
@@ -274,10 +476,24 @@ def main():
)
if not success:
raise RuntimeError("solvePnP fehlgeschlagen")
out["quality"] = {
"error": "solvepnp_failed",
"num_used_markers": len(set(used_markers))
}
out_file = Path(args.detections).with_suffix(
".camera_pose.json"
)
with open(out_file, "w", encoding="utf-8") as f:
json.dump(out, f, indent=2)
print("solvePnP fehlgeschlagen")
return
# ============================================================
# Kamera-Pose berechnen
# Kamera Pose
# ============================================================
R_wc, cam_pos = rvec_tvec_to_camera_pose(
@@ -285,10 +501,12 @@ def main():
tvec
)
roll, pitch, yaw = rotation_matrix_to_euler_zyx(R_wc)
roll, pitch, yaw = rotation_matrix_to_euler_zyx(
R_wc
)
# ============================================================
# Reprojektionsfehler
# Reprojektion
# ============================================================
projected, _ = cv2.projectPoints(
@@ -306,76 +524,126 @@ def main():
axis=1
)
rms = np.sqrt(np.mean(reproj_error ** 2))
max_err = np.max(reproj_error)
rms = float(
np.sqrt(np.mean(reproj_error ** 2))
)
max_err = float(
np.max(reproj_error)
)
# ============================================================
# Qualität
# ============================================================
marker_positions = np.array([
known_markers[mid]["position"]
for mid in set(used_markers)
])
marker_spread = compute_marker_spread(
marker_positions
)
viewing = compute_viewing_angles(
cam_pos,
known_markers,
set(used_markers)
)
quality_score = compute_pose_quality(
rms,
max_err,
len(set(used_markers)),
marker_spread,
viewing["mean"]
)
# ============================================================
# Ausgabe
# ============================================================
print()
print("==================================================")
print("KAMERA-POSE")
print("==================================================")
print()
print()
print("Position [m]")
print(f" X = {cam_pos[0]: .6f}")
print(f" Y = {cam_pos[1]: .6f}")
print(f" Z = {cam_pos[2]: .6f}")
print(f"X = {cam_pos[0]:.6f}")
print(f"Y = {cam_pos[1]:.6f}")
print(f"Z = {cam_pos[2]:.6f}")
print()
print("Orientation [deg]")
print(f" Roll = {roll: .3f}")
print(f" Pitch = {pitch: .3f}")
print(f" Yaw = {yaw: .3f}")
print(f"Roll = {roll:.3f}")
print(f"Pitch = {pitch:.3f}")
print(f"Yaw = {yaw:.3f}")
print()
print("Reprojection Error")
print(f" RMS = {rms:.3f} px")
print(f" MAX = {max_err:.3f} px")
print("Reprojection")
print(f"RMS = {rms:.3f}px")
print(f"MAX = {max_err:.3f}px")
print()
print("Quality")
print(f"Score = {quality_score:.1f}/100")
print(f"Marker Spread = {marker_spread:.3f}m")
if max_err > args.max_reprojection_error:
print("[WARNUNG] Reprojektionsfehler relativ hoch")
else:
print("[OK] Pose stabil")
print()
if viewing["mean"] is not None:
print(
f"Mean Viewing Angle = "
f"{viewing['mean']:.1f}deg"
)
# ============================================================
# JSON speichern
# ============================================================
out = {
"camera_pose": {
"position_m": {
"x": float(cam_pos[0]),
"y": float(cam_pos[1]),
"z": float(cam_pos[2]),
},
"orientation_deg": {
"roll": float(roll),
"pitch": float(pitch),
"yaw": float(yaw),
}
out["success"] = True
out["camera_pose"] = {
"position_m": {
"x": float(cam_pos[0]),
"y": float(cam_pos[1]),
"z": float(cam_pos[2]),
},
"quality": {
"reprojection_rms_px": float(rms),
"reprojection_max_px": float(max_err),
"num_markers_used": len(set(used_markers)),
"markers_used": sorted(set(used_markers))
}
"orientation_deg": {
"roll": float(roll),
"pitch": float(pitch),
"yaw": float(yaw),
},
"rotation_matrix_world_from_camera": (
R_wc.tolist()
)
}
out_file = Path(args.detections).with_suffix(".camera_pose.json")
out["quality"] = {
"pose_quality_score": quality_score,
"reprojection_rms_px": rms,
"reprojection_max_px": max_err,
"num_detected_markers": len(detections),
"num_used_markers": len(set(used_markers)),
"marker_spread_m": marker_spread,
"mean_viewing_angle_deg": viewing["mean"],
"max_viewing_angle_deg": viewing["max"],
"pose_stable":
max_err <= args.max_reprojection_error
}
out["used_markers"] = sorted(
list(set(used_markers))
)
out_file = Path(args.detections).with_suffix(
".camera_pose.json"
)
with open(out_file, "w", encoding="utf-8") as f:
json.dump(out, f, indent=2)
print(f"Pose gespeichert in:")
print()
print("Gespeichert:")
print(out_file)