Python Scripte in HomingApp
This commit is contained in:
chk
2441
data/robot/robot.json
Normal file
2441
data/robot/robot.json
Normal file
File diff suppressed because it is too large
Load Diff
595
data/robot/robot_running_2026_05_30.json
Normal file
595
data/robot/robot_running_2026_05_30.json
Normal file
@@ -0,0 +1,595 @@
|
|||||||
|
{
|
||||||
|
"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,
|
||||||
|
"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":[300, -500,1450],
|
||||||
|
"cameraPosition_c":[600, -500,600],
|
||||||
|
"cameraTarget": [210, -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.70, 0.85, 1.0],
|
||||||
|
"backgroundStrength": 0.20,
|
||||||
|
"sunEnergy": 0.35,
|
||||||
|
"areaEnergy": 120,
|
||||||
|
"exposure": -1.5,
|
||||||
|
"lensDirt": true,
|
||||||
|
"lensDirtStrength": 0.08,
|
||||||
|
"dofEnabled": true,
|
||||||
|
"dofFStop": 11,
|
||||||
|
"arucoDust": false,
|
||||||
|
"arucoDustStrength": 0.00005,
|
||||||
|
"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.20, 0.20],
|
||||||
|
"roughness": 0.35,
|
||||||
|
"metallic": 0.0
|
||||||
|
},
|
||||||
|
"markerBlack": {
|
||||||
|
"baseColor": [0.04, 0.04, 0.04],
|
||||||
|
"roughness": 0.80,
|
||||||
|
"metallic": 0.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"skeletonDefaults": {
|
||||||
|
"radius": 4,
|
||||||
|
"color": [0.85, 0.20, 0.20]
|
||||||
|
},
|
||||||
|
"markerDefaults": {
|
||||||
|
"size": 25,
|
||||||
|
"thickness": 1,
|
||||||
|
"color": [0.04, 0.04, 0.04]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"defaultPosition__": {
|
||||||
|
"x": 10,
|
||||||
|
"y": 4,
|
||||||
|
"z": 20,
|
||||||
|
"a": 10,
|
||||||
|
"b": 2,
|
||||||
|
"c": 9,
|
||||||
|
"e": 1
|
||||||
|
},
|
||||||
|
"defaultPosition": {
|
||||||
|
"x": 180,
|
||||||
|
"y": 86,
|
||||||
|
"z": -120,
|
||||||
|
"a": -60,
|
||||||
|
"b": 22,
|
||||||
|
"c": 91,
|
||||||
|
"e": 10
|
||||||
|
},
|
||||||
|
"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
|
||||||
|
},
|
||||||
|
"robot_test_poses": {
|
||||||
|
"sim04": {"x": 70, "y": 50,"z": -70,"a": 120,"b": 50,"c": 30,"e": 20},
|
||||||
|
"sim05": {"x": 180,"y": 86,"z": -120,"a": -60,"b": 22,"c": 91,"e": 10},
|
||||||
|
"sim06": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3},
|
||||||
|
"sim07": {"x": 30, "y": -2, "z": 95, "a": 20, "b": 23, "c": 9, "e": 9},
|
||||||
|
"sim08": {"x": 50, "y": -2, "z": 95, "a": 20, "b": 60, "c": 9, "e": 3},
|
||||||
|
"sim09": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
|
||||||
|
"sim09a": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
|
||||||
|
"sim09b": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
|
||||||
|
"sim010": {"x": 120, "y": 60, "z": -110, "a": 20, "b": 30, "c": 180, "e": 4},
|
||||||
|
"sim011": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
|
||||||
|
"sim012": {"x": 50, "y": 0, "z": 178, "a": 210, "b": 80, "c": 90, "e": 6}
|
||||||
|
},
|
||||||
|
"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": 4,
|
||||||
|
"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.20, 0.20]
|
||||||
|
},
|
||||||
|
"markers":[
|
||||||
|
{"id":210,"position":[20, -20, 0.3], "normal":[0,0,1]},
|
||||||
|
{"id":211,"position":[250, -10, 0.3], "normal":[0,0,1]},
|
||||||
|
{"id":215,"position":[250, -90, 0.3], "normal":[0,0,1]},
|
||||||
|
{"id":214,"position":[350, -10, 0.3], "normal":[0,0,1]},
|
||||||
|
{"id":208,"position":[350, -90, 0.3], "normal":[0,0,1]},
|
||||||
|
{"id":206,"position":[650, -10, 0.3], "normal":[0,0,1]},
|
||||||
|
{"id":205,"position":[750, -90, 0.3], "normal":[0,0,1]},
|
||||||
|
{"id":207,"position":[750, -10, 0.3], "normal":[0,0,1]},
|
||||||
|
{"id":217,"position":[650, -90, 0.3], "normal":[0,0,1]},
|
||||||
|
|
||||||
|
{"id": 46, "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, "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 48, "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 49, "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 50, "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 51, "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 52, "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 53, "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 54, "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 55, "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 56, "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 57, "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 58, "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 59, "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 60, "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 61, "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 62, "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 63, "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 64, "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 65, "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 66, "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 67, "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 68, "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 69, "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 70, "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 71, "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 72, "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 73, "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 74, "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 75, "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 76, "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 77, "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 78, "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 79, "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 80, "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 81, "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 82, "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 83, "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 84, "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 85, "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 86, "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 87, "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 88, "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 89, "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 90, "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 91, "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 92, "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 93, "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 94, "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 95, "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 96, "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 97, "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 98, "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 99, "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 100, "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 101, "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 102, "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 103, "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 104, "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||||
|
{"id": 105, "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.20, 0.80, 0.20]
|
||||||
|
},
|
||||||
|
"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.20, 0.20, 0.90]
|
||||||
|
},
|
||||||
|
"markers": [
|
||||||
|
{
|
||||||
|
"id": 198,
|
||||||
|
"name": "aruco_198",
|
||||||
|
"position": [0, -160, 35],
|
||||||
|
"normal": [0, 0, 1],
|
||||||
|
"size": 25,
|
||||||
|
"spin": 0
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": 229,
|
||||||
|
"name": "aruco_229",
|
||||||
|
"position": [0, -250, 35],
|
||||||
|
"normal": [0, 0, 1],
|
||||||
|
"size": 25,
|
||||||
|
"spin": 0
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": 242,
|
||||||
|
"name": "aruco_242",
|
||||||
|
"position": [0, -250, -35],
|
||||||
|
"normal": [0, 0, -1],
|
||||||
|
"size": 25,
|
||||||
|
"spin": 0
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": 243,
|
||||||
|
"name": "aruco_243",
|
||||||
|
"position": [0, -285, 0],
|
||||||
|
"normal": [0, -1, 0],
|
||||||
|
"size": 25,
|
||||||
|
"spin": 0
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"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.90, 0.20, 0.20]
|
||||||
|
},
|
||||||
|
"model": [
|
||||||
|
{
|
||||||
|
"stlFile": "surfaces/Ellebogen.stl",
|
||||||
|
"originOfModel": [90,0,0],
|
||||||
|
"rotationOfModelDegree": [0,-90,-90],
|
||||||
|
"material": "defaultPlastic"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"markers": [
|
||||||
|
{"id": 244, "name": "aruco_244", "position": [125, 0, 0], "normal": [1, 0, 0], "size": 25, "spin": 0},
|
||||||
|
{"id": 245, "name": "aruco_245", "position": [90, 0, -35], "normal": [0, 0, -1], "size": 25, "spin": 0},
|
||||||
|
{"id": 246, "name": "aruco_246", "position": [90, 0, 35], "normal": [0, 0, 1], "size": 25},
|
||||||
|
{"id": 247, "name": "aruco_247", "position": [52.5, 0, 35], "normal": [0, 0, 1], "size": 25},
|
||||||
|
{"id": 248, "name": "aruco_248", "position": [52.5, 0, -35], "normal": [0, 0, -1], "size": 25},
|
||||||
|
{"id": 232, "name": "aruco_232", "position": [90, 24.75, -24.75], "normal": [0, 1, -1], "size": 25},
|
||||||
|
{"id": 231, "name": "aruco_231", "position": [90, 24.75, 24.75], "normal": [0, 1, 1], "size": 25}
|
||||||
|
]
|
||||||
|
|
||||||
|
},
|
||||||
|
"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.20]
|
||||||
|
},
|
||||||
|
"model": [
|
||||||
|
{
|
||||||
|
"stlFile": "surfaces/Unterarm.stl",
|
||||||
|
"originOfModel": [0,-250,0],
|
||||||
|
"rotationOfModelDegree": [180, 0, -90],
|
||||||
|
"material": "defaultPlastic"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"markers":[
|
||||||
|
|
||||||
|
{"id":120, "position":[24.75, -112, -24.75],"normal":[1,0,-1]},
|
||||||
|
{"id":122, "name": "aruco_122", "position":[-35,-112,0], "normal":[-1,0,0]},
|
||||||
|
{"id":218, "name": "aruco_218", "position":[35,-112,0], "normal":[1,0,0]},
|
||||||
|
{"id":113, "name": "aruco_113", "position":[0, -182, 30],"normal":[0,0,1]},
|
||||||
|
{"id":114, "name": "aruco_114", "position":[ 24.75, -182, -24.75],"normal":[ 1,0,-1]},
|
||||||
|
{"id":115, "name": "aruco_115", "position":[-24.75, -182, -24.75],"normal":[-1,0,-1]},
|
||||||
|
{"id":124, "name": "aruco_124", "position":[-35,-219,0], "normal":[-1,0,0]},
|
||||||
|
{"id":219, "name": "aruco_219", "position":[35,-219,0], "normal":[1,0,0]}
|
||||||
|
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"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.20, 0.20]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"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.20, 0.80, 0.20]
|
||||||
|
},
|
||||||
|
"markers":[
|
||||||
|
{"id":40,"position":[12,-24,-17.1],"normal":[-10.98,0,-23.56]},
|
||||||
|
{"id":41,"position":[1.5,-2.2,25.8],"normal":[0,-25.6,9.5]},
|
||||||
|
{"id":42,"position":[13.9,-40,0],"normal":[1,-0.35,0.40], "spin": 27}
|
||||||
|
],
|
||||||
|
"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.20, 0.80, 0.20]
|
||||||
|
},
|
||||||
|
"markers":[
|
||||||
|
{"id":43,"position":[-12,-24,17.1],"normal":[10.98,0,23.56], "spin":90 },
|
||||||
|
{"id":44,"position":[-1.5,-2.2,-25.8],"normal":[0,-25.6,-9.5], "spin":90},
|
||||||
|
{"id":45,"position":[-13.9,-40,0],"normal":[-1,-0.35,-0.40], "spin": -27}
|
||||||
|
],
|
||||||
|
"model": [
|
||||||
|
{
|
||||||
|
"stlFile": "surfaces/Finger.stl",
|
||||||
|
"originOfModel": [-24,0,9.1],
|
||||||
|
"rotationOfModelDegree": [90, 90,0],
|
||||||
|
"material": "defaultPlastic"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
BIN
data/robot/surfaces/Base.stl
Normal file
BIN
data/robot/surfaces/Base.stl
Normal file
Binary file not shown.
BIN
data/robot/surfaces/Board.stl
Normal file
BIN
data/robot/surfaces/Board.stl
Normal file
Binary file not shown.
BIN
data/robot/surfaces/BoardRail.stl
Normal file
BIN
data/robot/surfaces/BoardRail.stl
Normal file
Binary file not shown.
BIN
data/robot/surfaces/Ellebogen.3mf
Normal file
BIN
data/robot/surfaces/Ellebogen.3mf
Normal file
Binary file not shown.
BIN
data/robot/surfaces/Ellebogen.stl
Normal file
BIN
data/robot/surfaces/Ellebogen.stl
Normal file
Binary file not shown.
BIN
data/robot/surfaces/Finger.stl
Normal file
BIN
data/robot/surfaces/Finger.stl
Normal file
Binary file not shown.
BIN
data/robot/surfaces/Holm.stl
Normal file
BIN
data/robot/surfaces/Holm.stl
Normal file
Binary file not shown.
BIN
data/robot/surfaces/Unterarm.3mf
Normal file
BIN
data/robot/surfaces/Unterarm.3mf
Normal file
Binary file not shown.
BIN
data/robot/surfaces/Unterarm.stl
Normal file
BIN
data/robot/surfaces/Unterarm.stl
Normal file
Binary file not shown.
BIN
data/robot/surfaces/boden.jpg
Normal file
BIN
data/robot/surfaces/boden.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 2.9 MiB |
1217
scripts/4_robotState_estimation_v6.py
Normal file
1217
scripts/4_robotState_estimation_v6.py
Normal file
File diff suppressed because it is too large
Load Diff
360
scripts/4b_revolute_angle.py
Normal file
360
scripts/4b_revolute_angle.py
Normal file
@@ -0,0 +1,360 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
4b_revolute_angle.py
|
||||||
|
--------------------
|
||||||
|
Generic revolute-joint angle estimator.
|
||||||
|
|
||||||
|
For each movable link (Arm1, Ellbow, Arm2 …) whose joint type is 'revolute',
|
||||||
|
this script estimates the rotation angle using the pairwise-vector method:
|
||||||
|
|
||||||
|
For every PAIR (m1, m2) of markers belonging to the target link:
|
||||||
|
v_model = local_pos_m2 - local_pos_m1 (in link's own frame)
|
||||||
|
v_obs = world_pos_m2 - world_pos_m1 (in world frame)
|
||||||
|
|
||||||
|
Both vectors are projected perpendicular to the joint axis (in world frame),
|
||||||
|
and the signed angle from v_model_perp to v_obs_perp is measured.
|
||||||
|
|
||||||
|
The joint axis in world frame is computed via FK using the already-known
|
||||||
|
joint values (from 4a, 4b-prev …), so it is ALWAYS correct — even for
|
||||||
|
deeply-nested joints whose world-frame axis differs from their local axis.
|
||||||
|
|
||||||
|
Pair weights = baseline_model × baseline_obs (longer baselines → more reliable).
|
||||||
|
|
||||||
|
How to use sequentially
|
||||||
|
-----------------------
|
||||||
|
Run 4b once per revolute joint, from root to tip:
|
||||||
|
|
||||||
|
python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Arm1 --x-mm 180
|
||||||
|
python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Ellbow --from-state state.json
|
||||||
|
python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Arm2 --from-state state.json
|
||||||
|
|
||||||
|
The --from-state flag reads the accumulated joint state JSON so you don't have
|
||||||
|
to pass every preceding value on the command line.
|
||||||
|
|
||||||
|
Output JSON
|
||||||
|
-----------
|
||||||
|
{
|
||||||
|
"link": "Arm1",
|
||||||
|
"joint": "y",
|
||||||
|
"mean_angle_deg": 86.3,
|
||||||
|
"circular_std_deg": 0.7,
|
||||||
|
"num_pairs": 6,
|
||||||
|
"joint_origin_world_mm": [290, 108, 61],
|
||||||
|
"joint_axis_world": [-1, 0, 0],
|
||||||
|
...
|
||||||
|
}
|
||||||
|
|
||||||
|
The file also contains the full accumulated state so the next 4b invocation
|
||||||
|
can read it via --from-state.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
from itertools import combinations
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Dict, List, Optional, Sequence, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from robot_fk import RobotFK
|
||||||
|
|
||||||
|
STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e")
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# I/O
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def _load_json(path: Path) -> dict:
|
||||||
|
with path.open("r", encoding="utf-8") as f:
|
||||||
|
return json.load(f)
|
||||||
|
|
||||||
|
|
||||||
|
def _load_observed_mm(aruco_json: dict) -> Dict[int, np.ndarray]:
|
||||||
|
result: Dict[int, np.ndarray] = {}
|
||||||
|
for m in aruco_json.get("markers", []):
|
||||||
|
mid = int(m.get("marker_id", m.get("id", -1)))
|
||||||
|
if mid < 0:
|
||||||
|
continue
|
||||||
|
if "position_mm" in m:
|
||||||
|
result[mid] = np.array(m["position_mm"], dtype=float)
|
||||||
|
elif "position_m" in m:
|
||||||
|
result[mid] = np.array(m["position_m"], dtype=float) * 1000.0
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def _load_state(path: Path) -> Dict[str, float]:
|
||||||
|
"""Load accumulated joint state from a previous 4b output JSON."""
|
||||||
|
raw = _load_json(path)
|
||||||
|
state = raw.get("accumulated_state", raw.get("state", {}))
|
||||||
|
return {k: float(v) for k, v in state.items() if k in STATE_KEYS}
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Angle maths
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def _project_perp(v: np.ndarray, axis: np.ndarray) -> np.ndarray:
|
||||||
|
"""Remove the component of v along axis."""
|
||||||
|
a = axis / (np.linalg.norm(axis) + 1e-15)
|
||||||
|
return v - np.dot(v, a) * a
|
||||||
|
|
||||||
|
|
||||||
|
def _signed_angle_rad(v_from: np.ndarray, v_to: np.ndarray,
|
||||||
|
axis: np.ndarray) -> float:
|
||||||
|
"""Signed angle rotating v_from onto v_to around axis (radians)."""
|
||||||
|
a = axis / (np.linalg.norm(axis) + 1e-15)
|
||||||
|
return math.atan2(float(np.dot(a, np.cross(v_from, v_to))),
|
||||||
|
float(np.dot(v_from, v_to)))
|
||||||
|
|
||||||
|
|
||||||
|
def _wrap(angle: float) -> float:
|
||||||
|
"""Wrap to (−π, π]."""
|
||||||
|
return (angle + math.pi) % (2.0 * math.pi) - math.pi
|
||||||
|
|
||||||
|
|
||||||
|
def _circular_mean_deg(angles_rad: np.ndarray,
|
||||||
|
weights: np.ndarray) -> Tuple[float, float, float]:
|
||||||
|
"""Returns mean_deg, circular_variance, circular_std_deg."""
|
||||||
|
w = np.clip(weights, 0, None)
|
||||||
|
if w.sum() < 1e-15:
|
||||||
|
w = np.ones_like(w)
|
||||||
|
s, c = np.sum(w * np.sin(angles_rad)), np.sum(w * np.cos(angles_rad))
|
||||||
|
mean = math.atan2(s, c)
|
||||||
|
R = math.sqrt(s*s + c*c) / w.sum()
|
||||||
|
R = float(np.clip(R, 0, 1))
|
||||||
|
c_var = 1.0 - R
|
||||||
|
c_std = math.sqrt(max(0.0, -2.0 * math.log(max(R, 1e-15))))
|
||||||
|
return math.degrees(mean), c_var, math.degrees(c_std)
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Core estimator (generic — works for any revolute joint)
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def estimate_revolute_angle(
|
||||||
|
fk: RobotFK,
|
||||||
|
observed_mm: Dict[int, np.ndarray],
|
||||||
|
link_name: str,
|
||||||
|
known_state: Dict[str, float],
|
||||||
|
min_baseline_mm: float = 15.0,
|
||||||
|
verbose: bool = True,
|
||||||
|
) -> dict:
|
||||||
|
"""
|
||||||
|
Estimate the revolute joint angle for `link_name`.
|
||||||
|
|
||||||
|
Parameters
|
||||||
|
----------
|
||||||
|
fk : RobotFK
|
||||||
|
observed_mm : world marker positions from step 3
|
||||||
|
link_name : e.g. "Arm1", "Ellbow", "Arm2"
|
||||||
|
known_state : already-estimated joint values (e.g. {"x": 180.0, "y": 86.0})
|
||||||
|
The target joint variable should NOT be in this dict.
|
||||||
|
min_baseline_mm : skip pairs with model or observed baseline shorter than this
|
||||||
|
verbose : print report
|
||||||
|
|
||||||
|
Returns
|
||||||
|
-------
|
||||||
|
dict with estimation results + updated accumulated_state
|
||||||
|
"""
|
||||||
|
|
||||||
|
# ── sanity checks ─────────────────────────────────────────
|
||||||
|
links = fk.links
|
||||||
|
if link_name not in links:
|
||||||
|
return {"status": "failed",
|
||||||
|
"reason": f"Link '{link_name}' not in robot.json"}
|
||||||
|
|
||||||
|
ji = links[link_name].get("jointToParent", {}) or {}
|
||||||
|
jtype = str(ji.get("type", "")).lower()
|
||||||
|
if jtype != "revolute":
|
||||||
|
return {"status": "failed",
|
||||||
|
"reason": f"Joint of '{link_name}' is not revolute (type='{jtype}')"}
|
||||||
|
|
||||||
|
var = str(ji.get("variable", "")).lower()
|
||||||
|
|
||||||
|
# ── FK: joint origin and axis in world ───────────────────
|
||||||
|
# Use known_state with the TARGET joint at 0
|
||||||
|
zero_state = dict(known_state)
|
||||||
|
zero_state[var] = 0.0
|
||||||
|
|
||||||
|
origin_world = fk.joint_origin_world(link_name, zero_state)
|
||||||
|
axis_world = fk.joint_axis_world(link_name, zero_state)
|
||||||
|
|
||||||
|
# ── collect matched markers ───────────────────────────────
|
||||||
|
model_local: Dict[int, np.ndarray] = {}
|
||||||
|
for m in links[link_name].get("markers", []):
|
||||||
|
mid = int(m.get("id", -1))
|
||||||
|
if mid >= 0 and "position" in m:
|
||||||
|
model_local[mid] = np.array(m["position"], dtype=float)
|
||||||
|
|
||||||
|
matched = {mid: (model_local[mid], observed_mm[mid])
|
||||||
|
for mid in model_local if mid in observed_mm}
|
||||||
|
|
||||||
|
if len(matched) < 2:
|
||||||
|
return {
|
||||||
|
"status": "failed",
|
||||||
|
"reason": (f"Need ≥2 matched markers, found {len(matched)}: "
|
||||||
|
f"{list(matched.keys())}. "
|
||||||
|
f"Model marker IDs: {list(model_local.keys())}"),
|
||||||
|
}
|
||||||
|
|
||||||
|
# ── pairwise estimation ───────────────────────────────────
|
||||||
|
ids = sorted(matched.keys())
|
||||||
|
angle_rad_list: List[float] = []
|
||||||
|
weight_list: List[float] = []
|
||||||
|
per_pair: List[dict] = []
|
||||||
|
|
||||||
|
for id1, id2 in combinations(ids, 2):
|
||||||
|
l1, o1 = matched[id1]
|
||||||
|
l2, o2 = matched[id2]
|
||||||
|
|
||||||
|
v_model = l2 - l1 # local frame, both in same link
|
||||||
|
v_obs = o2 - o1 # world frame
|
||||||
|
|
||||||
|
v_model_perp = _project_perp(v_model, axis_world)
|
||||||
|
v_obs_perp = _project_perp(v_obs, axis_world)
|
||||||
|
|
||||||
|
bl_model = float(np.linalg.norm(v_model_perp))
|
||||||
|
bl_obs = float(np.linalg.norm(v_obs_perp))
|
||||||
|
|
||||||
|
if bl_model < min_baseline_mm or bl_obs < min_baseline_mm:
|
||||||
|
per_pair.append({
|
||||||
|
"marker_ids": [id1, id2],
|
||||||
|
"skipped": True,
|
||||||
|
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}",
|
||||||
|
})
|
||||||
|
continue
|
||||||
|
|
||||||
|
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
|
||||||
|
w = bl_model * bl_obs
|
||||||
|
|
||||||
|
angle_rad_list.append(angle)
|
||||||
|
weight_list.append(w)
|
||||||
|
per_pair.append({
|
||||||
|
"marker_ids": [id1, id2],
|
||||||
|
"skipped": False,
|
||||||
|
"angle_deg": math.degrees(angle),
|
||||||
|
"baseline_model_mm": bl_model,
|
||||||
|
"baseline_obs_mm": bl_obs,
|
||||||
|
"weight": w,
|
||||||
|
})
|
||||||
|
|
||||||
|
if not angle_rad_list:
|
||||||
|
return {
|
||||||
|
"status": "failed",
|
||||||
|
"reason": "All pairs below min_baseline_mm. "
|
||||||
|
"Try --min-baseline 5 or check step-3 output.",
|
||||||
|
}
|
||||||
|
|
||||||
|
mean_deg, c_var, c_std_deg = _circular_mean_deg(
|
||||||
|
np.array(angle_rad_list), np.array(weight_list)
|
||||||
|
)
|
||||||
|
|
||||||
|
# ── verbose report ────────────────────────────────────────
|
||||||
|
if verbose:
|
||||||
|
print(f"\n── 4b: '{link_name}' angle ({var}) ──────────────────────")
|
||||||
|
print(f" Joint origin (world): [{', '.join(f'{v:.1f}' for v in origin_world)}] mm")
|
||||||
|
print(f" Joint axis (world): [{', '.join(f'{v:.3f}' for v in axis_world)}]")
|
||||||
|
print(f" Matched markers: {list(matched.keys())}")
|
||||||
|
print(f" Pairs used: {len(angle_rad_list)} / {len(list(combinations(ids, 2)))}")
|
||||||
|
print(f" Angle: {mean_deg:+.2f} ° circular_σ {c_std_deg:.2f} °")
|
||||||
|
if c_std_deg > 5.0:
|
||||||
|
print(f" [WARN] high spread – step-3 errors or marker overlap")
|
||||||
|
print(f"\n Pair detail:")
|
||||||
|
for pp in per_pair:
|
||||||
|
if pp["skipped"]:
|
||||||
|
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: SKIPPED – {pp['reason']}")
|
||||||
|
else:
|
||||||
|
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: "
|
||||||
|
f"{pp['angle_deg']:+7.2f}° "
|
||||||
|
f"bl_model={pp['baseline_model_mm']:.1f} "
|
||||||
|
f"bl_obs={pp['baseline_obs_mm']:.1f}")
|
||||||
|
|
||||||
|
# ── build accumulated state ───────────────────────────────
|
||||||
|
accumulated = dict(known_state)
|
||||||
|
accumulated[var] = mean_deg
|
||||||
|
|
||||||
|
return {
|
||||||
|
"status": "ok",
|
||||||
|
"link": link_name,
|
||||||
|
"joint": var,
|
||||||
|
"joint_origin_world_mm": origin_world.tolist(),
|
||||||
|
"joint_axis_world": axis_world.tolist(),
|
||||||
|
"mean_angle_deg": mean_deg,
|
||||||
|
"circular_variance": c_var,
|
||||||
|
"circular_std_deg": c_std_deg,
|
||||||
|
"num_pairs_used": len(angle_rad_list),
|
||||||
|
"num_markers_matched": len(matched),
|
||||||
|
"per_pair": per_pair,
|
||||||
|
"accumulated_state": accumulated,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# CLI
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
p = argparse.ArgumentParser(
|
||||||
|
description="4b: estimate revolute joint angle for any link")
|
||||||
|
p.add_argument("--robot", type=Path, default=Path("robot.json"))
|
||||||
|
p.add_argument("--aruco", type=Path,
|
||||||
|
default=Path("aruco_positions_optimized.json"))
|
||||||
|
p.add_argument("--link", type=str, required=True,
|
||||||
|
help="Link name, e.g. Arm1, Ellbow, Arm2")
|
||||||
|
p.add_argument("--from-state", type=Path, default=None,
|
||||||
|
help="JSON from a previous 4b run (carries accumulated state)")
|
||||||
|
|
||||||
|
# Manual joint-value overrides (for use without --from-state)
|
||||||
|
for k in STATE_KEYS:
|
||||||
|
p.add_argument(f"--{k}-mm" if k in ("x", "e") else f"--{k}-deg",
|
||||||
|
type=float, default=None,
|
||||||
|
help=f"Known value for joint '{k}'"
|
||||||
|
+ (" (mm)" if k in ("x", "e") else " (deg)"))
|
||||||
|
|
||||||
|
p.add_argument("--min-baseline", type=float, default=15.0,
|
||||||
|
help="Skip pairs with perpendicular baseline < this (mm)")
|
||||||
|
p.add_argument("--output", type=Path, default=None,
|
||||||
|
help="Save result JSON (readable by next 4b as --from-state)")
|
||||||
|
args = p.parse_args()
|
||||||
|
|
||||||
|
# Assemble known state
|
||||||
|
if args.from_state:
|
||||||
|
known_state = _load_state(args.from_state)
|
||||||
|
print(f" Loaded state from {args.from_state}: {known_state}")
|
||||||
|
else:
|
||||||
|
known_state = {}
|
||||||
|
|
||||||
|
# CLI overrides
|
||||||
|
for k in STATE_KEYS:
|
||||||
|
attr = f"{k}_mm" if k in ("x", "e") else f"{k}_deg"
|
||||||
|
v = getattr(args, attr, None)
|
||||||
|
if v is not None:
|
||||||
|
known_state[k] = float(v)
|
||||||
|
|
||||||
|
fk = RobotFK.from_file(args.robot)
|
||||||
|
aruco_json = _load_json(args.aruco)
|
||||||
|
observed_mm = _load_observed_mm(aruco_json)
|
||||||
|
|
||||||
|
result = estimate_revolute_angle(
|
||||||
|
fk = fk,
|
||||||
|
observed_mm = observed_mm,
|
||||||
|
link_name = args.link,
|
||||||
|
known_state = known_state,
|
||||||
|
min_baseline_mm = args.min_baseline,
|
||||||
|
verbose = True,
|
||||||
|
)
|
||||||
|
|
||||||
|
if args.output and result["status"] == "ok":
|
||||||
|
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
with args.output.open("w", encoding="utf-8") as f:
|
||||||
|
json.dump(result, f, indent=2)
|
||||||
|
print(f"\n Saved → {args.output}")
|
||||||
|
|
||||||
|
return 0 if result["status"] == "ok" else 1
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
218
scripts/5_camera_z_refine.py
Normal file
218
scripts/5_camera_z_refine.py
Normal file
@@ -0,0 +1,218 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
5_camera_z_refine.py
|
||||||
|
-----------------------
|
||||||
|
Refines camera z-positions using the FK-predicted z of the Ellbow link.
|
||||||
|
|
||||||
|
Background
|
||||||
|
----------
|
||||||
|
Camera poses from step 2 are estimated from board markers alone. Board
|
||||||
|
markers are roughly coplanar (z ≈ 0), so the camera's z-height above the
|
||||||
|
board is poorly constrained and can be 20–40 % off.
|
||||||
|
|
||||||
|
Once we have the elbow angle (step 3b, from 4b_revolute_angle.py),
|
||||||
|
FK predicts where the Ellbow markers *should* be in world space. The
|
||||||
|
difference between FK-predicted z and triangulated z gives a global
|
||||||
|
z-offset for the cameras.
|
||||||
|
|
||||||
|
Algorithm
|
||||||
|
---------
|
||||||
|
1. Load elbow-angle result (v8_ellbow_angle.json) → joint state dict
|
||||||
|
2. Run FK with that state → predicted world positions for Ellbow markers (mm)
|
||||||
|
3. Load triangulated Ellbow marker positions from aruco_positions_optimized.json
|
||||||
|
4. delta_z_mm = median(fk_z - triangulated_z) over matched markers
|
||||||
|
5. For each camera pose file:
|
||||||
|
a. camera center world C = -R_wc.T @ t_wc
|
||||||
|
b. C_new = C + [0, 0, delta_z_mm / 1000]
|
||||||
|
c. t_wc_new = -R_wc @ C_new
|
||||||
|
d. Save as *_camera_pose_v8.json
|
||||||
|
6. Save v8_z_correction.json summary
|
||||||
|
|
||||||
|
Usage
|
||||||
|
-----
|
||||||
|
python 5_camera_z_refine.py \\
|
||||||
|
--angle path/to/v8_ellbow_angle.json \\
|
||||||
|
--robot path/to/robot.json \\
|
||||||
|
--aruco path/to/aruco_positions_optimized.json \\
|
||||||
|
-pose path/to/render_a_camera_pose.json \\
|
||||||
|
-pose path/to/render_b_camera_pose.json \\
|
||||||
|
--outDir path/to/output_dir \\
|
||||||
|
[--elbowLink Ellbow]
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import copy
|
||||||
|
import json
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Dict, List, Optional
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
# robot_fk lives in the same directory as this script
|
||||||
|
sys.path.insert(0, str(Path(__file__).parent))
|
||||||
|
from robot_fk import RobotFK
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# I/O helpers
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def load_json(path: str) -> dict:
|
||||||
|
with open(path, "r", encoding="utf-8") as f:
|
||||||
|
return json.load(f)
|
||||||
|
|
||||||
|
|
||||||
|
def save_json(path: str, data: dict) -> None:
|
||||||
|
os.makedirs(os.path.dirname(path) or ".", exist_ok=True)
|
||||||
|
with open(path, "w", encoding="utf-8") as f:
|
||||||
|
json.dump(data, f, indent=2)
|
||||||
|
|
||||||
|
|
||||||
|
def load_triangulated_mm(aruco_json: dict) -> Dict[int, np.ndarray]:
|
||||||
|
"""Read marker world positions (mm) from aruco_positions_*.json."""
|
||||||
|
result: Dict[int, np.ndarray] = {}
|
||||||
|
for m in aruco_json.get("markers", []):
|
||||||
|
mid = int(m.get("marker_id", m.get("id", -1)))
|
||||||
|
if mid < 0:
|
||||||
|
continue
|
||||||
|
if "position_mm" in m:
|
||||||
|
result[mid] = np.array(m["position_mm"], dtype=float)
|
||||||
|
elif "position_m" in m:
|
||||||
|
result[mid] = np.array(m["position_m"], dtype=float) * 1000.0
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
# Main
|
||||||
|
# ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
parser = argparse.ArgumentParser(
|
||||||
|
description="5v8: refine camera z-positions using FK-predicted Ellbow marker z"
|
||||||
|
)
|
||||||
|
parser.add_argument("--angle", required=True,
|
||||||
|
help="v8_ellbow_angle.json from 4b_revolute_angle.py")
|
||||||
|
parser.add_argument("--robot", required=True, help="robot.json")
|
||||||
|
parser.add_argument("--aruco", required=True,
|
||||||
|
help="aruco_positions_optimized.json (first-pass triangulation)")
|
||||||
|
parser.add_argument("-pose", "--poses", action="append", required=True,
|
||||||
|
help="*_camera_pose.json files (repeat for each camera)")
|
||||||
|
parser.add_argument("--outDir", required=True, help="Output directory")
|
||||||
|
parser.add_argument("--elbowLink", default="Ellbow",
|
||||||
|
help="Link name used for z estimation (default: Ellbow)")
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
os.makedirs(args.outDir, exist_ok=True)
|
||||||
|
|
||||||
|
# ── 1. Load angle / state ────────────────────────────────
|
||||||
|
angle_data = load_json(args.angle)
|
||||||
|
if angle_data.get("status") != "ok":
|
||||||
|
print(f"[ERROR] Angle estimation was not ok: {angle_data.get('reason', '?')}")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
accumulated_state = angle_data.get("accumulated_state", {})
|
||||||
|
elbow_link = angle_data.get("link", args.elbowLink)
|
||||||
|
|
||||||
|
print(f"[INFO] Joint state from 4b: {accumulated_state}")
|
||||||
|
print(f"[INFO] Link used for z-ref: {elbow_link}")
|
||||||
|
|
||||||
|
# ── 2. FK prediction ─────────────────────────────────────
|
||||||
|
fk = RobotFK.from_file(args.robot)
|
||||||
|
T = fk.compute(accumulated_state)
|
||||||
|
all_fk = fk.all_markers_world(T) # marker_id -> {world_mm, link, …}
|
||||||
|
|
||||||
|
elbow_fk = {mid: v for mid, v in all_fk.items() if v["link"] == elbow_link}
|
||||||
|
|
||||||
|
if not elbow_fk:
|
||||||
|
print(f"[ERROR] No FK markers found for link '{elbow_link}'")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
print(f"[INFO] FK markers for '{elbow_link}': {sorted(elbow_fk.keys())}")
|
||||||
|
|
||||||
|
# ── 3. Load triangulated positions ───────────────────────
|
||||||
|
aruco_data = load_json(args.aruco)
|
||||||
|
triangulated = load_triangulated_mm(aruco_data)
|
||||||
|
|
||||||
|
# ── 4. Compute z-deltas ──────────────────────────────────
|
||||||
|
z_deltas: List[float] = []
|
||||||
|
print("\n[INFO] Per-marker z comparison (FK vs triangulated):")
|
||||||
|
for mid in sorted(elbow_fk.keys()):
|
||||||
|
fk_z = float(elbow_fk[mid]["world_mm"][2])
|
||||||
|
if mid not in triangulated:
|
||||||
|
print(f" Marker {mid:4d}: FK z={fk_z:8.1f} mm [NOT triangulated – skip]")
|
||||||
|
continue
|
||||||
|
obs_z = float(triangulated[mid][2])
|
||||||
|
delta = fk_z - obs_z
|
||||||
|
z_deltas.append(delta)
|
||||||
|
print(f" Marker {mid:4d}: FK z={fk_z:8.1f} mm obs z={obs_z:8.1f} mm Δz={delta:+7.1f} mm")
|
||||||
|
|
||||||
|
if not z_deltas:
|
||||||
|
print(f"\n[ERROR] No matched markers for '{elbow_link}' — cannot compute z correction.")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
delta_z_mm = float(np.median(z_deltas))
|
||||||
|
print(f"\n[INFO] z-correction (median): {delta_z_mm:+.1f} mm "
|
||||||
|
f"(from {len(z_deltas)} markers)")
|
||||||
|
if abs(delta_z_mm) < 1.0:
|
||||||
|
print("[INFO] Correction < 1 mm — cameras already well-calibrated in z.")
|
||||||
|
|
||||||
|
# ── 5. Apply correction to each camera pose ──────────────
|
||||||
|
corrected_files: List[str] = []
|
||||||
|
|
||||||
|
for pose_file in args.poses:
|
||||||
|
pose_data = load_json(pose_file)
|
||||||
|
|
||||||
|
pose_section = pose_data.get("camera_pose", {}) or {}
|
||||||
|
w2c = pose_section.get("world_to_camera", {}) or {}
|
||||||
|
|
||||||
|
R_wc = np.array(w2c.get("rotation_matrix", []), dtype=float).reshape(3, 3)
|
||||||
|
t_wc = np.array(w2c.get("translation_m", []), dtype=float).reshape(3)
|
||||||
|
|
||||||
|
# Camera centre in world (metres)
|
||||||
|
C_world = -R_wc.T @ t_wc
|
||||||
|
C_world_new = C_world + np.array([0.0, 0.0, delta_z_mm / 1000.0])
|
||||||
|
|
||||||
|
t_wc_new = -R_wc @ C_world_new
|
||||||
|
|
||||||
|
pose_new = copy.deepcopy(pose_data)
|
||||||
|
pose_new["camera_pose"]["world_to_camera"]["translation_m"] = \
|
||||||
|
[float(v) for v in t_wc_new.tolist()]
|
||||||
|
pose_new["camera_pose"]["camera_in_world"]["position_m"] = \
|
||||||
|
[float(v) for v in C_world_new.tolist()]
|
||||||
|
pose_new["camera_pose"]["camera_in_world"]["position_mm"] = \
|
||||||
|
[float(v * 1000.0) for v in C_world_new.tolist()]
|
||||||
|
pose_new["v8_z_correction_mm"] = delta_z_mm
|
||||||
|
pose_new["created_utc"] = time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime())
|
||||||
|
|
||||||
|
basename = os.path.basename(pose_file)
|
||||||
|
out_name = basename.replace("_camera_pose.json", "_camera_pose_v8.json")
|
||||||
|
if out_name == basename:
|
||||||
|
out_name = os.path.splitext(basename)[0] + "_v8.json"
|
||||||
|
|
||||||
|
out_path = os.path.join(args.outDir, out_name)
|
||||||
|
save_json(out_path, pose_new)
|
||||||
|
print(f"[INFO] Saved: {out_path}")
|
||||||
|
corrected_files.append(out_path)
|
||||||
|
|
||||||
|
# ── 6. Save correction summary ───────────────────────────
|
||||||
|
summary = {
|
||||||
|
"schema_version": "1.0",
|
||||||
|
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||||
|
"elbow_link": elbow_link,
|
||||||
|
"z_correction_mm": delta_z_mm,
|
||||||
|
"n_markers_used": len(z_deltas),
|
||||||
|
"z_deltas_mm": z_deltas,
|
||||||
|
"corrected_pose_files": corrected_files,
|
||||||
|
}
|
||||||
|
summary_path = os.path.join(args.outDir, "v8_z_correction.json")
|
||||||
|
save_json(summary_path, summary)
|
||||||
|
print(f"\n[INFO] Correction summary → {summary_path}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
145
scripts/9_evaluateMarker.py
Normal file
145
scripts/9_evaluateMarker.py
Normal file
@@ -0,0 +1,145 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
9_evaluateMarker.py
|
||||||
|
===================
|
||||||
|
Compare reconstructed markers against ground-truth (render_*.json).
|
||||||
|
|
||||||
|
Reports, per link and overall:
|
||||||
|
* 3D position error (mm)
|
||||||
|
* orientation error (deg) between measured and GT normal — only meaningful
|
||||||
|
when the detected file carries a MEASURED normal (aruco_marker_poses.json).
|
||||||
|
|
||||||
|
Backwards compatible: the original positional CLI
|
||||||
|
python 9_evaluateMarker.py detected.json original.json
|
||||||
|
still works and prints the familiar summary; --out adds a JSON report.
|
||||||
|
"""
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
from collections import defaultdict
|
||||||
|
from typing import Any, Dict, List, Optional
|
||||||
|
|
||||||
|
|
||||||
|
def dist3(a, b) -> float:
|
||||||
|
return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
|
||||||
|
|
||||||
|
|
||||||
|
def angle_deg(a, b) -> Optional[float]:
|
||||||
|
na = math.sqrt(sum(x * x for x in a))
|
||||||
|
nb = math.sqrt(sum(x * x for x in b))
|
||||||
|
if na < 1e-9 or nb < 1e-9:
|
||||||
|
return None
|
||||||
|
c = sum(x * y for x, y in zip(a, b)) / (na * nb)
|
||||||
|
c = max(-1.0, min(1.0, c))
|
||||||
|
ang = math.degrees(math.acos(c))
|
||||||
|
return min(ang, 180.0 - ang) # flip-invariant
|
||||||
|
|
||||||
|
|
||||||
|
def _pct(values: List[float], q: float) -> float:
|
||||||
|
if not values:
|
||||||
|
return 0.0
|
||||||
|
s = sorted(values)
|
||||||
|
idx = max(0, min(len(s) - 1, int(q * len(s)) - 1))
|
||||||
|
return s[idx]
|
||||||
|
|
||||||
|
|
||||||
|
def analyze(detected_file: str, original_file: str) -> Dict[str, Any]:
|
||||||
|
detected = json.load(open(detected_file, "r", encoding="utf-8"))
|
||||||
|
original = json.load(open(original_file, "r", encoding="utf-8"))
|
||||||
|
|
||||||
|
det_markers = detected.get("markers", detected if isinstance(detected, list) else [])
|
||||||
|
det_by_id = {int(m.get("marker_id", m.get("id", -1))): m for m in det_markers}
|
||||||
|
orig_by_id = {int(m["id"]): m for m in original}
|
||||||
|
|
||||||
|
det_ids = set(det_by_id) - {-1}
|
||||||
|
orig_ids = set(orig_by_id)
|
||||||
|
recognized = det_ids & orig_ids
|
||||||
|
missing = orig_ids - det_ids
|
||||||
|
|
||||||
|
per_link_pos: Dict[str, List[float]] = defaultdict(list)
|
||||||
|
per_link_ang: Dict[str, List[float]] = defaultdict(list)
|
||||||
|
rows = []
|
||||||
|
for mid in sorted(recognized):
|
||||||
|
o = orig_by_id[mid]
|
||||||
|
d = det_by_id[mid]
|
||||||
|
link = o.get("link", d.get("link", "?"))
|
||||||
|
pos_err = dist3(o["position_m"], d["position_m"]) * 1000.0 # mm
|
||||||
|
per_link_pos[link].append(pos_err)
|
||||||
|
ang_err = None
|
||||||
|
if o.get("normal") is not None and d.get("normal") is not None:
|
||||||
|
ang_err = angle_deg(o["normal"], d["normal"])
|
||||||
|
if ang_err is not None:
|
||||||
|
per_link_ang[link].append(ang_err)
|
||||||
|
rows.append({"marker_id": mid, "link": link, "pos_err_mm": pos_err, "normal_err_deg": ang_err})
|
||||||
|
|
||||||
|
all_pos = [r["pos_err_mm"] for r in rows]
|
||||||
|
all_ang = [r["normal_err_deg"] for r in rows if r["normal_err_deg"] is not None]
|
||||||
|
|
||||||
|
return {
|
||||||
|
"n_original": len(orig_ids),
|
||||||
|
"n_recognized": len(recognized),
|
||||||
|
"n_missing": len(missing),
|
||||||
|
"recognition_rate": (len(recognized) / len(orig_ids) * 100.0) if orig_ids else 0.0,
|
||||||
|
"per_link": {
|
||||||
|
ln: {
|
||||||
|
"n": len(per_link_pos[ln]),
|
||||||
|
"pos_mean_mm": sum(per_link_pos[ln]) / len(per_link_pos[ln]),
|
||||||
|
"pos_max_mm": max(per_link_pos[ln]),
|
||||||
|
"normal_mean_deg": (sum(per_link_ang[ln]) / len(per_link_ang[ln])) if per_link_ang.get(ln) else None,
|
||||||
|
"normal_max_deg": max(per_link_ang[ln]) if per_link_ang.get(ln) else None,
|
||||||
|
} for ln in sorted(per_link_pos, key=lambda k: -len(per_link_pos[k]))
|
||||||
|
},
|
||||||
|
"overall": {
|
||||||
|
"pos_mean_mm": (sum(all_pos) / len(all_pos)) if all_pos else 0.0,
|
||||||
|
"pos_p90_mm": _pct(all_pos, 0.9),
|
||||||
|
"pos_max_mm": max(all_pos) if all_pos else 0.0,
|
||||||
|
"normal_mean_deg": (sum(all_ang) / len(all_ang)) if all_ang else None,
|
||||||
|
"normal_p90_deg": _pct(all_ang, 0.9) if all_ang else None,
|
||||||
|
"normal_max_deg": max(all_ang) if all_ang else None,
|
||||||
|
},
|
||||||
|
"rows": rows,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def print_report(r: Dict[str, Any]) -> None:
|
||||||
|
# familiar summary (backwards compatible wording)
|
||||||
|
print(f"Erkannte Marker: {r['n_recognized']}")
|
||||||
|
print(f"Nicht erkannte Marker: {r['n_missing']}")
|
||||||
|
print(f"Gesamtzahl der Original-Marker: {r['n_original']}")
|
||||||
|
print(f"Erkennungsrate: {r['recognition_rate']:.2f}%")
|
||||||
|
o = r["overall"]
|
||||||
|
print(f"Gemittelter 3D-Abstand: {o['pos_mean_mm']/1000.0:.4f}m")
|
||||||
|
print(f"90%-Radius: {o['pos_p90_mm']/1000.0:.4f}m")
|
||||||
|
print(f"Schlechtester Abstand: {o['pos_max_mm']/1000.0:.4f}m")
|
||||||
|
if o["normal_mean_deg"] is not None:
|
||||||
|
print(f"Normalen-Fehler: mean {o['normal_mean_deg']:.2f}deg / p90 {o['normal_p90_deg']:.2f}deg / max {o['normal_max_deg']:.2f}deg")
|
||||||
|
|
||||||
|
print("\nPro Glied:")
|
||||||
|
print(f" {'link':>10} | {'n':>3} | {'pos mean/max [mm]':>18} | {'normal mean/max [deg]':>21}")
|
||||||
|
print(" " + "-" * 60)
|
||||||
|
for ln, s in r["per_link"].items():
|
||||||
|
nd = f"{s['normal_mean_deg']:6.2f} /{s['normal_max_deg']:6.2f}" if s["normal_mean_deg"] is not None else " - "
|
||||||
|
print(f" {ln:>10} | {s['n']:>3} | {s['pos_mean_mm']:7.2f} /{s['pos_max_mm']:7.2f} | {nd:>21}")
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
ap = argparse.ArgumentParser(description="Analysiert die Markererkennung (Position + Orientierung).")
|
||||||
|
ap.add_argument("detected_file", help="aruco_marker_poses.json oder aruco_positions_*.json")
|
||||||
|
ap.add_argument("original_file", help="Ground-Truth render_*.json")
|
||||||
|
ap.add_argument("--out", default=None, help="optional JSON-Report")
|
||||||
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
r = analyze(args.detected_file, args.original_file)
|
||||||
|
if r["n_recognized"] == 0:
|
||||||
|
print("Keine gemeinsamen Marker gefunden, um die Genauigkeit zu bewerten.")
|
||||||
|
return
|
||||||
|
print_report(r)
|
||||||
|
if args.out:
|
||||||
|
json.dump(r, open(args.out, "w", encoding="utf-8"), indent=2)
|
||||||
|
print(f"\n[INFO] wrote {args.out}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
Reference in New Issue
Block a user