Compare commits
3 Commits
222f0e55f7
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
|
908eb19c8d | ||
|
|
773e32c51c | ||
|
|
f7358cea8d |
3437
data/evaluations/Scene8/scene_reconstruction.json
Normal file
3437
data/evaluations/Scene8/scene_reconstruction.json
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
BIN
data/testPictures/calibration_cam0.npz
Normal file
BIN
data/testPictures/calibration_cam0.npz
Normal file
Binary file not shown.
BIN
data/testPictures/calibration_cam1.npz
Normal file
BIN
data/testPictures/calibration_cam1.npz
Normal file
Binary file not shown.
BIN
data/testPictures/calibration_cam2.npz
Normal file
BIN
data/testPictures/calibration_cam2.npz
Normal file
Binary file not shown.
BIN
data/testPictures/cam0_hires_1781074183695.jpg
Normal file
BIN
data/testPictures/cam0_hires_1781074183695.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 74 KiB |
BIN
data/testPictures/cam1_hires_1781074183695.jpg
Normal file
BIN
data/testPictures/cam1_hires_1781074183695.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 66 KiB |
BIN
data/testPictures/cam2_hires_1781074183695.jpg
Normal file
BIN
data/testPictures/cam2_hires_1781074183695.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 221 KiB |
433
data/testPictures/robot_1781069752019.json
Normal file
433
data/testPictures/robot_1781069752019.json
Normal file
@@ -0,0 +1,433 @@
|
||||
{
|
||||
"coordinateSystem": {"handedness": "right", "x": "right", "y": "backward", "z": "up"},
|
||||
"units": {"length": "mm", "rotation": "degree"},
|
||||
"vision_config": {"MarkerType": "DICT_4X4_250", "MarkerSize": 0.025},
|
||||
"renderingInfo": {
|
||||
"width": 1280,
|
||||
"height": 720,
|
||||
"renderDefaults": {"width": 1280, "height": 720, "dofFStop": 11},
|
||||
"cameraPosition__1": [-10, -800, 500],
|
||||
"cameraPosition__2": [-500, 300, 1200],
|
||||
"cameraPosition__3": [-200, -900, 200],
|
||||
"cameraPosition__4": [1200, 200, 300],
|
||||
"cameraPosition_a": [-300, -800, 500],
|
||||
"cameraPosition": [-200, 200, 1400],
|
||||
"cameraPosition_c": [600, -500, 600],
|
||||
"cameraTarget": [200, -200, 180],
|
||||
"cameraUpVector": [0, 0, 1],
|
||||
"lightPosition": [-500, -500, 500],
|
||||
"lightTarget": [0, 0, 0],
|
||||
"lightUpVector": [0, 0, 1],
|
||||
"metric": "mm",
|
||||
"showSkeleton": true,
|
||||
"showMarkers": true,
|
||||
"backgroundColor": [0.7, 0.85, 1.0],
|
||||
"backgroundStrength": 0.2,
|
||||
"sunEnergy": 0.35,
|
||||
"areaEnergy": 120,
|
||||
"exposure": -1.5,
|
||||
"lensDirt": true,
|
||||
"lensDirtStrength": 0.08,
|
||||
"dofEnabled": true,
|
||||
"dofFStop": 11.0,
|
||||
"arucoDust": true,
|
||||
"arucoDustStrength": 1.6,
|
||||
"markerOffsetMaxMm": 4.0,
|
||||
"markerOffsetSeed": 0,
|
||||
"markerRotationMaxDeg": 3,
|
||||
"motionBlur": true,
|
||||
"motionBlurMaxPx": 5.5,
|
||||
"focalErrorPct": 0.5,
|
||||
"principalErrorPx": 3.0,
|
||||
"residualDistortion": [0.02, -0.01],
|
||||
"localizedBlur": false,
|
||||
"localizedBlurStrength": 0.15,
|
||||
"vignette": true,
|
||||
"vignetteStrength": 0.08,
|
||||
"sensorNoise": true,
|
||||
"sensorNoiseStrength": 0.01,
|
||||
"lensDistortion": true,
|
||||
"lensDistortionStrength": 0.002,
|
||||
"materials": {
|
||||
"wood": {"baseColor": [0.72, 0.52, 0.33], "roughness": 0.85, "metallic": 0.0},
|
||||
"plaWhite": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.45, "metallic": 0.0},
|
||||
"steel": {"baseColor": [0.72, 0.72, 0.75], "roughness": 0.25, "metallic": 1.0},
|
||||
"powderCoatBlue": {"baseColor": [0.15, 0.25, 0.7], "roughness": 0.55, "metallic": 0.0},
|
||||
"defaultPlastic": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.4, "metallic": 0.0},
|
||||
"skeletonRed": {"baseColor": [0.85, 0.2, 0.2], "roughness": 0.35, "metallic": 0.0},
|
||||
"markerBlack": {"baseColor": [0.04, 0.04, 0.04], "roughness": 0.8, "metallic": 0.0}
|
||||
},
|
||||
"skeletonDefaults": {"radius": 4, "color": [0.85, 0.2, 0.2]},
|
||||
"markerDefaults": {"size": 25, "thickness": 1, "color": [0.04, 0.04, 0.04]},
|
||||
"defaultPosition": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3}
|
||||
},
|
||||
"defaultPosition__": {"x": 10, "y": 4, "z": 20, "a": 10, "b": 2, "c": 9, "e": 1},
|
||||
"defaultPosition": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
|
||||
"recognized": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null},
|
||||
"constraint_rules": {
|
||||
"rigid_distance": {"enabled": true, "mode": "mst", "weight": 1.0},
|
||||
"joint_axis_projection": {"enabled": true, "max_pairs": 2, "weight": 0.35},
|
||||
"chain_axis_projection": {"enabled": false, "max_depth": 3, "max_pairs": 2, "weight": 0.15},
|
||||
"axis_alignment_threshold": 0.95
|
||||
},
|
||||
"observation_weighting": {"enabled": true, "distance_weight": true, "marker_size_weight": true, "view_angle_weight": true},
|
||||
"multiview_calculation": {
|
||||
"combine_mode": "mean",
|
||||
"size_ref_px": 50.0,
|
||||
"border_ref_px": 120.0,
|
||||
"center_ref_norm": 0.01,
|
||||
"sharpness_ref": 2500.0,
|
||||
"homography_ref": 0.18,
|
||||
"size_factor": 0.3,
|
||||
"aspect_factor": 0.3,
|
||||
"border_factor": 0.01,
|
||||
"center_factor": 0.01,
|
||||
"sharpness_factor": 0.5,
|
||||
"homography_factor": 0.2,
|
||||
"normal_visibility_factor": 0.01,
|
||||
"spin_factor": 0.3,
|
||||
"weight_floor": 0.3
|
||||
},
|
||||
"pose_estimation": {
|
||||
"method": "hybrid",
|
||||
"marker_observation": "corner_pose",
|
||||
"use_normals": true,
|
||||
"normal_weight": 100.0,
|
||||
"robust_loss": "huber",
|
||||
"huber_delta_mm": 8.0,
|
||||
"max_iterations": 200,
|
||||
"min_cameras_per_marker": 2,
|
||||
"finger_block_joints": ["b", "c", "e"],
|
||||
"per_link_method": {}
|
||||
},
|
||||
"robot_test_poses": {
|
||||
"4": {"x": 70, "y": 50, "z": -70, "a": 120, "b": 50, "c": 30, "e": 20},
|
||||
"5": {"x": 180, "y": 86, "z": -120, "a": -60, "b": 22, "c": 91, "e": 10},
|
||||
"6": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3},
|
||||
"7": {"x": 30, "y": -2, "z": 95, "a": 20, "b": 23, "c": 9, "e": 9},
|
||||
"8": {"x": 50, "y": -2, "z": 95, "a": 20, "b": 60, "c": 9, "e": 3},
|
||||
"9": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
|
||||
"9a": {
|
||||
"x": 60,
|
||||
"y": -2,
|
||||
"z": 95,
|
||||
"a": 200,
|
||||
"b": 60,
|
||||
"c": 9,
|
||||
"e": 8,
|
||||
"rendering": {"width": 1440, "height": 1080, "dofFStop": 11}
|
||||
},
|
||||
"9b": {
|
||||
"x": 60,
|
||||
"y": -2,
|
||||
"z": 95,
|
||||
"a": 200,
|
||||
"b": 60,
|
||||
"c": 9,
|
||||
"e": 8,
|
||||
"rendering": {"width": 4896, "height": 3264, "dofFStop": 5.6}
|
||||
},
|
||||
"10": {"x": 120, "y": 60, "z": -110, "a": 20, "b": 30, "c": 180, "e": 4},
|
||||
"11": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
|
||||
"12": {"x": 50, "y": 0, "z": 178, "a": 210, "b": 80, "c": 90, "e": 6}
|
||||
},
|
||||
"test_camera_positions": {
|
||||
"a": [-300, -800, 800],
|
||||
"b": [300, -900, 1200],
|
||||
"c": [300, -900, 400],
|
||||
"d": [700, -800, 400],
|
||||
"e": [1200, -900, 400],
|
||||
"f": [500, -300, 1400],
|
||||
"g": [-200, 200, 1400]
|
||||
},
|
||||
"test_camera_targets": {
|
||||
"a": [210, -100, 180],
|
||||
"b": [310, -80, 180],
|
||||
"c": [210, -100, 150],
|
||||
"d": [210, -100, 150],
|
||||
"e": [210, -100, 50],
|
||||
"f": [200, -200, 180],
|
||||
"g": [200, -200, 180]
|
||||
},
|
||||
"movements": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null},
|
||||
"state_pose_params": {
|
||||
"numbers_of_Elements_to_consider_start": 3,
|
||||
"numbers_of_Elements_to_consider_final": 5,
|
||||
"solver_in_between_geometrical": false,
|
||||
"solver_after_geometrical": false,
|
||||
"geometric_passes_per_stage": 2,
|
||||
"revolute_search_coarse_deg": 5.0,
|
||||
"revolute_search_fine_deg": 1.0,
|
||||
"root_pose_min_markers": 3,
|
||||
"use_marker_normals_flip_tiebreak": true,
|
||||
"normal_flip_weight": 0.05
|
||||
},
|
||||
"links": {
|
||||
"Board": {
|
||||
"parent": null,
|
||||
"size": [1000, 200, 25],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"skeleton": {"from": [0, 0, 16], "to": [1000, 0, 16], "radius": 4, "color": [0.85, 0.2, 0.2]},
|
||||
"markers": [
|
||||
{
|
||||
"id": 46,
|
||||
"set": "A0",
|
||||
"position": [536.71, 185.44, -27.3],
|
||||
"normal": [0, 0, 1],
|
||||
"spin": 90,
|
||||
"info": "is placed on a white paper, A0_60Arucos_25mm_Seet223.pdf, with the following marker placements:"
|
||||
},
|
||||
{"id": 47, "set": "A0", "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 48, "set": "A0", "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 49, "set": "A0", "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 50, "set": "A0", "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 51, "set": "A0", "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 52, "set": "A0", "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 53, "set": "A0", "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 54, "set": "A0", "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 55, "set": "A0", "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 56, "set": "A0", "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 57, "set": "A0", "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 58, "set": "A0", "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 59, "set": "A0", "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 60, "set": "A0", "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 61, "set": "A0", "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 62, "set": "A0", "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 63, "set": "A0", "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 64, "set": "A0", "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 65, "set": "A0", "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 66, "set": "A0", "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 67, "set": "A0", "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 68, "set": "A0", "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 69, "set": "A0", "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 70, "set": "A0", "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 71, "set": "A0", "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 72, "set": "A0", "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 73, "set": "A0", "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 74, "set": "A0", "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 75, "set": "A0", "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 76, "set": "A0", "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 77, "set": "A0", "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 78, "set": "A0", "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 79, "set": "A0", "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 80, "set": "A0", "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 81, "set": "A0", "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 82, "set": "A0", "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 83, "set": "A0", "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 84, "set": "A0", "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 85, "set": "A0", "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 86, "set": "A0", "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 87, "set": "A0", "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 88, "set": "A0", "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 89, "set": "A0", "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 90, "set": "A0", "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 91, "set": "A0", "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 92, "set": "A0", "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 93, "set": "A0", "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 94, "set": "A0", "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 95, "set": "A0", "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 96, "set": "A0", "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 97, "set": "A0", "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 98, "set": "A0", "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 99, "set": "A0", "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 100, "set": "A0", "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 101, "set": "A0", "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 102, "set": "A0", "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 103, "set": "A0", "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 104, "set": "A0", "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 105, "set": "A0", "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Board.stl",
|
||||
"originOfModel": [0, 0, 0],
|
||||
"rotationOfModelDegree": [0, 0, -90],
|
||||
"material": "wood"
|
||||
},
|
||||
{
|
||||
"stlFile": "surfaces/BoardRail.stl",
|
||||
"originOfModel": [0, 0, 0],
|
||||
"rotationOfModelDegree": [0, 0, -90],
|
||||
"material": "steel"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Base": {
|
||||
"parent": "Board",
|
||||
"size": [150, 200, 150],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [0, 0, 16],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "x"
|
||||
},
|
||||
"skeleton": {"from": [0, 108, 45], "to": [110, 108, 45], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Base.stl",
|
||||
"originOfModel": [-30, 0, -35],
|
||||
"rotationOfModelDegree": [0, 0, 0],
|
||||
"material": "plaWhite"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Arm1": {
|
||||
"parent": "Base",
|
||||
"size": [70, 250, 70],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint1",
|
||||
"type": "revolute",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [110, 108, 45],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "y"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.2, 0.2, 0.9]},
|
||||
"markers": [ ],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Holm.stl",
|
||||
"originOfModel__": [-25, 29, -28.5],
|
||||
"originOfModel": [-29, 25, 28.5],
|
||||
"rotationOfModelDegree__": [0, 0, 0],
|
||||
"rotationOfModelDegree": [180, 0, -90],
|
||||
"material": "powderCoatBlue"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Ellbow": {
|
||||
"parent": "Arm1",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint2",
|
||||
"type": "revolute",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [0, -250, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "z"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [90, 0, 0], "radius": 4, "color": [0.9, 0.2, 0.2]},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Ellebogen.stl",
|
||||
"originOfModel": [90, 0, 0],
|
||||
"rotationOfModelDegree": [0, -90, -90],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
],
|
||||
"markers": [
|
||||
]
|
||||
},
|
||||
"Arm2": {
|
||||
"parent": "Ellbow",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint3",
|
||||
"type": "revolute",
|
||||
"axis": [0, -1, 0],
|
||||
"origin": [90, 0, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "a"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.95, 0.85, 0.2]},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Unterarm.stl",
|
||||
"originOfModel": [0, -250, 0],
|
||||
"rotationOfModelDegree": [180, 0, -90],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
],
|
||||
"markers": [
|
||||
]
|
||||
},
|
||||
"Hand": {
|
||||
"parent": "Arm2",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint4",
|
||||
"type": "revolute",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [0, -250, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "b"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -35, 0], "radius": 4, "color": [0.95, 0.55, 0.15]}
|
||||
},
|
||||
"Palm": {
|
||||
"parent": "Hand",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint3",
|
||||
"type": "revolute",
|
||||
"axis": [0, -1, 0],
|
||||
"origin": [0, 0, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "c"
|
||||
},
|
||||
"skeleton": {"from": [-50, -35, 0], "to": [50, -35, 0], "radius": 7, "color": [0.95, 0.2, 0.2]}
|
||||
},
|
||||
"FingerA": {
|
||||
"parent": "Palm",
|
||||
"size": [80, 60, 20],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [4, -35, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "e"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [24, 0, -9.1],
|
||||
"rotationOfModelDegree": [90, -90, 0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
},
|
||||
"FingerB": {
|
||||
"parent": "Palm",
|
||||
"size": [80, 60, 20],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [-4, -35, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "e"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [-24, 0, 9.1],
|
||||
"rotationOfModelDegree": [90, 90, 0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
357
doc/callibrate_scene_roadmap.md
Normal file
357
doc/callibrate_scene_roadmap.md
Normal file
@@ -0,0 +1,357 @@
|
||||
# Roadmap: Szenen-Kalibrierung der Board-/Loose-Marker (`callibrate_scene`)
|
||||
|
||||
Status: **Vorschlag / zur Abstimmung**
|
||||
Ort der Arbeit: `pipeline/` (nicht `approbot-pipeline/`, das bleibt die eingefrorene Kopie)
|
||||
Datum: 2026-06-04
|
||||
|
||||
> Eingearbeitete Entscheidungen:
|
||||
> 1. **Gelenkzustand UNBEKANNT** → wird mitgeschätzt (kein FK-Welt-Anker a priori).
|
||||
> 2. **Set-Definition direkt in `robot.json`** über ein optionales `"set"`-Feld je Marker.
|
||||
> Marker bleiben im bisherigen Format an ihrem Link. Gleiches `set` = ein starr zusammenhängendes
|
||||
> Objekt mit **fixen relativen Bezügen**. **Kein `set` = loser Marker** (fix, aber Lage z.Zt. unbekannt).
|
||||
> 3. **Welt = Roboter (Konvention 2)**, Roboter steht *nicht* bei 0/0/0.
|
||||
> 4. **Primär eine Aufnahme (7 Bilder)**, *ohne* zusätzliche Base-Marker; mehrere Posen als Fallback.
|
||||
> 5. **Ausgabe vorerst `robot.calibrated.json`** (Debugging); später in-place nach `robot.json`.
|
||||
> 6. **Code generisch & `robot.json`-unabhängig.** `robot.json` ist nur ein Beispiel und wird später
|
||||
> gegen ein anderes geprüft. KEINE festen Marker-IDs, Link-/Set-Namen, Achsen oder Gelenk-Variablen
|
||||
> im Code (Auto-Discovery aus den Daten). Daten-spezifisches (z.B. die `set`-Zuordnung) gehört in
|
||||
> `robot.json`, nicht in den Code.
|
||||
> 7. **Aktuelle Marker-Positionen = brauchbare Startwerte.** Die relativen Bezüge innerhalb jedes Sets
|
||||
> gelten als korrekt → direkte Grundlage für den Kabsch-Fit und die BA-Initialisierung.
|
||||
|
||||
---
|
||||
|
||||
## 0. Machbarkeit — Kurzurteil
|
||||
|
||||
**Ja, machbar** — die anspruchsvollere Variante, weil der Gelenkzustand mitgeschätzt wird und es
|
||||
keinen a-priori Welt-Anker gibt (weder Board noch Arm). Vorgehen:
|
||||
|
||||
> **Erst ankerlos rekonstruieren** (metrisch, aus der bekannten Marker-Größe), **dann den Roboter
|
||||
> in die Rekonstruktion einpassen** — der eingepasste Roboter definiert die Welt — **dann die Sets
|
||||
> einpassen** und die Marker-Positionen aktualisieren.
|
||||
|
||||
Bausteine im Bestand: Per-Marker-PnP (`solve_single_marker_pose`), Eck-Triangulation
|
||||
(`3b_corner_marker_poses.py`), Bündelausgleichung (`3_multiview_bundle_adjustment_v5...`),
|
||||
FK + θ-Schätzung (`pose_estimation.py`, `robot_fk.py`), Kabsch-Fit (`rigid_transform_no_scale`).
|
||||
|
||||
**Ein Beobachtbarkeits-Knackpunkt** für „eine Aufnahme genügt ohne Base-Marker" steht in §7 — er ist
|
||||
beherrschbar, aber bewusst zu entscheiden.
|
||||
|
||||
---
|
||||
|
||||
## 1. Problem & Zielbild
|
||||
|
||||
**Realität:** Höhe/Orientierung zwischen Board und Arm sind ungenau. Marker ~20–105 liegen fix,
|
||||
aber unbekannt: teils Board-Platte, teils darunterliegendes A0-Blatt, teils einzeln aufgeklebt.
|
||||
|
||||
**Vertrauenswürdig:** die *interne Geometrie* der Roboter-Links und die *relativen Bezüge innerhalb
|
||||
jedes Sets* (beide in `robot.json` hinterlegt), sowie die *Marker-Kantenlänge* (25 mm → Maßstab).
|
||||
**Unbekannt:** Gelenkwinkel, Kamera-Posen, Platzierung jedes Sets, Lage jedes losen Markers.
|
||||
|
||||
**Ziel:** Nach der Kalibrierung hat jedes Set (als starres Objekt korrekt platziert) und jeder lose
|
||||
Marker (einzeln vermessen) eine korrekte Pose in einem roboter-verankerten Weltsystem.
|
||||
|
||||
### Marker-Klassen (aus dem `set`-Feld abgeleitet)
|
||||
|
||||
| Klasse | Erkennung | bekannt | zu schätzen |
|
||||
|---|---|---|---|
|
||||
| **Arm-Marker** (Roboter) | liegen an Arm-Links (Arm1…Finger) | Lage je Link | — definieren via Fit die Welt |
|
||||
| **Set-Marker** (starr) | `"set": "A0"`, `"set":"Brett"`, … | interne Relativlage (fix) | 6-DoF-Platzierung je Set |
|
||||
| **Lose Marker** | **kein** `set`-Feld | nur „fix vorhanden" | je Marker eigene 6-DoF-Pose |
|
||||
|
||||
---
|
||||
|
||||
## 2. Set-Definition: `set`-Feld am Marker (kein Strukturumbau)
|
||||
|
||||
Marker bleiben **wie bisher** in der `markers`-Liste ihres Links. Ein optionales `"set"` gruppiert sie:
|
||||
|
||||
```jsonc
|
||||
"Board": {
|
||||
"parent": null, "mountPosition": [0,0,0], "mountRotation": [0,0,0],
|
||||
"markers": [
|
||||
{ "id": 210, "position": [ 20,-20,0.3], "normal": [0,0,1], "set": "A0" },
|
||||
{ "id": 211, "position": [250,-10,0.3], "normal": [0,0,1], "set": "A0" },
|
||||
{ "id": 215, "position": [250,-90,0.3], "normal": [0,0,1], "set": "Brett" },
|
||||
{ "id": 208, "position": [350,-90,0.3], "normal": [0,0,1], "set": "Brett" },
|
||||
{ "id": 205, "position": [750,-90,0.3], "normal": [0,0,1] } // kein set -> lose
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
- **Gleiches `set` ⇒ ein starres Objekt.** Die relativen Bezüge der Marker im Set gelten als **fix**;
|
||||
die Kalibrierung bestimmt nur die 6-DoF-Platzierung des ganzen Sets und schreibt die daraus
|
||||
resultierenden Positionen zurück (Format unverändert, relative Anordnung erhalten).
|
||||
- **Kein `set` ⇒ loser Marker.** Wird einzeln (Position + Normale + ggf. Spin) vermessen.
|
||||
- **Arm-Marker** brauchen kein `set`: ihr Link ist bereits ein starrer Körper und dient als
|
||||
Roboter-Referenz (sie werden *nicht* kalibriert, sondern definieren die Welt).
|
||||
- **Auto-Discovery** (Projekt-Konvention): Sets ergeben sich aus den `set`-Werten, nichts hartkodiert.
|
||||
|
||||
Hinweis: `robot_fk.py` / `all_markers_world()` bleiben unverändert — das `set`-Feld ist reine
|
||||
Zusatzinfo, die nur der Kalibrier-Treiber auswertet.
|
||||
|
||||
---
|
||||
|
||||
## 3. Algorithmus (Gelenkzustand unbekannt)
|
||||
|
||||
### Phase A — Ankerlose, metrische Rekonstruktion
|
||||
1. **Detektion** (Schritt 1) → Ecken je Kamera.
|
||||
2. **Per-Marker-PnP** je Kamera aus der bekannten Marker-Größe (`SOLVEPNP_IPPE_SQUARE`) → volle
|
||||
Marker-Pose *relativ zur Kamera*. Kein Welt-Anker nötig.
|
||||
3. **Relativer Posen-Graph:** gemeinsam gesehene Marker verknüpfen Kamerapaare → Init aller Kamera-
|
||||
und Marker-Posen in einem *beliebigen* Szenen-Frame `S`.
|
||||
4. **Globale Bündelausgleichung** (scipy `least_squares`, Huber): verfeinert alle Kamera- und
|
||||
Marker-Posen über die Reprojektion aller Ecken. Maßstab fix durch Marker-Größe.
|
||||
|
||||
→ konsistente, metrische 3D-Szene (Arm- **und** Set-/Loose-Marker) in `S`.
|
||||
|
||||
### Phase B — Roboter einpassen = Welt definieren
|
||||
5. Arm-Marker per ID → Link zuordnen. **Fit** von Gelenkwinkeln θ **und** der Platzierung der
|
||||
FK-Wurzel in `S`, sodass `FK(θ)` der Arm-Marker die rekonstruierten Arm-Positionen trifft
|
||||
(erweitert `pose_estimation.py` um eine freie Wurzel-Platzierung statt fixer Identität).
|
||||
6. In das Weltsystem rücktransformieren. Welt-Ursprung = FK-Wurzel-Frame (= heutiges „Board"-Frame),
|
||||
Roboter sitzt mit dem fertigen θ darin — *nicht* bei 0/0/0 (§6).
|
||||
|
||||
### Phase C — Set-Fit & Rückschreiben
|
||||
7. **Set-Marker (pro `set`):** Kabsch (`rigid_transform_no_scale`) bildet die **fixe interne Lage**
|
||||
auf die rekonstruierten Welt-Positionen ab → 6-DoF-Set-Platzierung → aktualisierte Positionen
|
||||
(= Platzierung ∘ interne Lage). Auch nicht gesehene Set-Marker erhalten so eine Position, sofern
|
||||
das Set über ≥3 nicht-kollineare Marker bestimmt ist.
|
||||
**Lose Marker:** triangulierte Pose direkt übernehmen.
|
||||
8. **Rückschreiben** nach `robot.calibrated.json` (Marker-Format unverändert, `set`-Felder erhalten)
|
||||
+ `calibration_report.json` (je Set die explizite Verschiebung/Verdrehung + RMS; je Marker Status).
|
||||
Nicht beobachtbare Größen → **`null`** (nie 0).
|
||||
|
||||
### Fallback — mehrere Posen (statische Kameras)
|
||||
Mehrere Gelenkzustände bei festen Kameras: Kamera-Posen + Set-/Loose-Posen + Wurzel-Platzierung sind
|
||||
**geteilte** Unbekannte, je Pose ein eigener θ-Satz. Löst die §7-Schwächen vollständig auf.
|
||||
|
||||
---
|
||||
|
||||
## 4. Eingaben & Ausgaben
|
||||
|
||||
**Eingaben:** `robot.json` (Arm-Geometrie + `set`-Felder + fixe interne Set-Lagen);
|
||||
Szenen-Ordner mit `render_*.png` **oder** vorhandene `*_aruco_detection.json`.
|
||||
(Gelenkzustand wird NICHT benötigt.)
|
||||
|
||||
**Ausgaben:** `robot.calibrated.json` (aktualisierte Marker-Positionen, Format wie bisher);
|
||||
`calibration_report.json` (je Set: Verschiebung/Verdrehung, RMS, #Kameras/#Marker, Status;
|
||||
je losem Marker: Pose oder `null`). Optional Viewer-Overlay Soll↔kalibriert.
|
||||
|
||||
---
|
||||
|
||||
## 5. Neue / geänderte Dateien
|
||||
|
||||
| Datei | Art | Inhalt |
|
||||
|---|---|---|
|
||||
| `pipeline/calibrate_scene.py` | **neu** | Treiber: Auto-Discovery Kameras+Sets, Phase A→B→C, schreibt `robot.calibrated.json`+Report |
|
||||
| `pipeline/scene_reconstruct.py` | **neu** | Phase A: Per-Marker-PnP, Posen-Graph, globale BA (ankerlos) |
|
||||
| `pipeline/robot_register.py` | **neu** | Phase B: Fit θ + freie Wurzel-Platzierung (nutzt `robot_fk`) |
|
||||
| `pipeline/marker_sets.py` | **neu** | liest `set`-Felder aus `robot.json`; Klassifizierung Arm/Set/Lose |
|
||||
| `3b_corner_marker_poses.py` | **erweitern** | volle Marker-**Rotation** (Normale + Spin) aus 4 Ecken |
|
||||
| `pose_estimation.py` | **erweitern** | optionale freie Wurzel-Platzierung (für Phase B wiederverwendbar) |
|
||||
|
||||
`2_estimate_camera_from_observations.py` / `robot_fk.py`: voraussichtlich **unverändert**.
|
||||
|
||||
---
|
||||
|
||||
## 6. Weltursprung (Konvention 2, Roboter nicht bei 0/0/0)
|
||||
|
||||
- Ursprung = FK-Wurzel-Frame (heute „Board"). Der Roboter sitzt mit seinem modellierten Versatz
|
||||
(`Base.jointToParent.origin` + Slider `x`) darin → **nicht** bei 0/0/0. Das deckt den Wunsch ab.
|
||||
- „Welt durch Roboter definiert" wird dadurch realisiert, dass die **Kalibrierung am Roboter
|
||||
verankert** (Fit θ + Wurzel-Platzierung über Arm-Marker), statt den Board-Markern zu vertrauen.
|
||||
Die Board-Positionen werden konsistent *neu* abgeleitet.
|
||||
- Der Kinematik-Baum bleibt unverändert. (Optionaler späterer Umbau „Base = Wurzel" möglich, aber
|
||||
für die Kalibrierung nicht nötig.)
|
||||
|
||||
---
|
||||
|
||||
## 7. Beobachtbarkeit — Einzelaufnahme ohne Base-Marker (wichtig)
|
||||
|
||||
Verifiziert an `robot.json`: `Base`, `Hand`, `Palm` haben **keine** Marker; erster markierter Link
|
||||
ist `Arm1`. Daraus folgt für EINE Pose mit unbekannten Gelenkwinkeln:
|
||||
|
||||
- **Slider `x` und `Joint1 y` sind nicht von der absoluten Roboter-Platzierung trennbar** (2-DoF-
|
||||
Gauge-Freiheit): eine Verschiebung entlang der Schiene ≙ Änderung von `x`; eine Drehung um die
|
||||
`Joint1`-Achse ≙ Änderung von `y`. Die Set-/Loose-Marker erben diese 2 Freiheitsgrade.
|
||||
- **Gut bestimmt aus einer Pose:** `z, a, b, c, e` und damit die gesamte Szene *relativ*.
|
||||
|
||||
Da Base-Marker mechanisch unerwünscht sind, der empfohlene Weg:
|
||||
|
||||
- **Gauge per Konvention fixieren** (Default für Einzelaufnahme): `x`,`y` auf die gefittete
|
||||
Konfiguration / nominale Schienen-Null setzen und die Welt so definieren. Ergebnis ist
|
||||
**in sich konsistent** → für künftige Pose-Schätzung (Board als Anker) voll nutzbar; lediglich
|
||||
der *absolute* Schienen-Nullpunkt und die `Joint1`-Null sind dann Konvention, kein Messwert.
|
||||
- **Mehrere Posen** (Fallback), wenn die absolute Basis-Lage / absolute `x`,`y` wirklich gebraucht
|
||||
werden — das löst die 2 Freiheitsgrade vollständig auf.
|
||||
- *(optional, falls je möglich:* ein einzelner Base-/Schlitten-Marker würde Einzelaufnahme voll
|
||||
beobachtbar machen — derzeit zurückgestellt.)*
|
||||
|
||||
QA: Reprojektions-RMS je Kamera; Set-Fit-Residuum (mm); Co-Visibility-Graph zusammenhängend?;
|
||||
≥3 nicht-kollineare Marker je Set; ≥2 Kameras je losem Marker (sonst Status `partial`/`unobserved`).
|
||||
|
||||
---
|
||||
|
||||
## 8. Validierung (Sim zuerst)
|
||||
|
||||
`pose.json` liefert in der Simulation GT-Gelenkwinkel **und** Kamera-Pos/Targets:
|
||||
1. Bekannte Sets künstlich verschieben/verdrehen → kalibrieren → Rück-Transform gegen Soll.
|
||||
2. Gefittete θ gegen GT-θ; gefittete Kamera-Posen gegen GT.
|
||||
3. Einzelaufnahme- vs. Mehrfach-Posen-Genauigkeit quantifizieren (belegt §7).
|
||||
4. Erst danach `data/recorded/`-Szenen.
|
||||
|
||||
---
|
||||
|
||||
## 9. Phasen / Meilensteine
|
||||
|
||||
- **P0 — `set`-Felder & Parser:** `set`-Felder in `robot.json` ergänzen; `marker_sets.py`
|
||||
(Arm/Set/Lose-Klassifizierung); FK-Welt-Positionen unverändert verifizieren.
|
||||
- **P1 — Ankerlose Rekonstruktion (Phase A):** Per-Marker-PnP + Posen-Graph + globale BA; gegen
|
||||
GT-Kamera-Posen (Sim) prüfen.
|
||||
- **P2 — Roboter-Registrierung (Phase B):** Fit θ + freie Wurzel-Platzierung; gegen GT-θ; §7-Gauge.
|
||||
- **P3 — Set-Fit & Rückschreiben (Phase C):** Kabsch + Loose → `robot.calibrated.json` + Report;
|
||||
Sim-Validierung mit künstlichem Offset.
|
||||
- **P4 — Mehrfach-Posen-Fallback.**
|
||||
- **P5 — Reale Szenen + Viewer-Overlay; danach in-place nach `robot.json`.**
|
||||
|
||||
---
|
||||
|
||||
## 10. Verbleibende kleinere Punkte
|
||||
|
||||
1. **Gauge-Konvention für Einzelaufnahme** (§7): `x`,`y` = gefittet, oder Schiene/`Joint1` auf
|
||||
nominal? (Beeinflusst nur den absoluten Nullpunkt, nicht die Set-Relativlagen.)
|
||||
2. **Set-Namensraum:** sind `set`-Namen global eindeutig oder pro Link? (Vorschlag: global, z.B.
|
||||
„A0", „Brett" — ein Set = ein physisches Objekt.)
|
||||
3. **Lose-Marker-Orientierung:** reicht Position + Normale, oder wird der Spin (Drehung um die
|
||||
Normale) gebraucht? (Bestimmt die nötige Genauigkeit von Phase C / 3b.)
|
||||
|
||||
---
|
||||
|
||||
## 11. Umsetzungs-Log: Mathematik & Anwendung
|
||||
|
||||
Pro abgeschlossener Phase: *was* gemacht wurde, *welche Mathematik* dahinter steckt, *wie* man es
|
||||
anwendet. (Wird mit jeder Phase fortgeschrieben.)
|
||||
|
||||
### P0 — Marker-Klassifizierung & `set`-Tags — ✅ erledigt (2026-06-04)
|
||||
|
||||
**Was gemacht**
|
||||
- Neu: `pipeline/marker_sets.py` — liest ein beliebiges `robot.json`, klassifiziert jeden Marker in
|
||||
`arm` / `set` / `loose` und gibt einen Report aus. Vollständig generisch (keine festen Namen/IDs).
|
||||
- Daten: `data/robot/robot.json` mit `set`-Tags versehen — `Brett` (9 Board-Oberflächen-Marker,
|
||||
z≈0.3) und `A0` (60 Papier-Marker, z≈−27.3). Chirurgisch eingefügt (kompaktes Custom-Format der
|
||||
Datei bleibt erhalten, nur +1 Zeile), FK numerisch exakt invariant.
|
||||
|
||||
**Mathematik / Logik**
|
||||
|
||||
1) *Statisch (Welt) vs. beweglich (Roboter):* Ein Link `L` ist beweglich, wenn auf dem Pfad
|
||||
`L → Wurzel` mindestens ein Gelenk vom Typ revolute/linear liegt:
|
||||
```
|
||||
movable(L) = ∃ A ∈ chain(L→root): type(jointToParent(A)) ∈ {revolute, linear}
|
||||
```
|
||||
Die Wurzel und nur über `fixed` angebundene Links sind statisch (= Welt). Generisch, da nur
|
||||
Gelenk-Typen geprüft werden — keine Link-Namen.
|
||||
|
||||
2) *Rollen* eines Markers `m` auf Link `L`:
|
||||
```
|
||||
role(m) = arm falls movable(L) → Welt-Referenz, NICHT kalibriert
|
||||
= set(s) falls ¬movable(L) ∧ m.set = s → starres Objekt, 6-DoF kalibrieren
|
||||
= loose falls ¬movable(L) ∧ m hat kein set → einzeln vermessen
|
||||
```
|
||||
|
||||
3) *Modell eines starren Sets `S`* (die eigentliche Kalibriergröße der Phase C):
|
||||
Marker `i` hat eine **bekannte, fixe** lokale Lage `p_iˡᵒᵏ` (die vertrauten relativen Bezüge).
|
||||
Das Set hat eine **unbekannte** starre Platzierung `(R_S, t_S) ∈ SE(3)`:
|
||||
```
|
||||
p_iʷᵉˡᵗ = R_S · p_iˡᵒᵏ + t_S R_S ∈ SO(3) (Verdrehung), t_S ∈ ℝ³ (Verschiebung)
|
||||
```
|
||||
`(R_S, t_S)` = die gesuchten 6 DoF je Set. Schätzung aus gemessenen Welt-Positionen `q_i`
|
||||
per **Kabsch / orthogonalem Procrustes** (ohne Skalierung):
|
||||
```
|
||||
min_{R∈SO(3), t} Σ_i ‖ R·p_iˡᵒᵏ + t − q_i ‖²
|
||||
p̄=mean(pˡᵒᵏ), q̄=mean(q), H = Σ (p_iˡᵒᵏ−p̄)(q_i−q̄)ᵀ, U Σ Vᵀ = svd(H)
|
||||
R = V·diag(1,1,det(V Uᵀ))·Uᵀ, t = q̄ − R·p̄
|
||||
```
|
||||
(vorhanden als `rigid_transform_no_scale`). Weil die aktuellen `robot.json`-Positionen brauchbare
|
||||
Startwerte sind und die relativen Bezüge stimmen, gilt `p_iˡᵒᵏ` = aktuelle Set-Positionen
|
||||
(ggf. zentriert) und `(R_S,t_S) ≈ Identität` als Initialwert.
|
||||
|
||||
4) *Loser Marker:* eigene unbekannte Pose `(R_m, t_m)`, einzeln aus den triangulierten Ecken
|
||||
(Position = Eckmittel, Orientierung aus Eckebene + Eckreihenfolge).
|
||||
|
||||
5) *FK-Invarianz:* `set` ist reine Metadaten; `p^welt = T_Link(θ)·p^lok` bleibt unberührt
|
||||
(verifiziert: max |Δ| = 0).
|
||||
|
||||
**Anwendung**
|
||||
```
|
||||
# Report (Mensch):
|
||||
python pipeline/marker_sets.py -robot data/robot/robot.json
|
||||
# Report (Maschine):
|
||||
python pipeline/marker_sets.py -robot data/robot/robot.json --json
|
||||
```
|
||||
```python
|
||||
from marker_sets import load_robot, classify_markers, get_sets, get_loose_markers, get_arm_markers
|
||||
data = load_robot("data/robot/robot.json")
|
||||
sets = get_sets(data) # {"A0":[MarkerInfo,...], "Brett":[...]}
|
||||
loose = get_loose_markers(data) # [MarkerInfo,...]
|
||||
arm = get_arm_markers(data) # {id: MarkerInfo}
|
||||
```
|
||||
*Neues `robot.json` vorbereiten:* an jeden Marker eines starren Objekts `"set": "<name>"` ergänzen
|
||||
(Name frei wählbar, ein Set = ein physisches Objekt); lose Marker ohne `set`; Arm-Marker (an
|
||||
beweglichen Links) brauchen keinen Eintrag. Der Code liest die Sets dann automatisch.
|
||||
|
||||
### P1 — Ankerlose, metrische Rekonstruktion (Phase A) — ✅ erledigt (2026-06-04)
|
||||
|
||||
**Was gemacht**
|
||||
- Neu: `pipeline/scene_reconstruct.py` — rekonstruiert aus den ArUco-Eckbeobachtungen ALLER Kameras
|
||||
die Kamera- und Marker-Posen in einem gemeinsamen Frame `S`, **ohne** Welt-Anker, **ohne**
|
||||
`robot.json` (nur Marker-Kantenlänge nötig). Vollständig generisch.
|
||||
|
||||
**Mathematik**
|
||||
|
||||
Konventionen: `E_c` = world(S)→Kamera c, `G_m` = Marker-lokal→world(S), also `M_{c←m} = E_c · G_m`.
|
||||
|
||||
1) *Per-Marker-PnP (tentativ):* für jedes (Kamera c, Marker m) löst IPPE_SQUARE die Pose eines
|
||||
Quadrats bekannter Größe. **Problem:** ein planares Quadrat hat eine **2-fache Flip-Ambiguität**
|
||||
→ naive Verkettung liefert ~⅓ gespiegelte Knoten (empirisch verifiziert).
|
||||
|
||||
2) *Flip-robuste Initialisierung* (Referenzkamera `c0`, `E_{c0}=I`), iterativ:
|
||||
- **Kamera-Pose per RANSAC-PnP** gegen die bereits platzierten 3D-Ecken
|
||||
`X_S = G_m · corner_local`. RANSAC verwirft gespiegelte Marker als Ausreißer.
|
||||
- **Marker-Ecken triangulieren** (lineares DLT über alle platzierten Kameras) — Triangulation
|
||||
ist **flip-frei**; daraus Marker-Pose via Kabsch `local→tri`. Korrigiert Flips aus Schritt 1.
|
||||
- Marker mit nur 1 Kamera sind nicht triangulierbar → als `insufficient_views` markiert
|
||||
(unbeobachtbar, **nicht** rekonstruiert; Konvention „unbekannt = null").
|
||||
|
||||
3) *Globale Bündelausgleichung* (`scipy.least_squares`, robuste Huber-Loss, **dünnbesetzte**
|
||||
Jacobi): minimiert die Reprojektion aller Ecken
|
||||
```
|
||||
min_{E_c, G_m} Σ_{(c,m)} Σ_{k=1..4} ρ( ‖ π(K_c, D_c; E_c·G_m·corner_k) − u_{c,m,k} ‖ )
|
||||
```
|
||||
Gauge: Referenzkamera `E_{c0}=I` fix (entfernt die 6-DoF-Starrkörperfreiheit).
|
||||
|
||||
4) *Similarity-Gauge / Skala:* Ankerlose SfM bestimmt die Struktur nur bis auf eine **7-DoF-
|
||||
Ähnlichkeit** (Rotation, Translation, **Skala**). Der absolute Maßstab kommt hier provisorisch aus
|
||||
der angenommenen Markergröße — empirisch ~0.93× gegenüber der echten Welt (konsistent über alle
|
||||
Szenen, also ein systematischer Markergrößen-/Rand-Versatz). **Die echte Skala + Lage fixiert
|
||||
erst Phase B über die bekannte mm-Geometrie des Roboters** (robuster als die Markergröße). Die
|
||||
*Form* ist bereits korrekt.
|
||||
|
||||
**Validierung (Sim, gegen FK-Ground-Truth)**
|
||||
- Reprojektion median **0.7–1.6 px** über Scene5/6/8/10/11/12 — besser als die bestehende,
|
||||
board-verankerte Pipeline (3.2–4.9 px), weil keine falschen Marker-Positionen angenommen werden.
|
||||
- Form (nach Similarity-Ausrichtung): Residuum **median ~3–7 mm** = Sensor-Rauschboden der Renders
|
||||
(`markerOffsetMaxMm: 4` + Rauschen); Übereinstimmung mit der bestehenden Triangulation **1.9 mm**.
|
||||
- Skalenfaktor konsistent **0.92–0.94** (→ Phase B).
|
||||
|
||||
**Anwendung**
|
||||
```
|
||||
python pipeline/scene_reconstruct.py --evalDir data/evaluations/Scene8
|
||||
# -> <evalDir>/scene_reconstruction.json (Kamera- & Marker-Posen in S, Reproj-Statistik,
|
||||
# Liste insufficient_view_markers)
|
||||
```
|
||||
```python
|
||||
import scene_reconstruct as sr
|
||||
res = sr.reconstruct("data/evaluations/Scene8") # dict; res["cameras"], res["markers"]
|
||||
```
|
||||
Voraussetzung: `*_aruco_detection.json` je Kamera (Pipeline-Schritt 1). Marker-Kantenlänge wird
|
||||
aus den Detektionen gelesen (Fallback `--markerSize`).
|
||||
@@ -351,6 +351,12 @@ def main():
|
||||
required=True
|
||||
)
|
||||
|
||||
parser.add_argument(
|
||||
'--saveDebugImage',
|
||||
action='store_true',
|
||||
help='Speichert ein Debug-JPG mit eingezeichneten Marker-Rahmen'
|
||||
)
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
out_dir = resolve_path(args.outDir)
|
||||
@@ -404,6 +410,9 @@ def main():
|
||||
detector_tuple
|
||||
)
|
||||
|
||||
# ids_raw: original numpy array für drawDetectedMarkers
|
||||
ids_raw = ids
|
||||
|
||||
detections = []
|
||||
|
||||
# --------------------------------------------------------
|
||||
@@ -601,6 +610,25 @@ def main():
|
||||
|
||||
print(f'Saved: {out_json}')
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Debug-Bild mit Marker-Rahmen
|
||||
# --------------------------------------------------------
|
||||
|
||||
if args.saveDebugImage:
|
||||
|
||||
debug_img = image.copy()
|
||||
|
||||
if corners_list and ids_raw is not None:
|
||||
cv2.aruco.drawDetectedMarkers(debug_img, corners_list, ids_raw)
|
||||
|
||||
debug_path = os.path.join(
|
||||
out_dir,
|
||||
f'{input_base}_debug.jpg'
|
||||
)
|
||||
|
||||
cv2.imwrite(debug_path, debug_img)
|
||||
print(f'Saved debug: {debug_path}')
|
||||
|
||||
|
||||
# ------------------------------------------------------------
|
||||
|
||||
|
||||
261
pipeline/marker_sets.py
Normal file
261
pipeline/marker_sets.py
Normal file
@@ -0,0 +1,261 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
marker_sets.py
|
||||
==============
|
||||
Klassifiziert die Marker aus robot.json in drei Rollen für die Szenen-Kalibrierung
|
||||
(siehe doc/callibrate_scene_roadmap.md, Phase P0):
|
||||
|
||||
arm : Marker an BEWEGLICHEN Roboter-Links (Arm1, Ellbow, Arm2, Finger, ...).
|
||||
Ihre Lage je Link ist bekannt -> Welt-Referenz, wird NICHT kalibriert.
|
||||
set : Marker an einem STATISCHEN Link (Welt-Wurzel) MIT "set"-Feld. Gleiches
|
||||
"set" = ein starres Objekt mit fixen relativen Bezügen -> es wird nur die
|
||||
6-DoF-Platzierung des ganzen Sets kalibriert.
|
||||
loose : Marker an einem statischen Link OHNE "set"-Feld. Lose, einzeln zu vermessen.
|
||||
|
||||
Die Set-Zugehörigkeit steht ausschließlich in robot.json (optionales Feld "set" am
|
||||
Marker). Hier wird nichts hartkodiert — die Sets ergeben sich per Auto-Discovery aus
|
||||
den vorhandenen "set"-Werten.
|
||||
|
||||
"Beweglich" = es liegt irgendwo auf der Kette bis zur Wurzel ein revolute/linear-Joint.
|
||||
Damit zählt die Wurzel (Board) und jeder rein über "fixed"-Joints angebundene Link als
|
||||
statisch (Welt), alles ab dem ersten Gelenk als Arm.
|
||||
|
||||
Public API
|
||||
----------
|
||||
data = load_robot("data/robot/robot.json")
|
||||
cls = classify_markers(data) # id -> MarkerInfo
|
||||
sets = get_sets(data) # set_name -> [MarkerInfo, ...]
|
||||
loose = get_loose_markers(data) # [MarkerInfo, ...]
|
||||
arm = get_arm_markers(data) # id -> MarkerInfo
|
||||
rep = set_summary(data) # serialisierbarer Report (für QA / --json)
|
||||
|
||||
CLI
|
||||
---
|
||||
python pipeline/marker_sets.py -robot data/robot/robot.json
|
||||
python pipeline/marker_sets.py -robot data/robot/robot.json --json
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
from dataclasses import dataclass, asdict
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Datentyp
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class MarkerInfo:
|
||||
id: int
|
||||
link: str
|
||||
role: str # "arm" | "set" | "loose"
|
||||
set_name: Optional[str] # nur bei role == "set"
|
||||
position: List[float] # im Link-Frame (mm), wie in robot.json
|
||||
normal: Optional[List[float]]
|
||||
spin: Optional[float]
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Laden
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def load_robot(path: str) -> Dict[str, Any]:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
return json.load(f)
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Link-Topologie: statisch (Welt) vs. beweglich (Arm)
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
_MOVABLE_JOINTS = ("revolute", "linear")
|
||||
|
||||
|
||||
def link_is_movable(links: Dict[str, Any], name: str) -> bool:
|
||||
"""
|
||||
True, wenn auf der Kette von `name` bis zur Wurzel mindestens ein
|
||||
revolute/linear-Joint liegt (Marker dort gehören zum beweglichen Roboter).
|
||||
"""
|
||||
seen = set()
|
||||
cur: Optional[str] = name
|
||||
while cur and cur in links and cur not in seen:
|
||||
seen.add(cur)
|
||||
joint = links[cur].get("jointToParent") or {}
|
||||
if str(joint.get("type", "")).lower() in _MOVABLE_JOINTS:
|
||||
return True
|
||||
cur = links[cur].get("parent")
|
||||
return False
|
||||
|
||||
|
||||
def root_links(links: Dict[str, Any]) -> List[str]:
|
||||
return [n for n, l in links.items()
|
||||
if not l.get("parent") or l.get("parent") not in links]
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Klassifizierung
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _set_name_of(marker: Dict[str, Any]) -> Optional[str]:
|
||||
val = marker.get("set")
|
||||
if val is None:
|
||||
return None
|
||||
s = str(val).strip()
|
||||
return s or None
|
||||
|
||||
|
||||
def classify_markers(robot_data: Dict[str, Any]) -> Dict[int, MarkerInfo]:
|
||||
"""
|
||||
Liefert id -> MarkerInfo über alle Links. Bei doppelten IDs gewinnt das erste
|
||||
Vorkommen (zusätzlich als Warnung in set_summary sichtbar).
|
||||
"""
|
||||
links = robot_data.get("links", {}) or {}
|
||||
out: Dict[int, MarkerInfo] = {}
|
||||
|
||||
for link_name, link in links.items():
|
||||
movable = link_is_movable(links, link_name)
|
||||
for mk in link.get("markers", []) or []:
|
||||
if "id" not in mk or "position" not in mk:
|
||||
continue
|
||||
mid = int(mk["id"])
|
||||
if mid in out:
|
||||
continue # erstes Vorkommen behalten
|
||||
set_name = _set_name_of(mk)
|
||||
if movable:
|
||||
role = "arm"
|
||||
set_name = None
|
||||
elif set_name is not None:
|
||||
role = "set"
|
||||
else:
|
||||
role = "loose"
|
||||
|
||||
normal = mk.get("normal")
|
||||
spin = mk.get("spin")
|
||||
out[mid] = MarkerInfo(
|
||||
id=mid,
|
||||
link=link_name,
|
||||
role=role,
|
||||
set_name=set_name,
|
||||
position=[float(v) for v in mk["position"]],
|
||||
normal=[float(v) for v in normal] if normal is not None else None,
|
||||
spin=float(spin) if spin is not None else None,
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def get_arm_markers(robot_data: Dict[str, Any]) -> Dict[int, MarkerInfo]:
|
||||
return {m.id: m for m in classify_markers(robot_data).values() if m.role == "arm"}
|
||||
|
||||
|
||||
def get_sets(robot_data: Dict[str, Any]) -> Dict[str, List[MarkerInfo]]:
|
||||
"""set_name -> Liste der Marker (role == 'set'), gruppiert nach 'set'-Wert."""
|
||||
sets: Dict[str, List[MarkerInfo]] = {}
|
||||
for m in classify_markers(robot_data).values():
|
||||
if m.role == "set" and m.set_name is not None:
|
||||
sets.setdefault(m.set_name, []).append(m)
|
||||
return sets
|
||||
|
||||
|
||||
def get_loose_markers(robot_data: Dict[str, Any]) -> List[MarkerInfo]:
|
||||
return [m for m in classify_markers(robot_data).values() if m.role == "loose"]
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# QA / Report
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _duplicate_ids(robot_data: Dict[str, Any]) -> List[int]:
|
||||
links = robot_data.get("links", {}) or {}
|
||||
seen, dups = set(), set()
|
||||
for link in links.values():
|
||||
for mk in link.get("markers", []) or []:
|
||||
if "id" not in mk:
|
||||
continue
|
||||
mid = int(mk["id"])
|
||||
(dups if mid in seen else seen).add(mid)
|
||||
return sorted(dups)
|
||||
|
||||
|
||||
def set_summary(robot_data: Dict[str, Any]) -> Dict[str, Any]:
|
||||
cls = classify_markers(robot_data)
|
||||
sets = get_sets(robot_data)
|
||||
loose = get_loose_markers(robot_data)
|
||||
arm = [m for m in cls.values() if m.role == "arm"]
|
||||
|
||||
warnings: List[str] = []
|
||||
for mid in _duplicate_ids(robot_data):
|
||||
warnings.append(f"Marker-ID {mid} kommt mehrfach vor (erstes Vorkommen gewertet)")
|
||||
for name, members in sets.items():
|
||||
links_used = sorted({m.link for m in members})
|
||||
if len(members) < 3:
|
||||
warnings.append(
|
||||
f"Set '{name}' hat nur {len(members)} Marker — 6-DoF-Platzierung "
|
||||
f"nicht voll bestimmbar (>=3 nicht-kollineare nötig)")
|
||||
if len(links_used) > 1:
|
||||
warnings.append(f"Set '{name}' verteilt sich über mehrere Links {links_used} "
|
||||
f"— ein Set sollte ein physisches Objekt sein")
|
||||
|
||||
return {
|
||||
"counts": {
|
||||
"total": len(cls),
|
||||
"arm": len(arm),
|
||||
"set": sum(len(v) for v in sets.values()),
|
||||
"loose": len(loose),
|
||||
"num_sets": len(sets),
|
||||
},
|
||||
"root_links": root_links(robot_data.get("links", {}) or {}),
|
||||
"sets": {name: sorted(m.id for m in members) for name, members in sorted(sets.items())},
|
||||
"loose_ids": sorted(m.id for m in loose),
|
||||
"arm_ids": sorted(m.id for m in arm),
|
||||
"warnings": warnings,
|
||||
}
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# CLI
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def main() -> None:
|
||||
ap = argparse.ArgumentParser(description="Marker aus robot.json in arm/set/loose klassifizieren")
|
||||
ap.add_argument("-robot", "--robot", required=True, help="Pfad zu robot.json")
|
||||
ap.add_argument("--json", action="store_true", help="Report als JSON ausgeben")
|
||||
args = ap.parse_args()
|
||||
|
||||
data = load_robot(args.robot)
|
||||
rep = set_summary(data)
|
||||
|
||||
if args.json:
|
||||
print(json.dumps(rep, indent=2, ensure_ascii=False))
|
||||
return
|
||||
|
||||
c = rep["counts"]
|
||||
print(f"robot.json: {args.robot}")
|
||||
print(f"Wurzel-Link(s): {rep['root_links']}")
|
||||
print(f"\nMarker gesamt: {c['total']} | arm: {c['arm']} set: {c['set']} "
|
||||
f"loose: {c['loose']} (Sets: {c['num_sets']})")
|
||||
|
||||
print("\nSets (starr, fixe interne Lage -> 6-DoF kalibrieren):")
|
||||
if rep["sets"]:
|
||||
for name, ids in rep["sets"].items():
|
||||
print(f" {name:10s} ({len(ids):3d}): {ids}")
|
||||
else:
|
||||
print(" (keine)")
|
||||
|
||||
print(f"\nLose Marker (einzeln zu vermessen): {rep['loose_ids'] or '(keine)'}")
|
||||
print(f"Arm-Marker (Welt-Referenz, nicht kalibriert): {rep['arm_ids']}")
|
||||
|
||||
if rep["warnings"]:
|
||||
print("\n[WARN]")
|
||||
for w in rep["warnings"]:
|
||||
print(f" - {w}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except Exception as exc: # pragma: no cover
|
||||
print(f"[ERROR] {exc}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
424
pipeline/scene_reconstruct.py
Normal file
424
pipeline/scene_reconstruct.py
Normal file
@@ -0,0 +1,424 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
scene_reconstruct.py — Phase A der Szenen-Kalibrierung
|
||||
========================================================
|
||||
Ankerlose, metrische Rekonstruktion ALLER Kamera- und Marker-Posen aus den
|
||||
ArUco-Eckbeobachtungen mehrerer Kameras — OHNE bekannten Welt-Anker, allein aus
|
||||
der bekannten Marker-Kantenlänge.
|
||||
|
||||
Vollständig generisch: kein robot.json nötig, keine festen Marker-IDs/Namen.
|
||||
|
||||
Warum nicht einfach Einzel-Marker-PnP?
|
||||
--------------------------------------
|
||||
Ein planares Quadrat hat bei PnP eine 2-fache Flip-Ambiguität. Verkettet man solche
|
||||
Einzelposen naiv, sind ~1/3 der Knoten gespiegelt -> unbrauchbar. Stattdessen:
|
||||
|
||||
1) je (Kamera, Marker): PnP (IPPE_SQUARE) als TENTATIVE Startpose.
|
||||
2) Referenzkamera c0 (meiste Beobachtungen) definiert den Szenen-Frame S (E_{c0}=I).
|
||||
3) Iteration (flip-robust):
|
||||
a) Kameras per cv2.solvePnPRansac gegen die bereits platzierten 3D-Ecken
|
||||
neu bestimmen -> geflippte Marker fallen als RANSAC-Ausreißer raus.
|
||||
b) Marker-Ecken über alle platzierten Kameras TRIANGULIEREN (DLT, flip-frei)
|
||||
und die Marker-Pose per Kabsch aus den triangulierten Ecken neu setzen.
|
||||
4) globale Bündelausgleichung (least_squares, Huber, dünnbesetzte Jacobi) über
|
||||
die Reprojektion ALLER Ecken. Gauge: c0 = Identität; Maßstab via Markergröße.
|
||||
|
||||
Geometrie-Konventionen
|
||||
----------------------
|
||||
- E_c = world(S) -> camera c (X_cam = E_c X_S)
|
||||
- G_m = marker m local -> world(S) (X_S = G_m X_local)
|
||||
- M_{c<-m} = E_c @ G_m (local -> camera)
|
||||
- Ecken-Reihenfolge wie ArUco/Pipeline: TL, TR, BR, BL.
|
||||
|
||||
Ein-/Ausgabe
|
||||
------------
|
||||
--evalDir : Ordner mit render_*_aruco_detection.json (Eckpunkte + Intrinsik)
|
||||
--out : scene_reconstruction.json (Default: <evalDir>/scene_reconstruction.json)
|
||||
|
||||
Das Ergebnis (Posen in S) ist Eingang für Phase B (robot_register.py).
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
import time
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
try:
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.sparse import lil_matrix
|
||||
HAVE_SCIPY = True
|
||||
except ImportError: # pragma: no cover
|
||||
HAVE_SCIPY = False
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# SE(3) / Geometrie-Helfer
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def rt_to_T(rvec, tvec) -> np.ndarray:
|
||||
R, _ = cv2.Rodrigues(np.asarray(rvec, dtype=float).reshape(3, 1))
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = np.asarray(tvec, dtype=float).reshape(3)
|
||||
return T
|
||||
|
||||
|
||||
def T_to_rt(T: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
rvec, _ = cv2.Rodrigues(np.ascontiguousarray(T[:3, :3]))
|
||||
return rvec.reshape(3), T[:3, 3].copy()
|
||||
|
||||
|
||||
def inv_T(T: np.ndarray) -> np.ndarray:
|
||||
R, t = T[:3, :3], T[:3, 3]
|
||||
Ti = np.eye(4)
|
||||
Ti[:3, :3] = R.T
|
||||
Ti[:3, 3] = -R.T @ t
|
||||
return Ti
|
||||
|
||||
|
||||
def local_corners(size_m: float) -> np.ndarray:
|
||||
"""Marker-Ecken im lokalen Frame (TL, TR, BR, BL), z=0."""
|
||||
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=float)
|
||||
|
||||
|
||||
def kabsch(P: np.ndarray, Q: np.ndarray) -> np.ndarray:
|
||||
"""R,t mit Q ≈ R P + t (ohne Skalierung); P,Q: Nx3. Liefert 4x4."""
|
||||
pc, qc = P.mean(0), Q.mean(0)
|
||||
H = (P - pc).T @ (Q - qc)
|
||||
U, _, Vt = np.linalg.svd(H)
|
||||
d = np.sign(np.linalg.det(Vt.T @ U.T))
|
||||
R = Vt.T @ np.diag([1.0, 1.0, d]) @ U.T
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = qc - R @ pc
|
||||
return T
|
||||
|
||||
|
||||
def triangulate_point(obs: List[Tuple]) -> Optional[np.ndarray]:
|
||||
"""Mehrbild-DLT eines 3D-Punkts. obs: [(K,D,R,t,uv), ...] mit X_cam=R X+t."""
|
||||
A = []
|
||||
for K, D, R, t, uv in obs:
|
||||
und = cv2.undistortPoints(np.array([[uv]], dtype=np.float64), K, D).reshape(2)
|
||||
x, y = float(und[0]), float(und[1])
|
||||
P = np.hstack([R, t.reshape(3, 1)])
|
||||
A.append(x * P[2] - P[0])
|
||||
A.append(y * P[2] - P[1])
|
||||
_, _, Vt = np.linalg.svd(np.asarray(A, dtype=float))
|
||||
X = Vt[-1]
|
||||
return X[:3] / X[3] if abs(X[3]) > 1e-12 else None
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Laden der Detektionen
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
class Cam:
|
||||
__slots__ = ("id", "K", "D", "obs")
|
||||
|
||||
def __init__(self, cid, K, D):
|
||||
self.id = cid
|
||||
self.K = K
|
||||
self.D = D
|
||||
self.obs: Dict[int, np.ndarray] = {} # marker_id -> (4,2) px
|
||||
|
||||
|
||||
def load_detections(eval_dir: str, cameras: Optional[List[str]] = None,
|
||||
default_size: Optional[float] = None
|
||||
) -> Tuple[Dict[str, Cam], Dict[int, float]]:
|
||||
cams: Dict[str, Cam] = {}
|
||||
sizes: Dict[int, float] = {}
|
||||
for det_path in sorted(glob.glob(os.path.join(eval_dir, "*_aruco_detection.json"))):
|
||||
m = re.match(r"render_([A-Za-z0-9]+)_aruco_detection\.json", os.path.basename(det_path))
|
||||
if not m:
|
||||
continue
|
||||
cid = m.group(1)
|
||||
if cameras and cid not in cameras:
|
||||
continue
|
||||
det = json.load(open(det_path, "r", encoding="utf-8"))
|
||||
K = np.array(det["camera"]["camera_matrix"], dtype=float).reshape(3, 3)
|
||||
D = np.array(det["camera"]["distortion_coefficients"], dtype=float).reshape(-1, 1)
|
||||
cam = Cam(cid, K, D)
|
||||
det_size = (det.get("vision_config", {}) or {}).get("MarkerSize", None)
|
||||
for d in det.get("detections", []):
|
||||
if d.get("type", "aruco") != "aruco":
|
||||
continue
|
||||
pts = d.get("image_points_px")
|
||||
if pts is None:
|
||||
continue
|
||||
mid = int(d["marker_id"])
|
||||
cam.obs[mid] = np.array(pts, dtype=float).reshape(4, 2)
|
||||
s = d.get("marker_size_m", det_size if det_size is not None else default_size)
|
||||
if s is not None:
|
||||
sizes[mid] = float(s)
|
||||
if cam.obs:
|
||||
cams[cid] = cam
|
||||
return cams, sizes
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Schritt 1: Per-Marker-PnP (tentative Startposen)
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def pnp_all(cams: Dict[str, Cam], sizes: Dict[int, float], default_size: float
|
||||
) -> Dict[Tuple[str, int], Dict]:
|
||||
out: Dict[Tuple[str, int], Dict] = {}
|
||||
for cid, cam in cams.items():
|
||||
for mid, corners_px in cam.obs.items():
|
||||
size = sizes.get(mid, default_size)
|
||||
ok, rvec, tvec = cv2.solvePnP(local_corners(size), corners_px, cam.K, cam.D,
|
||||
flags=cv2.SOLVEPNP_IPPE_SQUARE)
|
||||
if not ok:
|
||||
ok, rvec, tvec = cv2.solvePnP(local_corners(size), corners_px, cam.K, cam.D,
|
||||
flags=cv2.SOLVEPNP_ITERATIVE)
|
||||
if ok:
|
||||
out[(cid, mid)] = {"M": rt_to_T(rvec, tvec)}
|
||||
return out
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Schritt 2+3: flip-robuste Initialisierung
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def initialize_poses(cams, sizes, default_size, pnp, n_iter=6, min_pts_pnp=8,
|
||||
ransac_px=3.0) -> Tuple[Dict[str, np.ndarray], Dict[int, np.ndarray], str]:
|
||||
cam_ids = sorted(cams)
|
||||
all_mk = sorted({m for _, m in pnp})
|
||||
|
||||
# Referenzkamera = meiste Beobachtungen
|
||||
ref = max(cam_ids, key=lambda c: len(cams[c].obs))
|
||||
E: Dict[str, np.ndarray] = {ref: np.eye(4)}
|
||||
G: Dict[int, np.ndarray] = {}
|
||||
for m in cams[ref].obs: # tentative Startmarker aus c0
|
||||
if (ref, m) in pnp:
|
||||
G[m] = pnp[(ref, m)]["M"].copy()
|
||||
|
||||
def corners3D(m):
|
||||
return (G[m][:3, :3] @ local_corners(sizes.get(m, default_size)).T).T + G[m][:3, 3]
|
||||
|
||||
for _ in range(n_iter):
|
||||
# (a) Kameras gegen platzierte 3D-Ecken (RANSAC -> Flip-Ausreißer raus)
|
||||
for c in cam_ids:
|
||||
if c == ref:
|
||||
continue
|
||||
obj, img = [], []
|
||||
for m in cams[c].obs:
|
||||
if m in G:
|
||||
obj.append(corners3D(m))
|
||||
img.append(cams[c].obs[m])
|
||||
if sum(len(o) for o in obj) < min_pts_pnp:
|
||||
continue
|
||||
O = np.vstack(obj).astype(np.float64)
|
||||
I = np.vstack(img).astype(np.float64)
|
||||
ok, rvec, tvec, inl = cv2.solvePnPRansac(
|
||||
O, I, cams[c].K, cams[c].D, reprojectionError=ransac_px,
|
||||
iterationsCount=200, flags=cv2.SOLVEPNP_ITERATIVE)
|
||||
if ok and inl is not None and len(inl) >= 6:
|
||||
E[c] = rt_to_T(rvec, tvec)
|
||||
|
||||
# (b) Marker triangulieren (flip-frei) bzw. einfügen
|
||||
for m in all_mk:
|
||||
placed = [c for c in cam_ids if c in E and (c, m) in pnp]
|
||||
if len(placed) >= 2:
|
||||
tri, ok = [], True
|
||||
for ci in range(4):
|
||||
o = [(cams[c].K, cams[c].D, E[c][:3, :3], E[c][:3, 3], cams[c].obs[m][ci])
|
||||
for c in placed]
|
||||
X = triangulate_point(o)
|
||||
if X is None:
|
||||
ok = False
|
||||
break
|
||||
tri.append(X)
|
||||
if ok:
|
||||
G[m] = kabsch(local_corners(sizes.get(m, default_size)), np.array(tri))
|
||||
elif placed and m not in G:
|
||||
c = placed[0]
|
||||
G[m] = inv_T(E[c]) @ pnp[(c, m)]["M"]
|
||||
|
||||
return E, G, ref
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Schritt 4: Globale Bündelausgleichung (Referenzkamera fix)
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def reproj_stats(obs_pairs, cams, sizes, default_size, E, G) -> Tuple[float, float]:
|
||||
"""(RMS, Median) der Per-Beobachtungs-Reprojektionsfehler (px)."""
|
||||
per = []
|
||||
for (c, m) in obs_pairs:
|
||||
rvec, tvec = T_to_rt(E[c] @ G[m])
|
||||
proj, _ = cv2.projectPoints(local_corners(sizes.get(m, default_size)),
|
||||
rvec, tvec, cams[c].K, cams[c].D)
|
||||
d = proj.reshape(4, 2) - cams[c].obs[m]
|
||||
per.append(float(np.sqrt(np.mean(d * d))))
|
||||
if not per:
|
||||
return 0.0, 0.0
|
||||
a = np.asarray(per)
|
||||
return float(np.sqrt(np.mean(a * a))), float(np.median(a))
|
||||
|
||||
|
||||
def bundle_adjust(pnp, cams, sizes, default_size, E, G, ref,
|
||||
huber_px=2.0, max_nfev=120, verbose=0):
|
||||
if not HAVE_SCIPY:
|
||||
print("[WARN] scipy fehlt -> BA uebersprungen")
|
||||
return E, G
|
||||
cam_opt = [c for c in sorted(E) if c != ref]
|
||||
mk_ids = sorted(G)
|
||||
ci = {c: i for i, c in enumerate(cam_opt)}
|
||||
mi = {m: j for j, m in enumerate(mk_ids)}
|
||||
ncam, nmk = len(cam_opt), len(mk_ids)
|
||||
obs = [(c, m) for (c, m) in pnp if c in E and m in G]
|
||||
|
||||
def pack():
|
||||
x = np.zeros(6 * ncam + 6 * nmk)
|
||||
for c in cam_opt:
|
||||
r, t = T_to_rt(E[c]); x[6*ci[c]:6*ci[c]+3] = r; x[6*ci[c]+3:6*ci[c]+6] = t
|
||||
for m in mk_ids:
|
||||
r, t = T_to_rt(G[m]); o = 6*ncam + 6*mi[m]; x[o:o+3] = r; x[o+3:o+6] = t
|
||||
return x
|
||||
|
||||
def unpack(x):
|
||||
Ee = {ref: np.eye(4)}
|
||||
for c in cam_opt:
|
||||
Ee[c] = rt_to_T(x[6*ci[c]:6*ci[c]+3], x[6*ci[c]+3:6*ci[c]+6])
|
||||
Gg = {}
|
||||
for m in mk_ids:
|
||||
o = 6*ncam + 6*mi[m]
|
||||
Gg[m] = rt_to_T(x[o:o+3], x[o+3:o+6])
|
||||
return Ee, Gg
|
||||
|
||||
def residuals(x):
|
||||
Ee, Gg = unpack(x)
|
||||
res = np.empty(8 * len(obs))
|
||||
for k, (c, m) in enumerate(obs):
|
||||
rvec, tvec = T_to_rt(Ee[c] @ Gg[m])
|
||||
proj, _ = cv2.projectPoints(local_corners(sizes.get(m, default_size)),
|
||||
rvec, tvec, cams[c].K, cams[c].D)
|
||||
res[8*k:8*k+8] = (proj.reshape(4, 2) - cams[c].obs[m]).ravel()
|
||||
return res
|
||||
|
||||
Sp = lil_matrix((8 * len(obs), 6 * ncam + 6 * nmk), dtype=int)
|
||||
for k, (c, m) in enumerate(obs):
|
||||
rows = slice(8*k, 8*k+8)
|
||||
if c in ci:
|
||||
Sp[rows, 6*ci[c]:6*ci[c]+6] = 1
|
||||
o = 6*ncam + 6*mi[m]
|
||||
Sp[rows, o:o+6] = 1
|
||||
|
||||
sol = least_squares(residuals, pack(), jac_sparsity=Sp, method="trf",
|
||||
loss="huber", f_scale=huber_px, max_nfev=max_nfev, verbose=verbose)
|
||||
return unpack(sol.x)
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Hauptlauf
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def reconstruct(eval_dir, cameras=None, default_size=0.025,
|
||||
huber_px=2.0, max_nfev=120, verbose=0) -> Dict:
|
||||
cams, sizes = load_detections(eval_dir, cameras, default_size)
|
||||
if len(cams) < 2:
|
||||
raise RuntimeError(f"brauche >=2 Kameras, gefunden: {sorted(cams)}")
|
||||
pnp = pnp_all(cams, sizes, default_size)
|
||||
E0, G0, ref = initialize_poses(cams, sizes, default_size, pnp)
|
||||
|
||||
# Marker je #platzierter Kameras; nur >=2 sind triangulierbar/zuverlässig.
|
||||
ncams_of = {}
|
||||
for (c, m) in pnp:
|
||||
if c in E0 and m in G0:
|
||||
ncams_of[m] = ncams_of.get(m, 0) + 1
|
||||
weak = sorted(m for m in G0 if ncams_of.get(m, 0) < 2) # Einzelbild -> unbeobachtbar
|
||||
G0 = {m: T for m, T in G0.items() if ncams_of.get(m, 0) >= 2}
|
||||
|
||||
pairs0 = [(c, m) for (c, m) in pnp if c in E0 and m in G0]
|
||||
rms0, med0 = reproj_stats(pairs0, cams, sizes, default_size, E0, G0)
|
||||
E, G = bundle_adjust(pnp, cams, sizes, default_size, E0, G0, ref, huber_px, max_nfev, verbose)
|
||||
pairs = [(c, m) for (c, m) in pnp if c in E and m in G]
|
||||
rms1, med1 = reproj_stats(pairs, cams, sizes, default_size, E, G)
|
||||
dropped = [c for c in cams if c not in E]
|
||||
|
||||
cameras_out = []
|
||||
for c in sorted(E):
|
||||
Ec = E[c]
|
||||
rvec, _ = T_to_rt(Ec)
|
||||
cameras_out.append({
|
||||
"camera_id": c,
|
||||
"world_to_camera": {"rotation_matrix": Ec[:3, :3].tolist(),
|
||||
"translation_m": Ec[:3, 3].tolist(), "rvec_rad": rvec.tolist()},
|
||||
"center_m": (-Ec[:3, :3].T @ Ec[:3, 3]).tolist(),
|
||||
"num_markers": len(cams[c].obs),
|
||||
"is_reference": bool(c == ref),
|
||||
})
|
||||
markers_out = []
|
||||
for m in sorted(G):
|
||||
Gm = G[m]
|
||||
size = sizes.get(m, default_size)
|
||||
corners_S = (Gm[:3, :3] @ local_corners(size).T).T + Gm[:3, 3]
|
||||
rvec, _ = T_to_rt(Gm)
|
||||
markers_out.append({
|
||||
"marker_id": int(m),
|
||||
"pose_in_S": {"rotation_matrix": Gm[:3, :3].tolist(),
|
||||
"translation_m": Gm[:3, 3].tolist(), "rvec_rad": rvec.tolist()},
|
||||
"center_m": Gm[:3, 3].tolist(),
|
||||
"normal": (Gm[:3, :3] @ np.array([0.0, 0.0, 1.0])).tolist(),
|
||||
"corners_m": corners_S.tolist(),
|
||||
"num_cameras": int(ncams_of.get(m, 0)),
|
||||
})
|
||||
|
||||
return {
|
||||
"schema_version": "1.0",
|
||||
"stage": "scene_reconstruct_A",
|
||||
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"frame": "arbitrary S (reference camera pose = identity)",
|
||||
"reference_camera": ref,
|
||||
"summary": {"num_cameras": len(E), "num_markers": len(G), "num_observations": len(pairs),
|
||||
"reproj_rms_px_init": rms0, "reproj_median_px_init": med0,
|
||||
"reproj_rms_px_final": rms1, "reproj_median_px_final": med1,
|
||||
"dropped_cameras": dropped,
|
||||
"insufficient_view_markers": weak},
|
||||
"cameras": cameras_out,
|
||||
"markers": markers_out,
|
||||
}
|
||||
|
||||
|
||||
def main() -> None:
|
||||
ap = argparse.ArgumentParser(description="Phase A: ankerlose metrische Szenen-Rekonstruktion")
|
||||
ap.add_argument("--evalDir", required=True, help="Ordner mit render_*_aruco_detection.json")
|
||||
ap.add_argument("--cameras", default=None, help="Kommagetrennte Kamera-IDs (Default: alle)")
|
||||
ap.add_argument("--markerSize", type=float, default=0.025, help="Fallback-Kantenlänge (m)")
|
||||
ap.add_argument("--huberPx", type=float, default=2.0)
|
||||
ap.add_argument("--maxNfev", type=int, default=120)
|
||||
ap.add_argument("--out", default=None)
|
||||
ap.add_argument("-v", "--verbose", action="count", default=0)
|
||||
args = ap.parse_args()
|
||||
|
||||
cams = args.cameras.split(",") if args.cameras else None
|
||||
res = reconstruct(args.evalDir, cams, args.markerSize, args.huberPx, args.maxNfev, args.verbose)
|
||||
out = args.out or os.path.join(args.evalDir, "scene_reconstruction.json")
|
||||
json.dump(res, open(out, "w", encoding="utf-8"), indent=2)
|
||||
s = res["summary"]
|
||||
print(f"[INFO] Kameras={s['num_cameras']} Marker(>=2 Views)={s['num_markers']} "
|
||||
f"Obs={s['num_observations']} | RMS {s['reproj_rms_px_init']:.3f}->{s['reproj_rms_px_final']:.3f}px "
|
||||
f"(median {s['reproj_median_px_final']:.3f}px) | ref-Kamera={res['reference_camera']}")
|
||||
if s["dropped_cameras"]:
|
||||
print(f"[WARN] nicht verbundene Kameras verworfen: {s['dropped_cameras']}")
|
||||
if s["insufficient_view_markers"]:
|
||||
print(f"[INFO] {len(s['insufficient_view_markers'])} Marker mit <2 Views (unbeobachtbar, "
|
||||
f"nicht rekonstruiert): {s['insufficient_view_markers']}")
|
||||
print(f"[INFO] -> {out}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except Exception as exc:
|
||||
print(f"[ERROR] {exc}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
33
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.json
Normal file
33
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.json
Normal file
@@ -0,0 +1,33 @@
|
||||
{
|
||||
"page_format": "A3",
|
||||
"orientation": "portrait",
|
||||
"page_size_mm": {
|
||||
"width": 297.0,
|
||||
"height": 420.0
|
||||
},
|
||||
"seed": 223,
|
||||
"num_arucos": 10,
|
||||
"aruco_size_mm": 50.0,
|
||||
"aruco_dictionary": "DICT_4X4_250",
|
||||
"aruco_start_id": 46,
|
||||
"page_border_margin_mm": 20.0,
|
||||
"forbidden_rectangle_mm": {
|
||||
"x": 143.5,
|
||||
"y": 205.0,
|
||||
"w": 10.0,
|
||||
"h": 10.0
|
||||
},
|
||||
"forbidden_rectangle_margin_mm": 30.0,
|
||||
"placements": [
|
||||
{"id": 46, "position": [96.26, 119.02, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 47, "position": [158.68, 110.29, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 48, "position": [-43.31, 5.63, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 49, "position": [63.52, -4.25, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 50, "position": [27.77, 149.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 51, "position": [-98.03, 38.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 52, "position": [-120.71, 148.79, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 53, "position": [72.76, 59.77, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 54, "position": [-156.67, 34.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 55, "position": [-110.82, 93.01, -27.3], "normal": [0, 0, 1], "spin": 90}
|
||||
]
|
||||
}
|
||||
74
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.pdf
Normal file
74
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.pdf
Normal file
@@ -0,0 +1,74 @@
|
||||
%PDF-1.3
|
||||
%“Œ‹ž ReportLab Generated PDF document http://www.reportlab.com
|
||||
1 0 obj
|
||||
<<
|
||||
/F1 2 0 R /F2 3 0 R
|
||||
>>
|
||||
endobj
|
||||
2 0 obj
|
||||
<<
|
||||
/BaseFont /Helvetica /Encoding /WinAnsiEncoding /Name /F1 /Subtype /Type1 /Type /Font
|
||||
>>
|
||||
endobj
|
||||
3 0 obj
|
||||
<<
|
||||
/BaseFont /Times-Roman /Encoding /WinAnsiEncoding /Name /F2 /Subtype /Type1 /Type /Font
|
||||
>>
|
||||
endobj
|
||||
4 0 obj
|
||||
<<
|
||||
/Contents 8 0 R /MediaBox [ 0 0 841.8898 1190.551 ] /Parent 7 0 R /Resources <<
|
||||
/Font 1 0 R /ProcSet [ /PDF /Text /ImageB /ImageC /ImageI ]
|
||||
>> /Rotate 0 /Trans <<
|
||||
|
||||
>>
|
||||
/Type /Page
|
||||
>>
|
||||
endobj
|
||||
5 0 obj
|
||||
<<
|
||||
/PageMode /UseNone /Pages 7 0 R /Type /Catalog
|
||||
>>
|
||||
endobj
|
||||
6 0 obj
|
||||
<<
|
||||
/Author (OpenAI) /CreationDate (D:20260609100839+02'00') /Creator (ReportLab PDF Library - www.reportlab.com) /Keywords () /ModDate (D:20260609100839+02'00') /Producer (ReportLab PDF Library - www.reportlab.com)
|
||||
/Subject (A0 poster with random ArUco placement) /Title (A3_10Arucos_50mm_Seet223) /Trapped /False
|
||||
>>
|
||||
endobj
|
||||
7 0 obj
|
||||
<<
|
||||
/Count 1 /Kids [ 4 0 R ] /Type /Pages
|
||||
>>
|
||||
endobj
|
||||
8 0 obj
|
||||
<<
|
||||
/Filter [ /ASCII85Decode /FlateDecode ] /Length 2256
|
||||
>>
|
||||
stream
|
||||
Gasb^c)r<L%#+HM.C+V0R+R8eZUFpRXJ"GN0h@+g"WLbi='*AObm`^/.uekT^TKc`,iq!cbphBh5QC]L;bGq3dW[or@S)WAM(@-Kpc8<BGdBd2p\,bJT:b>G9Ol;/QtIaGHgCFun\\R[^AH)LDaL5K<a5F@5#HBrhRP!S9Uqq+)0EU[0C8;F^@>8hc6K6Q-e17IqXj](D4N^H9n1io5//-oU((jsCT;e\51G!$RnTZ;p57s"r=ne@2]#of7ERf;T(gFg`c@q3Mg:nHfM!N+P>$DsH3q.!3+"eSYI0L5WMthC6fq'?W/5'@0@#\6[V+To(cr+Jm>A-jSP#LkC\&faD@kR&*@VQ3F(gseG:WuLoc.FaHNRCE#&%so`Eirncr^:LCP^ZGJV@\d\#ea#Yb#dWDS<i2QX1QY$At)ei4K'%ceHsl`k5ka%LB[e!*C=QKO&X3@6KC%I4bki`j$jaC=eVV8,6=O/>>dH*Ak<JH@/?BeFma*r0\TJ%ba#eQ*3'7qR.B&alPF)V(sa3=n-*YcmdI:JQV#'Ei]QQ>:,?d"k'VQ@&n=r-B\F^/o(IQee!"-6Su;1[96Cg4Mjqtcn,8XX\5l:QUPiGQ#;QK_uSBM!YSb@Q+39DR2O0O:nJ.N]<)+8>6?]5CL^I>_h^+!]OhU<XpALd'*-Ao5_Ok,di,u"?Y>gUfIV;Kbl1Kp=d8KARCY8Sok8%=?Q*/1mAplQ`VSF$Ti5[jA:t\helFKE<i[Rf"@=qi$ECNdQr"+ilIKEfb6?2WEf6#u^T3_S(I6TF!N8)<ffa!KlR;fP4G>mgVa;g4kXm!f[kiPWB7PZsXGpt]DRlh:XoVst#(YMJRVcJuDqQXH"MN5Pco&iDG,G>ESjLAM-2-!JT+YocXgZQ*QIt_*@V;T$m(9?GDcVtSIHn7&c_Ek/I:juH@p:mc1*G.ABs/'uXMiSR@"@&nBX&kl@Q=FiH]-RZ2eKO*Yta#<]2a4D"]uj*J9>c1lf/be4,93Z3Fpm_oo.u4Yats4H4D[A]u@(".mqD#(h*q>Qa7NOYq`htWi[Dl+K[B`>%;X`!e@Ga(o'0D!0+%!/!mtb)ZqP^XN'k.)J^dta$%mhB)6B&Wh&p`mKboFfrbEnbOsZE+KUNCLPN+VkN3N,]4+SUrZa3EL7$fJ!7p65)OB/f0d!_:OUJBWC,*?&!N]K-1b<FqF<B*c+ua,C>_$t-J;-^7*JLTR\\#.q,QQEW,4)lf&9_2DoIaoo>O^g>,JY55Y#,;R*<X*'$ioTJ6u5t:>L&R'l_AM_mj`"".C-hN]Lb,Aou5=U7*b\e#i"'Yru5Ce4R[b%Cfr]`&[q?'r\pRfF5Uk-;m7iV3<6.-MqP3K3:G9W7].I%[`r!%&U*CB=P&J$fCdkGhfHj1`iRJkKQXtY#1m(c204)!Y,Mo(So0@%@km8_@KCA))qB+])e=ppN-YO[l3W.-:*<"hJH--pV@5W\okEl!O["aNs/.Z\lSI^dk-R*dPoUm/MEkhs[]cVC5[1tjVZWlfpX9/KT(g\7V=))K8^4^T]01\3h6m<,W<FQWggb3_JOojo,p>ae"L3^]fL$g=`U&luY\PQck]-m+F_B8:Q"ZYqS,o]u%5GIn!8(ilOKbgGUG%S7?C*O&?.K#,XWadI7qXP[f<_d`NM#pjIL1Dil1<G:b%m>XNj[t\9s(h![GJ(7=bqiTLc*3t=,]#[<SDuUD@Q4-F"E!8NEXXA@<W40BZP$HkV:#S4,ZrP)kpPP/qfcO!<%kbW`q]:2a;^u!HbjWd%rQg[oPY`D<P73oV"TRQOpK-6TH4,'MJVEHFNt@klU4_C(Ss;LtJp5Q)^0=<`86%cO*WX.o98]=u7tgY.3Bg(.r,#FDVSi@L[J#fREaIbRf:V7e#qh1k`7]rV/r(U;:ol]bC.pc%>*iZWf)u\m@Y>euOMsrkY^t"i0t:`9]R(0tY-<)R_RtHJ\XQk1s.Xm*6bdG7Gen/r,hM3Q)hZ%_5HImH*6e&@4HRR,'cUloT.UH'c!#K\HB1:%sCHb8\;NDZK*5N3FgfOH$a*>qUuYeu!HS]'Duq0?3W]Mf&9qoe,:u1:6/>jp&(j1?AU(fP3o5=,:_LL/TdL)D\?[hE6R.?]a_pN-Gge]Cc6,lNK,T?@,>+(W]\YXH*P#\i5.^D_^S7`j@5I_VY/M('*ZH&U*D-1q6km$)M6-Dp3).^j04<eZd=Dg0C0e@b0]P_QLG%YW`g&lT%@Lr1hUo08[Yk$ESR)7"&,Me(_W<Reju~>endstream
|
||||
endobj
|
||||
xref
|
||||
0 9
|
||||
0000000000 65535 f
|
||||
0000000073 00000 n
|
||||
0000000114 00000 n
|
||||
0000000221 00000 n
|
||||
0000000330 00000 n
|
||||
0000000533 00000 n
|
||||
0000000601 00000 n
|
||||
0000000936 00000 n
|
||||
0000000995 00000 n
|
||||
trailer
|
||||
<<
|
||||
/ID
|
||||
[<292de92922b99d411f4915f95f3215c2><292de92922b99d411f4915f95f3215c2>]
|
||||
% ReportLab generated PDF document -- digest (http://www.reportlab.com)
|
||||
|
||||
/Info 6 0 R
|
||||
/Root 5 0 R
|
||||
/Size 9
|
||||
>>
|
||||
startxref
|
||||
3342
|
||||
%%EOF
|
||||
33
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.json
Normal file
33
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.json
Normal file
@@ -0,0 +1,33 @@
|
||||
{
|
||||
"page_format": "A3",
|
||||
"orientation": "portrait",
|
||||
"page_size_mm": {
|
||||
"width": 297.0,
|
||||
"height": 420.0
|
||||
},
|
||||
"seed": 224,
|
||||
"num_arucos": 10,
|
||||
"aruco_size_mm": 50.0,
|
||||
"aruco_dictionary": "DICT_4X4_250",
|
||||
"aruco_start_id": 46,
|
||||
"page_border_margin_mm": 10.0,
|
||||
"forbidden_rectangle_mm": {
|
||||
"x": 143.5,
|
||||
"y": 205.0,
|
||||
"w": 10.0,
|
||||
"h": 10.0
|
||||
},
|
||||
"forbidden_rectangle_margin_mm": 30.0,
|
||||
"placements": [
|
||||
{"id": 46, "position": [-103.49, -14.02, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 47, "position": [17.33, -11.06, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 48, "position": [14.91, 182.28, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 49, "position": [-133.58, 94.98, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 50, "position": [82.51, 132.7, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 51, "position": [147.43, 136.07, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 52, "position": [159.13, -7.87, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 53, "position": [-151.21, 155.25, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 54, "position": [80.2, 56.76, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 55, "position": [94.41, -7.18, -27.3], "normal": [0, 0, 1], "spin": 90}
|
||||
]
|
||||
}
|
||||
74
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.pdf
Normal file
74
setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.pdf
Normal file
@@ -0,0 +1,74 @@
|
||||
%PDF-1.3
|
||||
%“Œ‹ž ReportLab Generated PDF document http://www.reportlab.com
|
||||
1 0 obj
|
||||
<<
|
||||
/F1 2 0 R /F2 3 0 R
|
||||
>>
|
||||
endobj
|
||||
2 0 obj
|
||||
<<
|
||||
/BaseFont /Helvetica /Encoding /WinAnsiEncoding /Name /F1 /Subtype /Type1 /Type /Font
|
||||
>>
|
||||
endobj
|
||||
3 0 obj
|
||||
<<
|
||||
/BaseFont /Times-Roman /Encoding /WinAnsiEncoding /Name /F2 /Subtype /Type1 /Type /Font
|
||||
>>
|
||||
endobj
|
||||
4 0 obj
|
||||
<<
|
||||
/Contents 8 0 R /MediaBox [ 0 0 841.8898 1190.551 ] /Parent 7 0 R /Resources <<
|
||||
/Font 1 0 R /ProcSet [ /PDF /Text /ImageB /ImageC /ImageI ]
|
||||
>> /Rotate 0 /Trans <<
|
||||
|
||||
>>
|
||||
/Type /Page
|
||||
>>
|
||||
endobj
|
||||
5 0 obj
|
||||
<<
|
||||
/PageMode /UseNone /Pages 7 0 R /Type /Catalog
|
||||
>>
|
||||
endobj
|
||||
6 0 obj
|
||||
<<
|
||||
/Author (OpenAI) /CreationDate (D:20260609100923+02'00') /Creator (ReportLab PDF Library - www.reportlab.com) /Keywords () /ModDate (D:20260609100923+02'00') /Producer (ReportLab PDF Library - www.reportlab.com)
|
||||
/Subject (A0 poster with random ArUco placement) /Title (A3_10Arucos_50mm_Seet224) /Trapped /False
|
||||
>>
|
||||
endobj
|
||||
7 0 obj
|
||||
<<
|
||||
/Count 1 /Kids [ 4 0 R ] /Type /Pages
|
||||
>>
|
||||
endobj
|
||||
8 0 obj
|
||||
<<
|
||||
/Filter [ /ASCII85Decode /FlateDecode ] /Length 2250
|
||||
>>
|
||||
stream
|
||||
Gasb_c&UjC%#"*@'L!jj:dg/icS[tN92<UA6')NeAET%I?9_ED,.k)fZS91([ejia<Cs'$fgA9\++!k^a-Yi]nbr7AB0)t2n"1-M`5hbYa%u/BqNCjUqq'TC"!('LN&_BBR?NH@p#kl@dp%:6qqEcB-&p',g[UnbJoaSu;a4ehU0RIA,Frc-0DbM%hrpOOVid%+B4;qBq""25\"5W<3W@E%ID!:i6lW*_ie.,%H)^[!2C[0;g+/JMe5$Z`I<.!)H7dZS4-0:3lcN+#F]S/t7j9gMYc[!98*<Od'up'm,PsA6/6%ZZ=X@]TZDLYp(B<J9M2u/tP_h8?J0es0eQ*fYF"/pf%FhE'PDlcA-WuW8?p0:n&f2:8j:JYh"2L?aJ2M7"le\q=#.`P2f>[l"9gd-nL7tLL%>OEW=N_Lh[pFT-]cp&?4m9C1lc6,l-KdJAF,3uF!FYX=,OD"c#\uFglsimhX_UXuWEi/K^.R#B6+'u28J9RVaHtpKDVlQ)Iin-O(B&5p/\IjcTp8Eg\.-!tQ76h9%^AH#7ZYa(19@4.Ye?j\o7YaFC!6X`Q@fC=\MkoAK\bJrG'cU/m1!IZ>LOn#n85krj+ae8+_G#4cH?.M2Lp/UehR>0di5WrQaD@GP8DUSYVmR'f";$bH&Ghs`^@?Y\R-Sj2h^".#@Vfd#eP.CfUYCN`q>PkP,R9q7/C%Nqb-m(l?UlO7pCCecpR;c,T,9Y(jdnI4*BAJ'4Q;kVlHnj#g99U</@q%6n^]t5%`u.YhL,4RU5'bVH5E6U-?Th!pgseKJd[&CZC/RQ33erEmP=cI@'$qYa.Z#\i5-sDh&&`7Pj>A>Q_d1L6sh-XFjem&<\]`mX,UicSL*Z*"[;V&@,`HD6mI[os-nTjK,cLV:O3];I+,ZjXR8#?-gF"DU"Rg`9J0s.N#3r<%;CZXrBaS6&=%s\IXLEb(1u6Ed)ssjUecd/S8:,aI:sG7\ZCXU).84\0:54L9Q$[&9d4:Yb"5Bo0Z%Y!,FRpXQ9Gdjt13$RpHpr_F(s:XEanMdi5WrQhhR*U).330>Q:D!3FF:J@-@@2QY*GE_4J8Yq?Gt?(N]lR>4+MXViG?_id-FU("<7,cQC#dLo+fLt+A[pBe)V=2"U,L9Qt2S`/FmjaaSi)o.nKJ9AOggiIPM!5$8\@Ei$#]!n&LD\DKK\R'eD]eSi!A(L=URkOMaQ]D<cRU1O#k68Z4\!V;V=g+c:_n6#ON>oZ:%KL9)`2r,5Z!jaL.At*>H5a,]4g=#!>!"_Rk4LSVog-&NBI%I)A%ABt7Y02=WDggh0iP38p+U$"h+U`a@QfLS\LeK<!FX/VbduYu28/>TVUW6/`8h!`<Z@e\2F^tflZt>C?&H/C(u6tk7U9ec\lNDp11_YN*+:F%J2LsoldfOr^$.t2>8s6V5QInI!3hak^fh4&lT%@L>W3U-'"&L92X4F(l(r9XQ:;HLXn1i(j_jo.Bre<52T/Wk\]kJddWL6dN#e^pW\/o(>BJtpCDd9-*Q'r;S@;M6@R/R_%:Dm_^m\NU-=]3>Jk1]#QOpJM5tANPPnfN#^m\MYh3.kIh-/$k253L8A(CX'T+_KG0uU.tMqMt7EAr;_W<1W^G$@>dD%0qJ#Qq0I&@1gH/ubbeT%EqfGu`/\;E8=U;Ok@og;hh"Mkb_b,)2b+O5eq6NR7@r<Bdl16^4ViaTFi:[A]oEkYhPoe?jM1a7g96a?CK"=dL(CE4ssrkM$[3l*qo6FPM"+;"!bD5E0U&533?^XuFq9p!.\EYhjT%He\VP1a'gr%Jlc]kH/(>!3eOlJ@0b:G+V&+CH4e!I/euU,uM#fCPb2);15E37E\Y:\"U?GUFE[!>?]6K>E:99CgGek]jU3\>+R8VgGm<$WYJsom=WGhYCJRT5o[OEDstAa%Pd9q!S6lE)ReK`b5'O=0GXr+`r]\Y2h^#D!7nht/'6rY8iRRdlT"sHjT)NN"2J(MJ2bY;>GTYapWPaTR.VF!eoCamo*4D9Dq]H%F7V!W(:3E<@*KY+g)'pnjUe\*%Bm11'$JdiRH`G?jO`_eCM0i2W<N>$l1^o&6%r/6k.KDr!#=%u\pXQ!23l_Ya(WT<3[(F*cpS]>qo=hC!&84d6e%reW(C9E=&S2:G)hU`^tK>"X;Qq!G8@MsNJ;U/rDnLZ"2LA\R!#qo]B-1O98;4Zs!AOi9`i,!">,A7<D3J0mYQS#Hf?17*XRAi(a9a*U6\HHE2'f?@VaLZH)cG9g])/R!CZ~>endstream
|
||||
endobj
|
||||
xref
|
||||
0 9
|
||||
0000000000 65535 f
|
||||
0000000073 00000 n
|
||||
0000000114 00000 n
|
||||
0000000221 00000 n
|
||||
0000000330 00000 n
|
||||
0000000533 00000 n
|
||||
0000000601 00000 n
|
||||
0000000936 00000 n
|
||||
0000000995 00000 n
|
||||
trailer
|
||||
<<
|
||||
/ID
|
||||
[<544804e2078c10a71a717f7cd03ded29><544804e2078c10a71a717f7cd03ded29>]
|
||||
% ReportLab generated PDF document -- digest (http://www.reportlab.com)
|
||||
|
||||
/Info 6 0 R
|
||||
/Root 5 0 R
|
||||
/Size 9
|
||||
>>
|
||||
startxref
|
||||
3336
|
||||
%%EOF
|
||||
417
setup/generateTabletopPDF/a3_aruco.py
Normal file
417
setup/generateTabletopPDF/a3_aruco.py
Normal file
@@ -0,0 +1,417 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import random
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import List, Tuple
|
||||
|
||||
from reportlab.pdfgen import canvas
|
||||
from reportlab.lib.units import mm
|
||||
from reportlab.pdfbase import pdfmetrics
|
||||
|
||||
try:
|
||||
import cv2
|
||||
except ImportError as exc:
|
||||
raise SystemExit(
|
||||
"OpenCV ist erforderlich. Installiere es mit: pip install opencv-contrib-python"
|
||||
) from exc
|
||||
|
||||
|
||||
# =========================
|
||||
# Header / Parameter
|
||||
# =========================
|
||||
|
||||
PAGE_FORMAT = "A3" # z.B. A0, A1, A2, A3, A4
|
||||
ORIENTATION = "portrait" # "portrait" oder "landscape"
|
||||
|
||||
NUM_ARUCOS = 10
|
||||
ARUCO_SIZE_MM = 50.0
|
||||
ARUCO_START_ID = 46 # erster Marker aus DICT_4X4_250
|
||||
SEED = 224 # Zufalls-Seed für reproduzierbare Verteilung
|
||||
|
||||
PAGE_BORDER_MARGIN_MM = 10.0 # Abstand aller Marker vom Seitenrand
|
||||
|
||||
FORBIDDEN_RECT_W_MM = 10.0
|
||||
FORBIDDEN_RECT_H_MM = 10.0
|
||||
FORBIDDEN_RECT_MARGIN_MM = 30.0 # keine ArUcos innerhalb dieses Abstands
|
||||
|
||||
LINE_WIDTH_MM = 1.0 # Linienstärke des Rechtecks
|
||||
TEXT_FONT = "Times-Roman"
|
||||
TEXT_SIZE_PT = 8
|
||||
TEXT_GAP_MM = 4.0
|
||||
|
||||
OUTPUT_BASENAME = f"{PAGE_FORMAT}_{NUM_ARUCOS}Arucos_{int(ARUCO_SIZE_MM)}mm_Seet{SEED}"
|
||||
|
||||
|
||||
# =========================
|
||||
# DIN-Formate
|
||||
# =========================
|
||||
|
||||
DIN_SIZES_MM = {
|
||||
"A0": (841.0, 1189.0),
|
||||
"A1": (594.0, 841.0),
|
||||
"A2": (420.0, 594.0),
|
||||
"A3": (297.0, 420.0),
|
||||
"A4": (210.0, 297.0),
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RectMM:
|
||||
x: float
|
||||
y: float
|
||||
w: float
|
||||
h: float
|
||||
|
||||
def intersects(self, other: "RectMM") -> bool:
|
||||
return not (
|
||||
self.x + self.w <= other.x
|
||||
or other.x + other.w <= self.x
|
||||
or self.y + self.h <= other.y
|
||||
or other.y + other.h <= self.y
|
||||
)
|
||||
|
||||
|
||||
def mm_to_pt(value_mm: float) -> float:
|
||||
return value_mm * mm
|
||||
|
||||
|
||||
def get_page_size_mm(page_format: str, orientation: str) -> Tuple[float, float]:
|
||||
if page_format not in DIN_SIZES_MM:
|
||||
raise ValueError(f"Unbekanntes Format: {page_format}. Unterstützt: {sorted(DIN_SIZES_MM)}")
|
||||
|
||||
w_mm, h_mm = DIN_SIZES_MM[page_format]
|
||||
if orientation.lower() == "portrait":
|
||||
return w_mm, h_mm
|
||||
if orientation.lower() == "landscape":
|
||||
return h_mm, w_mm
|
||||
raise ValueError("ORIENTATION muss 'portrait' oder 'landscape' sein.")
|
||||
|
||||
|
||||
def centered_rect(page_w_mm: float, page_h_mm: float, rect_w_mm: float, rect_h_mm: float) -> RectMM:
|
||||
return RectMM(
|
||||
x=(page_w_mm - rect_w_mm) / 2.0,
|
||||
y=(page_h_mm - rect_h_mm) / 2.0,
|
||||
w=rect_w_mm,
|
||||
h=rect_h_mm,
|
||||
)
|
||||
|
||||
|
||||
def expand_rect(rect: RectMM, margin_mm: float) -> RectMM:
|
||||
return RectMM(
|
||||
x=rect.x - margin_mm,
|
||||
y=rect.y - margin_mm,
|
||||
w=rect.w + 2.0 * margin_mm,
|
||||
h=rect.h + 2.0 * margin_mm,
|
||||
)
|
||||
|
||||
|
||||
def get_aruco_dictionary():
|
||||
# DICT_4X4_250 hat IDs 0..249
|
||||
return cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)
|
||||
|
||||
|
||||
def marker_module_pattern(marker_id: int) -> List[List[int]]:
|
||||
"""
|
||||
Liefert ein 6x6-Raster (inkl. schwarzem Rand).
|
||||
1 = schwarz, 0 = weiss
|
||||
"""
|
||||
aruco_dict = get_aruco_dictionary()
|
||||
img = cv2.aruco.generateImageMarker(aruco_dict, marker_id, 600) # nur zum Abtasten
|
||||
modules = 6
|
||||
cell = img.shape[0] // modules
|
||||
|
||||
pattern: List[List[int]] = []
|
||||
for r in range(modules):
|
||||
row: List[int] = []
|
||||
for c in range(modules):
|
||||
cy = int((r + 0.5) * cell)
|
||||
cx = int((c + 0.5) * cell)
|
||||
pixel = int(img[cy, cx]) # 0 = schwarz, 255 = weiss
|
||||
row.append(1 if pixel < 128 else 0)
|
||||
pattern.append(row)
|
||||
return pattern
|
||||
|
||||
|
||||
def draw_aruco_vector(
|
||||
c: canvas.Canvas,
|
||||
x_mm: float,
|
||||
y_mm: float,
|
||||
size_mm: float,
|
||||
marker_id: int,
|
||||
page_h_mm: float,
|
||||
) -> None:
|
||||
"""
|
||||
Zeichnet den Marker als Vektor-Rechtecke.
|
||||
x_mm, y_mm = linke obere Ecke in mm.
|
||||
"""
|
||||
pattern = marker_module_pattern(marker_id)
|
||||
modules = len(pattern)
|
||||
cell_mm = size_mm / modules
|
||||
|
||||
for r in range(modules):
|
||||
for col in range(modules):
|
||||
if pattern[r][col] == 1:
|
||||
cell_x_mm = x_mm + col * cell_mm
|
||||
#cell_y_mm = y_mm + (modules - 1 - r) * cell_mm
|
||||
cell_y_mm = y_mm + r * cell_mm
|
||||
c.rect(
|
||||
mm_to_pt(cell_x_mm),
|
||||
mm_to_pt(page_h_mm - cell_y_mm - cell_mm),
|
||||
mm_to_pt(cell_mm),
|
||||
mm_to_pt(cell_mm),
|
||||
stroke=0,
|
||||
fill=1,
|
||||
)
|
||||
|
||||
|
||||
def draw_aruco_label(
|
||||
c: canvas.Canvas,
|
||||
x_mm: float,
|
||||
y_mm: float,
|
||||
size_mm: float,
|
||||
page_h_mm: float,
|
||||
marker_id: int,
|
||||
) -> None:
|
||||
c.setFont(TEXT_FONT, TEXT_SIZE_PT)
|
||||
text = str(marker_id)
|
||||
text_width_pt = pdfmetrics.stringWidth(text, TEXT_FONT, TEXT_SIZE_PT)
|
||||
text_x_pt = mm_to_pt(x_mm + size_mm / 2.0) - text_width_pt / 2.0
|
||||
font = pdfmetrics.getFont(TEXT_FONT)
|
||||
ascent_mm = (font.face.ascent / 1000.0) * TEXT_SIZE_PT * 0.352777777777778
|
||||
text_baseline_y_mm = y_mm + size_mm + TEXT_GAP_MM + ascent_mm
|
||||
c.drawString(text_x_pt, mm_to_pt(page_h_mm - text_baseline_y_mm), text)
|
||||
|
||||
|
||||
def place_markers(
|
||||
page_w_mm: float,
|
||||
page_h_mm: float,
|
||||
num_markers: int,
|
||||
marker_size_mm: float,
|
||||
border_margin_mm: float,
|
||||
forbidden_area: RectMM,
|
||||
forbidden_margin_mm: float,
|
||||
seed: int,
|
||||
) -> List[dict]:
|
||||
rng = random.Random(seed)
|
||||
placed: List[RectMM] = []
|
||||
result: List[dict] = []
|
||||
|
||||
allowed_x_min = border_margin_mm
|
||||
allowed_y_min = border_margin_mm
|
||||
allowed_x_max = page_w_mm - border_margin_mm - marker_size_mm
|
||||
allowed_y_max = page_h_mm - border_margin_mm - marker_size_mm
|
||||
|
||||
if allowed_x_max < allowed_x_min or allowed_y_max < allowed_y_min:
|
||||
raise RuntimeError("Das Poster ist zu klein für Randabstand und Markergrösse.")
|
||||
|
||||
excluded = expand_rect(forbidden_area, forbidden_margin_mm)
|
||||
|
||||
attempts = 0
|
||||
max_attempts = 200000
|
||||
|
||||
while len(result) < num_markers and attempts < max_attempts:
|
||||
attempts += 1
|
||||
x = rng.uniform(allowed_x_min, allowed_x_max)
|
||||
y = rng.uniform(allowed_y_min, allowed_y_max)
|
||||
candidate = RectMM(x=x, y=y, w=marker_size_mm, h=marker_size_mm)
|
||||
|
||||
if any(candidate.intersects(other) for other in placed):
|
||||
continue
|
||||
if candidate.intersects(excluded):
|
||||
continue
|
||||
|
||||
placed.append(candidate)
|
||||
result.append(
|
||||
{
|
||||
"id": ARUCO_START_ID + len(result),
|
||||
"x_mm": round(x, 2),
|
||||
"y_mm": round(y, 2),
|
||||
"size_mm": marker_size_mm,
|
||||
}
|
||||
)
|
||||
|
||||
if len(result) < num_markers:
|
||||
raise RuntimeError(
|
||||
f"Nicht alle Marker konnten platziert werden: {len(result)} von {num_markers} "
|
||||
f"(nach {attempts} Versuchen)."
|
||||
)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def main() -> None:
|
||||
page_w_mm, page_h_mm = get_page_size_mm(PAGE_FORMAT, ORIENTATION)
|
||||
|
||||
if ARUCO_START_ID < 0 or ARUCO_START_ID + NUM_ARUCOS > 250:
|
||||
raise ValueError("ARUCO_START_ID + NUM_ARUCOS muss in den Bereich 0..249 von DICT_4X4_250 fallen.")
|
||||
|
||||
forbidden_rect = centered_rect(page_w_mm, page_h_mm, FORBIDDEN_RECT_W_MM, FORBIDDEN_RECT_H_MM)
|
||||
|
||||
# Neues lokales Koordinatensystem
|
||||
origin_x_mm = forbidden_rect.x + forbidden_rect.w - 90.0
|
||||
origin_y_mm = forbidden_rect.y
|
||||
|
||||
placements = place_markers(
|
||||
page_w_mm=page_w_mm,
|
||||
page_h_mm=page_h_mm,
|
||||
num_markers=NUM_ARUCOS,
|
||||
marker_size_mm=ARUCO_SIZE_MM,
|
||||
border_margin_mm=PAGE_BORDER_MARGIN_MM,
|
||||
forbidden_area=forbidden_rect,
|
||||
forbidden_margin_mm=FORBIDDEN_RECT_MARGIN_MM,
|
||||
seed=SEED,
|
||||
)
|
||||
|
||||
pdf_path = Path(f"{OUTPUT_BASENAME}.pdf")
|
||||
json_path = Path(f"{OUTPUT_BASENAME}.json")
|
||||
|
||||
c = canvas.Canvas(str(pdf_path), pagesize=(mm_to_pt(page_w_mm), mm_to_pt(page_h_mm)))
|
||||
c.setTitle(pdf_path.stem)
|
||||
c.setAuthor("OpenAI")
|
||||
c.setSubject("A0 poster with random ArUco placement")
|
||||
|
||||
# Weisser Hintergrund
|
||||
c.setFillColorRGB(1, 1, 1)
|
||||
c.rect(0, 0, mm_to_pt(page_w_mm), mm_to_pt(page_h_mm), stroke=0, fill=1)
|
||||
|
||||
# Rechteck mit 1 mm schwarzer Linie
|
||||
c.setStrokeColorRGB(0, 0, 0)
|
||||
c.setLineWidth(mm_to_pt(LINE_WIDTH_MM))
|
||||
c.rect(
|
||||
mm_to_pt(forbidden_rect.x),
|
||||
mm_to_pt(page_h_mm - forbidden_rect.y - forbidden_rect.h),
|
||||
mm_to_pt(forbidden_rect.w),
|
||||
mm_to_pt(forbidden_rect.h),
|
||||
stroke=1,
|
||||
fill=0,
|
||||
)
|
||||
|
||||
# Koordinatensystem
|
||||
|
||||
ARROW_LEN_MM = 50.0
|
||||
|
||||
# X-Achse (rot, nach unten)
|
||||
c.setStrokeColorRGB(1, 0, 0)
|
||||
c.setLineWidth(mm_to_pt(1.0))
|
||||
|
||||
c.line(
|
||||
mm_to_pt(origin_x_mm),
|
||||
mm_to_pt(page_h_mm - origin_y_mm),
|
||||
mm_to_pt(origin_x_mm),
|
||||
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)),
|
||||
)
|
||||
|
||||
# Pfeilspitze
|
||||
c.line(
|
||||
mm_to_pt(origin_x_mm),
|
||||
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)),
|
||||
mm_to_pt(origin_x_mm - 4),
|
||||
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM - 4)),
|
||||
)
|
||||
c.line(
|
||||
mm_to_pt(origin_x_mm),
|
||||
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)),
|
||||
mm_to_pt(origin_x_mm + 4),
|
||||
mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM - 4)),
|
||||
)
|
||||
|
||||
# Y-Achse (grün, nach rechts)
|
||||
c.setStrokeColorRGB(0, 0.7, 0)
|
||||
|
||||
c.line(
|
||||
mm_to_pt(origin_x_mm),
|
||||
mm_to_pt(page_h_mm - origin_y_mm),
|
||||
mm_to_pt(origin_x_mm + ARROW_LEN_MM),
|
||||
mm_to_pt(page_h_mm - origin_y_mm),
|
||||
)
|
||||
|
||||
# Pfeilspitze
|
||||
c.line(
|
||||
mm_to_pt(origin_x_mm + ARROW_LEN_MM),
|
||||
mm_to_pt(page_h_mm - origin_y_mm),
|
||||
mm_to_pt(origin_x_mm + ARROW_LEN_MM - 4),
|
||||
mm_to_pt(page_h_mm - origin_y_mm - 4),
|
||||
)
|
||||
c.line(
|
||||
mm_to_pt(origin_x_mm + ARROW_LEN_MM),
|
||||
mm_to_pt(page_h_mm - origin_y_mm),
|
||||
mm_to_pt(origin_x_mm + ARROW_LEN_MM - 4),
|
||||
mm_to_pt(page_h_mm - origin_y_mm + 4),
|
||||
)
|
||||
|
||||
c.setStrokeColorRGB(0, 0, 0)
|
||||
|
||||
# ArUcos zeichnen
|
||||
c.setFillColorRGB(0, 0, 0)
|
||||
for item in placements:
|
||||
draw_aruco_vector(
|
||||
c=c,
|
||||
x_mm=item["x_mm"],
|
||||
y_mm=item["y_mm"],
|
||||
size_mm=item["size_mm"],
|
||||
marker_id=item["id"],
|
||||
page_h_mm=page_h_mm,
|
||||
)
|
||||
draw_aruco_label(
|
||||
c=c,
|
||||
x_mm=item["x_mm"],
|
||||
y_mm=item["y_mm"],
|
||||
size_mm=item["size_mm"],
|
||||
page_h_mm=page_h_mm,
|
||||
marker_id=item["id"],
|
||||
)
|
||||
|
||||
c.showPage()
|
||||
c.save()
|
||||
|
||||
# JSON mit Positionen
|
||||
with json_path.open("w", encoding="utf-8") as f:
|
||||
meta = {
|
||||
"page_format": PAGE_FORMAT,
|
||||
"orientation": ORIENTATION,
|
||||
"page_size_mm": {"width": page_w_mm, "height": page_h_mm},
|
||||
"seed": SEED,
|
||||
"num_arucos": NUM_ARUCOS,
|
||||
"aruco_size_mm": ARUCO_SIZE_MM,
|
||||
"aruco_dictionary": "DICT_4X4_250",
|
||||
"aruco_start_id": ARUCO_START_ID,
|
||||
"page_border_margin_mm": PAGE_BORDER_MARGIN_MM,
|
||||
"forbidden_rectangle_mm": {
|
||||
"x": round(forbidden_rect.x, 2),
|
||||
"y": round(forbidden_rect.y, 2),
|
||||
"w": forbidden_rect.w,
|
||||
"h": forbidden_rect.h,
|
||||
},
|
||||
"forbidden_rectangle_margin_mm": FORBIDDEN_RECT_MARGIN_MM,
|
||||
}
|
||||
|
||||
f.write(json.dumps(meta, indent=2, ensure_ascii=False)[:-2])
|
||||
f.write(',\n "placements": [\n')
|
||||
|
||||
for index, p in enumerate(placements):
|
||||
item = {
|
||||
"id": p["id"],
|
||||
"position": [
|
||||
round((p["y_mm"] + ARUCO_SIZE_MM / 2) - origin_y_mm, 2),
|
||||
-1*round(origin_x_mm - (p["x_mm"] + ARUCO_SIZE_MM / 2), 2),
|
||||
-27.3,
|
||||
],
|
||||
"normal": [0, 0, 1],
|
||||
"spin": 90,
|
||||
}
|
||||
line = json.dumps(item, ensure_ascii=False)
|
||||
if index < len(placements) - 1:
|
||||
line += ","
|
||||
f.write(f" {line}\n")
|
||||
|
||||
f.write(" ]\n}\n")
|
||||
|
||||
print(f"PDF geschrieben: {pdf_path.resolve()}")
|
||||
print(f"JSON geschrieben: {json_path.resolve()}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
424
test/jRun.py
Normal file
424
test/jRun.py
Normal file
@@ -0,0 +1,424 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
jRun.py – Test-Pipeline: detect → camera-pose → ArUco-CSV
|
||||
|
||||
Ablauf:
|
||||
1. pipeline/1_detect_aruco_observations.py pro Bild → test/temp/
|
||||
2. pipeline/2_estimate_camera_from_observations.py pro Bild → test/temp/
|
||||
3. Positionen + Normalen per solvePnP aus Beobachtungen ableiten,
|
||||
über alle Kameras mitteln, nach ID sortieren
|
||||
4. test/temp/detections.csv schreiben: id, set, x, y, z, nx, ny, nz
|
||||
|
||||
Einheiten: x/y/z in mm (Weltframe des Roboters), nx/ny/nz dimensionslos.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import glob
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import subprocess
|
||||
import sys
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Pfade
|
||||
# ---------------------------------------------------------------------------
|
||||
SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
|
||||
PROJECT_ROOT = os.path.dirname(SCRIPT_DIR)
|
||||
PIPELINE_DIR = os.path.join(PROJECT_ROOT, "pipeline")
|
||||
DATA_DIR = os.path.join(PROJECT_ROOT, "data", "testPictures")
|
||||
TEMP_DIR = os.path.join(SCRIPT_DIR, "temp")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Hilfsfunktionen – Dateien
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def find_images() -> List[str]:
|
||||
return sorted(glob.glob(os.path.join(DATA_DIR, "cam*_hires_*.jpg")))
|
||||
|
||||
|
||||
def cam_id_from_path(img_path: str) -> str:
|
||||
m = re.match(r"(cam\d+)_", os.path.basename(img_path))
|
||||
if not m:
|
||||
raise ValueError(f"Kein Camera-ID in Dateiname: {img_path}")
|
||||
return m.group(1)
|
||||
|
||||
|
||||
def npz_path(cam_id: str) -> str:
|
||||
p = os.path.join(DATA_DIR, f"calibration_{cam_id}.npz")
|
||||
if not os.path.exists(p):
|
||||
raise FileNotFoundError(f"Kalibrierung nicht gefunden: {p}")
|
||||
return p
|
||||
|
||||
|
||||
def find_robot_json() -> str:
|
||||
candidates = glob.glob(os.path.join(DATA_DIR, "robot*.json"))
|
||||
if not candidates:
|
||||
raise FileNotFoundError(f"Kein robot*.json in {DATA_DIR}")
|
||||
return sorted(candidates)[0]
|
||||
|
||||
|
||||
def detection_json_path(img_path: str) -> str:
|
||||
base = os.path.splitext(os.path.basename(img_path))[0]
|
||||
return os.path.join(TEMP_DIR, f"{base}_aruco_detection.json")
|
||||
|
||||
|
||||
def camera_pose_json_path(img_path: str) -> str:
|
||||
base = os.path.splitext(os.path.basename(img_path))[0]
|
||||
return os.path.join(TEMP_DIR, f"{base}_camera_pose.json")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Subprocess-Wrapper
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def run_step(cmd: List[str]) -> None:
|
||||
print(f"\n>>> {' '.join(cmd)}")
|
||||
r = subprocess.run(cmd, text=True)
|
||||
if r.returncode != 0:
|
||||
raise RuntimeError(f"Befehl fehlgeschlagen (exit {r.returncode})")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Marker-Klassifizierung aus robot.json
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def load_marker_set_map(robot_path: str) -> Dict[int, str]:
|
||||
"""
|
||||
Gibt marker_id -> Set-Label zurück.
|
||||
'set'-Marker → set_name (z.B. 'Brett', 'A0')
|
||||
Arm/Loose → Link-Name
|
||||
"""
|
||||
if PIPELINE_DIR not in sys.path:
|
||||
sys.path.insert(0, PIPELINE_DIR)
|
||||
|
||||
# Import erst nach sys.path-Erweiterung
|
||||
from marker_sets import classify_markers # noqa: PLC0415
|
||||
|
||||
with open(robot_path, "r", encoding="utf-8") as f:
|
||||
robot_data = json.load(f)
|
||||
|
||||
classification = classify_markers(robot_data)
|
||||
result: Dict[int, str] = {}
|
||||
for mid, info in classification.items():
|
||||
if info.role == "set" and info.set_name:
|
||||
result[mid] = info.set_name
|
||||
else:
|
||||
result[mid] = info.link
|
||||
return result
|
||||
|
||||
|
||||
def load_a0_reference(robot_path: str) -> Dict[int, np.ndarray]:
|
||||
"""
|
||||
Welt-Referenz (x, y) in mm für die A0-Marker aus robot.json.
|
||||
|
||||
Nur die A0-Marker definieren (momentan) die Welt-Koordinaten, daher wird
|
||||
der dxy-Abgleich ausschließlich auf sie beschränkt. Position steht im
|
||||
Board-Frame = Welt-Frame (Board ist Wurzel-Link bei [0,0,0]).
|
||||
"""
|
||||
if PIPELINE_DIR not in sys.path:
|
||||
sys.path.insert(0, PIPELINE_DIR)
|
||||
|
||||
from marker_sets import classify_markers # noqa: PLC0415
|
||||
|
||||
with open(robot_path, "r", encoding="utf-8") as f:
|
||||
robot_data = json.load(f)
|
||||
|
||||
ref: Dict[int, np.ndarray] = {}
|
||||
for mid, info in classify_markers(robot_data).items():
|
||||
if info.role == "set" and info.set_name == "A0":
|
||||
ref[mid] = np.array(info.position[:2], dtype=float)
|
||||
return ref
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Positionen + Normalen per solvePnP
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _marker_local_corners(marker_size_m: float) -> np.ndarray:
|
||||
h = marker_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)
|
||||
|
||||
|
||||
def estimate_marker_poses(
|
||||
det_json: dict,
|
||||
pose_json: dict,
|
||||
) -> Dict[int, Tuple[np.ndarray, np.ndarray]]:
|
||||
"""
|
||||
Gibt pro erkannter Marker-ID: (pos_mm [3], normal_world [3]) zurück.
|
||||
|
||||
pos_mm: Marker-Mittelpunkt im Weltframe, Einheit mm
|
||||
normal_world: normierter Normalenvektor (Marker-Z-Achse im Weltframe)
|
||||
"""
|
||||
K = np.array(det_json["camera"]["camera_matrix"],
|
||||
dtype=np.float64).reshape(3, 3)
|
||||
D = np.array(det_json["camera"]["distortion_coefficients"],
|
||||
dtype=np.float64).reshape(-1, 1)
|
||||
marker_size_m = float(
|
||||
det_json.get("vision_config", {}).get("MarkerSize", 0.025)
|
||||
)
|
||||
|
||||
w2c = pose_json["camera_pose"]["world_to_camera"]
|
||||
R_wc = np.array(w2c["rotation_matrix"], dtype=np.float64).reshape(3, 3)
|
||||
t_wc = np.array(w2c["translation_m"], dtype=np.float64).reshape(3)
|
||||
|
||||
obj_corners = _marker_local_corners(marker_size_m)
|
||||
result: Dict[int, Tuple[np.ndarray, np.ndarray]] = {}
|
||||
|
||||
for det in det_json.get("detections", []):
|
||||
mid = int(det["marker_id"])
|
||||
corners_px = np.array(
|
||||
det["image_points_px"], dtype=np.float32
|
||||
).reshape(4, 2)
|
||||
|
||||
ok, rvec, tvec = cv2.solvePnP(
|
||||
obj_corners, corners_px,
|
||||
K.astype(np.float32), D.astype(np.float32),
|
||||
flags=cv2.SOLVEPNP_IPPE_SQUARE,
|
||||
)
|
||||
if not ok:
|
||||
ok, rvec, tvec = cv2.solvePnP(
|
||||
obj_corners, corners_px,
|
||||
K.astype(np.float32), D.astype(np.float32),
|
||||
flags=cv2.SOLVEPNP_ITERATIVE,
|
||||
)
|
||||
if not ok:
|
||||
continue
|
||||
|
||||
tvec_f = tvec.reshape(3).astype(np.float64)
|
||||
R_mc, _ = cv2.Rodrigues(rvec.reshape(3, 1))
|
||||
R_mc = R_mc.astype(np.float64)
|
||||
|
||||
# Weltframe-Position: x_world = R_wc.T @ (x_cam - t_wc)
|
||||
pos_world_m = R_wc.T @ (tvec_f - t_wc)
|
||||
|
||||
# Marker-Normale (Z-Achse) im Weltframe
|
||||
normal_cam = R_mc[:, 2]
|
||||
normal_world = R_wc.T @ normal_cam
|
||||
nlen = np.linalg.norm(normal_world)
|
||||
if nlen > 1e-9:
|
||||
normal_world /= nlen
|
||||
|
||||
result[mid] = (pos_world_m * 1000.0, normal_world)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Main
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def main() -> None:
|
||||
os.makedirs(TEMP_DIR, exist_ok=True)
|
||||
|
||||
images = find_images()
|
||||
if not images:
|
||||
print(f"[FEHLER] Keine cam*_hires_*.jpg in {DATA_DIR}")
|
||||
sys.exit(1)
|
||||
|
||||
robot_path = find_robot_json()
|
||||
print(f"Robot: {os.path.basename(robot_path)}")
|
||||
print(f"Bilder: {[os.path.basename(i) for i in images]}")
|
||||
print(f"Ausgabe-Ordner: {TEMP_DIR}")
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Schritt 1: ArUco-Erkennung
|
||||
# ------------------------------------------------------------------
|
||||
print("\n" + "=" * 60)
|
||||
print("Schritt 1: ArUco-Erkennung")
|
||||
print("=" * 60)
|
||||
for img in images:
|
||||
cam_id = cam_id_from_path(img)
|
||||
run_step([
|
||||
sys.executable,
|
||||
os.path.join(PIPELINE_DIR, "1_detect_aruco_observations.py"),
|
||||
"-i", img,
|
||||
"-npz", npz_path(cam_id),
|
||||
"-robot", robot_path,
|
||||
"-cameraId", cam_id,
|
||||
"-outDir", TEMP_DIR,
|
||||
"--saveDebugImage",
|
||||
])
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Schritt 2: Kamera-Positionen
|
||||
# ------------------------------------------------------------------
|
||||
print("\n" + "=" * 60)
|
||||
print("Schritt 2: Kamera-Poses")
|
||||
print("=" * 60)
|
||||
for img in images:
|
||||
run_step([
|
||||
sys.executable,
|
||||
os.path.join(PIPELINE_DIR, "2_estimate_camera_from_observations.py"),
|
||||
"-i", detection_json_path(img),
|
||||
"-robot", robot_path,
|
||||
"-outDir", TEMP_DIR,
|
||||
])
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Schritt 3: Beobachtungen zusammenführen
|
||||
# ------------------------------------------------------------------
|
||||
print("\n" + "=" * 60)
|
||||
print("Schritt 3: Positionen + Normalen")
|
||||
print("=" * 60)
|
||||
|
||||
marker_set_map = load_marker_set_map(robot_path)
|
||||
a0_ref = load_a0_reference(robot_path)
|
||||
|
||||
# mid -> Liste von (pos_mm, normal)
|
||||
observations: Dict[int, List[Tuple[np.ndarray, np.ndarray]]] = {}
|
||||
|
||||
# cam_id -> Welt-Position (mm) / QA-Metadaten
|
||||
camera_positions: Dict[str, np.ndarray] = {}
|
||||
camera_meta: Dict[str, dict] = {}
|
||||
|
||||
for img in images:
|
||||
det_path = detection_json_path(img)
|
||||
pose_path = camera_pose_json_path(img)
|
||||
|
||||
if not os.path.exists(det_path):
|
||||
print(f"[WARN] Fehlend: {det_path}")
|
||||
continue
|
||||
if not os.path.exists(pose_path):
|
||||
print(f"[WARN] Fehlend: {pose_path}")
|
||||
continue
|
||||
|
||||
with open(det_path, "r", encoding="utf-8") as f:
|
||||
det_json = json.load(f)
|
||||
with open(pose_path, "r", encoding="utf-8") as f:
|
||||
pose_json = json.load(f)
|
||||
|
||||
cam_id = cam_id_from_path(img)
|
||||
ciw = pose_json["camera_pose"]["camera_in_world"]["position_mm"]
|
||||
camera_positions[cam_id] = np.array(ciw, dtype=float)
|
||||
est = pose_json.get("estimation", {})
|
||||
camera_meta[cam_id] = {
|
||||
"rms_px": est.get("residual_rms_px"),
|
||||
"num_markers": est.get("num_used_markers"),
|
||||
}
|
||||
|
||||
cam_name = os.path.basename(img)
|
||||
poses = estimate_marker_poses(det_json, pose_json)
|
||||
print(f" {cam_name}: {len(poses)} Marker mit Pose")
|
||||
|
||||
for mid, pn in poses.items():
|
||||
observations.setdefault(mid, []).append(pn)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Mittelwert über alle Kameras + CSV schreiben
|
||||
# ------------------------------------------------------------------
|
||||
rows = []
|
||||
for mid in sorted(observations.keys()):
|
||||
obs_list = observations[mid]
|
||||
positions = np.array([p for p, _ in obs_list])
|
||||
normals = np.array([n for _, n in obs_list])
|
||||
|
||||
pos_mean = positions.mean(axis=0)
|
||||
|
||||
n_sum = normals.sum(axis=0)
|
||||
n_norm = np.linalg.norm(n_sum)
|
||||
normal_mean = n_sum / n_norm if n_norm > 1e-9 else np.array([0.0, 0.0, 1.0])
|
||||
|
||||
set_label = marker_set_map.get(mid, "?")
|
||||
|
||||
# dxy: planarer Abgleich gegen robot.json — nur für A0 definiert
|
||||
if mid in a0_ref:
|
||||
d = pos_mean[:2] - a0_ref[mid]
|
||||
dxy = round(float(np.hypot(d[0], d[1])), 2)
|
||||
else:
|
||||
dxy = ""
|
||||
|
||||
rows.append({
|
||||
"id": mid,
|
||||
"set": set_label,
|
||||
"SeenByCount": len(obs_list),
|
||||
"x": round(float(pos_mean[0]), 2),
|
||||
"y": round(float(pos_mean[1]), 2),
|
||||
"z": round(float(pos_mean[2]), 2),
|
||||
"nx": round(float(normal_mean[0]), 4),
|
||||
"ny": round(float(normal_mean[1]), 4),
|
||||
"nz": round(float(normal_mean[2]), 4),
|
||||
"dxy": dxy,
|
||||
})
|
||||
|
||||
# Kamera-Positionen als eigene Zeilen (oben in der CSV)
|
||||
camera_rows = []
|
||||
for cam_id in sorted(camera_positions.keys()):
|
||||
pos = camera_positions[cam_id]
|
||||
camera_rows.append({
|
||||
"id": cam_id,
|
||||
"set": "CAMERA",
|
||||
"SeenByCount": "",
|
||||
"x": round(float(pos[0]), 2),
|
||||
"y": round(float(pos[1]), 2),
|
||||
"z": round(float(pos[2]), 2),
|
||||
"nx": "",
|
||||
"ny": "",
|
||||
"nz": "",
|
||||
"dxy": "",
|
||||
})
|
||||
|
||||
csv_path = os.path.join(TEMP_DIR, "detections.csv")
|
||||
fieldnames = ["id", "set", "SeenByCount", "x", "y", "z", "nx", "ny", "nz", "dxy"]
|
||||
|
||||
with open(csv_path, "w", newline="", encoding="utf-8") as f:
|
||||
writer = csv.DictWriter(f, fieldnames=fieldnames)
|
||||
writer.writeheader()
|
||||
writer.writerows(camera_rows + rows)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Konsolenübersicht
|
||||
# ------------------------------------------------------------------
|
||||
print(f"\nGeschrieben: {csv_path} "
|
||||
f"({len(rows)} Marker + {len(camera_rows)} Kameras)\n")
|
||||
|
||||
# Kamera-Positionen + Reprojektions-RMS (schlechte Kamera erkennen)
|
||||
print("Kamera-Positionen (Welt, mm):")
|
||||
chdr = f"{'cam':>5} {'x':>9} {'y':>9} {'z':>9} {'rms_px':>7} {'#mk':>4}"
|
||||
print(chdr)
|
||||
print("-" * len(chdr))
|
||||
for cam_id in sorted(camera_positions.keys()):
|
||||
pos = camera_positions[cam_id]
|
||||
meta = camera_meta.get(cam_id, {})
|
||||
rms = meta.get("rms_px")
|
||||
nmk = meta.get("num_markers")
|
||||
rms_s = f"{rms:7.2f}" if rms is not None else " n/a"
|
||||
print(f"{cam_id:>5} {pos[0]:>9.1f} {pos[1]:>9.1f} {pos[2]:>9.1f} "
|
||||
f"{rms_s} {str(nmk):>4}")
|
||||
|
||||
# Marker-Tabelle
|
||||
print()
|
||||
hdr = (f"{'id':>5} {'set':<12} {'cams':>4} {'x':>8} {'y':>8} {'z':>8} "
|
||||
f"{'nx':>7} {'ny':>7} {'nz':>7} {'dxy':>7}")
|
||||
print(hdr)
|
||||
print("-" * len(hdr))
|
||||
for row in rows:
|
||||
dxy_s = f"{row['dxy']:7.2f}" if row['dxy'] != "" else " -"
|
||||
print(
|
||||
f"{row['id']:>5} {row['set']:<12} {row['SeenByCount']:>4} "
|
||||
f"{row['x']:>8.1f} {row['y']:>8.1f} {row['z']:>8.1f} "
|
||||
f"{row['nx']:>7.4f} {row['ny']:>7.4f} {row['nz']:>7.4f} {dxy_s}"
|
||||
)
|
||||
|
||||
# A0-Abgleich-Statistik (planarer Fehler gegen robot.json)
|
||||
dxys = np.array([row["dxy"] for row in rows if row["dxy"] != ""], dtype=float)
|
||||
if dxys.size:
|
||||
print(f"\nA0 dxy (Welt-Abgleich, mm): n={dxys.size} "
|
||||
f"mean={dxys.mean():.2f} median={np.median(dxys):.2f} "
|
||||
f"max={dxys.max():.2f}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
2956
test/temp/cam0_hires_1781074183695_aruco_detection.json
Normal file
2956
test/temp/cam0_hires_1781074183695_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
719
test/temp/cam0_hires_1781074183695_camera_pose.json
Normal file
719
test/temp/cam0_hires_1781074183695_camera_pose.json
Normal file
@@ -0,0 +1,719 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-10T10:41:07Z",
|
||||
"source": {
|
||||
"detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\test\\temp\\cam0_hires_1781074183695_aruco_detection.json",
|
||||
"robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\data\\testPictures\\robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam0",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1429.6978759765625,
|
||||
0.0,
|
||||
633.3245239257812
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1414.5067138671875,
|
||||
468.4399108886719
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.0862322673201561,
|
||||
0.14179007709026337,
|
||||
0.0014998731203377247,
|
||||
-0.004277258180081844,
|
||||
-0.7496029734611511
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 37,
|
||||
"used_marker_ids": [
|
||||
95,
|
||||
97,
|
||||
51,
|
||||
55,
|
||||
54,
|
||||
47,
|
||||
79,
|
||||
96,
|
||||
85,
|
||||
62,
|
||||
57,
|
||||
105,
|
||||
59,
|
||||
48,
|
||||
102,
|
||||
86,
|
||||
71,
|
||||
92,
|
||||
72,
|
||||
84,
|
||||
65,
|
||||
80,
|
||||
89,
|
||||
60,
|
||||
56,
|
||||
63,
|
||||
99,
|
||||
68,
|
||||
46,
|
||||
87,
|
||||
67,
|
||||
50,
|
||||
98,
|
||||
76,
|
||||
70,
|
||||
100,
|
||||
91
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.013977914197306877,
|
||||
0.0012063861436378766,
|
||||
0.00041790882674225575,
|
||||
0.000417864448222557
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 0.8488907668283678,
|
||||
"residual_median_px": 0.6015952243821542,
|
||||
"residual_max_px": 3.215993321685572,
|
||||
"sigma2_normalized": 1.900175232935359e-07
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.7153297066688538,
|
||||
-0.6744856238365173,
|
||||
0.18268170952796936
|
||||
],
|
||||
[
|
||||
-0.24956846237182617,
|
||||
-0.4907773733139038,
|
||||
-0.834777295589447
|
||||
],
|
||||
[
|
||||
0.6527013182640076,
|
||||
0.5515493750572205,
|
||||
-0.5193979740142822
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.5062417984008789,
|
||||
0.09014879167079926,
|
||||
0.7660393714904785
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.0691228499009884,
|
||||
-0.7015145339853286,
|
||||
0.6341981126084956
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
-0.11536678671836853,
|
||||
-0.719718337059021,
|
||||
0.5656145811080933
|
||||
],
|
||||
"position_mm": [
|
||||
-115.36678314208984,
|
||||
-719.7183227539062,
|
||||
565.6145629882812
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 133.28041076660156,
|
||||
"pitch": -40.74557876586914,
|
||||
"yaw": -19.2331600189209
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
2.384673507521362e-07,
|
||||
-4.007478825357904e-08,
|
||||
-8.83056895699674e-08,
|
||||
-2.241640771133345e-08,
|
||||
5.7720270356521624e-08,
|
||||
1.378850799895014e-07
|
||||
],
|
||||
[
|
||||
-4.0074788253579336e-08,
|
||||
1.2484717569362809e-07,
|
||||
5.533958978882963e-08,
|
||||
-4.109574324899393e-09,
|
||||
-6.375572286576732e-08,
|
||||
-3.447971573741165e-08
|
||||
],
|
||||
[
|
||||
-8.830568956996727e-08,
|
||||
5.533958978882929e-08,
|
||||
3.5195680429658137e-07,
|
||||
7.077668257887362e-08,
|
||||
-9.83714238710889e-08,
|
||||
-1.8066440288226506e-07
|
||||
],
|
||||
[
|
||||
-2.2416407711333367e-08,
|
||||
-4.10957432489948e-09,
|
||||
7.07766825788736e-08,
|
||||
2.2608963627527025e-08,
|
||||
-1.477120449173928e-08,
|
||||
-4.530858533789268e-08
|
||||
],
|
||||
[
|
||||
5.772027035652173e-08,
|
||||
-6.375572286576727e-08,
|
||||
-9.837142387108908e-08,
|
||||
-1.4771204491739334e-08,
|
||||
5.6162582570999035e-08,
|
||||
7.030683489124922e-08
|
||||
],
|
||||
[
|
||||
1.3788507998950147e-07,
|
||||
-3.44797157374115e-08,
|
||||
-1.8066440288226543e-07,
|
||||
-4.53085853378928e-08,
|
||||
7.030683489124922e-08,
|
||||
2.3454528563310696e-07
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.02797931616962056,
|
||||
0.020244730206848253,
|
||||
0.03399126405526364
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.00015036277340993356,
|
||||
0.0002369864607335175,
|
||||
0.0004842987565884378
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0003928571281192472,
|
||||
0.00037055936963847897,
|
||||
0.0005785760565158723
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
0.39285712811924717,
|
||||
0.37055936963847896,
|
||||
0.5785760565158723
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.034843440181318734,
|
||||
"pitch": 0.02499050875561387,
|
||||
"yaw": 0.029035517784143414
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
259.25,
|
||||
854.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
258.490478515625,
|
||||
853.5327758789062
|
||||
],
|
||||
"reprojection_error_px": 0.8917237602301682,
|
||||
"confidence": 0.8258754592596019
|
||||
},
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
538.5,
|
||||
857.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
538.7420654296875,
|
||||
858.910888671875
|
||||
],
|
||||
"reprojection_error_px": 1.1858575718599158,
|
||||
"confidence": 0.9204299158225671
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
129.5,
|
||||
750.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
128.6764678955078,
|
||||
750.4482421875
|
||||
],
|
||||
"reprojection_error_px": 0.8770763390572774,
|
||||
"confidence": 0.6584739246254625
|
||||
},
|
||||
{
|
||||
"marker_id": 55,
|
||||
"observed_center_px": [
|
||||
402.75,
|
||||
765.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
402.4529724121094,
|
||||
765.8202514648438
|
||||
],
|
||||
"reprojection_error_px": 0.6429713221634116,
|
||||
"confidence": 0.7393243794945659
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
557.0,
|
||||
796.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
556.9273071289062,
|
||||
797.4539184570312
|
||||
],
|
||||
"reprojection_error_px": 0.7076619586053134,
|
||||
"confidence": 0.7478392159262601
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
510.5,
|
||||
750.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
511.28216552734375,
|
||||
750.1286010742188
|
||||
],
|
||||
"reprojection_error_px": 0.7915305498499524,
|
||||
"confidence": 0.7141846645068768
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
343.25,
|
||||
648.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
342.9404296875,
|
||||
648.327880859375
|
||||
],
|
||||
"reprojection_error_px": 0.5234676181611774,
|
||||
"confidence": 0.545522217065577
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
443.5,
|
||||
642.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
444.098388671875,
|
||||
641.9876708984375
|
||||
],
|
||||
"reprojection_error_px": 0.5985156717861821,
|
||||
"confidence": 0.5196626187985754
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
725.5,
|
||||
682.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
725.919189453125,
|
||||
681.0695190429688
|
||||
],
|
||||
"reprojection_error_px": 1.2526991209083898,
|
||||
"confidence": 0.4333112154683819
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
476.0,
|
||||
615.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
476.35174560546875,
|
||||
615.0320434570312
|
||||
],
|
||||
"reprojection_error_px": 0.35320214340387446,
|
||||
"confidence": 0.48909895359164274
|
||||
},
|
||||
{
|
||||
"marker_id": 57,
|
||||
"observed_center_px": [
|
||||
877.0,
|
||||
674.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
877.175048828125,
|
||||
674.4921875
|
||||
],
|
||||
"reprojection_error_px": 0.3116237753833712,
|
||||
"confidence": 0.3447832172321244
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
697.0,
|
||||
632.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
697.4603271484375,
|
||||
631.9234008789062
|
||||
],
|
||||
"reprojection_error_px": 0.7378127337168187,
|
||||
"confidence": 0.3941136916116856
|
||||
},
|
||||
{
|
||||
"marker_id": 59,
|
||||
"observed_center_px": [
|
||||
805.5,
|
||||
596.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
805.6655883789062,
|
||||
596.3532104492188
|
||||
],
|
||||
"reprojection_error_px": 0.42995518235972685,
|
||||
"confidence": 0.272074573917431
|
||||
},
|
||||
{
|
||||
"marker_id": 48,
|
||||
"observed_center_px": [
|
||||
902.25,
|
||||
601.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
902.2860107421875,
|
||||
601.1778564453125
|
||||
],
|
||||
"reprojection_error_px": 0.32415003220668187,
|
||||
"confidence": 0.27826623535156253
|
||||
},
|
||||
{
|
||||
"marker_id": 102,
|
||||
"observed_center_px": [
|
||||
771.25,
|
||||
547.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
770.7193603515625,
|
||||
547.3040771484375
|
||||
],
|
||||
"reprojection_error_px": 0.6931274241000027,
|
||||
"confidence": 0.27992431449450633
|
||||
},
|
||||
{
|
||||
"marker_id": 86,
|
||||
"observed_center_px": [
|
||||
79.25,
|
||||
320.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
78.74799346923828,
|
||||
321.10186767578125
|
||||
],
|
||||
"reprojection_error_px": 0.988781620970154,
|
||||
"confidence": 0.1358137908003952
|
||||
},
|
||||
{
|
||||
"marker_id": 71,
|
||||
"observed_center_px": [
|
||||
912.0,
|
||||
552.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
912.014404296875,
|
||||
551.594970703125
|
||||
],
|
||||
"reprojection_error_px": 0.40528535021083606,
|
||||
"confidence": 0.23938019040412045
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
730.25,
|
||||
523.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
730.6965942382812,
|
||||
523.346923828125
|
||||
],
|
||||
"reprojection_error_px": 0.6015952243821542,
|
||||
"confidence": 0.2632574407582938
|
||||
},
|
||||
{
|
||||
"marker_id": 72,
|
||||
"observed_center_px": [
|
||||
231.25,
|
||||
355.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
231.26632690429688,
|
||||
356.1981201171875
|
||||
],
|
||||
"reprojection_error_px": 1.198231356213527,
|
||||
"confidence": 0.2033684790201144
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
151.5,
|
||||
328.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
152.23899841308594,
|
||||
329.2857971191406
|
||||
],
|
||||
"reprojection_error_px": 1.078700962729356,
|
||||
"confidence": 0.16272265589141124
|
||||
},
|
||||
{
|
||||
"marker_id": 65,
|
||||
"observed_center_px": [
|
||||
965.0,
|
||||
541.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
965.0440673828125,
|
||||
541.2013549804688
|
||||
],
|
||||
"reprojection_error_px": 0.30187875367233796,
|
||||
"confidence": 0.1894175373214682
|
||||
},
|
||||
{
|
||||
"marker_id": 80,
|
||||
"observed_center_px": [
|
||||
1046.25,
|
||||
545.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1046.1553955078125,
|
||||
545.08056640625
|
||||
],
|
||||
"reprojection_error_px": 0.194056055388887,
|
||||
"confidence": 0.16189579196314718
|
||||
},
|
||||
{
|
||||
"marker_id": 89,
|
||||
"observed_center_px": [
|
||||
1159.25,
|
||||
521.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1157.6162109375,
|
||||
524.2700805664062
|
||||
],
|
||||
"reprojection_error_px": 3.215993321685572,
|
||||
"confidence": 0.17985049003304185
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
167.5,
|
||||
310.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
167.38864135742188,
|
||||
310.4362487792969
|
||||
],
|
||||
"reprojection_error_px": 0.4502374314901596,
|
||||
"confidence": 0.15039999323852157
|
||||
},
|
||||
{
|
||||
"marker_id": 56,
|
||||
"observed_center_px": [
|
||||
311.0,
|
||||
355.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
310.8277587890625,
|
||||
356.5285339355469
|
||||
],
|
||||
"reprojection_error_px": 0.7973594694636303,
|
||||
"confidence": 0.20911960257932388
|
||||
},
|
||||
{
|
||||
"marker_id": 63,
|
||||
"observed_center_px": [
|
||||
885.75,
|
||||
511.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
885.6793212890625,
|
||||
511.126708984375
|
||||
],
|
||||
"reprojection_error_px": 0.14211317572143223,
|
||||
"confidence": 0.2003141260801938
|
||||
},
|
||||
{
|
||||
"marker_id": 99,
|
||||
"observed_center_px": [
|
||||
1091.75,
|
||||
505.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1091.5972900390625,
|
||||
505.013671875
|
||||
],
|
||||
"reprojection_error_px": 0.15332074985320285,
|
||||
"confidence": 0.15127373626685678
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
385.25,
|
||||
339.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
384.7447204589844,
|
||||
338.80047607421875
|
||||
],
|
||||
"reprojection_error_px": 0.5432469158017581,
|
||||
"confidence": 0.181058749706097
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
338.25,
|
||||
339.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
338.55401611328125,
|
||||
339.2395324707031
|
||||
],
|
||||
"reprojection_error_px": 0.3041962628044907,
|
||||
"confidence": 0.16268174014312184
|
||||
},
|
||||
{
|
||||
"marker_id": 87,
|
||||
"observed_center_px": [
|
||||
1008.25,
|
||||
466.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1008.1798706054688,
|
||||
466.3894348144531
|
||||
],
|
||||
"reprojection_error_px": 0.36732190923735014,
|
||||
"confidence": 0.13144713990357454
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
272.5,
|
||||
301.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
272.3302001953125,
|
||||
301.4171142578125
|
||||
],
|
||||
"reprojection_error_px": 0.18894978150261477,
|
||||
"confidence": 0.1526939471328476
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
357.25,
|
||||
318.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
357.13116455078125,
|
||||
318.4410400390625
|
||||
],
|
||||
"reprojection_error_px": 0.33102586221249186,
|
||||
"confidence": 0.16588357002730827
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
294.0,
|
||||
270.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
294.2515869140625,
|
||||
269.8180236816406
|
||||
],
|
||||
"reprojection_error_px": 0.7269027955170481,
|
||||
"confidence": 0.12006603211516774
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
488.0,
|
||||
318.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
488.0938720703125,
|
||||
318.10589599609375
|
||||
],
|
||||
"reprojection_error_px": 0.6509085445996369,
|
||||
"confidence": 0.14193112858785442
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
328.0,
|
||||
272.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
328.2170715332031,
|
||||
272.0340270996094
|
||||
],
|
||||
"reprojection_error_px": 0.3062096409819492,
|
||||
"confidence": 0.11352543707690875
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
578.5,
|
||||
294.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
579.1131591796875,
|
||||
294.27313232421875
|
||||
],
|
||||
"reprojection_error_px": 0.7767669919866956,
|
||||
"confidence": 0.13400150965155774
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
416.75,
|
||||
242.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
417.3974609375,
|
||||
241.78074645996094
|
||||
],
|
||||
"reprojection_error_px": 0.7996277574143927,
|
||||
"confidence": 0.11558895571856645
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
test/temp/cam0_hires_1781074183695_debug.jpg
Normal file
BIN
test/temp/cam0_hires_1781074183695_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 205 KiB |
1850
test/temp/cam1_hires_1781074183695_aruco_detection.json
Normal file
1850
test/temp/cam1_hires_1781074183695_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
551
test/temp/cam1_hires_1781074183695_camera_pose.json
Normal file
551
test/temp/cam1_hires_1781074183695_camera_pose.json
Normal file
@@ -0,0 +1,551 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-10T10:41:08Z",
|
||||
"source": {
|
||||
"detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\test\\temp\\cam1_hires_1781074183695_aruco_detection.json",
|
||||
"robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\data\\testPictures\\robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam1",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1335.5843505859375,
|
||||
0.0,
|
||||
669.8739013671875
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1340.3487548828125,
|
||||
434.5127258300781
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.013518857769668102,
|
||||
0.6387264132499695,
|
||||
-0.004291425924748182,
|
||||
-0.003142312401905656,
|
||||
-1.9807322025299072
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 25,
|
||||
"used_marker_ids": [
|
||||
57,
|
||||
59,
|
||||
85,
|
||||
105,
|
||||
54,
|
||||
97,
|
||||
92,
|
||||
102,
|
||||
66,
|
||||
47,
|
||||
95,
|
||||
55,
|
||||
62,
|
||||
69,
|
||||
96,
|
||||
79,
|
||||
103,
|
||||
58,
|
||||
64,
|
||||
51,
|
||||
74,
|
||||
52,
|
||||
75,
|
||||
81,
|
||||
77
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.00821410924365016,
|
||||
0.0005796130478329388,
|
||||
0.0004528899483522234,
|
||||
0.0004528893564058897
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 0.867342582216324,
|
||||
"residual_median_px": 0.608681178523418,
|
||||
"residual_max_px": 2.7179420081717525,
|
||||
"sigma2_normalized": 2.3307814675638535e-07
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.980829656124115,
|
||||
0.1278911679983139,
|
||||
0.14702728390693665
|
||||
],
|
||||
[
|
||||
0.16805413365364075,
|
||||
0.9370887875556946,
|
||||
0.30597788095474243
|
||||
],
|
||||
[
|
||||
-0.09864574670791626,
|
||||
0.32482072710990906,
|
||||
-0.9406170845031738
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
0.2543368637561798,
|
||||
0.17617078125476837,
|
||||
1.0045982599258423
|
||||
],
|
||||
"rvec_rad": [
|
||||
0.22767542290885773,
|
||||
2.968432623677477,
|
||||
0.48528339118991104
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.3189542591571808,
|
||||
-0.5239294767379761,
|
||||
0.8536434769630432
|
||||
],
|
||||
"position_mm": [
|
||||
318.9542541503906,
|
||||
-523.9295043945312,
|
||||
853.6434936523438
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 160.94879150390625,
|
||||
"pitch": 5.661191940307617,
|
||||
"yaw": 170.2774200439453
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
3.127441931746991e-07,
|
||||
1.2611994564482237e-07,
|
||||
-2.6481242745835294e-07,
|
||||
2.220874904109554e-08,
|
||||
-5.424247207491378e-08,
|
||||
-2.3469346032411482e-08
|
||||
],
|
||||
[
|
||||
1.2611994564482083e-07,
|
||||
1.7736366090593526e-06,
|
||||
1.7073582936930538e-06,
|
||||
-2.0513038317965798e-08,
|
||||
5.817162316400782e-08,
|
||||
-4.3575632542245767e-07
|
||||
],
|
||||
[
|
||||
-2.648124274583587e-07,
|
||||
1.7073582936930396e-06,
|
||||
5.575466237723722e-06,
|
||||
-2.143261716718035e-08,
|
||||
8.436780693695763e-08,
|
||||
-5.750157395485433e-07
|
||||
],
|
||||
[
|
||||
2.220874904109555e-08,
|
||||
-2.051303831796606e-08,
|
||||
-2.143261716718066e-08,
|
||||
1.1352143805196684e-08,
|
||||
-5.900072683314034e-09,
|
||||
-4.703659593185704e-09
|
||||
],
|
||||
[
|
||||
-5.4242472074913865e-08,
|
||||
5.817162316400724e-08,
|
||||
8.436780693695687e-08,
|
||||
-5.9000726833139505e-09,
|
||||
2.211014317418639e-08,
|
||||
-1.0277230205531951e-08
|
||||
],
|
||||
[
|
||||
-2.3469346032410817e-08,
|
||||
-4.3575632542245735e-07,
|
||||
-5.750157395485471e-07,
|
||||
-4.703659593184902e-09,
|
||||
-1.0277230205531706e-08,
|
||||
2.2015022890204316e-07
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.032041826154000676,
|
||||
0.07630534399918094,
|
||||
0.13528923079637759
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.00010654643966457389,
|
||||
0.0001486947987462453,
|
||||
0.00046920169320031565
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0015986459647155456,
|
||||
0.0009965541542375047,
|
||||
0.000655580178244062
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
1.5986459647155455,
|
||||
0.9965541542375047,
|
||||
0.655580178244062
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.06336728811161631,
|
||||
"pitch": 0.09289304619911497,
|
||||
"yaw": 0.025599763400525364
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 57,
|
||||
"observed_center_px": [
|
||||
52.25,
|
||||
319.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
51.970375061035156,
|
||||
318.21563720703125
|
||||
],
|
||||
"reprojection_error_px": 0.8327154961238747,
|
||||
"confidence": 0.5244924829597363
|
||||
},
|
||||
{
|
||||
"marker_id": 59,
|
||||
"observed_center_px": [
|
||||
51.75,
|
||||
452.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
51.892948150634766,
|
||||
451.67559814453125
|
||||
],
|
||||
"reprojection_error_px": 0.5919220095044975,
|
||||
"confidence": 0.4943639237071812
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
233.25,
|
||||
371.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
233.19097900390625,
|
||||
371.65509033203125
|
||||
],
|
||||
"reprojection_error_px": 0.16594122173065323,
|
||||
"confidence": 0.8483075854962995
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
218.25,
|
||||
444.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
218.5349578857422,
|
||||
443.7501525878906
|
||||
],
|
||||
"reprojection_error_px": 0.3789785297142959,
|
||||
"confidence": 0.8026039835274208
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
477.75,
|
||||
306.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
477.47686767578125,
|
||||
306.69378662109375
|
||||
],
|
||||
"reprojection_error_px": 0.27885697140504484,
|
||||
"confidence": 0.8452719950987326
|
||||
},
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
527.5,
|
||||
255.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
526.965087890625,
|
||||
254.7095489501953
|
||||
],
|
||||
"reprojection_error_px": 0.608681178523418,
|
||||
"confidence": 0.8803422600053223
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
61.75,
|
||||
588.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
62.09781265258789,
|
||||
587.8051147460938
|
||||
],
|
||||
"reprojection_error_px": 0.7770708831223688,
|
||||
"confidence": 0.5831582761878896
|
||||
},
|
||||
{
|
||||
"marker_id": 102,
|
||||
"observed_center_px": [
|
||||
39.0,
|
||||
537.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
38.78061294555664,
|
||||
536.7982177734375
|
||||
],
|
||||
"reprojection_error_px": 0.5022328741680533,
|
||||
"confidence": 0.250937972298748
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
667.25,
|
||||
227.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
666.9827270507812,
|
||||
227.5442657470703
|
||||
],
|
||||
"reprojection_error_px": 0.3372853572460938,
|
||||
"confidence": 0.8298789941103211
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
486.75,
|
||||
370.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
486.16741943359375,
|
||||
370.6946716308594
|
||||
],
|
||||
"reprojection_error_px": 0.614245195516966,
|
||||
"confidence": 0.7781680532624052
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
717.25,
|
||||
350.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
717.3355712890625,
|
||||
350.58428955078125
|
||||
],
|
||||
"reprojection_error_px": 0.5905224167328221,
|
||||
"confidence": 0.7551712168920252
|
||||
},
|
||||
{
|
||||
"marker_id": 55,
|
||||
"observed_center_px": [
|
||||
580.25,
|
||||
389.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
580.1473388671875,
|
||||
389.7716979980469
|
||||
],
|
||||
"reprojection_error_px": 0.29044639838191394,
|
||||
"confidence": 0.7273519115777719
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
427.25,
|
||||
536.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.4496154785156,
|
||||
537.495849609375
|
||||
],
|
||||
"reprojection_error_px": 0.7720997209349723,
|
||||
"confidence": 0.670160195967072
|
||||
},
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
964.75,
|
||||
299.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
964.7708129882812,
|
||||
298.9970703125
|
||||
],
|
||||
"reprojection_error_px": 0.021018171900598445,
|
||||
"confidence": 0.7128402360059218
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
474.25,
|
||||
513.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
474.6187744140625,
|
||||
513.740966796875
|
||||
],
|
||||
"reprojection_error_px": 0.4405219241573996,
|
||||
"confidence": 0.6404186396481667
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
562.25,
|
||||
534.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
562.2368774414062,
|
||||
535.1456909179688
|
||||
],
|
||||
"reprojection_error_px": 0.6458242509316087,
|
||||
"confidence": 0.6430894222572451
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
840.0,
|
||||
447.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
840.52978515625,
|
||||
447.9445495605469
|
||||
],
|
||||
"reprojection_error_px": 0.6915899244243344,
|
||||
"confidence": 0.6444201033439911
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
911.75,
|
||||
394.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
912.2774658203125,
|
||||
394.775146484375
|
||||
],
|
||||
"reprojection_error_px": 0.5280648987334423,
|
||||
"confidence": 0.6422534651483102
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
1011.0,
|
||||
418.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1009.9371337890625,
|
||||
417.3528747558594
|
||||
],
|
||||
"reprojection_error_px": 1.2443695849532412,
|
||||
"confidence": 0.6368641043980565
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
758.5,
|
||||
482.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
758.6611328125,
|
||||
483.0947265625
|
||||
],
|
||||
"reprojection_error_px": 1.1065215903484336,
|
||||
"confidence": 0.6455595507517459
|
||||
},
|
||||
{
|
||||
"marker_id": 74,
|
||||
"observed_center_px": [
|
||||
890.5,
|
||||
834.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
891.2932739257812,
|
||||
835.595703125
|
||||
],
|
||||
"reprojection_error_px": 1.1595245995489538,
|
||||
"confidence": 0.4237258557628202
|
||||
},
|
||||
{
|
||||
"marker_id": 52,
|
||||
"observed_center_px": [
|
||||
895.75,
|
||||
902.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
896.4219360351562,
|
||||
903.2128295898438
|
||||
],
|
||||
"reprojection_error_px": 0.8159100836344867,
|
||||
"confidence": 0.3439829645330524
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
1039.0,
|
||||
861.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1037.73388671875,
|
||||
859.344970703125
|
||||
],
|
||||
"reprojection_error_px": 2.7179420081717525,
|
||||
"confidence": 0.4233448853603915
|
||||
},
|
||||
{
|
||||
"marker_id": 81,
|
||||
"observed_center_px": [
|
||||
846.75,
|
||||
870.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
847.3787231445312,
|
||||
871.1409301757812
|
||||
],
|
||||
"reprojection_error_px": 1.0904354041330793,
|
||||
"confidence": 0.3887985352320142
|
||||
},
|
||||
{
|
||||
"marker_id": 77,
|
||||
"observed_center_px": [
|
||||
985.75,
|
||||
867.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
985.7974853515625,
|
||||
866.8070678710938
|
||||
],
|
||||
"reprojection_error_px": 0.1986898713505852,
|
||||
"confidence": 0.4081027110350248
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
test/temp/cam1_hires_1781074183695_debug.jpg
Normal file
BIN
test/temp/cam1_hires_1781074183695_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 174 KiB |
3433
test/temp/cam2_hires_1781074183695_aruco_detection.json
Normal file
3433
test/temp/cam2_hires_1781074183695_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
929
test/temp/cam2_hires_1781074183695_camera_pose.json
Normal file
929
test/temp/cam2_hires_1781074183695_camera_pose.json
Normal file
@@ -0,0 +1,929 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-10T10:41:08Z",
|
||||
"source": {
|
||||
"detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\test\\temp\\cam2_hires_1781074183695_aruco_detection.json",
|
||||
"robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\data\\testPictures\\robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam2",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1365.456298828125,
|
||||
0.0,
|
||||
916.1585083007812
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1364.6678466796875,
|
||||
557.509033203125
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.042227353900671005,
|
||||
-0.2937094569206238,
|
||||
-0.000249923556111753,
|
||||
-0.0038161256816238165,
|
||||
0.5750380158424377
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 52,
|
||||
"used_marker_ids": [
|
||||
61,
|
||||
75,
|
||||
83,
|
||||
77,
|
||||
52,
|
||||
101,
|
||||
74,
|
||||
64,
|
||||
81,
|
||||
69,
|
||||
73,
|
||||
82,
|
||||
58,
|
||||
103,
|
||||
51,
|
||||
95,
|
||||
86,
|
||||
66,
|
||||
84,
|
||||
79,
|
||||
60,
|
||||
55,
|
||||
72,
|
||||
97,
|
||||
54,
|
||||
47,
|
||||
53,
|
||||
96,
|
||||
56,
|
||||
67,
|
||||
62,
|
||||
46,
|
||||
70,
|
||||
98,
|
||||
50,
|
||||
68,
|
||||
90,
|
||||
85,
|
||||
76,
|
||||
105,
|
||||
91,
|
||||
59,
|
||||
57,
|
||||
102,
|
||||
92,
|
||||
100,
|
||||
48,
|
||||
71,
|
||||
94,
|
||||
63,
|
||||
65,
|
||||
49
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011984790416146193,
|
||||
0.000748124316348008,
|
||||
0.0004621256111282346,
|
||||
0.0004621231525591624
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 0.8931551476129429,
|
||||
"residual_median_px": 0.6460402304256327,
|
||||
"residual_max_px": 2.4510862935844595,
|
||||
"sigma2_normalized": 2.2663277596682428e-07
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.2814928889274597,
|
||||
-0.959563136100769,
|
||||
-0.0006156218587420881
|
||||
],
|
||||
[
|
||||
-0.7972064018249512,
|
||||
0.2342216819524765,
|
||||
-0.5564190149307251
|
||||
],
|
||||
[
|
||||
0.5340633988380432,
|
||||
-0.15613722801208496,
|
||||
-0.8309016227722168
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
0.16960781812667847,
|
||||
0.19306619465351105,
|
||||
0.8885678648948669
|
||||
],
|
||||
"rvec_rad": [
|
||||
1.6251837047350812,
|
||||
-2.1708495805581816,
|
||||
0.659184269040976
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
-0.27289459109306335,
|
||||
0.2562676668167114,
|
||||
0.8458425998687744
|
||||
],
|
||||
"position_mm": [
|
||||
-272.89459228515625,
|
||||
256.2676696777344,
|
||||
845.8425903320312
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -169.35748291015625,
|
||||
"pitch": -32.28041458129883,
|
||||
"yaw": -109.44808197021484
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
1.5063737842098413e-07,
|
||||
-8.335910592336172e-08,
|
||||
-3.064318777732698e-08,
|
||||
1.296037575122895e-09,
|
||||
3.825991719294466e-08,
|
||||
7.338024221745554e-08
|
||||
],
|
||||
[
|
||||
-8.335910592336e-08,
|
||||
1.9572401471349003e-07,
|
||||
4.902855952330072e-08,
|
||||
-3.2496406832607055e-08,
|
||||
-3.877361500380251e-08,
|
||||
-1.0148736472989194e-07
|
||||
],
|
||||
[
|
||||
-3.064318777732759e-08,
|
||||
4.9028559523301544e-08,
|
||||
6.795918188810849e-07,
|
||||
5.7317179496685195e-08,
|
||||
-6.365580225321214e-08,
|
||||
-1.6150762608367604e-07
|
||||
],
|
||||
[
|
||||
1.2960375751224031e-09,
|
||||
-3.249640683260679e-08,
|
||||
5.731717949668508e-08,
|
||||
1.832623332284755e-08,
|
||||
-1.018243938937513e-09,
|
||||
2.879104601402566e-09
|
||||
],
|
||||
[
|
||||
3.82599171929445e-08,
|
||||
-3.877361500380285e-08,
|
||||
-6.365580225321209e-08,
|
||||
-1.0182439389374238e-09,
|
||||
2.1148437851966976e-08,
|
||||
3.4895901940506535e-08
|
||||
],
|
||||
[
|
||||
7.338024221745488e-08,
|
||||
-1.0148736472989244e-07,
|
||||
-1.6150762608367557e-07,
|
||||
2.8791046014027618e-09,
|
||||
3.489590194050643e-08,
|
||||
1.4353963669281618e-07
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.02223765595627221,
|
||||
0.02534805788125073,
|
||||
0.04723312755300941
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.00013537441901204065,
|
||||
0.00014542502484774406,
|
||||
0.00037886625172059885
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0003990009598470209,
|
||||
0.000607790728802823,
|
||||
0.0005472769194608717
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
0.39900095984702094,
|
||||
0.6077907288028229,
|
||||
0.5472769194608716
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.05086200168889332,
|
||||
"pitch": 0.02925593897609651,
|
||||
"yaw": 0.017037341044152724
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 61,
|
||||
"observed_center_px": [
|
||||
678.75,
|
||||
1048.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
678.7637939453125,
|
||||
1049.9586181640625
|
||||
],
|
||||
"reprojection_error_px": 1.7086738435089337,
|
||||
"confidence": 0.1298805194008731
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
901.0,
|
||||
987.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
900.0938110351562,
|
||||
989.9572143554688
|
||||
],
|
||||
"reprojection_error_px": 2.3859953166324357,
|
||||
"confidence": 0.9054705142666829
|
||||
},
|
||||
{
|
||||
"marker_id": 83,
|
||||
"observed_center_px": [
|
||||
654.75,
|
||||
950.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
654.1168823242188,
|
||||
949.4424438476562
|
||||
],
|
||||
"reprojection_error_px": 0.8436272010805596,
|
||||
"confidence": 0.8383768107463128
|
||||
},
|
||||
{
|
||||
"marker_id": 77,
|
||||
"observed_center_px": [
|
||||
887.0,
|
||||
923.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
886.1251831054688,
|
||||
923.5886840820312
|
||||
],
|
||||
"reprojection_error_px": 1.0544446630308655,
|
||||
"confidence": 0.8372378253881325
|
||||
},
|
||||
{
|
||||
"marker_id": 52,
|
||||
"observed_center_px": [
|
||||
831.75,
|
||||
824.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
831.6743774414062,
|
||||
824.7091674804688
|
||||
],
|
||||
"reprojection_error_px": 0.08594222489286081,
|
||||
"confidence": 0.7438876362292666
|
||||
},
|
||||
{
|
||||
"marker_id": 101,
|
||||
"observed_center_px": [
|
||||
694.5,
|
||||
830.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
694.2298583984375,
|
||||
829.5364379882812
|
||||
],
|
||||
"reprojection_error_px": 0.5365316613243687,
|
||||
"confidence": 0.6912989810207925
|
||||
},
|
||||
{
|
||||
"marker_id": 74,
|
||||
"observed_center_px": [
|
||||
923.0,
|
||||
801.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
922.6452026367188,
|
||||
800.9827270507812
|
||||
],
|
||||
"reprojection_error_px": 0.3552175724341925,
|
||||
"confidence": 0.7063299460473174
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
1438.75,
|
||||
820.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1437.7794189453125,
|
||||
823.000732421875
|
||||
],
|
||||
"reprojection_error_px": 2.4510862935844595,
|
||||
"confidence": 0.6757268014633911
|
||||
},
|
||||
{
|
||||
"marker_id": 81,
|
||||
"observed_center_px": [
|
||||
873.25,
|
||||
761.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
873.4539794921875,
|
||||
761.24560546875
|
||||
],
|
||||
"reprojection_error_px": 0.32607393450409006,
|
||||
"confidence": 0.6474169705665015
|
||||
},
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
1536.0,
|
||||
750.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1536.01904296875,
|
||||
750.9163208007812
|
||||
],
|
||||
"reprojection_error_px": 0.6665928624074666,
|
||||
"confidence": 0.6112865494409404
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
617.75,
|
||||
711.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
618.2041015625,
|
||||
710.8424682617188
|
||||
],
|
||||
"reprojection_error_px": 0.7990971254560385,
|
||||
"confidence": 0.532833258968179
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
667.75,
|
||||
701.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
668.7314453125,
|
||||
701.2522583007812
|
||||
],
|
||||
"reprojection_error_px": 1.100446137059598,
|
||||
"confidence": 0.552022973773092
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
1427.0,
|
||||
721.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1427.1419677734375,
|
||||
721.8328247070312
|
||||
],
|
||||
"reprojection_error_px": 0.16436173760828193,
|
||||
"confidence": 0.6045651933799014
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
1353.0,
|
||||
667.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1353.5479736328125,
|
||||
666.92724609375
|
||||
],
|
||||
"reprojection_error_px": 0.6359600508344548,
|
||||
"confidence": 0.602350818516097
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
1297.0,
|
||||
603.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1297.596435546875,
|
||||
602.8515014648438
|
||||
],
|
||||
"reprojection_error_px": 0.7173119573085042,
|
||||
"confidence": 0.5578670422625701
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
1407.5,
|
||||
552.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1407.8885498046875,
|
||||
551.9653930664062
|
||||
],
|
||||
"reprojection_error_px": 0.48163477591670184,
|
||||
"confidence": 0.4802479007916047
|
||||
},
|
||||
{
|
||||
"marker_id": 86,
|
||||
"observed_center_px": [
|
||||
640.25,
|
||||
541.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
641.0107421875,
|
||||
541.1596069335938
|
||||
],
|
||||
"reprojection_error_px": 0.8334243309981626,
|
||||
"confidence": 0.3964057961714926
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
1494.25,
|
||||
502.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1494.6441650390625,
|
||||
501.90594482421875
|
||||
],
|
||||
"reprojection_error_px": 0.5232017220929314,
|
||||
"confidence": 0.45475830315312954
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
672.5,
|
||||
487.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
673.2105102539062,
|
||||
487.0281066894531
|
||||
],
|
||||
"reprojection_error_px": 0.7110659652225932,
|
||||
"confidence": 0.36929083534733026
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
1206.0,
|
||||
461.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1206.052001953125,
|
||||
461.0175476074219
|
||||
],
|
||||
"reprojection_error_px": 0.2381980645263715,
|
||||
"confidence": 0.4098258244985548
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
635.0,
|
||||
468.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
635.5394897460938,
|
||||
468.3836669921875
|
||||
],
|
||||
"reprojection_error_px": 0.5518899843691694,
|
||||
"confidence": 0.36321795262481693
|
||||
},
|
||||
{
|
||||
"marker_id": 55,
|
||||
"observed_center_px": [
|
||||
1338.0,
|
||||
459.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1338.171875,
|
||||
459.31719970703125
|
||||
],
|
||||
"reprojection_error_px": 0.46567919130967816,
|
||||
"confidence": 0.4226507579685383
|
||||
},
|
||||
{
|
||||
"marker_id": 72,
|
||||
"observed_center_px": [
|
||||
742.5,
|
||||
437.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
743.2981567382812,
|
||||
437.74658203125
|
||||
],
|
||||
"reprojection_error_px": 0.9400254744548894,
|
||||
"confidence": 0.32333953337853494
|
||||
},
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
1433.5,
|
||||
414.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1433.2255859375,
|
||||
414.3753967285156
|
||||
],
|
||||
"reprojection_error_px": 0.3013785874317596,
|
||||
"confidence": 0.3750450731634656
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
1380.75,
|
||||
388.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1380.5369873046875,
|
||||
388.4062805175781
|
||||
],
|
||||
"reprojection_error_px": 0.2327181766637924,
|
||||
"confidence": 0.3804229609837889
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
1332.0,
|
||||
398.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1331.9332275390625,
|
||||
397.9681701660156
|
||||
],
|
||||
"reprojection_error_px": 0.536005162153778,
|
||||
"confidence": 0.3734523056846838
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
709.75,
|
||||
401.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
710.3843383789062,
|
||||
401.6002502441406
|
||||
],
|
||||
"reprojection_error_px": 0.6422112506050203,
|
||||
"confidence": 0.30688470757026015
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
1209.25,
|
||||
401.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1209.242919921875,
|
||||
401.32574462890625
|
||||
],
|
||||
"reprojection_error_px": 0.1743991452423911,
|
||||
"confidence": 0.3473105376216546
|
||||
},
|
||||
{
|
||||
"marker_id": 56,
|
||||
"observed_center_px": [
|
||||
758.0,
|
||||
379.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
758.4189453125,
|
||||
379.59234619140625
|
||||
],
|
||||
"reprojection_error_px": 0.4290023239248972,
|
||||
"confidence": 0.3172821134080634
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
636.5,
|
||||
383.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
637.0377197265625,
|
||||
383.2904968261719
|
||||
],
|
||||
"reprojection_error_px": 0.7073087523087564,
|
||||
"confidence": 0.28880951617505063
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
1180.5,
|
||||
374.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1180.4544677734375,
|
||||
373.8306579589844
|
||||
],
|
||||
"reprojection_error_px": 0.17535652400488683,
|
||||
"confidence": 0.3341836586203227
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
729.0,
|
||||
351.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
729.3017578125,
|
||||
351.3228759765625
|
||||
],
|
||||
"reprojection_error_px": 0.3499009818269637,
|
||||
"confidence": 0.26347861637214165
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
582.5,
|
||||
325.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
582.8350219726562,
|
||||
325.0391540527344
|
||||
],
|
||||
"reprojection_error_px": 0.5697532003189067,
|
||||
"confidence": 0.2570583858267052
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
569.75,
|
||||
351.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
570.3342895507812,
|
||||
350.63134765625
|
||||
],
|
||||
"reprojection_error_px": 1.046876866424377,
|
||||
"confidence": 0.26500110579975544
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
689.75,
|
||||
327.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
690.3238525390625,
|
||||
327.2581481933594
|
||||
],
|
||||
"reprojection_error_px": 0.7557942420289565,
|
||||
"confidence": 0.2553101300914065
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
736.75,
|
||||
317.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
737.05517578125,
|
||||
317.24169921875
|
||||
],
|
||||
"reprojection_error_px": 0.30528865100247043,
|
||||
"confidence": 0.2627003031266385
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
556.75,
|
||||
296.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
557.29150390625,
|
||||
295.6229553222656
|
||||
],
|
||||
"reprojection_error_px": 1.2503824159405754,
|
||||
"confidence": 0.2463831284328039
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
1279.75,
|
||||
261.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1279.4200439453125,
|
||||
260.87799072265625
|
||||
],
|
||||
"reprojection_error_px": 0.7041069088758003,
|
||||
"confidence": 0.2657187200890806
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
714.75,
|
||||
230.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
715.0855102539062,
|
||||
230.44677734375
|
||||
],
|
||||
"reprojection_error_px": 0.45222904566109196,
|
||||
"confidence": 0.21252735432479583
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1223.5,
|
||||
256.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1222.8546142578125,
|
||||
255.816650390625
|
||||
],
|
||||
"reprojection_error_px": 0.7773767684748337,
|
||||
"confidence": 0.2621913434075788
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
531.5,
|
||||
237.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
531.8168334960938,
|
||||
236.93191528320312
|
||||
],
|
||||
"reprojection_error_px": 0.6504642263070076,
|
||||
"confidence": 0.22193151443517992
|
||||
},
|
||||
{
|
||||
"marker_id": 59,
|
||||
"observed_center_px": [
|
||||
1192.5,
|
||||
180.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1191.9222412109375,
|
||||
180.2024688720703
|
||||
],
|
||||
"reprojection_error_px": 0.6498692102462452,
|
||||
"confidence": 0.22338557625759903
|
||||
},
|
||||
{
|
||||
"marker_id": 57,
|
||||
"observed_center_px": [
|
||||
1285.75,
|
||||
179.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1284.944091796875,
|
||||
179.25888061523438
|
||||
],
|
||||
"reprojection_error_px": 0.8412054383882214,
|
||||
"confidence": 0.2198605610785733
|
||||
},
|
||||
{
|
||||
"marker_id": 102,
|
||||
"observed_center_px": [
|
||||
1128.25,
|
||||
175.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1127.7625732421875,
|
||||
175.20362854003906
|
||||
],
|
||||
"reprojection_error_px": 0.4896275692100215,
|
||||
"confidence": 0.22088674806143707
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
1092.25,
|
||||
186.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1091.6705322265625,
|
||||
186.0386962890625
|
||||
],
|
||||
"reprojection_error_px": 0.6167918276927576,
|
||||
"confidence": 0.20880094929023507
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
684.25,
|
||||
149.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
684.1082763671875,
|
||||
149.09165954589844
|
||||
],
|
||||
"reprojection_error_px": 0.2125024411687107,
|
||||
"confidence": 0.1658427624477632
|
||||
},
|
||||
{
|
||||
"marker_id": 48,
|
||||
"observed_center_px": [
|
||||
1207.5,
|
||||
130.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1206.74365234375,
|
||||
130.34719848632812
|
||||
],
|
||||
"reprojection_error_px": 0.7625675857649254,
|
||||
"confidence": 0.19719918051995877
|
||||
},
|
||||
{
|
||||
"marker_id": 71,
|
||||
"observed_center_px": [
|
||||
1149.25,
|
||||
98.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1148.265380859375,
|
||||
98.3095474243164
|
||||
],
|
||||
"reprojection_error_px": 1.002869401103468,
|
||||
"confidence": 0.18035085894910577
|
||||
},
|
||||
{
|
||||
"marker_id": 94,
|
||||
"observed_center_px": [
|
||||
668.75,
|
||||
102.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
668.649658203125,
|
||||
102.52938079833984
|
||||
],
|
||||
"reprojection_error_px": 0.2968536789078286,
|
||||
"confidence": 0.13931516180114967
|
||||
},
|
||||
{
|
||||
"marker_id": 63,
|
||||
"observed_center_px": [
|
||||
1094.0,
|
||||
89.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1093.3001708984375,
|
||||
89.75130462646484
|
||||
],
|
||||
"reprojection_error_px": 0.743582400730686,
|
||||
"confidence": 0.18347683233254594
|
||||
},
|
||||
{
|
||||
"marker_id": 65,
|
||||
"observed_center_px": [
|
||||
1142.0,
|
||||
63.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1140.761962890625,
|
||||
63.32167434692383
|
||||
],
|
||||
"reprojection_error_px": 1.2791443505947548,
|
||||
"confidence": 0.1712791339969131
|
||||
},
|
||||
{
|
||||
"marker_id": 49,
|
||||
"observed_center_px": [
|
||||
659.75,
|
||||
20.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
659.8107299804688,
|
||||
22.313045501708984
|
||||
],
|
||||
"reprojection_error_px": 2.3138426099248632,
|
||||
"confidence": 0.03034300498209627
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
test/temp/cam2_hires_1781074183695_debug.jpg
Normal file
BIN
test/temp/cam2_hires_1781074183695_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 384 KiB |
70
test/temp/detections.csv
Normal file
70
test/temp/detections.csv
Normal file
@@ -0,0 +1,70 @@
|
||||
id,set,SeenByCount,x,y,z,nx,ny,nz,dxy
|
||||
cam0,CAMERA,,-115.37,-719.72,565.61,,,,
|
||||
cam1,CAMERA,,318.95,-523.93,853.64,,,,
|
||||
cam2,CAMERA,,-272.89,256.27,845.84,,,,
|
||||
0,?,2,513.41,-99.44,-19.39,-0.5015,-0.865,0.0182,
|
||||
46,A0,2,565.64,209.9,-55.56,0.0354,0.015,0.9993,37.88
|
||||
47,A0,3,362.8,-287.49,-65.76,-0.333,-0.3131,0.8894,18.59
|
||||
48,A0,2,761.75,-329.77,-88.34,-0.6704,0.4149,0.6152,73.62
|
||||
49,A0,1,1092.66,151.29,-83.59,-0.0347,-0.025,0.9991,86.95
|
||||
50,A0,2,630.74,265.32,-79.87,0.0073,-0.0024,1.0,78.39
|
||||
51,A0,3,176.56,-163.97,-69.44,-0.26,0.2559,0.9311,11.94
|
||||
52,A0,2,96.93,219.89,-55.7,-0.0341,-0.0377,0.9987,11.46
|
||||
53,A0,1,518.69,210.86,-64.22,0.0166,0.0014,0.9999,32.47
|
||||
54,A0,3,357.23,-330.04,-55.94,-0.2949,0.3095,0.904,14.97
|
||||
55,A0,3,296.01,-261.27,-57.66,-0.291,0.0608,0.9548,12.36
|
||||
56,A0,2,538.59,201.55,-67.84,-0.0012,0.0013,1.0,51.71
|
||||
57,A0,3,635.81,-362.16,-68.53,0.0282,-0.001,0.9996,33.01
|
||||
58,A0,2,49.92,-219.72,-67.42,0.0418,-0.0158,0.999,1.62
|
||||
59,A0,3,664.52,-276.95,-69.92,-0.4034,-0.2722,0.8736,38.35
|
||||
60,A0,2,465.81,316.89,-63.04,-0.4284,-0.7766,0.462,45.64
|
||||
61,A0,1,-11.44,338.72,-61.53,0.0583,-0.0284,0.9979,11.35
|
||||
62,A0,3,423.81,-171.25,-62.26,-0.0275,-0.2428,0.9697,19.49
|
||||
63,A0,2,858.89,-229.54,-87.46,-0.0167,0.0026,0.9999,81.75
|
||||
64,A0,2,-21.74,-191.34,-56.41,-0.2806,0.4588,0.8431,3.14
|
||||
65,A0,2,876.99,-304.2,-81.63,0.0086,-0.0228,0.9997,73.92
|
||||
66,A0,2,221.89,-378.26,-70.96,-0.3787,0.5606,0.7364,19.32
|
||||
67,A0,2,570.28,307.19,-74.97,-0.665,-0.023,0.7465,61.98
|
||||
68,A0,2,615.28,185.13,-67.64,0.0036,0.0103,0.9999,44.0
|
||||
69,A0,2,7.51,-289.23,-71.43,0.0655,0.0143,0.9977,8.02
|
||||
70,A0,2,646.35,344.94,-66.82,0.013,0.0124,0.9998,63.0
|
||||
71,A0,2,801.68,-284.13,-67.44,-0.0049,0.0036,1.0,51.93
|
||||
72,A0,2,470.53,219.63,-59.99,-0.8891,-0.3928,-0.2349,38.9
|
||||
73,A0,1,227.77,334.47,-38.57,0.0887,-0.003,0.9961,6.19
|
||||
74,A0,2,93.61,158.28,-59.95,-0.4017,0.1293,0.9066,13.78
|
||||
75,A0,2,-31.35,211.98,-57.21,-0.1993,-0.6229,0.7565,18.29
|
||||
76,A0,2,725.6,203.41,-58.89,-0.5971,-0.6684,0.4436,54.51
|
||||
77,A0,2,14.87,208.14,-61.15,-0.2433,-0.6323,0.7355,16.9
|
||||
79,A0,3,324.61,-146.67,-56.54,-0.0051,-0.0055,1.0,17.5
|
||||
80,A0,1,920.78,-313.59,-61.98,0.0261,-0.0109,0.9996,61.4
|
||||
81,A0,2,134.74,179.65,-55.28,0.1542,-0.6375,0.7549,10.94
|
||||
82,A0,1,235.39,299.34,-56.31,0.0119,-0.0171,0.9998,16.36
|
||||
83,A0,1,60.54,343.24,-73.55,-0.0211,-0.0083,0.9997,16.87
|
||||
84,A0,2,433.15,283.86,-57.97,0.0108,0.0308,0.9995,36.13
|
||||
85,A0,3,530.97,-312.83,-65.01,-0.5392,0.0418,0.8411,26.39
|
||||
86,A0,2,371.18,293.37,-37.99,0.0359,0.0184,0.9992,8.4
|
||||
87,A0,1,992.42,-224.08,-54.87,0.0164,-0.0161,0.9997,53.39
|
||||
89,A0,1,1087.35,-337.82,-77.4,-0.8097,-0.3076,-0.4998,104.15
|
||||
90,A0,1,716.28,322.0,-98.36,-0.9961,-0.0881,-0.0008,73.33
|
||||
91,A0,2,797.63,384.71,-85.91,-0.6357,-0.6612,0.3984,93.42
|
||||
92,A0,3,689.76,-175.44,-72.48,-0.4179,0.1772,0.8911,45.65
|
||||
94,A0,1,912.67,170.82,-55.05,0.0401,-0.0161,0.9991,37.08
|
||||
95,A0,3,196.36,-271.55,-63.02,-0.263,-0.3771,0.8881,10.62
|
||||
96,A0,3,386.86,-188.18,-58.96,-0.0153,-0.2337,0.9722,17.17
|
||||
97,A0,3,317.16,-361.37,-54.79,-0.242,-0.4685,0.8497,12.92
|
||||
98,A0,2,648.49,382.72,-96.14,-0.001,0.016,0.9999,99.69
|
||||
99,A0,1,1032.86,-294.32,-67.94,-0.7854,-0.2944,-0.5445,78.57
|
||||
100,A0,2,848.99,192.39,-61.29,-0.6582,-0.6214,0.4251,49.94
|
||||
101,A0,1,131.96,300.1,-59.87,0.0067,0.0277,0.9996,14.34
|
||||
102,A0,3,683.89,-213.5,-68.38,-0.2212,-0.1903,0.9565,35.49
|
||||
103,A0,2,111.85,-193.49,-59.79,0.0184,0.0312,0.9993,8.43
|
||||
105,A0,3,553.96,-263.47,-68.08,-0.4035,-0.285,0.8695,29.26
|
||||
201,?,1,897.13,61.75,90.23,-0.9978,0.0489,0.0451,
|
||||
208,?,3,683.3,-76.41,-55.32,-0.3861,-0.3324,0.8605,
|
||||
210,?,2,153.21,-2.51,-63.41,-0.3954,0.267,0.8788,
|
||||
211,?,3,359.71,3.01,-41.85,0.0415,0.0337,0.9986,
|
||||
214,?,3,595.34,16.16,-69.24,-0.9274,-0.1985,0.3172,
|
||||
215,?,3,353.19,-91.68,-33.15,-0.2708,-0.4026,0.8744,
|
||||
217,?,1,764.54,-10.41,-25.65,0.106,0.004,0.9944,
|
||||
218,?,1,1121.04,-225.1,28.11,-0.7647,-0.6357,0.1053,
|
||||
219,?,1,1127.16,-337.07,20.54,-0.9004,-0.4335,0.038,
|
||||
|
Reference in New Issue
Block a user