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)

765
programs/3_fuse_markers_world.py Normal file
View File

@@ -0,0 +1,765 @@
#!/usr/bin/env python3
"""
3_fuse_markers_world.py
PHASE 1B
---------
Fusioniert Marker-Weltkoordinaten aus mehreren Kameras.
EINGABE:
--json *.camera_pose.json (mehrfach möglich)
--robots robot.json
Das Script findet automatisch:
*.aruco_detection.json
Beispiel:
snapshot_video0_1779690911822_aruco_detection.camera_pose.json
->
snapshot_video0_1779690911822_aruco_detection.json
FEATURES:
- mehrere Kameras (2..5)
- automatische Detection-Datei-Erkennung
- bekannte Marker aus robot.json
- unbekannte Marker triangulieren
- gewichtete Marker-Fusion
- Qualitätsmetriken
- CSV Export
- JSON Export
- Kamera Export
- robuste Fehlerbehandlung
- vorbereitet für spätere Rigid-Body Constraints
OUTPUT:
fused_markers.csv
fused_markers.json
Benötigt:
pip install opencv-python numpy
"""
import argparse
import csv
import json
import math
from collections import defaultdict
from pathlib import Path
import cv2
import numpy as np
# ============================================================
# JSON HELPERS
# ============================================================
def load_json(path):
with open(path, "r", encoding="utf-8") as f:
return json.load(f)
def save_json(path, data):
with open(path, "w", encoding="utf-8") as f:
json.dump(data, f, indent=2)
# ============================================================
# FILE MATCHING
# ============================================================
def find_detection_json(camera_pose_json_path):
"""
Findet automatisch die passende
*_aruco_detection.json Datei.
Beispiel:
INPUT:
snapshot_video0_123_aruco_detection.camera_pose.json
OUTPUT:
snapshot_video0_123_aruco_detection.json
"""
pose_path = Path(camera_pose_json_path)
name = pose_path.name
if not name.endswith(".camera_pose.json"):
raise ValueError(
f"Expected .camera_pose.json: {pose_path}"
)
detection_name = name.replace(
".camera_pose.json",
".json"
)
detection_path = pose_path.with_name(
detection_name
)
if not detection_path.exists():
raise FileNotFoundError(
"Matching detection JSON not found:\n"
f"{detection_path}"
)
return detection_path
# ============================================================
# CAMERA POSE
# ============================================================
def extract_camera_pose(camera_pose_data):
if "camera_pose" not in camera_pose_data:
raise ValueError("camera_pose missing")
pose = camera_pose_data["camera_pose"]
pos = pose["position_m"]
cam_pos = np.array([
pos["x"],
pos["y"],
pos["z"]
], dtype=np.float32)
if "rotation_matrix_world_from_camera" in pose:
R_wc = np.array(
pose["rotation_matrix_world_from_camera"],
dtype=np.float32
)
else:
ori = pose["orientation_deg"]
R_wc = euler_to_rotation_matrix(
ori["roll"],
ori["pitch"],
ori["yaw"]
)
return R_wc, cam_pos
# ============================================================
# INTRINSICS
# ============================================================
def extract_intrinsics(detection_data):
if "camera" not in detection_data:
raise ValueError("camera section missing")
camera = detection_data["camera"]
if "camera_matrix" not in camera:
raise ValueError("camera_matrix missing")
if "distortion_coefficients" not in camera:
raise ValueError("distortion_coefficients missing")
K = np.array(
camera["camera_matrix"],
dtype=np.float32
)
D = np.array(
camera["distortion_coefficients"],
dtype=np.float32
).reshape(-1, 1)
return K, D
# ============================================================
# ROTATION HELPERS
# ============================================================
def euler_to_rotation_matrix(
roll_deg,
pitch_deg,
yaw_deg
):
r = math.radians(roll_deg)
p = math.radians(pitch_deg)
y = math.radians(yaw_deg)
Rx = np.array([
[1, 0, 0],
[0, math.cos(r), -math.sin(r)],
[0, math.sin(r), math.cos(r)]
])
Ry = np.array([
[math.cos(p), 0, math.sin(p)],
[0, 1, 0],
[-math.sin(p), 0, math.cos(p)]
])
Rz = np.array([
[math.cos(y), -math.sin(y), 0],
[math.sin(y), math.cos(y), 0],
[0, 0, 1]
])
return Rz @ Ry @ Rx
# ============================================================
# ROBOT MARKERS
# ============================================================
def build_known_marker_lookup(robot_data):
"""
Nur Marker mit ABSOLUTER Weltposition.
relPos wird in Phase 2 verwendet.
"""
lookup = {}
for marker in robot_data.get("Marker", []):
marker_id = int(marker.get("id", -1))
if marker_id < 0:
continue
if "position" not in marker:
continue
pos = marker["position"]
if pos is None:
continue
if len(pos) != 3:
continue
lookup[marker_id] = np.array(
pos,
dtype=np.float32
)
return lookup
# ============================================================
# MARKER POSE REL CAMERA
# ============================================================
def estimate_marker_pose_camera(
image_points,
marker_size,
K,
D
):
half = marker_size / 2.0
object_points = np.array([
[-half, half, 0],
[half, half, 0],
[half, -half, 0],
[-half, -half, 0]
], dtype=np.float32)
image_points = np.array(
image_points,
dtype=np.float32
)
success, rvec, tvec = cv2.solvePnP(
object_points,
image_points,
K,
D,
flags=cv2.SOLVEPNP_IPPE_SQUARE
)
if not success:
return None
R_mc, _ = cv2.Rodrigues(rvec)
return {
"rvec": rvec,
"tvec": tvec.reshape(3),
"R_mc": R_mc
}
# ============================================================
# MARKER WORLD TRANSFORM
# ============================================================
def marker_world_position(
cam_world_pos,
R_wc,
t_mc
):
"""
Marker Mittelpunkt in Weltkoordinaten.
X_world = R_wc * X_cam + C
"""
return (
R_wc @ t_mc.reshape(3)
) + cam_world_pos
# ============================================================
# WEIGHTING
# ============================================================
def compute_marker_weight(
detection,
camera_pose_data
):
"""
Qualitätsgewicht.
Verwendet:
- confidence
- reprojection RMS
- Bildzentrum
- Markerfläche
- Sharpness
"""
confidence = float(
detection.get("confidence", 0.5)
)
quality = detection.get("quality", {})
area_px = float(
quality.get("area_px", 1000)
)
sharpness = quality.get(
"sharpness", {}
)
lap_var = float(
sharpness.get(
"laplacian_var",
500
)
)
geometry = quality.get(
"geometry", {}
)
dist_center = float(
geometry.get(
"distance_to_center_norm",
0.5
)
)
pose_quality = camera_pose_data.get(
"quality",
{}
)
reproj = float(
pose_quality.get(
"reprojection_rms_px",
10.0
)
)
reproj_weight = 1.0 / (1.0 + reproj)
area_weight = min(
area_px / 2000.0,
1.0
)
sharpness_weight = min(
lap_var / 5000.0,
1.0
)
center_weight = 1.0 - dist_center
weight = (
confidence *
reproj_weight *
area_weight *
sharpness_weight *
center_weight
)
return max(weight, 1e-6)
# ============================================================
# FUSION
# ============================================================
def weighted_average(points, weights):
points = np.array(points)
weights = np.array(weights)
if len(points) == 1:
return points[0]
total_weight = np.sum(weights)
if total_weight < 1e-9:
return np.mean(points, axis=0)
return np.sum(
points * weights[:, None],
axis=0
) / total_weight
# ============================================================
# MAIN
# ============================================================
def main():
parser = argparse.ArgumentParser()
parser.add_argument(
"--json",
action="append",
required=True,
help="*.camera_pose.json"
)
parser.add_argument(
"--robots",
required=True,
help="robot.json"
)
parser.add_argument(
"--outdir",
default="."
)
args = parser.parse_args()
outdir = Path(args.outdir)
outdir.mkdir(
parents=True,
exist_ok=True
)
# ========================================================
# robot.json
# ========================================================
robot_data = load_json(
args.robots
)
known_markers = build_known_marker_lookup(
robot_data
)
# ========================================================
# globale Observationen
# ========================================================
observations = defaultdict(list)
camera_exports = []
# ========================================================
# alle Kameras
# ========================================================
for json_file in args.json:
print()
print("================================================")
print("LOAD CAMERA")
print("================================================")
print(json_file)
# ----------------------------------------------------
# camera pose json
# ----------------------------------------------------
camera_pose_data = load_json(
json_file
)
# ----------------------------------------------------
# detection json automatisch finden
# ----------------------------------------------------
detection_json = find_detection_json(
json_file
)
print(
f"Detection JSON:\n{detection_json}"
)
detection_data = load_json(
detection_json
)
# ----------------------------------------------------
# intrinsics
# ----------------------------------------------------
K, D = extract_intrinsics(
detection_data
)
# ----------------------------------------------------
# kamerapose
# ----------------------------------------------------
R_wc, cam_world_pos = extract_camera_pose(
camera_pose_data
)
camera_name = Path(
json_file
).stem
# ----------------------------------------------------
# camera export
# ----------------------------------------------------
camera_exports.append({
"camera": camera_name,
"x": float(cam_world_pos[0]),
"y": float(cam_world_pos[1]),
"z": float(cam_world_pos[2])
})
# ----------------------------------------------------
# detections
# ----------------------------------------------------
detections = detection_data.get(
"detections",
[]
)
print(
f"Detections: {len(detections)}"
)
# ----------------------------------------------------
# marker durchlaufen
# ----------------------------------------------------
for det in detections:
marker_id = int(
det["marker_id"]
)
marker_size = float(
det["marker_size_m"]
)
pose = estimate_marker_pose_camera(
det["image_points_px"],
marker_size,
K,
D
)
if pose is None:
continue
world_pos = marker_world_position(
cam_world_pos,
R_wc,
pose["tvec"]
)
weight = compute_marker_weight(
det,
camera_pose_data
)
observations[marker_id].append({
"world_pos": world_pos,
"weight": weight,
"camera": camera_name,
"confidence": float(
det.get("confidence", 0.5)
),
"known_marker": marker_id in known_markers
})
# ========================================================
# fusion
# ========================================================
fused_markers = []
print()
print("================================================")
print("FUSE MARKERS")
print("================================================")
for marker_id, obs_list in observations.items():
points = [
o["world_pos"]
for o in obs_list
]
weights = [
o["weight"]
for o in obs_list
]
fused = weighted_average(
points,
weights
)
spread = 0.0
if len(points) > 1:
dists = [
np.linalg.norm(p - fused)
for p in points
]
spread = float(
np.mean(dists)
)
known = marker_id in known_markers
mean_conf = float(np.mean([
o["confidence"]
for o in obs_list
]))
mean_weight = float(np.mean(weights))
print(
f"Marker {marker_id:3d} | "
f"cams={len(obs_list)} | "
f"spread={spread:.4f}m | "
f"known={known}"
)
fused_markers.append({
"marker_id": marker_id,
"x": float(fused[0]),
"y": float(fused[1]),
"z": float(fused[2]),
"num_cameras": len(obs_list),
"spread_m": spread,
"known_marker": known,
"mean_confidence": mean_conf,
"mean_weight": mean_weight
})
# ========================================================
# CSV EXPORT
# ========================================================
csv_file = outdir / "fused_markers.csv"
with open(
csv_file,
"w",
newline="",
encoding="utf-8"
) as f:
writer = csv.DictWriter(
f,
fieldnames=[
"marker_id",
"x",
"y",
"z",
"num_cameras",
"spread_m",
"known_marker",
"mean_confidence",
"mean_weight"
]
)
writer.writeheader()
for row in fused_markers:
writer.writerow(row)
# ========================================================
# JSON EXPORT
# ========================================================
export_json = {
"fused_markers": fused_markers,
"cameras": camera_exports
}
json_file = outdir / "fused_markers.json"
save_json(
json_file,
export_json
)
# ========================================================
# DONE
# ========================================================
print()
print("================================================")
print("EXPORT")
print("================================================")
print(csv_file)
print(json_file)
print()
# ============================================================
# ENTRY
# ============================================================
if __name__ == "__main__":
main()

1135
programs/3_fuse_markers_world____.py Normal file
View File

File diff suppressed because it is too large Load Diff