arbeiten an unitTests

Abhängigkeit macht Probleme

programs/03a_cameraPose.py

@@ -22,9 +22,17 @@ def load_json(path):
 def load_robot_markers(robot_json):
     markers = {}
     for m in robot_json["Marker"]:
-        if m.get("on") == "Base":
-            mid = int(m["id"])
-            markers[mid] = np.array(m["relPos"], dtype=np.float32)
+        if m.get("on") != "Board":
+            continue
+        if "id" not in m:
+            continue
+        pos = m.get("position")
+        if pos is None:
+            pos = m.get("relPos")
+        if pos is None:
+            continue
+        mid = int(m["id"])
+        markers[mid] = np.array(pos, dtype=np.float32)
     return markers
 
 
@@ -49,62 +57,95 @@ def marker_corners_world(center, size_m):
     ], dtype=np.float32)
 
 
-# ------------------------------------------------------------
-# Build correspondences for one camera
-# ------------------------------------------------------------
-
-def build_correspondences(camera_id, scene_markers, robot_markers, marker_size_m):
-
-    obj_pts = []
-    img_pts = []
-
-    for marker_id, marker_data in scene_markers.items():
-        mid = int(marker_id)
-        
-        if mid not in robot_markers:
-            continue
-
-        # Find observations for this camera
-        for obs in marker_data.get("observations", []):
-            if obs.get("camera_id") == camera_id:
-                center = robot_markers[mid]
-                obj_corners = marker_corners_world(center, marker_size_m)
-                img_corners = np.array(obs["corners_px"], dtype=np.float32)
-
-                obj_pts.append(obj_corners)
-                img_pts.append(img_corners)
-
-    if len(obj_pts) == 0:
-        return None, None
-
-    obj_pts = np.vstack(obj_pts).astype(np.float32)
-    img_pts = np.vstack(img_pts).astype(np.float32)
-
-    return obj_pts, img_pts
+def marker_corners_local(size_m):
+    h = size_m / 2.0
+    return np.array([
+        [-h,  h, 0.0],
+        [ h,  h, 0.0],
+        [ h, -h, 0.0],
+        [-h, -h, 0.0],
+    ], dtype=np.float32)
 
 
 # ------------------------------------------------------------
-# Solve PnP
+# Solve single marker pose
 # ------------------------------------------------------------
 
-def solve_camera(obj_pts, img_pts, K, dist):
-
-    if obj_pts is None or len(obj_pts) < 6:
-        raise RuntimeError("Not enough correspondences for PnP")
-
+def solve_marker_pose(corners_px, K, dist, marker_size_m):
+    obj_pts = marker_corners_local(marker_size_m)
     ok, rvec, tvec = cv2.solvePnP(
         obj_pts,
-        img_pts,
+        corners_px,
         K,
         dist,
-        flags=cv2.SOLVEPNP_ITERATIVE
+        flags=cv2.SOLVEPNP_IPPE_SQUARE
     )
 
     if not ok:
-        raise RuntimeError("solvePnP failed")
+        ok, rvec, tvec = cv2.solvePnP(
+            obj_pts,
+            corners_px,
+            K,
+            dist,
+            flags=cv2.SOLVEPNP_ITERATIVE
+        )
 
-    R, _ = cv2.Rodrigues(rvec)
-    return R, tvec
+    if not ok:
+        return None, None
+
+    return rvec, tvec
+
+
+def rigid_transform_no_scale(A: np.ndarray, B: np.ndarray):
+    """Find R, t such that B ≈ R A + t for A,B: Nx3."""
+    assert A.shape == B.shape and A.shape[1] == 3, "A and B must be Nx3"
+    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)
+
+
+# ------------------------------------------------------------
+# Estimate camera pose from board marker center correspondences
+# ------------------------------------------------------------
+
+def build_camera_pose_from_board_markers(camera_id, scene_markers, robot_markers, K, dist, marker_size_m):
+    cam_centers = []
+    world_centers = []
+
+    for marker_id, marker_data in scene_markers.items():
+        mid = int(marker_id)
+        if mid not in robot_markers:
+            continue
+
+        for obs in marker_data.get("observations", []):
+            if obs.get("camera_id") != camera_id:
+                continue
+
+            corners_px = np.array(obs["corners_px"], dtype=np.float32)
+            rvec, tvec = solve_marker_pose(corners_px, K, dist, marker_size_m)
+            if rvec is None:
+                continue
+
+            cam_centers.append(tvec.flatten())
+            world_centers.append(robot_markers[mid])
+            break
+
+    if len(cam_centers) < 3:
+        return None, None
+
+    A = np.vstack(cam_centers)
+    B = np.vstack(world_centers)
+    R, t = rigid_transform_no_scale(A, B)
+    return R, t
 
 
 # ------------------------------------------------------------
@@ -126,6 +167,7 @@ def main():
     robot = load_json(args.robot)
 
     robot_markers = load_robot_markers(robot)
+    print(f"[INFO] Loaded {len(robot_markers)} board markers from robot.json")
 
     result = {
         "camera_poses": {}
@@ -142,19 +184,19 @@ def main():
         K = np.array(cam["camera_matrix"], dtype=np.float32)
         dist = np.array(cam["distortion_coefficients"], dtype=np.float32)
 
-        obj_pts, img_pts = build_correspondences(
+        R, t = build_camera_pose_from_board_markers(
             cam_id,
             scene["markers"],
             robot_markers,
+            K,
+            dist,
             args.marker_size
         )
 
-        if obj_pts is None:
-            print(f"[WARN] Camera {cam_id}: no valid markers")
+        if R is None:
+            print(f"[WARN] Camera {cam_id}: not enough board markers for pose estimation")
             continue
 
-        R, t = solve_camera(obj_pts, img_pts, K, dist)
-
         result["camera_poses"][cam_id] = {
             "R_world_from_cam": R.tolist(),
             "t_world_from_cam": t.flatten().tolist()