unfinished step 3a

programs/03a_cameraPose.py

@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+
+import argparse
+import json
+import numpy as np
+import cv2
+
+
+# ------------------------------------------------------------
+# Load JSON
+# ------------------------------------------------------------
+
+def load_json(path):
+    with open(path, "r", encoding="utf-8") as f:
+        return json.load(f)
+
+
+# ------------------------------------------------------------
+# Robot model: marker centers in world coordinates
+# ------------------------------------------------------------
+
+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)
+    return markers
+
+
+# ------------------------------------------------------------
+# Marker geometry (world frame)
+# ------------------------------------------------------------
+
+def marker_corners_world(center, size_m):
+    """
+    Returns 4 corners in consistent OpenCV order:
+    TL, TR, BR, BL
+    Marker lies in XY plane (z=0)
+    """
+    h = size_m / 2.0
+    x, y, z = center
+
+    return np.array([
+        [x - h, y + h, z],
+        [x + h, y + h, z],
+        [x + h, y - h, z],
+        [x - h, y - h, z],
+    ], dtype=np.float32)
+
+
+# ------------------------------------------------------------
+# Build correspondences for one camera
+# ------------------------------------------------------------
+
+def build_correspondences(camera, robot_markers, marker_size_m):
+
+    obj_pts = []
+    img_pts = []
+
+    for obs in camera["observations"]:
+        mid = int(obs["marker_id"])
+
+        if mid not in robot_markers:
+            continue
+
+        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
+
+
+# ------------------------------------------------------------
+# Solve PnP
+# ------------------------------------------------------------
+
+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")
+
+    ok, rvec, tvec = cv2.solvePnP(
+        obj_pts,
+        img_pts,
+        K,
+        dist,
+        flags=cv2.SOLVEPNP_ITERATIVE
+    )
+
+    if not ok:
+        raise RuntimeError("solvePnP failed")
+
+    R, _ = cv2.Rodrigues(rvec)
+    return R, tvec
+
+
+# ------------------------------------------------------------
+# Main
+# ------------------------------------------------------------
+
+def main():
+
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument("-scene", required=True)
+    parser.add_argument("-robot", required=True)
+    parser.add_argument("--marker_size", type=float, default=0.025)
+    parser.add_argument("-out", default="camera_poses.json")
+
+    args = parser.parse_args()
+
+    scene = load_json(args.scene)
+    robot = load_json(args.robot)
+
+    robot_markers = load_robot_markers(robot)
+
+    result = {
+        "camera_poses": {}
+    }
+
+    # --------------------------------------------------------
+    # Each camera independently
+    # --------------------------------------------------------
+
+    for cam_id, cam in scene["cameras"].items():
+
+        print(f"[INFO] Solving camera {cam_id}")
+
+        K = np.array(cam["camera_matrix"], dtype=np.float32)
+        dist = np.array(cam["distortion_coefficients"], dtype=np.float32)
+
+        obj_pts, img_pts = build_correspondences(
+            cam,
+            robot_markers,
+            args.marker_size
+        )
+
+        if obj_pts is None:
+            print(f"[WARN] Camera {cam_id}: no valid markers")
+            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()
+        }
+
+        print(f"[OK] Camera {cam_id} solved")
+
+    # --------------------------------------------------------
+    # Save
+    # --------------------------------------------------------
+
+    with open(args.out, "w", encoding="utf-8") as f:
+        json.dump(result, f, indent=2)
+
+    print(f"[DONE] Saved -> {args.out}")
+
+
+if __name__ == "__main__":
+    main()

test/03a_cameraPose.test.js

@@ -0,0 +1,49 @@
+// test/03a_cameraPose.test.js
+const { execSync } = require('child_process');
+const fs = require('fs');
+const path = require('path');
+
+describe('Camera Pose Script', () => {
+  const projectRoot = path.resolve(__dirname, '..');
+  const scriptPath = path.resolve(projectRoot, 'programs/03a_cameraPose.py');
+  const timestamp = 11778819665744;
+  const sceneFile = path.resolve(projectRoot, `test/data/screenShots/scene_${timestamp}.json`);
+  const robotDir = path.resolve(projectRoot, 'test/data/robot');
+  const robotFile = path.resolve(robotDir, 'robot.json');
+  const outputFile = path.resolve(projectRoot, `test/data/screenShots/scene_${timestamp}_cameras.json`);
+
+  test('should exist and be executable', () => {
+    expect(fs.existsSync(scriptPath)).toBe(true);
+  });
+
+  test('should have scene file', () => {
+    expect(fs.existsSync(sceneFile)).toBe(true);
+  });
+
+  test('should have robot file', () => {
+    expect(fs.existsSync(robotFile)).toBe(true);
+  });
+
+  test('should compute camera poses with timestamp parameter', () => {
+    // Führe das Python-Skript mit den korrekten Parametern aus
+    const cmd = `python "${scriptPath}" -scene "${sceneFile}" -robot "${robotFile}" -out "${outputFile}"`;
+    
+    try {
+      execSync(cmd, { stdio: 'inherit', cwd: projectRoot });  
+    } catch (error) {
+      throw new Error(`Failed to compute camera poses: ${error.message}`);
+    }
+    // Überprüfe, ob die erwartete Ausgabedatei erstellt wurde
+    expect(fs.existsSync(outputFile)).toBe(true);
+    
+    // Prüfe den Inhalt der JSON-Datei
+    const jsonData = JSON.parse(fs.readFileSync(outputFile, 'utf8'));
+    expect(jsonData).toBeDefined();
+    expect(typeof jsonData).toBe('object');
+    expect(jsonData).toHaveProperty('camera_poses');
+  });
+
+  test('should handle timestamp parameter correctly', () => {
+    expect(timestamp).toBe(11778819665744);
+  });
+});

