Old Python new unitTest

Die alte Datei neu mit UnitTest überprüfen

programs/03a_cameraPose.py

@@ -53,24 +53,26 @@ def marker_corners_world(center, size_m):
 # Build correspondences for one camera
 # ------------------------------------------------------------
 
-def build_correspondences(camera, robot_markers, marker_size_m):
+def build_correspondences(camera_id, scene_markers, robot_markers, marker_size_m):
 
     obj_pts = []
     img_pts = []
 
-    for obs in camera["observations"]:
-        mid = int(obs["marker_id"])
-
+    for marker_id, marker_data in scene_markers.items():
+        mid = int(marker_id)
+        
         if mid not in robot_markers:
             continue
 
-        center = robot_markers[mid]
+        # 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_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)
+                obj_pts.append(obj_corners)
+                img_pts.append(img_corners)
 
     if len(obj_pts) == 0:
         return None, None
@@ -141,7 +143,8 @@ def main():
         dist = np.array(cam["distortion_coefficients"], dtype=np.float32)
 
         obj_pts, img_pts = build_correspondences(
-            cam,
+            cam_id,
+            scene["markers"],
             robot_markers,
             args.marker_size
         )