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":
+        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(m["relPos"], dtype=np.float32)
+        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_IPPE_SQUARE
+    )
+
+    if not ok:
+        ok, rvec, tvec = cv2.solvePnP(
+            obj_pts,
+            corners_px,
             K,
             dist,
             flags=cv2.SOLVEPNP_ITERATIVE
         )
 
     if not ok:
-        raise RuntimeError("solvePnP failed")
+        return None, None
 
-    R, _ = cv2.Rodrigues(rvec)
-    return R, tvec
+    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()

test/01_read_ArUco.test.js

@@ -4,9 +4,26 @@ const fs = require('fs');
 const path = require('path');
 
 describe('Aruco Detection Script', () => {
-  const scriptPath = 'programs/01_read_Aruco_jpg_to_json.py';
+  const scriptPath = 'programs/01_read_AruCo_jpg_to_json.py';
   const calibrationFile = 'data/settings/callibration_cam0.npz';
-  const screenshotsDir = 'test/data/screenShots';
+  const sourceScreenshotsDir = path.join(__dirname, 'data', 'screenShots');
+  const screenshotFiles = [
+    'snapshot_video0_1778819665744.jpg',
+    'snapshot_video1_1778819665744.jpg',
+  ];
+
+  beforeEach(() => {
+    screenshotFiles.forEach((file) => {
+      const src = path.join(sourceScreenshotsDir, file);
+      if (!fs.existsSync(src)) {
+        throw new Error(`Missing test fixture screenshot: ${src}`);
+      }
+    });
+  });
+
+  afterEach(() => {
+    // Keep generated detection JSON files in the screenshot directory.
+  });
 
   test('should exist and be executable', () => {
     expect(fs.existsSync(scriptPath)).toBe(true);
@@ -17,38 +34,28 @@ describe('Aruco Detection Script', () => {
   });
 
   test('should have screenshot directory', () => {
-    expect(fs.existsSync(screenshotsDir)).toBe(true);
+    expect(fs.existsSync(sourceScreenshotsDir)).toBe(true);
   });
 
   test('should process screenshots successfully', () => {
-    const screenshots = fs.readdirSync(screenshotsDir)
-      .filter(file => file.endsWith('.jpg') || file.endsWith('.png'));
+    const screenshots = fs.readdirSync(sourceScreenshotsDir)
+      .filter(file => file.endsWith('.jpg') || file.endsWith('.png'))
+      .filter(file => screenshotFiles.includes(file));
 
     expect(screenshots.length).toBeGreaterThan(0);
 
     screenshots.forEach(screenshot => {
-      const screenshotPath = path.join(screenshotsDir, screenshot);
+      const screenshotPath = path.join(sourceScreenshotsDir, screenshot);
       const jsonPath = screenshotPath.replace(/\.(jpg|png)$/i, '_aruco_detection.json');
-      
-      // Lösche alte JSON-Datei, falls vorhanden
-      if (fs.existsSync(jsonPath)) {
-        fs.unlinkSync(jsonPath);
-      }
-      
-      // Führe das Python-Skript aus
-      const cmd = `python ${scriptPath} -i ${screenshotPath} -npz ${calibrationFile} -cameraId 0`;
+      const cmd = `python ${scriptPath} -i "${screenshotPath}" -npz "${calibrationFile}" -cameraId 0`;
 
       try {
         execSync(cmd, { stdio: 'inherit' });
-        
-        // Überprüfe, ob JSON-Datei erstellt wurde
         expect(fs.existsSync(jsonPath)).toBe(true);
 
-        // Lese und prüfe JSON-Inhalt
         const jsonData = JSON.parse(fs.readFileSync(jsonPath, 'utf8'));
         expect(jsonData).toBeDefined();
         expect(typeof jsonData).toBe('object');
-        
       } catch (error) {
         fail(`Failed to process ${screenshot}: ${error.message}`);
       }

test/02_build_scene.test.js

@@ -6,35 +6,46 @@ const path = require('path');
 describe('Build Scene JSON Script', () => {
   const scriptPath = 'programs/02_build_scene_json.py';
   const timestamp = 1778819665744;
-  const testDir = './test/data/screenShots';
+  const screenshotDir = path.join(__dirname, 'data', 'screenShots');
+  const detectionFiles = [
+    'snapshot_video0_1778819665744_aruco_detection.json',
+    'snapshot_video1_1778819665744_aruco_detection.json',
+  ];
+
+  beforeEach(() => {
+    detectionFiles.forEach((file) => {
+      const src = path.join(screenshotDir, file);
+      if (!fs.existsSync(src)) {
+        throw new Error(`Missing test fixture detection file: ${src}`);
+      }
+    });
+  });
+
+  afterEach(() => {
+    // Keep the generated scene JSON in the screenshot directory for real-world behavior.
+  });
 
   test('should exist and be executable', () => {
     expect(fs.existsSync(scriptPath)).toBe(true);
   });
 
   test('should build scene JSON with timestamp parameter', () => {
-    // Überprüfe, ob der Test-Ordner existiert
-    expect(fs.existsSync(testDir)).toBe(true);
-    
-    // Führe das Python-Skript mit den korrekten Parametern aus
-    const cmd = `python ${scriptPath} -timestamp ${timestamp} -dir ${testDir}`;
+    const cmd = `python ${scriptPath} -timestamp ${timestamp} -dir "${screenshotDir}"`;
 
     try {
-      //execSync(cmd, { stdio: 'inherit' });
-      //execSync(cmd, { stdio: 'pipe' });
-      execSync(cmd);
-      console.log("TEST START", process.pid, Date.now());
+      execSync(cmd, { stdio: 'inherit' });
     } catch (error) {
       throw new Error(`Failed to build scene JSON: ${error.message}`);
     }
-    // Überprüfe, ob die erwartete Ausgabedatei erstellt wurde
-    const expectedJsonFile = `./test/data/screenShots/scene_${timestamp}.json`;
+
+    const expectedJsonFile = path.join(screenshotDir, `scene_${timestamp}.json`);
     expect(fs.existsSync(expectedJsonFile)).toBe(true);
 
-    // Prüfe den Inhalt der JSON-Datei
     const jsonData = JSON.parse(fs.readFileSync(expectedJsonFile, 'utf8'));
     expect(jsonData).toBeDefined();
     expect(typeof jsonData).toBe('object');
+    expect(jsonData.cameras).toBeDefined();
+    expect(Object.keys(jsonData.cameras).sort()).toEqual(['0', '1']);
   });
 
   test('should handle timestamp parameter correctly', () => {

test/03a_cameraPose.test.js

@@ -7,10 +7,35 @@ describe('Camera Pose Script', () => {
   const projectRoot = path.resolve(__dirname, '..');
   const scriptPath = path.resolve(projectRoot, 'programs/03a_cameraPose.py');
   const timestamp = 1778819665744;
-  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`);
+  const screenshotDir = path.join(__dirname, 'data', 'screenShots');
+  const detectionFiles = [
+    'snapshot_video0_1778819665744_aruco_detection.json',
+    'snapshot_video1_1778819665744_aruco_detection.json',
+  ];
+  const robotFile = path.resolve(projectRoot, 'test/data/robot/robot.json');
+  const sceneFile = path.join(screenshotDir, `scene_${timestamp}.json`);
+  const outputFile = path.join(screenshotDir, `scene_${timestamp}_cameras.json`);
+
+  beforeEach(() => {
+    detectionFiles.forEach((file) => {
+      const src = path.join(screenshotDir, file);
+      if (!fs.existsSync(src)) {
+        throw new Error(`Missing test fixture detection file: ${src}`);
+      }
+    });
+
+    if (!fs.existsSync(sceneFile)) {
+      throw new Error(`Missing scene file generated by step 2: ${sceneFile}`);
+    }
+
+    if (fs.existsSync(outputFile)) {
+      fs.unlinkSync(outputFile);
+    }
+  });
+
+  afterEach(() => {
+    // Keep the generated scene and camera JSON in the screenshot directory.
+  });
 
   test('should exist and be executable', () => {
     expect(fs.existsSync(scriptPath)).toBe(true);

test/AA_readTwoImages.test.js

@@ -1,30 +1,50 @@
-
 const { execSync } = require('child_process');
 const fs = require('fs');
+const os = require('os');
 const path = require('path');
 
 const BASE_PATH = path.join(__dirname, '..');
 const PYTHON_CMD = process.platform === 'win32' ? 'python' : 'python3';
+const screenshotFiles = [
+  'snapshot_video0_1778819665744.jpg',
+  'snapshot_video1_1778819665744.jpg',
+];
+
+let tempDir;
 
 describe('Camera Pose Script', () => {
+  beforeEach(() => {
+    tempDir = fs.mkdtempSync(path.join(os.tmpdir(), 'AA_readTwoImages-'));
+    const sourceDir = path.join(__dirname, 'data', 'screenShots');
 
-   test('should build scene JSON with timestamp parameter', () => {
+    screenshotFiles.forEach((file) => {
+      const src = path.join(sourceDir, file);
+      const dst = path.join(tempDir, file);
+      if (!fs.existsSync(src)) {
+        throw new Error(`Missing test fixture screenshot: ${src}`);
+      }
+      fs.copyFileSync(src, dst);
+    });
+  });
 
-        const outDir = "test/data/screenShots";
-        const strFile0 = path.join(outDir, "snapshot_video0_1778819665744.jpg");
-        const strFile1 = path.join(outDir, "snapshot_video1_1778819665744.jpg");
-
-        const command2 = `${PYTHON_CMD} ${path.join(BASE_PATH, 'programs/readTwoImages.py')} \
-        -i ${strFile0} \
-        -i ${strFile1} \
-        -npz ${path.join(BASE_PATH, 'data/settings/callibration_cam0.npz')} \
-        -npz ${path.join(BASE_PATH, 'data/settings/callibration_cam1.npz')} \
-        -settings ${path.join(BASE_PATH, 'data/settings/settings1m.json')}`;
+  afterEach(() => {
+    if (tempDir && fs.existsSync(tempDir)) {
+      fs.rmSync(tempDir, { recursive: true, force: true });
+    }
+  });
 
+  test('should execute readTwoImages without modifying shared fixtures', () => {
+    const strFile0 = path.join(tempDir, 'snapshot_video0_1778819665744.jpg');
+    const strFile1 = path.join(tempDir, 'snapshot_video1_1778819665744.jpg');
+    const command2 = `${PYTHON_CMD} "${path.join(BASE_PATH, 'programs/readTwoImages.py')}" \
+      -i "${strFile0}" \
+      -i "${strFile1}" \
+      -npz "${path.join(BASE_PATH, 'data/settings/callibration_cam0.npz')}" \
+      -npz "${path.join(BASE_PATH, 'data/settings/callibration_cam1.npz')}" \
+      -settings "${path.join(BASE_PATH, 'data/settings/settings1m.json')}"`;
 
     try {
       execSync(command2, { stdio: 'inherit' });
-                
     } catch (error) {
       fail(`Failed to execute command: ${error.message}`);
     }

test/data/screenShots/snapshot_video0_1778819665744_two_cam.csv

@@ -1,19 +0,0 @@
-id,x_mm,y_mm,z_mm,roll_deg,pitch_deg,yaw_deg,seen_by
-camera 0,-288.32,-668.94,620.08,-127.297,-0.964,-47.004
-camera 1,19.49,-429.99,990.98,-160.689,-18.539,-15.506
-178,439.69,-460.64,28.28,20.063,23.695,124.140,2
-197,293.21,-130.34,56.04,41.568,89.171,-137.649,1
-198,330.33,-45.10,96.09,-2.443,0.707,-2.481,3
-200,259.80,-22.21,120.92,1.031,0.666,6.589,3
-201,222.85,54.74,107.27,87.712,0.106,-89.274,3
-204,257.00,127.56,129.27,-3.728,2.213,-4.119,3
-205,799.81,-91.06,1.74,154.524,-44.854,-117.977,3
-207,860.56,34.00,-80.07,77.108,-60.238,-49.231,2
-210,12.46,40.52,-36.47,-1.952,-0.234,1.515,1
-211,198.84,-0.23,0.85,-0.958,0.368,0.705,3
-215,199.06,-90.46,0.87,1.174,1.061,-0.775,3
-217,626.85,21.82,-50.90,1.053,2.929,-0.136,2
-226,423.39,-131.40,92.73,143.471,-41.661,-93.691,3
-228,418.38,-237.27,59.10,2.243,-36.678,0.143,2
-229,333.27,-135.08,95.13,125.337,-42.171,-76.089,3
-243,355.53,-153.74,39.82,91.028,1.926,2.884,1