diff --git a/programs/03a_cameraPose.py b/programs/03a_cameraPose.py index 31bebe6..898933b 100644 --- a/programs/03a_cameraPose.py +++ b/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() diff --git a/test/01_read_ArUco.test.js b/test/01_read_ArUco.test.js index 1a9dcbb..a37c133 100644 --- a/test/01_read_ArUco.test.js +++ b/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}`); } diff --git a/test/02_build_scene.test.js b/test/02_build_scene.test.js index 2394d97..aa7f37f 100644 --- a/test/02_build_scene.test.js +++ b/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', () => { diff --git a/test/03a_cameraPose.test.js b/test/03a_cameraPose.test.js index 7f601c4..0c1a3b0 100644 --- a/test/03a_cameraPose.test.js +++ b/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); diff --git a/test/AA_readTwoImages.test.js b/test/AA_readTwoImages.test.js index 6d10a1c..6e82ce8 100644 --- a/test/AA_readTwoImages.test.js +++ b/test/AA_readTwoImages.test.js @@ -1,32 +1,52 @@ - 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"); + afterEach(() => { + if (tempDir && fs.existsSync(tempDir)) { + fs.rmSync(tempDir, { recursive: true, force: true }); + } + }); - 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')}`; + 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}`); - } - }); + try { + execSync(command2, { stdio: 'inherit' }); + } catch (error) { + fail(`Failed to execute command: ${error.message}`); + } + }); }); \ No newline at end of file diff --git a/test/data/screenShots/snapshot_video0_1778819665744_two_cam.csv b/test/data/screenShots/snapshot_video0_1778819665744_two_cam.csv deleted file mode 100644 index 430c7c6..0000000 --- a/test/data/screenShots/snapshot_video0_1778819665744_two_cam.csv +++ /dev/null @@ -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 diff --git a/test/data/screenShots/snapshot_video0_1778819665744_two_cam_annotated.jpg b/test/data/screenShots/snapshot_video0_1778819665744_two_cam_annotated.jpg deleted file mode 100644 index f61e4bc..0000000 Binary files a/test/data/screenShots/snapshot_video0_1778819665744_two_cam_annotated.jpg and /dev/null differ diff --git a/test/data/screenShots/snapshot_video0_1778819665744_two_cam_overlay.png b/test/data/screenShots/snapshot_video0_1778819665744_two_cam_overlay.png deleted file mode 100644 index 6cc1598..0000000 Binary files a/test/data/screenShots/snapshot_video0_1778819665744_two_cam_overlay.png and /dev/null differ diff --git a/test/data/screenShots/snapshot_video0_1779690911822.jpg b/test/data/screenShots/snapshot_video0_1779690911822.jpg new file mode 100644 index 0000000..fd4d347 Binary files /dev/null and b/test/data/screenShots/snapshot_video0_1779690911822.jpg differ diff --git a/test/data/screenShots/snapshot_video1_1778819665744_two_cam_annotated.jpg b/test/data/screenShots/snapshot_video1_1778819665744_two_cam_annotated.jpg deleted file mode 100644 index f6cc42f..0000000 Binary files a/test/data/screenShots/snapshot_video1_1778819665744_two_cam_annotated.jpg and /dev/null differ diff --git a/test/data/screenShots/snapshot_video1_1778819665744_two_cam_overlay.png b/test/data/screenShots/snapshot_video1_1778819665744_two_cam_overlay.png deleted file mode 100644 index 36d7c80..0000000 Binary files a/test/data/screenShots/snapshot_video1_1778819665744_two_cam_overlay.png and /dev/null differ diff --git a/test/data/screenShots/snapshot_video1_1779690911822.jpg b/test/data/screenShots/snapshot_video1_1779690911822.jpg new file mode 100644 index 0000000..9997f36 Binary files /dev/null and b/test/data/screenShots/snapshot_video1_1779690911822.jpg differ