diff --git a/programs/03a_cameraPose.py b/programs/03a_cameraPose.py new file mode 100644 index 0000000..55b7fed --- /dev/null +++ b/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() \ No newline at end of file diff --git a/test/00_testSetup.json b/test/00_testSetup.json new file mode 100644 index 0000000..e69de29 diff --git a/test/03a_cameraPose.test.js b/test/03a_cameraPose.test.js new file mode 100644 index 0000000..8315886 --- /dev/null +++ b/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); + }); +}); \ No newline at end of file diff --git a/test/data/robot/robot.json b/test/data/robot/robot.json new file mode 100644 index 0000000..eb7739a --- /dev/null +++ b/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]} + ] +} \ No newline at end of file