test/data/robot/robot.json

@@ -0,0 +1,47 @@
+{
+    "recognized":{"x":null, "y":null, "z": null, "a":null, "b":null, "c":null, "e": null},
+    "Elements":["Board","Base","Arm1","Joint1","Arm2","Finger1","Finger2"], 
+    "ElementLength":{"Arm1":250, "Arm2":250, "Finger1":100, "Finger2":100},
+    "Joints":{
+        "jointA":{"name":"Slider",  "type":"lninear", "axis":[1,0,0],"parent":"Board","child":"Base"},
+        "jointB":{"name":"Shoulder","type":"revolute","axis":[1,0,0],"parent":"Base","child":"Arm1","origin":[-89.5, 115, 52], "originSource":[null, "229_198_Foto_5_2026", "Fuson"]},
+        "jointC":{"name":"EllbowLift","type":"revolute","axis":[1,0,0],"parent":"Arm1","child":"Joint1", "origin":[null, null, null]},
+        "jointD":{"name":"EllbowTwist","type":"revolute","axis":[0,1,0],"parent":"Joint1","child":"Arm2", "origin":[null, null, null]}
+    },
+    "MarkerType":"DICT_4X4_250",
+    "Marker":[
+        {"id":205,"on":"Board","position":[0.80, -0.090, 0.0]},
+        {"id":207,"on":"Board","position":[0.80, 0.0, 0.0]},        
+        {"id":208,"on":"Board","position":[0.50, -0.090, 0.0]},
+        {"id":210,"on":"Board","position":[0.00,  0.0,   0.0]},
+        {"id":211,"on":"Board","position":[0.20,  0.0,   0.0]},
+        {"id":214,"on":"Board","position":[0.40,  0.0,   0.0]},
+        {"id":215,"on":"Board","position":[0.20, -0.090, 0.0]},
+        {"id":217,"on":"Board","position":[0.60, -0.090, 0.0]},
+
+        {"id":200,"on":"Base","relPos":[-163.8,   6.5,  55], "relPosSource":["226_FotoAverage_5_2026",null,null]},
+        {"id":201,"on":"Base","relPos":[-164.8,  97.5,  74.5], "relPosSource":["226_FotoAverage_5_2026",null,null]},
+        {"id":204,"on":"Base","relPos":[-158.5,152.5,111]},
+              
+        {"id":198,"on":"Arm1","relPos":[-89.5,-160, 35],"relPosSource":["Fusion",null,null]},  
+        {"id":229,"on":"Arm1","relPos":[-89.5,-250, 35],"relPosSource":["Fusion",null,null]},        
+        {"id":242,"on":"Arm1","relPos":[-89.5,-250,-35]},        
+        {"id":243,"on":"Arm1","relPos":[-89.5,-285,  0]}, 
+
+        {"id":222,"on":"Joint1", "relPos":[0,0, -35]},
+        {"id":226,"on":"Joint1", "relPos":[0,0, 35]},
+
+        {"id":228,"on":"Arm2", "relPos":[-24.75, 112, 24.75],  "relPosSource":["Fusion","Fusion","Fusion"]},
+        {"id": -1,"on":"Arm2", "relPos":[-24.75, 182, 24.75],  "relPosSource":["Fusion","Fusion","Fusion"]},
+        {"id": -1,"on":"Arm2", "relPos":[-35,112,0],           "relPosSource":["Fusion","Fusion","Fusion"]},
+        {"id": -1,"on":"Arm2", "relPos":[-35,219,0],           "relPosSource":["Fusion","Fusion","Fusion"]},
+        {"id":223,"on":"Arm2", "relPos":[-28.67,112,-20.08],   "relPosSource":["Fusion","Fusion","Fusion"]},
+        {"id": -1,"on":"Arm2", "relPos":[0,182,-30],           "relPosSource":["Fusion","Fusion","Fusion"]},
+        {"id":218,"on":"Arm2", "relPos":[35,112,0],            "relPosSource":["Fusion","Fusion","Fusion"]},
+        {"id":219,"on":"Arm2", "relPos":[35,219,0],            "relPosSource":["Fusion","Fusion","Fusion"]},
+        {"id": -1,"on":"Arm2", "relPos":[24.75, 182, 24.75],   "relPosSource":["Fusion","Fusion","Fusion"]},
+        
+        {"id":218,"on":"Finger1","name":"A1","relPos":[-1.70,-25.14, 38.04]},
+        {"id":222,"on":"Finger1","name":"B1","relPos":[-14.55, 0.84, 74.79]}
+    ]
+}