diff --git a/programs/03a_cameraPose.py b/programs/03a_cameraPose.py index 55b7fed..31bebe6 100644 --- a/programs/03a_cameraPose.py +++ b/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 ) diff --git a/test/00_testSetup.json b/test/00_testSetup.json index e69de29..492e2a5 100644 --- a/test/00_testSetup.json +++ b/test/00_testSetup.json @@ -0,0 +1,16 @@ +[ + { + "name":"Test 0", + "robot":"test/data/robot/robot.json", + "image":[ + { + "timestamp":11778819665744, + "file":"test/data/screenShots/snapshot_video0_11778819665744.jpg" + }, + { + "timestamp":11778819665744, + "file":"test/data/screenShots/snapshot_video1_11778819665744.jpg" + } + ] + } +] \ No newline at end of file diff --git a/test/03a_cameraPose.test.js b/test/03a_cameraPose.test.js index 8315886..7f601c4 100644 --- a/test/03a_cameraPose.test.js +++ b/test/03a_cameraPose.test.js @@ -6,7 +6,7 @@ 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 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'); @@ -44,6 +44,6 @@ describe('Camera Pose Script', () => { }); test('should handle timestamp parameter correctly', () => { - expect(timestamp).toBe(11778819665744); + expect(timestamp).toBe(1778819665744); }); }); \ No newline at end of file diff --git a/test/AA_readTwoImages.test.js b/test/AA_readTwoImages.test.js new file mode 100644 index 0000000..6d10a1c --- /dev/null +++ b/test/AA_readTwoImages.test.js @@ -0,0 +1,32 @@ + +const { execSync } = require('child_process'); +const fs = require('fs'); +const path = require('path'); + +const BASE_PATH = path.join(__dirname, '..'); +const PYTHON_CMD = process.platform === 'win32' ? 'python' : 'python3'; + +describe('Camera Pose Script', () => { + + test('should build scene JSON with timestamp parameter', () => { + + 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')}`; + + + 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 new file mode 100644 index 0000000..430c7c6 --- /dev/null +++ b/test/data/screenShots/snapshot_video0_1778819665744_two_cam.csv @@ -0,0 +1,19 @@ +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 new file mode 100644 index 0000000..f61e4bc Binary files /dev/null and b/test/data/screenShots/snapshot_video0_1778819665744_two_cam_annotated.jpg 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 new file mode 100644 index 0000000..6cc1598 Binary files /dev/null and b/test/data/screenShots/snapshot_video0_1778819665744_two_cam_overlay.png 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 new file mode 100644 index 0000000..f6cc42f Binary files /dev/null and b/test/data/screenShots/snapshot_video1_1778819665744_two_cam_annotated.jpg 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 new file mode 100644 index 0000000..36d7c80 Binary files /dev/null and b/test/data/screenShots/snapshot_video1_1778819665744_two_cam_overlay.png differ