Compare commits
38 Commits
b854b44111
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
|
1bbcb535aa | ||
|
|
c1f29bc1ee | ||
|
|
dba2744687 | ||
|
|
33dcbe72bf | ||
|
|
fab7032d56 | ||
|
|
9bf49eff8d | ||
|
|
da2a5d5ae6 | ||
|
|
7818604c02 | ||
| 1db62e08df | |||
|
|
ce829d3875 | ||
| fe08ebc08c | |||
| b9df99540d | |||
| 2c0aeb718a | |||
| f9db05d073 | |||
|
|
aa78116837 | ||
|
|
d36ef6189d | ||
|
|
eb403dab36 | ||
|
|
5f8e1a0189 | ||
|
|
498499bf13 | ||
|
|
a3986beb6e | ||
|
|
855f917d24 | ||
|
|
f585c83689 | ||
|
|
366de4aad9 | ||
|
|
eae6b6098a | ||
|
|
5f6d28673a | ||
|
|
08d1c21d1e | ||
|
|
f929c19f4b | ||
|
|
578b955508 | ||
|
|
0234c1ef1d | ||
|
|
ad208b7d21 | ||
|
|
f983d69a0c | ||
|
|
ef4c7e6144 | ||
|
|
90f84d1221 | ||
|
|
13316b0d03 | ||
|
|
7ea5eda908 | ||
|
|
8ec601a087 | ||
|
|
2582b2adc9 | ||
|
|
a79729ca1f |
11
.claude/launch.json
Normal file
11
.claude/launch.json
Normal file
@@ -0,0 +1,11 @@
|
||||
{
|
||||
"version": "0.0.1",
|
||||
"configurations": [
|
||||
{
|
||||
"name": "appRobotHoming",
|
||||
"runtimeExecutable": "node",
|
||||
"runtimeArgs": ["server/server.js"],
|
||||
"port": 2093
|
||||
}
|
||||
]
|
||||
}
|
||||
49
data/homing/20260625_172504/aruco_marker_poses.csv
Normal file
49
data/homing/20260625_172504/aruco_marker_poses.csv
Normal file
@@ -0,0 +1,49 @@
|
||||
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
|
||||
0,unknown,,2,505.2,-100.88,-7.16,0.02189,0.03624,0.9991,,,,,,24.24
|
||||
46,Board,A0,2,537.83,185.51,-27.88,-0.08568,-0.05354,0.99488,537.44,185.2,-27.2,0.841,-0.679,23.2
|
||||
47,Board,A0,2,343.18,-286.29,-27.16,-0.03123,0.01004,0.99946,343.18,-286.05,-27.49,0.407,0.326,24.03
|
||||
50,Board,A0,2,574.07,210.38,-26.23,-0.00073,0.03209,0.99948,574.23,211.48,-27.15,1.448,0.925,24.06
|
||||
51,Board,A0,2,166.73,-171.08,-27.09,-0.03386,0.01172,0.99936,167.18,-170.93,-27.76,0.818,0.667,24.31
|
||||
53,Board,A0,2,487.37,212.32,-27.38,-0.07361,-0.01564,0.99716,487.08,212.19,-27.28,0.33,-0.103,23.53
|
||||
54,Board,A0,3,341.07,-330.3,-27.22,-0.03362,0.04017,0.99863,341.05,-330.09,-27.5,0.351,0.28,24.36
|
||||
55,Arm1,A0,2,282.65,-261.75,-26.65,-0.05274,0.01696,0.99846,,,,,,24.32
|
||||
58,Board,A0,2,48.62,-216.5,-27.86,-0.00162,-0.0094,0.99995,49.3,-216.52,-27.93,0.684,0.068,24.24
|
||||
62,Board,A0,3,404.15,-174.92,-26.96,0.01614,-0.01502,0.99976,404.07,-174.84,-27.4,0.454,0.439,23.97
|
||||
64,Board,A0,2,-22.59,-186.68,-26.97,-0.00648,0.01133,0.99991,-21.95,-186.37,-28.04,1.286,1.074,24.24
|
||||
66,Board,A0,2,208.51,-363.21,-27.64,-0.04961,0.03216,0.99825,208.41,-362.24,-27.7,0.98,0.055,24.38
|
||||
68,Board,A0,2,574.39,169.06,-26.11,-0.00044,0.05167,0.99866,574.4,170.26,-27.15,1.591,1.04,24.52
|
||||
73,Board,A0,2,221.97,337.22,-30.29,-0.05172,0.1462,0.9879,223.01,334.05,-27.67,4.242,-2.621,26.49
|
||||
76,Board,A0,2,686.21,165.02,-27.13,-0.04548,-0.03114,0.99848,685.86,166,-26.98,1.054,-0.149,23.28
|
||||
79,Board,A0,2,311.51,-157.95,-27.04,0.05023,-0.04899,0.99754,311.73,-158.5,-27.54,0.776,0.496,23.36
|
||||
82,Board,A0,2,219.37,300.46,-29.79,0.00751,0.1232,0.99235,220.31,298.19,-27.68,3.238,-2.111,25.14
|
||||
85,Board,A0,3,503.91,-313.51,-27.06,0.00515,-0.00866,0.99995,503.43,-312.87,-27.25,0.816,0.191,24.1
|
||||
90,Board,A0,2,644.93,316.2,-28.16,-0.02384,-0.04569,0.99867,644.39,315.79,-27.04,1.307,-1.118,23.32
|
||||
91,Board,A0,2,725.49,327.85,-27.51,-0.02708,0.03694,0.99895,724.61,327.11,-26.92,1.289,-0.587,24.21
|
||||
92,Board,A0,2,644.7,-186.93,-25.55,-0.05883,-0.0185,0.9981,644.42,-185.49,-27.04,2.094,1.489,23.69
|
||||
95,Board,A0,3,184.77,-273.26,-27.61,-0.03697,0.01835,0.99915,185.04,-272.99,-27.73,0.401,0.121,24.34
|
||||
96,Board,A0,3,369.2,-185.74,-27.61,-0.04836,0.02152,0.9986,369.1,-186.1,-27.46,0.398,-0.148,24.06
|
||||
103,Board,A0,3,104.63,-186.32,-27.25,-0.01721,0.02455,0.99955,105.03,-186.33,-27.85,0.721,0.6,24.42
|
||||
105,Board,A0,3,524.3,-267.15,-27.08,-0.016,0.01938,0.99968,523.86,-266.44,-27.22,0.848,0.137,23.92
|
||||
118,unknown,,3,322.99,-174.22,47.16,0.02335,-0.99462,0.10089,,,,,,24.14
|
||||
122,Ellbow,,3,359.95,-173.78,46.07,0.00204,-0.99361,0.11286,,,,,,24.26
|
||||
143,Arm2,,2,340.07,-138.22,229.66,-0.72695,-0.68151,0.08421,,,,,,24.03
|
||||
144,Arm2,,3,362.93,-157.15,158.16,-0.05456,-0.9917,0.11642,,,,,,24.31
|
||||
146,Arm2,,2,337.31,-147.71,160.02,-0.68833,-0.72303,0.05854,,,,,,24.0
|
||||
147,FingerA,,3,383.25,-144.05,226.5,0.43778,-0.89607,0.07357,,,,,,23.85
|
||||
148,Arm2,,3,367.21,-142.91,264.34,-0.04936,-0.99586,0.07639,,,,,,24.28
|
||||
178,FingerB,,2,287.73,-121.4,315.33,-0.67179,-0.7215,-0.16775,,,,,,23.25
|
||||
179,FingerB,,2,329.15,-139.64,311.4,-0.61251,-0.17201,0.77152,,,,,,23.53
|
||||
198,Arm1,,2,268.53,-53.98,84.4,0.00864,0.01319,0.99988,,,,,,24.79
|
||||
200,unknown,,2,199.66,-28.82,110.04,-0.15134,-0.01695,0.98834,,,,,,23.71
|
||||
204,unknown,,2,198.48,115.43,120.91,0.03546,0.04428,0.99839,,,,,,24.17
|
||||
208,Board,rail,2,626.35,-98.62,-6.79,-0.00084,0.01537,0.99988,631.01,-98.43,-7.71,4.748,0.917,24.04
|
||||
210,Board,rail,2,129.9,-7.86,-5.67,0.06219,0.02253,0.99781,122.63,-13.98,-0.72,10.715,-4.946,23.24
|
||||
214,unknown,,2,531.62,-8.59,-6.83,-0.02922,0.04196,0.99869,,,,,,24.25
|
||||
217,Board,rail,2,730.01,-8.51,-5.0,-0.08513,-0.01438,0.99627,732.39,-23.88,7.39,19.881,-12.386,23.12
|
||||
229,Arm1,,3,271.17,-142.11,79.1,0.01062,-0.04516,0.99892,,,,,,23.94
|
||||
243,Arm1,,2,270.3,-176.61,43.73,0.02044,-0.9996,-0.01934,,,,,,24.13
|
||||
|
||||
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
|
||||
cam0,335.33,-885.66,468.37,-0.06588,0.89247,-0.44628
|
||||
cam1,296.81,-462.82,780.61,0.09503,0.4331,-0.89632
|
||||
cam2,712.91,-665.53,724.31,-0.42887,0.59445,-0.68022
|
||||
|
2002
data/homing/20260625_172504/aruco_marker_poses.json
Normal file
2002
data/homing/20260625_172504/aruco_marker_poses.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
data/homing/20260625_172504/cam0.jpg
Normal file
BIN
data/homing/20260625_172504/cam0.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 98 KiB |
2645
data/homing/20260625_172504/cam0_aruco_detection.json
Normal file
2645
data/homing/20260625_172504/cam0_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
481
data/homing/20260625_172504/cam0_camera_pose.json
Normal file
481
data/homing/20260625_172504/cam0_camera_pose.json
Normal file
@@ -0,0 +1,481 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:25:09Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_172504/cam0_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam0",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1424.7584228515625,
|
||||
0.0,
|
||||
635.95947265625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1421.5770263671875,
|
||||
482.1744384765625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.05634751915931702,
|
||||
0.33765655755996704,
|
||||
0.002130246954038739,
|
||||
-0.004022662527859211,
|
||||
-1.182201862335205
|
||||
]
|
||||
},
|
||||
"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": 20,
|
||||
"used_marker_ids": [
|
||||
97,
|
||||
66,
|
||||
85,
|
||||
54,
|
||||
105,
|
||||
69,
|
||||
47,
|
||||
95,
|
||||
58,
|
||||
64,
|
||||
103,
|
||||
62,
|
||||
96,
|
||||
208,
|
||||
51,
|
||||
79,
|
||||
210,
|
||||
68,
|
||||
50,
|
||||
91
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011403454671871994,
|
||||
0.001728926659833975,
|
||||
0.0016691755968926973,
|
||||
0.0016691753847773315
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 3.4096867660331136,
|
||||
"residual_median_px": 1.5691231791727809,
|
||||
"residual_max_px": 12.026369061549262,
|
||||
"sigma2_normalized": 3.2778193707572165e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.9973888397216797,
|
||||
0.04563671350479126,
|
||||
-0.05597161129117012
|
||||
],
|
||||
[
|
||||
-0.02958603762090206,
|
||||
-0.4488010108470917,
|
||||
-0.8931418061256409
|
||||
],
|
||||
[
|
||||
-0.0658801719546318,
|
||||
0.8924656510353088,
|
||||
-0.4462788999080658
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.2678244411945343,
|
||||
0.030760858207941055,
|
||||
1.0215346813201904
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.0344335845398724,
|
||||
0.011289329253343375,
|
||||
-0.08570511152514877
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.33533409237861633,
|
||||
-0.885656476020813,
|
||||
0.4683726131916046
|
||||
],
|
||||
"position_mm": [
|
||||
335.3341064453125,
|
||||
-885.656494140625,
|
||||
468.37261962890625
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 116.56741333007812,
|
||||
"pitch": 3.777391195297241,
|
||||
"yaw": -1.6990946531295776
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
9.702707532310775e-06,
|
||||
1.2273484057683694e-06,
|
||||
3.575388647845507e-06,
|
||||
-1.2199370555784652e-07,
|
||||
-2.1440042443296295e-06,
|
||||
1.039618282302021e-07
|
||||
],
|
||||
[
|
||||
1.22734840576837e-06,
|
||||
3.358446334387221e-06,
|
||||
1.8878909236797321e-07,
|
||||
4.651297819399596e-07,
|
||||
-8.835069052569738e-07,
|
||||
6.206962238461149e-07
|
||||
],
|
||||
[
|
||||
3.5753886478454758e-06,
|
||||
1.8878909236796024e-07,
|
||||
1.143731789507973e-05,
|
||||
-7.216885572151019e-07,
|
||||
-2.2145565854988443e-06,
|
||||
-3.0088157401175667e-06
|
||||
],
|
||||
[
|
||||
-1.219937055578466e-07,
|
||||
4.651297819399599e-07,
|
||||
-7.216885572151033e-07,
|
||||
2.2942348340442282e-07,
|
||||
3.6332485943604063e-08,
|
||||
3.2363949947789566e-07
|
||||
],
|
||||
[
|
||||
-2.1440042443296215e-06,
|
||||
-8.835069052569716e-07,
|
||||
-2.2145565854988506e-06,
|
||||
3.633248594360388e-08,
|
||||
9.86494679764419e-07,
|
||||
6.579597825341255e-07
|
||||
],
|
||||
[
|
||||
1.039618282302198e-07,
|
||||
6.206962238461218e-07,
|
||||
-3.0088157401175595e-06,
|
||||
3.2363949947789524e-07,
|
||||
6.579597825341204e-07,
|
||||
2.970696318843356e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.1784715940965861,
|
||||
0.10500061405868079,
|
||||
0.19376919211544102
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0004789817151044733,
|
||||
0.0009932243854056438,
|
||||
0.0017235708047084566
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0024386451901829424,
|
||||
0.0013863680263389562,
|
||||
0.002737631446074117
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2.4386451901829425,
|
||||
1.3863680263389562,
|
||||
2.7376314460741167
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.19011944203297743,
|
||||
"pitch": 0.14270986978198957,
|
||||
"yaw": 0.1181609242753551
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
676.25,
|
||||
910.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
675.9551391601562,
|
||||
911.3131713867188
|
||||
],
|
||||
"reprojection_error_px": 0.8649801263910383,
|
||||
"confidence": 0.42385670146087856
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
480.5,
|
||||
921.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
480.5653381347656,
|
||||
919.2376098632812
|
||||
],
|
||||
"reprojection_error_px": 1.763600880544741,
|
||||
"confidence": 0.2797311732321247
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
1080.0,
|
||||
843.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1077.551025390625,
|
||||
842.6098022460938
|
||||
],
|
||||
"reprojection_error_px": 2.605749158768581,
|
||||
"confidence": 0.6761152978036918
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
753.5,
|
||||
868.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
753.16845703125,
|
||||
868.4518432617188
|
||||
],
|
||||
"reprojection_error_px": 0.33502210609070604,
|
||||
"confidence": 0.6745646371332027
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1098.25,
|
||||
783.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1096.3798828125,
|
||||
782.7687377929688
|
||||
],
|
||||
"reprojection_error_px": 2.0080046589625047,
|
||||
"confidence": 0.53843639257073
|
||||
},
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
130.25,
|
||||
818.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
131.2628631591797,
|
||||
816.5780639648438
|
||||
],
|
||||
"reprojection_error_px": 1.9548048201489217,
|
||||
"confidence": 0.6309056746154269
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
755.0,
|
||||
810.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
754.8045043945312,
|
||||
810.226318359375
|
||||
],
|
||||
"reprojection_error_px": 0.19692473653729947,
|
||||
"confidence": 0.558404255319149
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
461.25,
|
||||
799.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
462.01556396484375,
|
||||
799.7348022460938
|
||||
],
|
||||
"reprojection_error_px": 0.7657148006869643,
|
||||
"confidence": 0.5573620390355706
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
243.0,
|
||||
742.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
244.86248779296875,
|
||||
742.42578125
|
||||
],
|
||||
"reprojection_error_px": 1.8639659872994379,
|
||||
"confidence": 0.4721784486231997
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
139.75,
|
||||
714.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
141.61831665039062,
|
||||
715.1568603515625
|
||||
],
|
||||
"reprojection_error_px": 2.197483328524737,
|
||||
"confidence": 0.4471706966400147
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
350.75,
|
||||
708.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
352.2232971191406,
|
||||
708.7933959960938
|
||||
],
|
||||
"reprojection_error_px": 1.6733444379103959,
|
||||
"confidence": 0.3922909999211629
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
851.5,
|
||||
685.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
851.7431640625,
|
||||
686.1382446289062
|
||||
],
|
||||
"reprojection_error_px": 0.9209274032584249,
|
||||
"confidence": 0.3564780454484243
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
793.25,
|
||||
698.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
793.7991333007812,
|
||||
698.2991943359375
|
||||
],
|
||||
"reprojection_error_px": 0.6253516072450701,
|
||||
"confidence": 0.3348906742607279
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
1202.5,
|
||||
583.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1209.5457763671875,
|
||||
583.9437866210938
|
||||
],
|
||||
"reprojection_error_px": 7.108705775496229,
|
||||
"confidence": 0.28965074531908364
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
458.0,
|
||||
690.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
459.341064453125,
|
||||
691.0894775390625
|
||||
],
|
||||
"reprojection_error_px": 1.4649019204351656,
|
||||
"confidence": 0.34674061533734285
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
696.25,
|
||||
672.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
697.0543212890625,
|
||||
673.733154296875
|
||||
],
|
||||
"reprojection_error_px": 1.6872105394342276,
|
||||
"confidence": 0.3256815826862768
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
439.25,
|
||||
532.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.3565673828125,
|
||||
530.716796875
|
||||
],
|
||||
"reprojection_error_px": 12.026369061549262,
|
||||
"confidence": 0.15571821530659996
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
1027.25,
|
||||
434.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1027.0755615234375,
|
||||
434.6865539550781
|
||||
],
|
||||
"reprojection_error_px": 0.47011502613700773,
|
||||
"confidence": 0.09323781394061914
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
1017.0,
|
||||
413.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1016.9035034179688,
|
||||
413.9068298339844
|
||||
],
|
||||
"reprojection_error_px": 0.41811733301008686,
|
||||
"confidence": 0.08307692198670814
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
1165.5,
|
||||
355.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1165.020751953125,
|
||||
355.35980224609375
|
||||
],
|
||||
"reprojection_error_px": 0.6180072633772071,
|
||||
"confidence": 0.06871920537654881
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_172504/cam0_debug.jpg
Normal file
BIN
data/homing/20260625_172504/cam0_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 209 KiB |
BIN
data/homing/20260625_172504/cam1.jpg
Normal file
BIN
data/homing/20260625_172504/cam1.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 108 KiB |
3662
data/homing/20260625_172504/cam1_aruco_detection.json
Normal file
3662
data/homing/20260625_172504/cam1_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
761
data/homing/20260625_172504/cam1_camera_pose.json
Normal file
761
data/homing/20260625_172504/cam1_camera_pose.json
Normal file
@@ -0,0 +1,761 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:25:13Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_172504/cam1_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam1",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1367.5723876953125,
|
||||
0.0,
|
||||
672.1165771484375
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1372.3011474609375,
|
||||
445.8396911621094
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.01016925647854805,
|
||||
0.7656787633895874,
|
||||
-0.0031530377455055714,
|
||||
-0.00288817984983325,
|
||||
-2.490830183029175
|
||||
]
|
||||
},
|
||||
"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": 40,
|
||||
"used_marker_ids": [
|
||||
54,
|
||||
95,
|
||||
58,
|
||||
85,
|
||||
47,
|
||||
103,
|
||||
59,
|
||||
48,
|
||||
105,
|
||||
51,
|
||||
102,
|
||||
96,
|
||||
62,
|
||||
71,
|
||||
92,
|
||||
63,
|
||||
208,
|
||||
210,
|
||||
217,
|
||||
74,
|
||||
75,
|
||||
52,
|
||||
68,
|
||||
76,
|
||||
46,
|
||||
53,
|
||||
101,
|
||||
50,
|
||||
100,
|
||||
82,
|
||||
60,
|
||||
67,
|
||||
73,
|
||||
94,
|
||||
70,
|
||||
104,
|
||||
98,
|
||||
90,
|
||||
91,
|
||||
88
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011372574297308506,
|
||||
0.0021949616225605375,
|
||||
0.002143709970633132,
|
||||
0.002143709690342172
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 4.23364353800265,
|
||||
"residual_median_px": 1.0679787737296105,
|
||||
"residual_max_px": 19.348939630537274,
|
||||
"sigma2_normalized": 4.968098633993014e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.9951881766319275,
|
||||
0.019756384193897247,
|
||||
-0.0959698036313057
|
||||
],
|
||||
[
|
||||
-0.02385592833161354,
|
||||
0.9011316895484924,
|
||||
0.4328886866569519
|
||||
],
|
||||
[
|
||||
0.09503374248743057,
|
||||
0.43309515714645386,
|
||||
-0.8963242769241333
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
0.37943923473358154,
|
||||
0.0862211212515831,
|
||||
0.8719164729118347
|
||||
],
|
||||
"rvec_rad": [
|
||||
0.0032072073338993575,
|
||||
-2.967110340660477,
|
||||
-0.6774876643558738
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.2968088686466217,
|
||||
-0.4628157615661621,
|
||||
0.7806104421615601
|
||||
],
|
||||
"position_mm": [
|
||||
296.8088684082031,
|
||||
-462.8157653808594,
|
||||
780.6104125976562
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 154.21060180664062,
|
||||
"pitch": -5.453261375427246,
|
||||
"yaw": -178.62680053710938
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
2.2460757709580997e-06,
|
||||
-1.0592111231264017e-07,
|
||||
-5.911479445197522e-07,
|
||||
9.807677828663091e-09,
|
||||
6.656021402664132e-07,
|
||||
1.8807173299942092e-07
|
||||
],
|
||||
[
|
||||
-1.059211123126279e-07,
|
||||
7.819602866884685e-06,
|
||||
-8.896995228371429e-07,
|
||||
-8.928123497147304e-07,
|
||||
8.447874310868068e-07,
|
||||
-3.883503279890717e-06
|
||||
],
|
||||
[
|
||||
-5.911479445197253e-07,
|
||||
-8.896995228370399e-07,
|
||||
1.8982339503369444e-05,
|
||||
-6.420456812663701e-07,
|
||||
-7.934435641861599e-07,
|
||||
3.082335762957178e-07
|
||||
],
|
||||
[
|
||||
9.807677828661003e-09,
|
||||
-8.928123497147341e-07,
|
||||
-6.42045681266356e-07,
|
||||
2.4448642118127635e-07,
|
||||
-7.775978286233749e-08,
|
||||
3.809119252748882e-07
|
||||
],
|
||||
[
|
||||
6.656021402664136e-07,
|
||||
8.447874310868029e-07,
|
||||
-7.934435641861794e-07,
|
||||
-7.775978286233639e-08,
|
||||
4.178437168687293e-07,
|
||||
-3.614395664399821e-07
|
||||
],
|
||||
[
|
||||
1.880717329994141e-07,
|
||||
-3.883503279890717e-06,
|
||||
3.08233576295777e-07,
|
||||
3.809119252748862e-07,
|
||||
-3.614395664399844e-07,
|
||||
2.809559727575109e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.08586868930820345,
|
||||
0.16021935571577825,
|
||||
0.24963041613501347
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0004944556817160425,
|
||||
0.000646408320544166,
|
||||
0.001676174134025194
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0028646163342597826,
|
||||
0.0025089246532298257,
|
||||
0.001967934179216506
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2.8646163342597823,
|
||||
2.5089246532298257,
|
||||
1.967934179216506
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.1994418033453169,
|
||||
"pitch": 0.16062277696661237,
|
||||
"yaw": 0.07987033548886921
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
735.25,
|
||||
38.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
735.0857543945312,
|
||||
38.939788818359375
|
||||
],
|
||||
"reprojection_error_px": 0.4694580105501786,
|
||||
"confidence": 0.2985929580100399
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
1005.5,
|
||||
139.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1004.7282104492188,
|
||||
139.58082580566406
|
||||
],
|
||||
"reprojection_error_px": 0.9659284275868637,
|
||||
"confidence": 0.9357112460666233
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
1234.75,
|
||||
235.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1234.0849609375,
|
||||
235.2240447998047
|
||||
],
|
||||
"reprojection_error_px": 0.6655453606389707,
|
||||
"confidence": 0.36770453019575644
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
458.0,
|
||||
68.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
458.2991027832031,
|
||||
69.70124816894531
|
||||
],
|
||||
"reprojection_error_px": 1.2379255382753527,
|
||||
"confidence": 0.8685664374455692
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
731.5,
|
||||
117.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
731.38525390625,
|
||||
117.72209930419922
|
||||
],
|
||||
"reprojection_error_px": 0.485844027498816,
|
||||
"confidence": 0.9524913713047937
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
1128.75,
|
||||
284.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1128.236083984375,
|
||||
284.30267333984375
|
||||
],
|
||||
"reprojection_error_px": 0.5504974858473882,
|
||||
"confidence": 0.9144001450649527
|
||||
},
|
||||
{
|
||||
"marker_id": 59,
|
||||
"observed_center_px": [
|
||||
262.25,
|
||||
123.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
263.9138488769531,
|
||||
125.33772277832031
|
||||
],
|
||||
"reprojection_error_px": 2.479035718842208,
|
||||
"confidence": 0.9376517069038668
|
||||
},
|
||||
{
|
||||
"marker_id": 48,
|
||||
"observed_center_px": [
|
||||
148.5,
|
||||
51.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
149.4849395751953,
|
||||
51.862728118896484
|
||||
],
|
||||
"reprojection_error_px": 1.3093531891436279,
|
||||
"confidence": 0.5517245546325658
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
432.0,
|
||||
147.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
432.3002014160156,
|
||||
149.29176330566406
|
||||
],
|
||||
"reprojection_error_px": 1.5707179826022124,
|
||||
"confidence": 0.9442241264421224
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
1019.75,
|
||||
308.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1019.1754150390625,
|
||||
307.9459228515625
|
||||
],
|
||||
"reprojection_error_px": 0.5771240900522866,
|
||||
"confidence": 0.9297130863840988
|
||||
},
|
||||
{
|
||||
"marker_id": 102,
|
||||
"observed_center_px": [
|
||||
239.25,
|
||||
213.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
240.9429473876953,
|
||||
215.61300659179688
|
||||
],
|
||||
"reprojection_error_px": 2.5173129361648683,
|
||||
"confidence": 0.9498751926981491
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
689.75,
|
||||
280.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
689.872314453125,
|
||||
280.85595703125
|
||||
],
|
||||
"reprojection_error_px": 0.37638575097840954,
|
||||
"confidence": 0.8766456914396368
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
634.75,
|
||||
296.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
634.6029052734375,
|
||||
297.34588623046875
|
||||
],
|
||||
"reprojection_error_px": 0.6137729696270053,
|
||||
"confidence": 0.8625841801430425
|
||||
},
|
||||
{
|
||||
"marker_id": 71,
|
||||
"observed_center_px": [
|
||||
63.5,
|
||||
113.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
64.10832977294922,
|
||||
114.1216812133789
|
||||
],
|
||||
"reprojection_error_px": 0.8698003470479402,
|
||||
"confidence": 0.6962965934908386
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
256.25,
|
||||
273.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
257.945556640625,
|
||||
274.84429931640625
|
||||
],
|
||||
"reprojection_error_px": 2.327381067178508,
|
||||
"confidence": 0.8785732433921414
|
||||
},
|
||||
{
|
||||
"marker_id": 63,
|
||||
"observed_center_px": [
|
||||
36.75,
|
||||
191.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
37.489715576171875,
|
||||
191.14720153808594
|
||||
],
|
||||
"reprojection_error_px": 0.7468243819019208,
|
||||
"confidence": 0.23403229407345516
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
295.25,
|
||||
413.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
288.3956298828125,
|
||||
413.3526916503906
|
||||
],
|
||||
"reprojection_error_px": 6.8658752994838395,
|
||||
"confidence": 0.7372980849807326
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
1059.0,
|
||||
559.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1073.6019287109375,
|
||||
555.7957763671875
|
||||
],
|
||||
"reprojection_error_px": 15.127861931469159,
|
||||
"confidence": 0.6136138322804316
|
||||
},
|
||||
{
|
||||
"marker_id": 217,
|
||||
"observed_center_px": [
|
||||
162.25,
|
||||
532.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
146.7255401611328,
|
||||
520.9512939453125
|
||||
],
|
||||
"reprojection_error_px": 19.348939630537274,
|
||||
"confidence": 0.6320655742155583
|
||||
},
|
||||
{
|
||||
"marker_id": 74,
|
||||
"observed_center_px": [
|
||||
1086.0,
|
||||
734.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1084.8804931640625,
|
||||
735.0576782226562
|
||||
],
|
||||
"reprojection_error_px": 1.1610174177882524,
|
||||
"confidence": 0.49861888008985367
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
1251.5,
|
||||
801.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1249.17626953125,
|
||||
800.1643676757812
|
||||
],
|
||||
"reprojection_error_px": 2.8131749250632194,
|
||||
"confidence": 0.06713952659324364
|
||||
},
|
||||
{
|
||||
"marker_id": 52,
|
||||
"observed_center_px": [
|
||||
1075.25,
|
||||
807.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1073.9066162109375,
|
||||
807.3248291015625
|
||||
],
|
||||
"reprojection_error_px": 1.354756379713829,
|
||||
"confidence": 0.44660041827617547
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
421.5,
|
||||
733.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
422.2747497558594,
|
||||
733.5101318359375
|
||||
],
|
||||
"reprojection_error_px": 0.9276161243969003,
|
||||
"confidence": 0.453656743367513
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
274.0,
|
||||
722.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
274.78045654296875,
|
||||
723.6485595703125
|
||||
],
|
||||
"reprojection_error_px": 1.19017717876916,
|
||||
"confidence": 0.42914121819077033
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
472.5,
|
||||
751.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
472.7325744628906,
|
||||
751.6512451171875
|
||||
],
|
||||
"reprojection_error_px": 0.4637763737575183,
|
||||
"confidence": 0.42705375163400877
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
541.0,
|
||||
783.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
541.320556640625,
|
||||
783.3278198242188
|
||||
],
|
||||
"reprojection_error_px": 0.32986737469810884,
|
||||
"confidence": 0.4239630899600427
|
||||
},
|
||||
{
|
||||
"marker_id": 101,
|
||||
"observed_center_px": [
|
||||
1031.25,
|
||||
900.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1029.7542724609375,
|
||||
900.5302124023438
|
||||
],
|
||||
"reprojection_error_px": 1.4960326401403623,
|
||||
"confidence": 0.3366794154513476
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
427.25,
|
||||
777.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.572998046875,
|
||||
777.9152221679688
|
||||
],
|
||||
"reprojection_error_px": 0.5260581593870906,
|
||||
"confidence": 0.3955545216798782
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
120.0,
|
||||
723.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
122.01116180419922,
|
||||
723.669677734375
|
||||
],
|
||||
"reprojection_error_px": 2.018306799327716,
|
||||
"confidence": 0.4621067158671878
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
894.75,
|
||||
890.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
893.3291625976562,
|
||||
890.5499267578125
|
||||
],
|
||||
"reprojection_error_px": 1.523547952267044,
|
||||
"confidence": 0.37908960001789416
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
613.0,
|
||||
860.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
612.6180419921875,
|
||||
860.841552734375
|
||||
],
|
||||
"reprojection_error_px": 0.39277706514463534,
|
||||
"confidence": 0.3907528879258137
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
497.75,
|
||||
837.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
497.95806884765625,
|
||||
838.1924438476562
|
||||
],
|
||||
"reprojection_error_px": 0.48892658313275084,
|
||||
"confidence": 0.3865272468846743
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
889.25,
|
||||
925.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
887.6505126953125,
|
||||
926.299560546875
|
||||
],
|
||||
"reprojection_error_px": 1.6912647434798707,
|
||||
"confidence": 0.15682847151496923
|
||||
},
|
||||
{
|
||||
"marker_id": 94,
|
||||
"observed_center_px": [
|
||||
29.0,
|
||||
720.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
31.336706161499023,
|
||||
720.2244873046875
|
||||
],
|
||||
"reprojection_error_px": 2.3528924604549735,
|
||||
"confidence": 0.09146699566173024
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
401.25,
|
||||
866.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
401.60943603515625,
|
||||
867.041259765625
|
||||
],
|
||||
"reprojection_error_px": 0.46262999734212534,
|
||||
"confidence": 0.35896437880813437
|
||||
},
|
||||
{
|
||||
"marker_id": 104,
|
||||
"observed_center_px": [
|
||||
107.5,
|
||||
792.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
109.24372100830078,
|
||||
792.2886352539062
|
||||
],
|
||||
"reprojection_error_px": 1.7674482353344945,
|
||||
"confidence": 0.3880719051456888
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
436.5,
|
||||
882.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
436.5618896484375,
|
||||
883.1439819335938
|
||||
],
|
||||
"reprojection_error_px": 0.8961216581333906,
|
||||
"confidence": 0.36026641726859315
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
351.75,
|
||||
880.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
352.1582336425781,
|
||||
880.5147094726562
|
||||
],
|
||||
"reprojection_error_px": 0.4865447685943754,
|
||||
"confidence": 0.3567245846660417
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
254.25,
|
||||
887.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
255.22390747070312,
|
||||
886.9551391601562
|
||||
],
|
||||
"reprojection_error_px": 0.9749401296709685,
|
||||
"confidence": 0.3494656541641164
|
||||
},
|
||||
{
|
||||
"marker_id": 88,
|
||||
"observed_center_px": [
|
||||
199.75,
|
||||
872.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
201.13644409179688,
|
||||
872.6148681640625
|
||||
],
|
||||
"reprojection_error_px": 1.3911944201992585,
|
||||
"confidence": 0.347903013426907
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_172504/cam1_debug.jpg
Normal file
BIN
data/homing/20260625_172504/cam1_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 257 KiB |
BIN
data/homing/20260625_172504/cam2.jpg
Normal file
BIN
data/homing/20260625_172504/cam2.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 220 KiB |
2521
data/homing/20260625_172504/cam2_aruco_detection.json
Normal file
2521
data/homing/20260625_172504/cam2_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
495
data/homing/20260625_172504/cam2_camera_pose.json
Normal file
495
data/homing/20260625_172504/cam2_camera_pose.json
Normal file
@@ -0,0 +1,495 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:25:15Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_172504/cam2_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam2",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1388.99072265625,
|
||||
0.0,
|
||||
933.082763671875
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1394.8729248046875,
|
||||
562.4996948242188
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.019531700760126114,
|
||||
-0.11213663965463638,
|
||||
0.0026758278254419565,
|
||||
0.0007694826927036047,
|
||||
0.05339815095067024
|
||||
]
|
||||
},
|
||||
"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": 21,
|
||||
"used_marker_ids": [
|
||||
85,
|
||||
92,
|
||||
105,
|
||||
54,
|
||||
93,
|
||||
66,
|
||||
217,
|
||||
62,
|
||||
96,
|
||||
95,
|
||||
79,
|
||||
76,
|
||||
103,
|
||||
64,
|
||||
46,
|
||||
90,
|
||||
53,
|
||||
86,
|
||||
84,
|
||||
82,
|
||||
73
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.014228673477920699,
|
||||
0.0012360717022841035,
|
||||
0.0009070175499997285,
|
||||
0.0009070142696966918
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 1.7852212006525887,
|
||||
"residual_median_px": 1.0469264348760319,
|
||||
"residual_max_px": 6.647095648204168,
|
||||
"sigma2_normalized": 9.597873663078857e-07
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.7617526054382324,
|
||||
0.6427322030067444,
|
||||
0.08141452819108963
|
||||
],
|
||||
[
|
||||
0.48559483885765076,
|
||||
-0.4832412004470825,
|
||||
-0.7284748554229736
|
||||
],
|
||||
[
|
||||
-0.42887139320373535,
|
||||
0.5944520831108093,
|
||||
-0.6802176833152771
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.1742696762084961,
|
||||
-0.1401522308588028,
|
||||
1.1940648555755615
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.1767839356074785,
|
||||
0.8396398702141942,
|
||||
-0.2585585732707637
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.7129078507423401,
|
||||
-0.6655329465866089,
|
||||
0.7243147492408752
|
||||
],
|
||||
"position_mm": [
|
||||
712.9078369140625,
|
||||
-665.532958984375,
|
||||
724.3147583007812
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 138.84930419921875,
|
||||
"pitch": 25.395954132080078,
|
||||
"yaw": 32.51630783081055
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
2.4924502189037485e-06,
|
||||
7.284908255672299e-07,
|
||||
2.0024504778922095e-07,
|
||||
6.993863992084647e-08,
|
||||
-3.359070245275866e-07,
|
||||
5.600958340179941e-09
|
||||
],
|
||||
[
|
||||
7.284908255672354e-07,
|
||||
1.064107327675121e-06,
|
||||
-2.3219104124960886e-08,
|
||||
2.319589057300187e-07,
|
||||
-2.846525717659831e-07,
|
||||
1.5012786826699554e-07
|
||||
],
|
||||
[
|
||||
2.0024504778920997e-07,
|
||||
-2.3219104124972556e-08,
|
||||
4.278959925593973e-06,
|
||||
-1.4661732034947215e-07,
|
||||
-6.23228741847165e-07,
|
||||
-1.5703080494058352e-06
|
||||
],
|
||||
[
|
||||
6.993863992084778e-08,
|
||||
2.3195890573001912e-07,
|
||||
-1.4661732034946982e-07,
|
||||
1.0962701995068561e-07,
|
||||
-2.8204197612513988e-08,
|
||||
1.4125379662533936e-07
|
||||
],
|
||||
[
|
||||
-3.359070245275866e-07,
|
||||
-2.8465257176598095e-07,
|
||||
-6.232287418471685e-07,
|
||||
-2.8204197612513405e-08,
|
||||
2.305112527858795e-07,
|
||||
2.649685210729187e-07
|
||||
],
|
||||
[
|
||||
5.600958340183888e-09,
|
||||
1.5012786826699972e-07,
|
||||
-1.5703080494058346e-06,
|
||||
1.4125379662534013e-07,
|
||||
2.6496852107291726e-07,
|
||||
1.078572601871589e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.09045568752546955,
|
||||
0.05910379253809281,
|
||||
0.11852002706372318
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0003310997130030251,
|
||||
0.00048011587433231107,
|
||||
0.0010385435002307746
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0015926294915772563,
|
||||
0.001282414773137875,
|
||||
0.0014297273307869451
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
1.5926294915772563,
|
||||
1.282414773137875,
|
||||
1.4297273307869451
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.12744173458500993,
|
||||
"pitch": 0.08524351737408967,
|
||||
"yaw": 0.06595345807707714
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
943.25,
|
||||
1039.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
943.3576049804688,
|
||||
1037.9586181640625
|
||||
],
|
||||
"reprojection_error_px": 1.0469264348760319,
|
||||
"confidence": 0.25017342512163626
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
1262.25,
|
||||
1041.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1262.399658203125,
|
||||
1040.8380126953125
|
||||
],
|
||||
"reprojection_error_px": 0.9241852745384849,
|
||||
"confidence": 0.18638720024967376
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1019.5,
|
||||
1007.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1019.3496704101562,
|
||||
1006.0000610351562
|
||||
],
|
||||
"reprojection_error_px": 1.2589465443049024,
|
||||
"confidence": 0.7893291944675445
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
726.75,
|
||||
892.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
727.0918579101562,
|
||||
891.9924926757812
|
||||
],
|
||||
"reprojection_error_px": 0.4279916503422691,
|
||||
"confidence": 0.712278812924601
|
||||
},
|
||||
{
|
||||
"marker_id": 93,
|
||||
"observed_center_px": [
|
||||
1893.5,
|
||||
971.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1894.048095703125,
|
||||
971.9451904296875
|
||||
],
|
||||
"reprojection_error_px": 0.70611855836639,
|
||||
"confidence": 0.042321015168188404
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
548.25,
|
||||
804.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
549.4153442382812,
|
||||
803.2486572265625
|
||||
],
|
||||
"reprojection_error_px": 1.5364616961092168,
|
||||
"confidence": 0.5883494177188935
|
||||
},
|
||||
{
|
||||
"marker_id": 217,
|
||||
"observed_center_px": [
|
||||
1527.25,
|
||||
916.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1529.0672607421875,
|
||||
922.8938598632812
|
||||
],
|
||||
"reprojection_error_px": 6.647095648204168,
|
||||
"confidence": 0.45290577054800996
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
962.0,
|
||||
802.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
961.2557373046875,
|
||||
802.291748046875
|
||||
],
|
||||
"reprojection_error_px": 0.7728491674409725,
|
||||
"confidence": 0.6057547967936824
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
912.25,
|
||||
783.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
911.0736694335938,
|
||||
783.0982055664062
|
||||
],
|
||||
"reprojection_error_px": 1.18608395635878,
|
||||
"confidence": 0.5545391608146377
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
630.5,
|
||||
709.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
631.0589599609375,
|
||||
709.0905151367188
|
||||
],
|
||||
"reprojection_error_px": 0.6929026563578145,
|
||||
"confidence": 0.4695004591117706
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
875.75,
|
||||
715.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
875.3380126953125,
|
||||
715.4541625976562
|
||||
],
|
||||
"reprojection_error_px": 0.6131861090513578,
|
||||
"confidence": 0.5142115324222218
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
1551.75,
|
||||
746.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1552.177490234375,
|
||||
744.9830322265625
|
||||
],
|
||||
"reprojection_error_px": 1.576051752365359,
|
||||
"confidence": 0.29593493306278906
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
648.5,
|
||||
590.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
648.8373413085938,
|
||||
590.6143798828125
|
||||
],
|
||||
"reprojection_error_px": 0.7009007054415375,
|
||||
"confidence": 0.39305998326235797
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
541.0,
|
||||
511.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
541.9498901367188,
|
||||
512.197021484375
|
||||
],
|
||||
"reprojection_error_px": 1.3413206043684687,
|
||||
"confidence": 0.3099175748319284
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
1382.0,
|
||||
629.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1381.5179443359375,
|
||||
628.320556640625
|
||||
],
|
||||
"reprojection_error_px": 0.8330791930264032,
|
||||
"confidence": 0.26236530151152454
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
1573.0,
|
||||
614.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1573.05712890625,
|
||||
612.7874145507812
|
||||
],
|
||||
"reprojection_error_px": 1.4637007577355878,
|
||||
"confidence": 0.21907023701319386
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
1341.0,
|
||||
580.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1340.56494140625,
|
||||
579.7637939453125
|
||||
],
|
||||
"reprojection_error_px": 0.49504472552160556,
|
||||
"confidence": 0.22146320976627604
|
||||
},
|
||||
{
|
||||
"marker_id": 86,
|
||||
"observed_center_px": [
|
||||
1260.75,
|
||||
466.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1259.335205078125,
|
||||
466.6405944824219
|
||||
],
|
||||
"reprojection_error_px": 1.4217635103809503,
|
||||
"confidence": 0.19461123347835588
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
1285.5,
|
||||
508.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1284.366455078125,
|
||||
507.854248046875
|
||||
],
|
||||
"reprojection_error_px": 1.200643035340168,
|
||||
"confidence": 0.17943691614102802
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
1130.0,
|
||||
393.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1129.2742919921875,
|
||||
393.4766540527344
|
||||
],
|
||||
"reprojection_error_px": 0.7260834289920938,
|
||||
"confidence": 0.18215071600887703
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
1154.0,
|
||||
379.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1152.795654296875,
|
||||
379.095703125
|
||||
],
|
||||
"reprojection_error_px": 1.2081422353226527,
|
||||
"confidence": 0.17728829216377978
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_172504/cam2_debug.jpg
Normal file
BIN
data/homing/20260625_172504/cam2_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 353 KiB |
2487
data/homing/20260625_172504/robot_1781069752019.json
Normal file
2487
data/homing/20260625_172504/robot_1781069752019.json
Normal file
File diff suppressed because it is too large
Load Diff
59
data/homing/20260625_172504/robot_state.json
Normal file
59
data/homing/20260625_172504/robot_state.json
Normal file
@@ -0,0 +1,59 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:25:19Z",
|
||||
"method": "hybrid",
|
||||
"seeded": true,
|
||||
"movements": {
|
||||
"x": {
|
||||
"value": 162.42504894251783,
|
||||
"unit": "mm",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 4
|
||||
},
|
||||
"y": {
|
||||
"value": -1.169420311588133,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 4
|
||||
},
|
||||
"z": {
|
||||
"value": 98.46924561003269,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 1
|
||||
},
|
||||
"a": {
|
||||
"value": 89.51769789582495,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 4
|
||||
},
|
||||
"b": {
|
||||
"value": -47.89793601064412,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
},
|
||||
"c": {
|
||||
"value": -64.66160343747586,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
},
|
||||
"e": {
|
||||
"value": 15.8937800585978,
|
||||
"unit": "mm",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
}
|
||||
},
|
||||
"residual_rms": 22.202829667894964,
|
||||
"num_markers": 43
|
||||
}
|
||||
105
data/homing/20260625_172504/state_Arm1.json
Normal file
105
data/homing/20260625_172504/state_Arm1.json
Normal file
@@ -0,0 +1,105 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm1",
|
||||
"joint": "y",
|
||||
"method": "primary",
|
||||
"joint_origin_world_mm": [
|
||||
216.50906842923365,
|
||||
108.3968,
|
||||
46.3163
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 8.73396517409613,
|
||||
"circular_variance": 0.07161679045871494,
|
||||
"circular_std_deg": 22.088350114431325,
|
||||
"num_pairs_used": 6,
|
||||
"num_markers_matched": 4,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
198
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 2.7136316277566275,
|
||||
"baseline_model_mm": 120.70271952197267,
|
||||
"baseline_obs_mm": 235.58340824705388,
|
||||
"weight": 28435.558049674528
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 36.09512656392027,
|
||||
"baseline_model_mm": 63.35571402801804,
|
||||
"baseline_obs_mm": 159.67340571245236,
|
||||
"weight": 10116.222630197835
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 88.90480962311312,
|
||||
"baseline_model_mm": 34.32559540634365,
|
||||
"baseline_obs_mm": 110.45934908729735,
|
||||
"weight": 3791.582925618644
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
198,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -3.440668721283733,
|
||||
"baseline_model_mm": 89.99999999999997,
|
||||
"baseline_obs_mm": 88.29072557685865,
|
||||
"weight": 7946.165301917275
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
198,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -2.7040713039960176,
|
||||
"baseline_model_mm": 129.8075498574717,
|
||||
"baseline_obs_mm": 129.20056719980755,
|
||||
"weight": 16771.209068402644
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
229,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -0.7105184107327543,
|
||||
"baseline_model_mm": 49.49747468305837,
|
||||
"baseline_obs_mm": 49.40955380679017,
|
||||
"weight": 2445.6481386528067
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 106.50906842923364,
|
||||
"y": 8.73396517409613
|
||||
}
|
||||
}
|
||||
101
data/homing/20260625_172504/state_Arm2.json
Normal file
101
data/homing/20260625_172504/state_Arm2.json
Normal file
@@ -0,0 +1,101 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm2",
|
||||
"joint": "a",
|
||||
"method": "primary",
|
||||
"joint_origin_world_mm": [
|
||||
306.50906842923365,
|
||||
-138.70421127250364,
|
||||
84.27799422070137
|
||||
],
|
||||
"joint_axis_world": [
|
||||
0.0,
|
||||
0.13361549880725562,
|
||||
0.9910332479177922
|
||||
],
|
||||
"mean_angle_deg": 93.37084948672933,
|
||||
"circular_variance": 0.0007324693649812808,
|
||||
"circular_std_deg": 2.193370075603702,
|
||||
"num_pairs_used": 4,
|
||||
"num_markers_matched": 4,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
144
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 89.94688841244773,
|
||||
"baseline_model_mm": 26.68445427585132,
|
||||
"baseline_obs_mm": 24.643765187064083,
|
||||
"weight": 657.6054253190281
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
146
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": true,
|
||||
"reason": "bl_model=0.3 bl_obs=2.8 < 15.0"
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 93.80406057538599,
|
||||
"baseline_model_mm": 26.836171485515575,
|
||||
"baseline_obs_mm": 28.679452701596173,
|
||||
"weight": 769.6467108107678
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
146
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 92.82624404009415,
|
||||
"baseline_model_mm": 26.791733799812224,
|
||||
"baseline_obs_mm": 27.197454261300408,
|
||||
"weight": 728.6669546013292
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": true,
|
||||
"reason": "bl_model=0.4 bl_obs=4.3 < 15.0"
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
146,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 96.11666161271195,
|
||||
"baseline_model_mm": 26.946706663338293,
|
||||
"baseline_obs_mm": 31.282017166008377,
|
||||
"weight": 842.9473404099408
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 106.50906842923364,
|
||||
"y": 8.73396517409613,
|
||||
"z": 88.94460334069886,
|
||||
"a": 93.37084948672933
|
||||
}
|
||||
}
|
||||
54
data/homing/20260625_172504/state_Ellbow.json
Normal file
54
data/homing/20260625_172504/state_Ellbow.json
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Ellbow",
|
||||
"joint": "z",
|
||||
"method": "fallback_1_child_axis",
|
||||
"joint_origin_world_mm": [
|
||||
216.50906842923365,
|
||||
-138.70421127250364,
|
||||
84.27799422070137
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 88.94460334069886,
|
||||
"circular_variance": 5.079874864089007e-07,
|
||||
"circular_std_deg": 0.05775162282873076,
|
||||
"num_pairs_used": 2,
|
||||
"num_markers_matched": 1,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
146
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "fallback_1_child_axis",
|
||||
"skipped": false,
|
||||
"angle_deg": 89.03275897168004,
|
||||
"baseline_model_mm": 70.00000071428568,
|
||||
"baseline_obs_mm": 70.28215356251216,
|
||||
"weight": 4919.750799577387
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "fallback_1_child_axis",
|
||||
"skipped": false,
|
||||
"angle_deg": 88.90676969215383,
|
||||
"baseline_model_mm": 106.99999999999999,
|
||||
"baseline_obs_mm": 107.1349128404376,
|
||||
"weight": 11463.43567392682
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 106.50906842923364,
|
||||
"y": 8.73396517409613,
|
||||
"z": 88.94460334069886
|
||||
}
|
||||
}
|
||||
61
data/homing/20260625_175916/aruco_marker_poses.csv
Normal file
61
data/homing/20260625_175916/aruco_marker_poses.csv
Normal file
@@ -0,0 +1,61 @@
|
||||
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
|
||||
0,unknown,,3,505.79,-100.06,-8.17,0.00654,-0.01478,0.99987,,,,,,23.71
|
||||
46,Board,A0,2,537.9,185.52,-27.5,0.0061,0.00117,0.99998,537.44,185.2,-27.2,0.638,-0.305,23.83
|
||||
47,Board,A0,3,343.34,-286.18,-27.52,-0.00826,0.02432,0.99967,343.18,-286.05,-27.49,0.21,-0.034,24.1
|
||||
50,Board,A0,3,574.64,211.77,-27.34,0.00242,0.0109,0.99994,574.23,211.48,-27.15,0.539,-0.186,24.07
|
||||
51,Board,A0,3,166.89,-171.06,-27.49,-0.04142,0.00081,0.99914,167.18,-170.93,-27.76,0.417,0.272,24.2
|
||||
53,Board,A0,3,487.16,212.93,-27.58,0.06677,0.02171,0.99753,487.08,212.19,-27.28,0.798,-0.296,23.92
|
||||
54,Board,A0,3,341.31,-330.18,-27.55,-0.01014,0.01262,0.99987,341.05,-330.09,-27.5,0.275,-0.049,24.21
|
||||
55,Arm1,A0,3,282.71,-261.9,-26.63,-0.03413,0.00638,0.9994,,,,,,24.37
|
||||
56,Arm1,A0,2,500.02,169.42,-27.88,-0.01336,0.00216,0.99991,,,,,,23.7
|
||||
58,Board,A0,3,48.69,-216.76,-27.87,-0.0193,0.00188,0.99981,49.3,-216.52,-27.93,0.658,0.059,24.44
|
||||
60,Board,A0,2,435.69,286.11,-29.34,-0.04419,0.06927,0.99662,435.46,283.95,-27.36,2.937,-1.981,24.83
|
||||
62,Board,A0,3,404.22,-174.75,-27.14,0.00893,-0.00881,0.99992,404.07,-174.84,-27.4,0.313,0.259,23.96
|
||||
64,Board,A0,2,-22.33,-187.15,-26.56,0.02977,0.00062,0.99956,-21.95,-186.37,-28.04,1.719,1.484,23.66
|
||||
66,Board,A0,2,207.97,-362.78,-28.65,-0.08,0.0928,0.99247,208.41,-362.24,-27.7,1.18,-0.951,25.14
|
||||
67,Board,A0,2,524.61,268.02,-28.51,-0.0163,-0.00123,0.99987,524.1,266.85,-27.22,1.818,-1.29,23.6
|
||||
68,Board,A0,3,575.05,170.15,-27.14,0.01722,0.03205,0.99934,574.4,170.26,-27.15,0.663,0.006,24.1
|
||||
69,Board,A0,2,6.91,-280.99,-27.23,-0.00089,0.03112,0.99952,6.58,-279.46,-28,1.744,0.77,24.07
|
||||
70,Board,A0,2,603.56,300.57,-28.34,-0.06715,0.04327,0.9968,603.03,299.84,-27.11,1.527,-1.233,23.99
|
||||
73,Board,A0,2,222.01,337.1,-29.9,-0.0437,0.11256,0.99268,223.01,334.05,-27.67,3.914,-2.235,25.6
|
||||
75,Board,A0,2,-27.07,199.16,-28.78,-0.06027,0.14192,0.98804,-24.94,196.46,-28.04,3.523,-0.736,26.71
|
||||
76,Board,A0,2,686.17,164.68,-26.47,-0.08323,0.00998,0.99648,685.86,166,-26.98,1.45,0.514,23.85
|
||||
77,Arm1,A0,2,17.53,194.86,-28.25,-0.02333,0.14876,0.9886,,,,,,26.07
|
||||
79,Board,A0,2,311.53,-157.96,-26.95,0.02919,-0.01163,0.99951,311.73,-158.5,-27.54,0.827,0.59,24.03
|
||||
82,Board,A0,2,219.4,300.84,-30.04,0.00365,0.102,0.99478,220.31,298.19,-27.68,3.663,-2.363,25.25
|
||||
85,Board,A0,3,504.05,-313.36,-27.37,-0.00147,-0.03538,0.99937,503.43,-312.87,-27.25,0.799,-0.124,24.1
|
||||
88,Board,A0,2,767.2,313.41,-25.61,0.05089,-0.00066,0.9987,767.09,314.94,-26.86,1.978,1.252,23.68
|
||||
90,Board,A0,2,644.9,315.86,-27.76,-0.00476,-0.03483,0.99938,644.39,315.79,-27.04,0.882,-0.718,23.64
|
||||
91,Board,A0,2,725.06,326.79,-26.9,-0.04318,-0.00677,0.99904,724.61,327.11,-26.92,0.553,0.023,23.76
|
||||
92,Board,A0,2,644.97,-186.59,-26.03,0.03072,-0.01393,0.99943,644.42,-185.49,-27.04,1.592,1.007,24.18
|
||||
94,Board,A0,2,875.22,168.95,-22.69,0.07264,0.05485,0.99585,876.38,172.13,-26.7,5.244,4.006,24.44
|
||||
95,Board,A0,3,184.79,-273.16,-27.83,-0.00258,0.02727,0.99962,185.04,-272.99,-27.73,0.315,-0.097,24.35
|
||||
96,Board,A0,3,369.18,-185.71,-27.51,0.02262,0.01808,0.99958,369.1,-186.1,-27.46,0.4,-0.049,24.16
|
||||
97,Board,A0,2,303.36,-359.54,-26.35,-0.00502,0.02288,0.99973,303.02,-359.03,-27.55,1.344,1.197,24.46
|
||||
98,Board,A0,2,577.15,315.74,-29.24,-0.02274,0.04433,0.99876,576.48,314.67,-27.15,2.445,-2.094,24.33
|
||||
100,Board,A0,2,803.8,169.01,-24.93,0.01883,-0.01657,0.99969,803.92,171.12,-26.81,2.829,1.882,23.24
|
||||
103,Board,A0,3,104.64,-186.37,-27.12,-0.00058,0.0069,0.99998,105.03,-186.33,-27.85,0.825,0.726,24.45
|
||||
104,Board,A0,2,827.17,235.41,-24.77,0.08284,0.04199,0.99568,827.64,237.83,-26.77,3.174,1.996,24.77
|
||||
105,Board,A0,3,524.5,-266.88,-27.45,-0.00083,0.01085,0.99994,523.86,-266.44,-27.22,0.811,-0.234,24.19
|
||||
118,unknown,,3,323.07,-174.22,47.26,0.01395,-0.99423,0.10635,,,,,,24.41
|
||||
122,Ellbow,,3,360.07,-173.45,46.18,0.01114,-0.99429,0.10615,,,,,,24.51
|
||||
143,Arm2,,2,339.81,-138.06,229.48,-0.73183,-0.67663,0.08126,,,,,,24.18
|
||||
144,Arm2,,3,362.95,-156.86,158.05,-0.03961,-0.99309,0.11048,,,,,,24.37
|
||||
147,FingerA,,3,382.82,-143.67,226.6,0.42827,-0.9012,0.06646,,,,,,24.01
|
||||
148,Arm2,,3,366.84,-142.43,264.66,-0.04437,-0.99493,0.09027,,,,,,24.39
|
||||
178,FingerB,,2,287.31,-120.45,315.29,-0.66387,-0.73035,-0.1608,,,,,,22.88
|
||||
179,FingerB,,2,329.61,-140.32,313.07,-0.6954,-0.15177,0.70241,,,,,,26.39
|
||||
198,Arm1,,2,268.82,-53.79,84.52,0.01757,0.02587,0.99951,,,,,,24.56
|
||||
200,unknown,,2,199.61,-28.62,110.03,-0.06794,-0.01026,0.99764,,,,,,23.25
|
||||
204,unknown,,2,198.89,116.09,120.31,0.05577,0.03761,0.99774,,,,,,24.15
|
||||
208,Board,rail,3,626.32,-98.56,-6.78,0.00383,-0.01334,0.9999,631.01,-98.43,-7.71,4.781,0.93,23.72
|
||||
210,Board,rail,2,129.76,-7.26,-6.27,0.028,0.01002,0.99956,122.63,-13.98,-0.72,11.263,-5.553,23.35
|
||||
214,unknown,,3,532.15,-7.88,-7.33,-0.02081,0.05531,0.99825,,,,,,24.05
|
||||
217,Board,rail,2,730.24,-8.43,-5.0,-0.01833,-0.01881,0.99966,732.39,-23.88,7.39,19.917,-12.387,23.61
|
||||
229,Arm1,,3,271.15,-142.19,79.27,0.01827,-0.03009,0.99938,,,,,,24.04
|
||||
243,Arm1,,3,270.52,-175.76,42.77,0.0038,-0.99933,-0.03654,,,,,,24.16
|
||||
|
||||
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
|
||||
cam0,335.07,-885.3,469.64,-0.06533,0.89189,-0.44751
|
||||
cam1,297.15,-462.72,780.65,0.09465,0.43305,-0.89639
|
||||
cam2,714.94,-666.29,727.22,-0.42921,0.59338,-0.68094
|
||||
|
2542
data/homing/20260625_175916/aruco_marker_poses.json
Normal file
2542
data/homing/20260625_175916/aruco_marker_poses.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
data/homing/20260625_175916/cam0.jpg
Normal file
BIN
data/homing/20260625_175916/cam0.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 103 KiB |
2628
data/homing/20260625_175916/cam0_aruco_detection.json
Normal file
2628
data/homing/20260625_175916/cam0_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
481
data/homing/20260625_175916/cam0_camera_pose.json
Normal file
481
data/homing/20260625_175916/cam0_camera_pose.json
Normal file
@@ -0,0 +1,481 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:59:21Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_175916/cam0_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam0",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1424.7584228515625,
|
||||
0.0,
|
||||
635.95947265625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1421.5770263671875,
|
||||
482.1744384765625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.05634751915931702,
|
||||
0.33765655755996704,
|
||||
0.002130246954038739,
|
||||
-0.004022662527859211,
|
||||
-1.182201862335205
|
||||
]
|
||||
},
|
||||
"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": 20,
|
||||
"used_marker_ids": [
|
||||
97,
|
||||
66,
|
||||
85,
|
||||
54,
|
||||
105,
|
||||
69,
|
||||
47,
|
||||
95,
|
||||
58,
|
||||
64,
|
||||
103,
|
||||
96,
|
||||
62,
|
||||
51,
|
||||
79,
|
||||
208,
|
||||
210,
|
||||
68,
|
||||
50,
|
||||
53
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011436738437523918,
|
||||
0.0017627388219078695,
|
||||
0.0017082741597643208,
|
||||
0.0017082738298200922
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 3.4895581122703203,
|
||||
"residual_median_px": 1.3632084656881505,
|
||||
"residual_max_px": 12.179917236270976,
|
||||
"sigma2_normalized": 3.433175856049273e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.99742591381073,
|
||||
0.04507605358958244,
|
||||
-0.055764634162187576
|
||||
],
|
||||
[
|
||||
-0.029563840478658676,
|
||||
-0.4500020146369934,
|
||||
-0.8925380110740662
|
||||
],
|
||||
[
|
||||
-0.06532628834247589,
|
||||
0.891889214515686,
|
||||
-0.44751107692718506
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.26810967922210693,
|
||||
0.0306912399828434,
|
||||
1.021647572517395
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.035801112416453,
|
||||
0.010908616696792009,
|
||||
-0.08515448496251522
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.33506736159324646,
|
||||
-0.8853000402450562,
|
||||
0.46964067220687866
|
||||
],
|
||||
"position_mm": [
|
||||
335.0673522949219,
|
||||
-885.300048828125,
|
||||
469.64068603515625
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 116.64550018310547,
|
||||
"pitch": 3.745587110519409,
|
||||
"yaw": -1.6977574825286865
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
1.0402983243682025e-05,
|
||||
9.607061849723336e-07,
|
||||
3.175419172829312e-06,
|
||||
-1.413862159845341e-07,
|
||||
-2.12955432250041e-06,
|
||||
2.312142235999549e-07
|
||||
],
|
||||
[
|
||||
9.607061849723307e-07,
|
||||
3.7788686477185887e-06,
|
||||
-1.6924039364567714e-07,
|
||||
5.562539816390373e-07,
|
||||
-8.841983839385075e-07,
|
||||
6.874403655011122e-07
|
||||
],
|
||||
[
|
||||
3.175419172829249e-06,
|
||||
-1.692403936456681e-07,
|
||||
1.3583347408234157e-05,
|
||||
-9.175016077988046e-07,
|
||||
-2.2808655354386924e-06,
|
||||
-3.2773776598692575e-06
|
||||
],
|
||||
[
|
||||
-1.4138621598453382e-07,
|
||||
5.562539816390341e-07,
|
||||
-9.175016077987983e-07,
|
||||
2.6164306620048117e-07,
|
||||
4.0225581275842607e-08,
|
||||
3.496020393352873e-07
|
||||
],
|
||||
[
|
||||
-2.1295543225004017e-06,
|
||||
-8.841983839385094e-07,
|
||||
-2.280865535438699e-06,
|
||||
4.02255812758387e-08,
|
||||
9.903676474491735e-07,
|
||||
6.66954432381659e-07
|
||||
],
|
||||
[
|
||||
2.3121422359997266e-07,
|
||||
6.874403655011114e-07,
|
||||
-3.277377659869258e-06,
|
||||
3.4960203933526876e-07,
|
||||
6.669544323816537e-07,
|
||||
3.1141328985088255e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.18479983617802564,
|
||||
0.11137905544845887,
|
||||
0.2111669934581791
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0005115105729117251,
|
||||
0.0009951721697521356,
|
||||
0.0017646905956877612
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0026703652741227927,
|
||||
0.0018374002535456223,
|
||||
0.0027539784841622054
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2.670365274122793,
|
||||
1.8374002535456224,
|
||||
2.7539784841622055
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.1722531066068174,
|
||||
"pitch": 0.1564002711188888,
|
||||
"yaw": 0.12201801252145385
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
676.0,
|
||||
910.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
675.7621459960938,
|
||||
911.7245483398438
|
||||
],
|
||||
"reprojection_error_px": 1.247434633072338,
|
||||
"confidence": 0.42385670146087856
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
479.5,
|
||||
922.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
480.50054931640625,
|
||||
919.6788330078125
|
||||
],
|
||||
"reprojection_error_px": 2.7589850735869397,
|
||||
"confidence": 0.27078087716396265
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
1079.0,
|
||||
843.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1076.984619140625,
|
||||
842.904296875
|
||||
],
|
||||
"reprojection_error_px": 2.1856289218368885,
|
||||
"confidence": 0.6759853525830334
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
753.5,
|
||||
868.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
752.8966064453125,
|
||||
868.8074340820312
|
||||
],
|
||||
"reprojection_error_px": 0.606120825922678,
|
||||
"confidence": 0.6610788202218779
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1098.25,
|
||||
784.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1095.7879638671875,
|
||||
782.9950561523438
|
||||
],
|
||||
"reprojection_error_px": 2.6592356150248286,
|
||||
"confidence": 0.5825558453258135
|
||||
},
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
130.5,
|
||||
818.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
131.20993041992188,
|
||||
816.9083862304688
|
||||
],
|
||||
"reprojection_error_px": 1.302160444400257,
|
||||
"confidence": 0.5841161997457144
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
755.0,
|
||||
810.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
754.5029907226562,
|
||||
810.5115966796875
|
||||
],
|
||||
"reprojection_error_px": 0.4971445511574386,
|
||||
"confidence": 0.5487375886524822
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
461.0,
|
||||
800.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
461.86224365234375,
|
||||
800.0243530273438
|
||||
],
|
||||
"reprojection_error_px": 0.8912803555986298,
|
||||
"confidence": 0.5215660558894334
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
243.25,
|
||||
743.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
244.70860290527344,
|
||||
742.6357421875
|
||||
],
|
||||
"reprojection_error_px": 1.5033982137941357,
|
||||
"confidence": 0.468912299986827
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
139.5,
|
||||
714.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
141.43846130371094,
|
||||
715.3218383789062
|
||||
],
|
||||
"reprojection_error_px": 2.346249970897008,
|
||||
"confidence": 0.4334442420959473
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
350.5,
|
||||
708.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
352.01885986328125,
|
||||
708.9456787109375
|
||||
],
|
||||
"reprojection_error_px": 1.78920191946218,
|
||||
"confidence": 0.3938267188343575
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
793.0,
|
||||
697.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
793.4209594726562,
|
||||
698.4274291992188
|
||||
],
|
||||
"reprojection_error_px": 0.7975695565737103,
|
||||
"confidence": 0.36075364145684324
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
851.25,
|
||||
685.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
851.3250732421875,
|
||||
686.2469482421875
|
||||
],
|
||||
"reprojection_error_px": 0.7507114413671468,
|
||||
"confidence": 0.34135471017020086
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
457.75,
|
||||
690.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
459.0975646972656,
|
||||
691.2110595703125
|
||||
],
|
||||
"reprojection_error_px": 1.4242564869760441,
|
||||
"confidence": 0.3676955439548635
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
695.75,
|
||||
672.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
696.7139892578125,
|
||||
673.8245849609375
|
||||
],
|
||||
"reprojection_error_px": 2.0635856097717857,
|
||||
"confidence": 0.3187317073170732
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
1201.75,
|
||||
583.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1208.861572265625,
|
||||
583.9735107421875
|
||||
],
|
||||
"reprojection_error_px": 7.177895461370356,
|
||||
"confidence": 0.28568227626765064
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
439.0,
|
||||
532.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.0049133300781,
|
||||
530.6361694335938
|
||||
],
|
||||
"reprojection_error_px": 12.179917236270976,
|
||||
"confidence": 0.16251118951659882
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
1027.25,
|
||||
434.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1026.4912109375,
|
||||
434.3568420410156
|
||||
],
|
||||
"reprojection_error_px": 0.7662741435661348,
|
||||
"confidence": 0.09654486228169104
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
1016.5,
|
||||
413.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1016.3207397460938,
|
||||
413.5331726074219
|
||||
],
|
||||
"reprojection_error_px": 0.18230375891269784,
|
||||
"confidence": 0.08377973241313476
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
909.0,
|
||||
416.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
909.6261596679688,
|
||||
416.78497314453125
|
||||
],
|
||||
"reprojection_error_px": 0.6879575734700244,
|
||||
"confidence": 0.07916194626380657
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_175916/cam0_debug.jpg
Normal file
BIN
data/homing/20260625_175916/cam0_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 223 KiB |
BIN
data/homing/20260625_175916/cam1.jpg
Normal file
BIN
data/homing/20260625_175916/cam1.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 110 KiB |
3558
data/homing/20260625_175916/cam1_aruco_detection.json
Normal file
3558
data/homing/20260625_175916/cam1_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
761
data/homing/20260625_175916/cam1_camera_pose.json
Normal file
761
data/homing/20260625_175916/cam1_camera_pose.json
Normal file
@@ -0,0 +1,761 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:59:25Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_175916/cam1_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam1",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1367.5723876953125,
|
||||
0.0,
|
||||
672.1165771484375
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1372.3011474609375,
|
||||
445.8396911621094
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.01016925647854805,
|
||||
0.7656787633895874,
|
||||
-0.0031530377455055714,
|
||||
-0.00288817984983325,
|
||||
-2.490830183029175
|
||||
]
|
||||
},
|
||||
"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": 40,
|
||||
"used_marker_ids": [
|
||||
54,
|
||||
95,
|
||||
58,
|
||||
85,
|
||||
47,
|
||||
103,
|
||||
59,
|
||||
105,
|
||||
48,
|
||||
51,
|
||||
102,
|
||||
96,
|
||||
62,
|
||||
71,
|
||||
92,
|
||||
208,
|
||||
63,
|
||||
210,
|
||||
217,
|
||||
74,
|
||||
75,
|
||||
52,
|
||||
76,
|
||||
68,
|
||||
46,
|
||||
53,
|
||||
50,
|
||||
101,
|
||||
82,
|
||||
100,
|
||||
73,
|
||||
60,
|
||||
67,
|
||||
70,
|
||||
94,
|
||||
90,
|
||||
104,
|
||||
98,
|
||||
91,
|
||||
88
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011863062568042395,
|
||||
0.0021882720514259653,
|
||||
0.0021293863444762583,
|
||||
0.0021293859894700134
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 4.205255463207004,
|
||||
"residual_median_px": 1.1858116613812175,
|
||||
"residual_max_px": 19.118438507028245,
|
||||
"sigma2_normalized": 4.9019293968833265e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.995226263999939,
|
||||
0.019625136628746986,
|
||||
-0.09560120105743408
|
||||
],
|
||||
[
|
||||
-0.023808155208826065,
|
||||
0.9011572599411011,
|
||||
0.4328380525112152
|
||||
],
|
||||
[
|
||||
0.09464621543884277,
|
||||
0.4330478608608246,
|
||||
-0.8963881134986877
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
0.37944144010543823,
|
||||
0.08616430312395096,
|
||||
0.8720222115516663
|
||||
],
|
||||
"rvec_rad": [
|
||||
0.0032728478486759383,
|
||||
-2.9675124187665194,
|
||||
-0.6774800690352322
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.2971478998661041,
|
||||
-0.4627215266227722,
|
||||
0.7806501984596252
|
||||
],
|
||||
"position_mm": [
|
||||
297.14788818359375,
|
||||
-462.7215270996094,
|
||||
780.6502075195312
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 154.2146453857422,
|
||||
"pitch": -5.430957317352295,
|
||||
"yaw": -178.6295928955078
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
2.216368778099703e-06,
|
||||
-1.0420377127470311e-07,
|
||||
-5.846035681663414e-07,
|
||||
9.638753004284797e-09,
|
||||
6.568277931365646e-07,
|
||||
1.8540529905122184e-07
|
||||
],
|
||||
[
|
||||
-1.0420377127470325e-07,
|
||||
7.716359917594955e-06,
|
||||
-8.780258686751835e-07,
|
||||
-8.798584488389978e-07,
|
||||
8.337615617564991e-07,
|
||||
-3.831747509912712e-06
|
||||
],
|
||||
[
|
||||
-5.846035681663423e-07,
|
||||
-8.780258686751325e-07,
|
||||
1.8740126796208265e-05,
|
||||
-6.332974421687337e-07,
|
||||
-7.826466185078999e-07,
|
||||
3.040527671220899e-07
|
||||
],
|
||||
[
|
||||
9.638753004284528e-09,
|
||||
-8.798584488389991e-07,
|
||||
-6.332974421687222e-07,
|
||||
2.409326607990254e-07,
|
||||
-7.666433629844329e-08,
|
||||
3.751828231192777e-07
|
||||
],
|
||||
[
|
||||
6.568277931365642e-07,
|
||||
8.337615617564963e-07,
|
||||
-7.826466185079048e-07,
|
||||
-7.66643362984426e-08,
|
||||
4.122731304646649e-07,
|
||||
-3.5672545335086356e-07
|
||||
],
|
||||
[
|
||||
1.8540529905122184e-07,
|
||||
-3.831747509912709e-06,
|
||||
3.040527671221047e-07,
|
||||
3.7518282311927725e-07,
|
||||
-3.567254533508643e-07,
|
||||
2.7716081204320043e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.08529894195541628,
|
||||
0.15915814568050488,
|
||||
0.24803267374807242
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0004908489185065252,
|
||||
0.0006420849869485074,
|
||||
0.0016648147405738587
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0027539612430056413,
|
||||
0.002145451671454086,
|
||||
0.0019549734255131183
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2.7539612430056413,
|
||||
2.145451671454086,
|
||||
1.9549734255131184
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.19816528973825537,
|
||||
"pitch": 0.158142424860459,
|
||||
"yaw": 0.04543296464811841
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
735.25,
|
||||
38.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
735.1257934570312,
|
||||
38.84978103637695
|
||||
],
|
||||
"reprojection_error_px": 0.6125067811164705,
|
||||
"confidence": 0.29867757253933086
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
1005.25,
|
||||
139.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1004.7445678710938,
|
||||
139.50558471679688
|
||||
],
|
||||
"reprojection_error_px": 0.714896875632619,
|
||||
"confidence": 0.9333333333333333
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
1234.75,
|
||||
235.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1234.0479736328125,
|
||||
235.1490020751953
|
||||
],
|
||||
"reprojection_error_px": 0.7092542569779415,
|
||||
"confidence": 0.36770453019575644
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
458.0,
|
||||
68.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
458.30291748046875,
|
||||
69.59838104248047
|
||||
],
|
||||
"reprojection_error_px": 1.6268315699341729,
|
||||
"confidence": 0.875342401743355
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
731.75,
|
||||
117.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
731.4143676757812,
|
||||
117.63695526123047
|
||||
],
|
||||
"reprojection_error_px": 0.7199729591239207,
|
||||
"confidence": 0.9759139241315488
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
1128.75,
|
||||
284.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1128.21533203125,
|
||||
284.2250061035156
|
||||
],
|
||||
"reprojection_error_px": 0.6012416152520633,
|
||||
"confidence": 0.9144001450649527
|
||||
},
|
||||
{
|
||||
"marker_id": 59,
|
||||
"observed_center_px": [
|
||||
262.0,
|
||||
123.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
263.8606872558594,
|
||||
125.23478698730469
|
||||
],
|
||||
"reprojection_error_px": 2.5439424041120047,
|
||||
"confidence": 0.9428090418229809
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
432.25,
|
||||
147.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
432.2873840332031,
|
||||
149.19923400878906
|
||||
],
|
||||
"reprojection_error_px": 1.449716102610904,
|
||||
"confidence": 0.9442241264421224
|
||||
},
|
||||
{
|
||||
"marker_id": 48,
|
||||
"observed_center_px": [
|
||||
148.5,
|
||||
50.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
149.40765380859375,
|
||||
51.741458892822266
|
||||
],
|
||||
"reprojection_error_px": 1.3441823434382303,
|
||||
"confidence": 0.5647167996853729
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
1019.5,
|
||||
307.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1019.1678466796875,
|
||||
307.8690490722656
|
||||
],
|
||||
"reprojection_error_px": 0.49651087191997795,
|
||||
"confidence": 0.8824030145295596
|
||||
},
|
||||
{
|
||||
"marker_id": 102,
|
||||
"observed_center_px": [
|
||||
239.5,
|
||||
213.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
240.8688507080078,
|
||||
215.52737426757812
|
||||
],
|
||||
"reprojection_error_px": 2.24339286525173,
|
||||
"confidence": 0.9331395642876747
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
690.0,
|
||||
280.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
689.8768310546875,
|
||||
280.7818908691406
|
||||
],
|
||||
"reprojection_error_px": 0.12723056481812098,
|
||||
"confidence": 0.9012948266053811
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
634.75,
|
||||
296.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
634.6005249023438,
|
||||
297.2737731933594
|
||||
],
|
||||
"reprojection_error_px": 0.5446844617769289,
|
||||
"confidence": 0.8643569912878776
|
||||
},
|
||||
{
|
||||
"marker_id": 71,
|
||||
"observed_center_px": [
|
||||
63.25,
|
||||
113.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
63.99217224121094,
|
||||
114.01245880126953
|
||||
],
|
||||
"reprojection_error_px": 0.7872129686354927,
|
||||
"confidence": 0.6618010073234457
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
256.25,
|
||||
273.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
257.8680114746094,
|
||||
274.7709045410156
|
||||
],
|
||||
"reprojection_error_px": 2.220610671605798,
|
||||
"confidence": 0.8785732433921414
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
295.25,
|
||||
413.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
288.319091796875,
|
||||
413.3032531738281
|
||||
],
|
||||
"reprojection_error_px": 6.945291300358831,
|
||||
"confidence": 0.7372980849807326
|
||||
},
|
||||
{
|
||||
"marker_id": 63,
|
||||
"observed_center_px": [
|
||||
36.0,
|
||||
190.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
37.349727630615234,
|
||||
191.05543518066406
|
||||
],
|
||||
"reprojection_error_px": 1.3838552404184128,
|
||||
"confidence": 0.23122213258874602
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
1059.0,
|
||||
560.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1073.5731201171875,
|
||||
555.7078857421875
|
||||
],
|
||||
"reprojection_error_px": 15.192039848292012,
|
||||
"confidence": 0.6479471317434429
|
||||
},
|
||||
{
|
||||
"marker_id": 217,
|
||||
"observed_center_px": [
|
||||
162.0,
|
||||
532.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
146.59190368652344,
|
||||
520.931640625
|
||||
],
|
||||
"reprojection_error_px": 19.118438507028245,
|
||||
"confidence": 0.6089399477645175
|
||||
},
|
||||
{
|
||||
"marker_id": 74,
|
||||
"observed_center_px": [
|
||||
1085.75,
|
||||
734.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1084.8079833984375,
|
||||
734.9703979492188
|
||||
],
|
||||
"reprojection_error_px": 0.9674557011249624,
|
||||
"confidence": 0.4941465175073702
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
1251.5,
|
||||
801.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1249.0706787109375,
|
||||
800.055419921875
|
||||
],
|
||||
"reprojection_error_px": 2.8263781642957686,
|
||||
"confidence": 0.07129070123540064
|
||||
},
|
||||
{
|
||||
"marker_id": 52,
|
||||
"observed_center_px": [
|
||||
1075.0,
|
||||
807.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1073.82666015625,
|
||||
807.2386474609375
|
||||
],
|
||||
"reprojection_error_px": 1.1973633531819778,
|
||||
"confidence": 0.4747611026689374
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
273.75,
|
||||
722.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
274.6397705078125,
|
||||
723.6699829101562
|
||||
],
|
||||
"reprojection_error_px": 1.4698814804706541,
|
||||
"confidence": 0.4464638052540792
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
421.5,
|
||||
733.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
422.1742858886719,
|
||||
733.5136108398438
|
||||
],
|
||||
"reprojection_error_px": 0.847618755377099,
|
||||
"confidence": 0.453656743367513
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
472.25,
|
||||
751.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
472.6407165527344,
|
||||
751.65087890625
|
||||
],
|
||||
"reprojection_error_px": 0.41883632713964764,
|
||||
"confidence": 0.4433609532391809
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
541.25,
|
||||
783.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
541.2369384765625,
|
||||
783.3223876953125
|
||||
],
|
||||
"reprojection_error_px": 0.07355665725931064,
|
||||
"confidence": 0.41918904165916515
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
427.0,
|
||||
777.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.4671630859375,
|
||||
777.926025390625
|
||||
],
|
||||
"reprojection_error_px": 0.49922568744740276,
|
||||
"confidence": 0.4069334148740479
|
||||
},
|
||||
{
|
||||
"marker_id": 101,
|
||||
"observed_center_px": [
|
||||
1031.0,
|
||||
900.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1029.668212890625,
|
||||
900.4525146484375
|
||||
],
|
||||
"reprojection_error_px": 1.3470965397955643,
|
||||
"confidence": 0.34672467375183735
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
894.5,
|
||||
890.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
893.2540283203125,
|
||||
890.4962158203125
|
||||
],
|
||||
"reprojection_error_px": 1.2700660048814185,
|
||||
"confidence": 0.39232527116476396
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
120.0,
|
||||
723.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
121.81710815429688,
|
||||
723.7096557617188
|
||||
],
|
||||
"reprojection_error_px": 1.8291630826238716,
|
||||
"confidence": 0.4207088246724057
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
889.0,
|
||||
926.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
887.5708618164062,
|
||||
926.2489624023438
|
||||
],
|
||||
"reprojection_error_px": 1.4291385602573807,
|
||||
"confidence": 0.1469791980355697
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
613.0,
|
||||
860.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
612.533935546875,
|
||||
860.8355102539062
|
||||
],
|
||||
"reprojection_error_px": 0.5742675377756796,
|
||||
"confidence": 0.36564926628565203
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
497.75,
|
||||
838.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
497.8586120605469,
|
||||
838.2026977539062
|
||||
],
|
||||
"reprojection_error_px": 0.22996295165716743,
|
||||
"confidence": 0.363884422773956
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
401.25,
|
||||
866.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
401.48388671875,
|
||||
867.0733032226562
|
||||
],
|
||||
"reprojection_error_px": 0.39903379679866513,
|
||||
"confidence": 0.35896437880813437
|
||||
},
|
||||
{
|
||||
"marker_id": 94,
|
||||
"observed_center_px": [
|
||||
29.0,
|
||||
720.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
31.11460304260254,
|
||||
720.2709350585938
|
||||
],
|
||||
"reprojection_error_px": 2.1147066710213593,
|
||||
"confidence": 0.09031730482356817
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
351.5,
|
||||
880.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
352.0174560546875,
|
||||
880.55810546875
|
||||
],
|
||||
"reprojection_error_px": 0.6022372857988869,
|
||||
"confidence": 0.36026641726859315
|
||||
},
|
||||
{
|
||||
"marker_id": 104,
|
||||
"observed_center_px": [
|
||||
107.5,
|
||||
792.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
109.0373764038086,
|
||||
792.3465576171875
|
||||
],
|
||||
"reprojection_error_px": 1.540405654502856,
|
||||
"confidence": 0.40599116793083023
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
436.5,
|
||||
882.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
436.4425048828125,
|
||||
883.1728515625
|
||||
],
|
||||
"reprojection_error_px": 1.174259969580457,
|
||||
"confidence": 0.35391110957760685
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
254.25,
|
||||
887.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
255.05325317382812,
|
||||
887.015869140625
|
||||
],
|
||||
"reprojection_error_px": 0.8034099146071898,
|
||||
"confidence": 0.3494656541641164
|
||||
},
|
||||
{
|
||||
"marker_id": 88,
|
||||
"observed_center_px": [
|
||||
199.5,
|
||||
873.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
200.95053100585938,
|
||||
872.6802978515625
|
||||
],
|
||||
"reprojection_error_px": 1.4853449642002237,
|
||||
"confidence": 0.33862598740745903
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_175916/cam1_debug.jpg
Normal file
BIN
data/homing/20260625_175916/cam1_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 260 KiB |
BIN
data/homing/20260625_175916/cam2.jpg
Normal file
BIN
data/homing/20260625_175916/cam2.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 282 KiB |
3092
data/homing/20260625_175916/cam2_aruco_detection.json
Normal file
3092
data/homing/20260625_175916/cam2_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
761
data/homing/20260625_175916/cam2_camera_pose.json
Normal file
761
data/homing/20260625_175916/cam2_camera_pose.json
Normal file
@@ -0,0 +1,761 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:59:28Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_175916/cam2_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam2",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1388.99072265625,
|
||||
0.0,
|
||||
933.082763671875
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1394.8729248046875,
|
||||
562.4996948242188
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.019531700760126114,
|
||||
-0.11213663965463638,
|
||||
0.0026758278254419565,
|
||||
0.0007694826927036047,
|
||||
0.05339815095067024
|
||||
]
|
||||
},
|
||||
"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": 40,
|
||||
"used_marker_ids": [
|
||||
85,
|
||||
92,
|
||||
105,
|
||||
97,
|
||||
54,
|
||||
208,
|
||||
217,
|
||||
47,
|
||||
93,
|
||||
66,
|
||||
62,
|
||||
96,
|
||||
94,
|
||||
95,
|
||||
79,
|
||||
100,
|
||||
104,
|
||||
76,
|
||||
69,
|
||||
103,
|
||||
51,
|
||||
58,
|
||||
68,
|
||||
50,
|
||||
88,
|
||||
46,
|
||||
91,
|
||||
64,
|
||||
90,
|
||||
53,
|
||||
72,
|
||||
98,
|
||||
67,
|
||||
70,
|
||||
60,
|
||||
84,
|
||||
86,
|
||||
82,
|
||||
75,
|
||||
73
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.00929587415047541,
|
||||
0.0010748985788974661,
|
||||
0.001013950655392918,
|
||||
0.001013950511849939
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 1.9984633198313304,
|
||||
"residual_median_px": 1.1048397349885588,
|
||||
"residual_max_px": 8.012141964697209,
|
||||
"sigma2_normalized": 1.1114547464625753e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.7610290050506592,
|
||||
0.6436193585395813,
|
||||
0.08117286115884781
|
||||
],
|
||||
[
|
||||
0.4864314794540405,
|
||||
-0.4833734929561615,
|
||||
-0.7278286218643188
|
||||
],
|
||||
[
|
||||
-0.42920777201652527,
|
||||
0.5933837294578552,
|
||||
-0.6809379458427429
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.17428335547447205,
|
||||
-0.14054450392723083,
|
||||
1.1974174976348877
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.177352630181243,
|
||||
0.841105236753405,
|
||||
-0.2590449733390141
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.7149407863616943,
|
||||
-0.6662914156913757,
|
||||
0.7272217273712158
|
||||
],
|
||||
"position_mm": [
|
||||
714.9407958984375,
|
||||
-666.2914428710938,
|
||||
727.2217407226562
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 138.93040466308594,
|
||||
"pitch": 25.41729164123535,
|
||||
"yaw": 32.58573913574219
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
1.7325268790245518e-06,
|
||||
5.981655510564799e-07,
|
||||
1.9008081078750225e-07,
|
||||
6.084320732905797e-08,
|
||||
-2.3354702118446124e-07,
|
||||
-3.602729870649897e-08
|
||||
],
|
||||
[
|
||||
5.981655510564792e-07,
|
||||
7.185403999584639e-07,
|
||||
-1.733161988537022e-07,
|
||||
1.3590794895526218e-07,
|
||||
-1.804162779559688e-07,
|
||||
1.5814835072347804e-07
|
||||
],
|
||||
[
|
||||
1.9008081078749777e-07,
|
||||
-1.733161988537059e-07,
|
||||
2.5200605751354745e-06,
|
||||
-7.330065793590657e-08,
|
||||
-3.490611313350714e-07,
|
||||
-1.0443589786543664e-06
|
||||
],
|
||||
[
|
||||
6.084320732905791e-08,
|
||||
1.3590794895526253e-07,
|
||||
-7.330065793590603e-08,
|
||||
6.430560370646186e-08,
|
||||
-2.0737599537247602e-08,
|
||||
9.272757844523572e-08
|
||||
],
|
||||
[
|
||||
-2.3354702118446045e-07,
|
||||
-1.8041627795596826e-07,
|
||||
-3.490611313350726e-07,
|
||||
-2.073759953724742e-08,
|
||||
1.4131779018249e-07,
|
||||
1.552658806009075e-07
|
||||
],
|
||||
[
|
||||
-3.6027298706497375e-08,
|
||||
1.5814835072348006e-07,
|
||||
-1.0443589786543662e-06,
|
||||
9.272757844523604e-08,
|
||||
1.552658806009069e-07,
|
||||
7.158822480370525e-07
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.07541584873239728,
|
||||
0.048567777257391795,
|
||||
0.09095532342017693
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0002535854958519155,
|
||||
0.0003759225853583288,
|
||||
0.0008460982496359702
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.001238859522904557,
|
||||
0.0010725601282025278,
|
||||
0.0013428043149285806
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
1.2388595229045571,
|
||||
1.0725601282025277,
|
||||
1.3428043149285807
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.09670418188338105,
|
||||
"pitch": 0.07031307438362219,
|
||||
"yaw": 0.04214847006011442
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
943.0,
|
||||
1037.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
942.2024536132812,
|
||||
1035.979736328125
|
||||
],
|
||||
"reprojection_error_px": 1.7167649434302272,
|
||||
"confidence": 0.28339330788046485
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
1260.5,
|
||||
1040.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1260.0,
|
||||
1039.166259765625
|
||||
],
|
||||
"reprojection_error_px": 1.1935212170729015,
|
||||
"confidence": 0.186820630468236
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1018.75,
|
||||
1005.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1017.9473266601562,
|
||||
1004.2492065429688
|
||||
],
|
||||
"reprojection_error_px": 1.48619270710366,
|
||||
"confidence": 0.868630415221559
|
||||
},
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
650.75,
|
||||
884.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
651.09619140625,
|
||||
883.6798095703125
|
||||
],
|
||||
"reprojection_error_px": 0.47156166195399984,
|
||||
"confidence": 0.7472812484733762
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
727.5,
|
||||
891.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
727.0736083984375,
|
||||
890.49853515625
|
||||
],
|
||||
"reprojection_error_px": 1.0884583736414755,
|
||||
"confidence": 0.7927270034536174
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
1312.75,
|
||||
909.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1318.954833984375,
|
||||
914.5689697265625
|
||||
],
|
||||
"reprojection_error_px": 8.012141964697209,
|
||||
"confidence": 0.5865958415608975
|
||||
},
|
||||
{
|
||||
"marker_id": 217,
|
||||
"observed_center_px": [
|
||||
1525.0,
|
||||
915.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1526.02783203125,
|
||||
921.9722290039062
|
||||
],
|
||||
"reprojection_error_px": 6.553334034174421,
|
||||
"confidence": 0.48103075735306616
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
779.5,
|
||||
850.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
779.2447509765625,
|
||||
849.6724243164062
|
||||
],
|
||||
"reprojection_error_px": 0.8660447887040601,
|
||||
"confidence": 0.7324762772043267
|
||||
},
|
||||
{
|
||||
"marker_id": 93,
|
||||
"observed_center_px": [
|
||||
1890.0,
|
||||
969.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1890.317138671875,
|
||||
971.253173828125
|
||||
],
|
||||
"reprojection_error_px": 1.5362644612040617,
|
||||
"confidence": 0.0615171510658591
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
549.25,
|
||||
803.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
550.2374877929688,
|
||||
801.9854125976562
|
||||
],
|
||||
"reprojection_error_px": 1.6044667149644478,
|
||||
"confidence": 0.526209880769093
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
961.5,
|
||||
801.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
960.479248046875,
|
||||
801.3424072265625
|
||||
],
|
||||
"reprojection_error_px": 1.0328455993265524,
|
||||
"confidence": 0.5777637144454008
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
911.5,
|
||||
782.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
910.5089721679688,
|
||||
782.1806640625
|
||||
],
|
||||
"reprojection_error_px": 1.0412068021481435,
|
||||
"confidence": 0.5986996695409863
|
||||
},
|
||||
{
|
||||
"marker_id": 94,
|
||||
"observed_center_px": [
|
||||
1813.75,
|
||||
889.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1813.9437255859375,
|
||||
890.62109375
|
||||
],
|
||||
"reprojection_error_px": 1.1377085738166388,
|
||||
"confidence": 0.321218994735393
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
631.25,
|
||||
708.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
631.6299438476562,
|
||||
708.2338256835938
|
||||
],
|
||||
"reprojection_error_px": 0.640931550393094,
|
||||
"confidence": 0.5102620497459525
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
875.5,
|
||||
714.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
875.01416015625,
|
||||
714.7401733398438
|
||||
],
|
||||
"reprojection_error_px": 0.5419627173032531,
|
||||
"confidence": 0.5178115536596077
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
1710.25,
|
||||
832.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1710.6463623046875,
|
||||
831.8109130859375
|
||||
],
|
||||
"reprojection_error_px": 0.7949489617009169,
|
||||
"confidence": 0.31927346015206026
|
||||
},
|
||||
{
|
||||
"marker_id": 104,
|
||||
"observed_center_px": [
|
||||
1769.75,
|
||||
795.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1770.6925048828125,
|
||||
794.8803100585938
|
||||
],
|
||||
"reprojection_error_px": 1.1279765412478604,
|
||||
"confidence": 0.2820489477598529
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
1550.0,
|
||||
745.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1549.9093017578125,
|
||||
744.649169921875
|
||||
],
|
||||
"reprojection_error_px": 0.8556506255348009,
|
||||
"confidence": 0.2957408773689774
|
||||
},
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
465.5,
|
||||
587.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
467.173583984375,
|
||||
587.0509643554688
|
||||
],
|
||||
"reprojection_error_px": 1.7327770666811397,
|
||||
"confidence": 0.3850070054256877
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
649.0,
|
||||
589.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
649.4606323242188,
|
||||
590.1286010742188
|
||||
],
|
||||
"reprojection_error_px": 0.7793083142275168,
|
||||
"confidence": 0.4188423377379467
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
720.75,
|
||||
621.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
720.8779907226562,
|
||||
620.9248046875
|
||||
],
|
||||
"reprojection_error_px": 0.14844514174617424,
|
||||
"confidence": 0.43643037974141263
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
569.25,
|
||||
573.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
570.5189208984375,
|
||||
573.7750244140625
|
||||
],
|
||||
"reprojection_error_px": 1.2691676279320252,
|
||||
"confidence": 0.37500699513057323
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
1415.5,
|
||||
663.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1414.463134765625,
|
||||
662.5946044921875
|
||||
],
|
||||
"reprojection_error_px": 1.2266347402207098,
|
||||
"confidence": 0.25940485673792224
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
1438.0,
|
||||
635.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1437.16650390625,
|
||||
634.8544921875
|
||||
],
|
||||
"reprojection_error_px": 0.922573665375857,
|
||||
"confidence": 0.270957436835909
|
||||
},
|
||||
{
|
||||
"marker_id": 88,
|
||||
"observed_center_px": [
|
||||
1720.5,
|
||||
694.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1720.8408203125,
|
||||
693.2443237304688
|
||||
],
|
||||
"reprojection_error_px": 0.8289783530016113,
|
||||
"confidence": 0.21331579725472802
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
1380.75,
|
||||
628.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1379.9970703125,
|
||||
628.1585693359375
|
||||
],
|
||||
"reprojection_error_px": 0.8267272903932755,
|
||||
"confidence": 0.23649772201893263
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
1672.75,
|
||||
657.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1672.596923828125,
|
||||
656.64208984375
|
||||
],
|
||||
"reprojection_error_px": 0.8714597813971704,
|
||||
"confidence": 0.22421739747061642
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
541.75,
|
||||
511.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
543.0106811523438,
|
||||
511.8523864746094
|
||||
],
|
||||
"reprojection_error_px": 1.5218014555032349,
|
||||
"confidence": 0.3194003123985894
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
1571.25,
|
||||
614.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1571.232177734375,
|
||||
612.768798828125
|
||||
],
|
||||
"reprojection_error_px": 1.2313301583159495,
|
||||
"confidence": 0.21534147510121324
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
1340.25,
|
||||
579.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1339.28076171875,
|
||||
579.6795654296875
|
||||
],
|
||||
"reprojection_error_px": 0.9717941523468644,
|
||||
"confidence": 0.2131836038878541
|
||||
},
|
||||
{
|
||||
"marker_id": 72,
|
||||
"observed_center_px": [
|
||||
1282.0,
|
||||
562.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1280.0421142578125,
|
||||
563.0445556640625
|
||||
],
|
||||
"reprojection_error_px": 1.9799190939764175,
|
||||
"confidence": 0.2329113678114362
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
1493.25,
|
||||
573.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1492.691650390625,
|
||||
572.2074584960938
|
||||
],
|
||||
"reprojection_error_px": 1.6404842509340254,
|
||||
"confidence": 0.19836555127576866
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
1411.0,
|
||||
570.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1410.1534423828125,
|
||||
569.2794799804688
|
||||
],
|
||||
"reprojection_error_px": 1.111669419280521,
|
||||
"confidence": 0.22146320976627604
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
1516.0,
|
||||
598.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1515.696044921875,
|
||||
597.093505859375
|
||||
],
|
||||
"reprojection_error_px": 1.1957706246675956,
|
||||
"confidence": 0.17917987028547397
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
1326.5,
|
||||
509.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1325.19873046875,
|
||||
509.093017578125
|
||||
],
|
||||
"reprojection_error_px": 1.3634284303456619,
|
||||
"confidence": 0.19672907121069258
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
1284.5,
|
||||
508.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1283.4097900390625,
|
||||
507.8693542480469
|
||||
],
|
||||
"reprojection_error_px": 1.0980100506965966,
|
||||
"confidence": 0.1992299923059727
|
||||
},
|
||||
{
|
||||
"marker_id": 86,
|
||||
"observed_center_px": [
|
||||
1259.0,
|
||||
466.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1258.5447998046875,
|
||||
466.7055358886719
|
||||
],
|
||||
"reprojection_error_px": 0.4994519189518402,
|
||||
"confidence": 0.20697942435059868
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
1129.75,
|
||||
393.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1128.9288330078125,
|
||||
393.5796813964844
|
||||
],
|
||||
"reprojection_error_px": 0.8386439386067119,
|
||||
"confidence": 0.16919547786983627
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
862.5,
|
||||
325.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
861.7324829101562,
|
||||
326.8608703613281
|
||||
],
|
||||
"reprojection_error_px": 1.7843726640496438,
|
||||
"confidence": 0.21628605150283992
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
1153.5,
|
||||
379.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1152.4403076171875,
|
||||
379.2215881347656
|
||||
],
|
||||
"reprojection_error_px": 1.08261223328565,
|
||||
"confidence": 0.1485104612681437
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_175916/cam2_debug.jpg
Normal file
BIN
data/homing/20260625_175916/cam2_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 458 KiB |
2487
data/homing/20260625_175916/robot_1781069752019.json
Normal file
2487
data/homing/20260625_175916/robot_1781069752019.json
Normal file
File diff suppressed because it is too large
Load Diff
59
data/homing/20260625_175916/robot_state.json
Normal file
59
data/homing/20260625_175916/robot_state.json
Normal file
@@ -0,0 +1,59 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:59:33Z",
|
||||
"method": "hybrid",
|
||||
"seeded": true,
|
||||
"movements": {
|
||||
"x": {
|
||||
"value": 110.01986467359067,
|
||||
"unit": "mm",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 6
|
||||
},
|
||||
"y": {
|
||||
"value": 1.0330890750947754,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 6
|
||||
},
|
||||
"z": {
|
||||
"value": 96.2744445958484,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 1
|
||||
},
|
||||
"a": {
|
||||
"value": 86.75801106951552,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 3
|
||||
},
|
||||
"b": {
|
||||
"value": 62.05866240456044,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
},
|
||||
"c": {
|
||||
"value": -80.56572105590129,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
},
|
||||
"e": {
|
||||
"value": 35.00722900692152,
|
||||
"unit": "mm",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
}
|
||||
},
|
||||
"residual_rms": 33.572073291771,
|
||||
"num_markers": 55
|
||||
}
|
||||
222
data/homing/20260625_175916/state_Arm1.json
Normal file
222
data/homing/20260625_175916/state_Arm1.json
Normal file
@@ -0,0 +1,222 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm1",
|
||||
"joint": "y",
|
||||
"method": "primary",
|
||||
"joint_origin_world_mm": [
|
||||
154.64058080413625,
|
||||
108.3154,
|
||||
53.4964
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 7.073578785211089,
|
||||
"circular_variance": 0.007972526914742928,
|
||||
"circular_std_deg": 7.249434372855015,
|
||||
"num_pairs_used": 15,
|
||||
"num_markers_matched": 6,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
56
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 0.20885786426191633,
|
||||
"baseline_model_mm": 430.4301189508002,
|
||||
"baseline_obs_mm": 431.321568455382,
|
||||
"weight": 185653.79401629578
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
77
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 0.1534985213298885,
|
||||
"baseline_model_mm": 455.14017577005876,
|
||||
"baseline_obs_mm": 456.76224811573417,
|
||||
"weight": 207890.84989252244
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
198
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 2.7294530137510984,
|
||||
"baseline_model_mm": 120.70271952197267,
|
||||
"baseline_obs_mm": 235.9270100140718,
|
||||
"weight": 28477.031717386148
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 36.070256539732,
|
||||
"baseline_model_mm": 63.35571402801802,
|
||||
"baseline_obs_mm": 159.8295466346313,
|
||||
"weight": 10126.115049811471
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 89.62500483491822,
|
||||
"baseline_model_mm": 34.32559540634365,
|
||||
"baseline_obs_mm": 110.61945936866468,
|
||||
"weight": 3797.0788063572545
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
56,
|
||||
77
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -0.8278666326016993,
|
||||
"baseline_model_mm": 24.720487454740837,
|
||||
"baseline_obs_mm": 25.442346313142185,
|
||||
"weight": 628.9472028532032
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
56,
|
||||
198
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 16.060958275347453,
|
||||
"baseline_model_mm": 332.5358726513577,
|
||||
"baseline_obs_mm": 249.91702454137288,
|
||||
"weight": 83106.37584629621
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
56,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 10.575835506835253,
|
||||
"baseline_model_mm": 421.3102260804976,
|
||||
"baseline_obs_mm": 329.51739043275734,
|
||||
"weight": 138829.0462606806
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
56,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 8.204562901697962,
|
||||
"baseline_model_mm": 452.5694494770941,
|
||||
"baseline_obs_mm": 352.3360154416916,
|
||||
"weight": 159456.51653939928
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
77,
|
||||
198
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 14.349880680055369,
|
||||
"baseline_model_mm": 356.9731122927888,
|
||||
"baseline_obs_mm": 273.03257450221435,
|
||||
"weight": 97465.28787736819
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
77,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 9.66556769141446,
|
||||
"baseline_model_mm": 445.86971516352173,
|
||||
"baseline_obs_mm": 353.78452354827397,
|
||||
"weight": 157741.80474373116
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
77,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 7.573295647776105,
|
||||
"baseline_model_mm": 477.27969043318825,
|
||||
"baseline_obs_mm": 377.3635068156209,
|
||||
"weight": 180107.93771374188
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
198,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -3.397605318552869,
|
||||
"baseline_model_mm": 90.00000000000003,
|
||||
"baseline_obs_mm": 88.54992394070814,
|
||||
"weight": 7969.493154663735
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
198,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -3.2531512518968957,
|
||||
"baseline_model_mm": 129.8075498574717,
|
||||
"baseline_obs_mm": 128.9115321451203,
|
||||
"weight": 16733.690136130772
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
229,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -2.3933465309361535,
|
||||
"baseline_model_mm": 49.49747468305833,
|
||||
"baseline_obs_mm": 49.589805881590635,
|
||||
"weight": 2454.5701611618097
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 44.640580804136256,
|
||||
"y": 7.073578785211089
|
||||
}
|
||||
}
|
||||
65
data/homing/20260625_175916/state_Arm2.json
Normal file
65
data/homing/20260625_175916/state_Arm2.json
Normal file
@@ -0,0 +1,65 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm2",
|
||||
"joint": "a",
|
||||
"method": "primary",
|
||||
"joint_origin_world_mm": [
|
||||
244.64058080413622,
|
||||
-139.78180740318305,
|
||||
84.28236561327859
|
||||
],
|
||||
"joint_axis_world": [
|
||||
0.0,
|
||||
0.13416044351804374,
|
||||
0.9909596234938344
|
||||
],
|
||||
"mean_angle_deg": 92.49229573636204,
|
||||
"circular_variance": 0.0005036790852294137,
|
||||
"circular_std_deg": 1.8187344232244496,
|
||||
"num_pairs_used": 2,
|
||||
"num_markers_matched": 3,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
144
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 90.53857082520275,
|
||||
"baseline_model_mm": 26.684454275851333,
|
||||
"baseline_obs_mm": 24.841154956793282,
|
||||
"weight": 662.8726636038881
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 94.18507945848495,
|
||||
"baseline_model_mm": 26.836171485515596,
|
||||
"baseline_obs_mm": 28.506933940749555,
|
||||
"weight": 765.0169677602199
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": true,
|
||||
"reason": "bl_model=0.4 bl_obs=3.9 < 15.0"
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 44.640580804136256,
|
||||
"y": 7.073578785211089,
|
||||
"z": 90.63649643342701,
|
||||
"a": 92.49229573636204
|
||||
}
|
||||
}
|
||||
41
data/homing/20260625_175916/state_Ellbow.json
Normal file
41
data/homing/20260625_175916/state_Ellbow.json
Normal file
@@ -0,0 +1,41 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Ellbow",
|
||||
"joint": "z",
|
||||
"method": "fallback_1_child_axis",
|
||||
"joint_origin_world_mm": [
|
||||
154.64058080413625,
|
||||
-139.78180740318305,
|
||||
84.28236561327859
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 90.63649643342701,
|
||||
"circular_variance": 0.0,
|
||||
"circular_std_deg": 0.0,
|
||||
"num_pairs_used": 1,
|
||||
"num_markers_matched": 1,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "fallback_1_child_axis",
|
||||
"skipped": false,
|
||||
"angle_deg": 90.63649643342701,
|
||||
"baseline_model_mm": 107.0,
|
||||
"baseline_obs_mm": 107.57847026231472,
|
||||
"weight": 11510.896318067675
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 44.640580804136256,
|
||||
"y": 7.073578785211089,
|
||||
"z": 90.63649643342701
|
||||
}
|
||||
}
|
||||
@@ -14,6 +14,15 @@ die vollständige Pose aller Gelenke.
|
||||
**Homing** (dieser Prozess): bei jedem Einschalten, automatisch.
|
||||
**Kalibrierung** (`doc/Kalibrierung.md`): nur nach mechanischen Änderungen.
|
||||
|
||||
Dieses Dokument ist der **allgemeine Ablauf**. Technische Detail-Doku je Teilstrecke:
|
||||
|
||||
| Doku | Inhalt |
|
||||
|---|---|
|
||||
| [`Homing_0_Camera.md`](Homing_0_Camera.md) | Foto → Kamera-Pose → trianguliertes `aruco_marker_poses.json` (Schritte 1–3b) |
|
||||
| [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md) | `aruco_marker_poses.json` → Gelenkwinkel je Link (4b-Kette, Schätz-Methoden/Fallbacks) |
|
||||
| `Homing_2_improvement.md` *(geplant)* | Bekannte Probleme, Genauigkeits-Befunde, Verbesserungsideen |
|
||||
| [`Homing_5_Pose.md`](Homing_5_Pose.md) | Verfeinerungsschritt NACH der 4b-Kette (Bundle-Adjustment, `5_pose_estimation.py`) — noch nicht verdrahtet |
|
||||
|
||||
---
|
||||
|
||||
## Kinematik-Kette
|
||||
@@ -76,9 +85,11 @@ X-Position aus Marker-Positionen schätzen
|
||||
│ → state_Arm2.json
|
||||
▼
|
||||
4b_revolute_angle.py --link Hand --from-state state_Arm2.json
|
||||
│ → state_Hand.json ← accumulated_state enthält x,y,z,a,b,c,e
|
||||
│ → state_Hand.json ← accumulated_state: x,y,z,a,b (4b-Primärkette)
|
||||
│ Fallback 5_pose_estimation.py liefert alle 7 (…,c,e). Nur bekannte
|
||||
│ Achsen gehen ins G92; wirklich fehlende werden weggelassen.
|
||||
▼
|
||||
POST ROBOT_URL/api/state
|
||||
G92 über Driver-WebSocket (DRIVER_WS_URL) — setzt Motorposition ohne Bewegung
|
||||
```
|
||||
|
||||
**Schritte 1–3b** sind dieselbe Board-Pipeline wie in der Kalibrierung.
|
||||
@@ -98,7 +109,7 @@ X-Slider-Position über `--x-mm`.
|
||||
| X-Schätzung | `server/homingOrchestrator.js` → `estimateXFromMarkers()` | Pro Arm-Marker `beobachtetes_x − Modell_x(slider=0)`, gemittelt — rechnet den kinematischen Gelenk-Offset (z.B. Arm1.origin.x=110) heraus. Nur x-zuverlässige Ketten (x-Rotation: Arm1/Ellbow). Fallback: roher Mittelwert |
|
||||
| Homing-Orchestrator | `server/homingOrchestrator.js` → `runHoming()` | Kompletter Ablauf als SSE-Stream |
|
||||
| Backend-Route | `POST /api/homing/run` | SSE-Stream, startet `runHoming()` |
|
||||
| State senden | `POST /api/homing/send-state` | Weiterleitung an `ROBOT_URL/api/state` |
|
||||
| State senden | `POST /api/homing/send-state` | Baut `G92` (fehlende c/e → 0) und sendet es als Plain-Text-G-Code über den Driver-WebSocket (`DRIVER_WS_URL`, `server/driverClient.js`). Der Driver verarbeitet G92 intern als M92 = Motorposition setzen ohne Bewegung. Kein HTTP `/api/state` (gibt es am Driver nicht) |
|
||||
| Run-Daten | `GET /api/homing/run-data?run=ts` | Debug-Bilder (base64) + finalState |
|
||||
| Frontend | `public/index.html` + `public/client.js` | Homing-Buttons, Fortschrittsbalken, Tree View; schreibt Teil-Pose als `G92`-GCode ins Eingabefeld |
|
||||
| Board-Viewer (Homing) | `public/boardViewer.html?mode=homing` | Skelett + Arm-Marker per FK (Three.js): Marker-Quadrat spin-korrekt rotiert + Orientierungszeiger zu Ecke 0 (Modell-Seite); gemessene Marker als Kugeln + Fehlerlinien; progressiver Update je erkanntem Gelenk |
|
||||
@@ -146,8 +157,8 @@ die aktuelle Konfiguration.
|
||||
## Offene Punkte
|
||||
|
||||
- [~] **Arm-Marker eintragen** (Nutzer): `links.Arm1/Ellbow/Arm2/Hand.markers` in `robot.json` — Arm1 + Ellbow eingetragen, Arm2/Hand offen
|
||||
- [~] **Erstmals testen**: Homing-Run mit echtem Roboter — Arm1 erkannt (x, y); Ellbow scheitert noch an fehlenden Markern
|
||||
- [~] **Erstmals testen**: Homing-Run mit echtem Roboter — Arm1 (x, y) und Ellbow (z) laufen durch; Arm2/Hand noch zu prüfen. Ellbow brach zunächst ab (4b: nur 2 Marker sichtbar, deren Verbindungsvektor parallel zur Gelenkachse liegt → keine Baseline), seit 2026-06-16 über Fallback-1/-2 in `4b_revolute_angle.py` gelöst — Details: [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md)
|
||||
- [x] **X-Schätzung verfeinern** (2026-06-14): `estimateXFromMarkers()` rechnet den kinematischen Gelenk-Offset heraus statt rohem Mittelwert — behebt den ~110 mm Versatz der Modell-Marker
|
||||
- [x] **Unit-Test für X-Schätzung** (2026-06-14): reine Geometrie nach `server/homingXEstimate.cjs` ausgelöst, `test/homingXEstimate.test.js` (9 Tests, inkl. Regression gegen den Offset-Bug)
|
||||
- [ ] **y-Restfehler** (~2°): erkannt 30° → ausgegeben 28°; vermutlich X-Rest-Rauschen + 4b-Fit-Residuum, noch zu untersuchen
|
||||
- [ ] **robot.json via Driver-API** (optional): wenn Driver `GET ROBOT_URL/api/robot/config` bereitstellt
|
||||
- [x] **robot.json via Driver-API** (2026-06-17): `server/robotConfig.js` — `fetchRobot()`/`pushRobot()`/`robotCachePath`; automatischer Fallback auf lokale Datei wenn `ROBOT_URL` nicht gesetzt
|
||||
|
||||
34
doc/Homing_0_Camera.md
Normal file
34
doc/Homing_0_Camera.md
Normal file
@@ -0,0 +1,34 @@
|
||||
# Homing 0 – Kamera & Triangulation
|
||||
|
||||
> Technische Detail-Doku zu [`Homing.md`](Homing.md) — dieser Teil: Foto → trianguliertes
|
||||
> `aruco_marker_poses.json`.
|
||||
> Status: Gerüst, wird sukzessive ausgebaut.
|
||||
|
||||
---
|
||||
|
||||
## Scope
|
||||
|
||||
Schritte 1–3b der Homing-Pipeline (siehe `Homing.md` → Ablauf), a.k.a. „Board-Pipeline":
|
||||
|
||||
| Schritt | Script | Pro | Ergebnis |
|
||||
|---|---|---|---|
|
||||
| 1 | `1_detect_aruco_observations.py` | Kamera | 2D-Marker-Ecken (mit NPZ-Intrinsik entzerrt) |
|
||||
| 2 | `2_estimate_camera_from_observations.py` | Kamera | Kamera-Pose relativ zum Board (`cam*_camera_pose.json`) |
|
||||
| 3b | `3b_corner_marker_poses.py` | einmal, ≥2 Kamera-Posen | `aruco_marker_poses.json` (3D-Weltpositionen aller Marker) |
|
||||
|
||||
Ausgelagert in `runBoardPipeline()` (`server/server.js`) — gemeinsam genutzt von
|
||||
Kalibrierung und Homing.
|
||||
|
||||
## Offene Punkte / noch zu dokumentieren
|
||||
|
||||
- [ ] Kamera-Intrinsik-Format (NPZ-Inhalt, Verzeichnis)
|
||||
- [ ] Wie `3b` mehrere Kamera-Beobachtungen eines Markers kombiniert (Triangulation/Mittelung, `num_cameras`-Feld)
|
||||
- [ ] Fehlerquellen: Verdeckung, Belichtung, `num_cameras < 2` → Marker fehlt komplett
|
||||
- [ ] Format von `aruco_marker_poses.json` (Felder `position_mm`, `corners_m`, `normal`, `edge_length_mm`)
|
||||
- [ ] Zusammenhang mit `Kalibrierung.md` / `Kalibrierung_Marker.md`
|
||||
|
||||
## Verweise
|
||||
|
||||
- Allgemeiner Ablauf: [`Homing.md`](Homing.md)
|
||||
- Nächster Schritt (Gelenkwinkel aus den triangulierten Positionen): [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md)
|
||||
- Kalibrierung: `Kalibrierung.md`, `Kalibrierung_Marker.md`
|
||||
84
doc/Homing_1_StepByStep.md
Normal file
84
doc/Homing_1_StepByStep.md
Normal file
@@ -0,0 +1,84 @@
|
||||
# Homing 1 – Schritt für Schritt: Gelenkwinkel-Schätzung (4b)
|
||||
|
||||
> Technische Detail-Doku zu [`Homing.md`](Homing.md) — dieser Teil: `aruco_marker_poses.json`
|
||||
> → Gelenkwinkel je Link.
|
||||
> Status: Gerüst, wird sukzessive ausgebaut.
|
||||
|
||||
---
|
||||
|
||||
## Scope
|
||||
|
||||
Die 4b-Kette aus `Homing.md` → Ablauf: `scripts/4b_revolute_angle.py`, sequenziell
|
||||
Arm1 → Ellbow → Arm2 → Hand. Jeder Aufruf bekommt den Zustand des vorherigen Schritts
|
||||
über `--from-state` (`accumulated_state`).
|
||||
|
||||
## Schätz-Methoden (Prioritätsreihenfolge je Gelenk)
|
||||
|
||||
Pro Gelenk wird in dieser Reihenfolge versucht, eine Schätzung zu finden. Jede
|
||||
nächste Stufe ist ein **reiner Fallback** — sie greift nur, wenn die vorherige Stufe
|
||||
**keine einzige** brauchbare Baseline liefert (z. B. Marker nicht sichtbar, oder
|
||||
Marker-Paar zufällig parallel zur Drehachse):
|
||||
|
||||
1. **Primär** (`TIER_PRIMARY`) — zwei Marker auf dem Ziel-Link selbst.
|
||||
`v_model`/`v_obs`-Differenzvektor, ⟂ zur Gelenkachse projiziert, Winkel zwischen
|
||||
beiden gemessen. Braucht nur die Achs*richtung* (aus FK der schon gelösten
|
||||
Vorgänger), nicht die Pivot-*Position*.
|
||||
2. **Fallback-1** (`TIER_FALLBACK_1`, implementiert 2026-06-16) — zwei Marker auf
|
||||
dem **direkten Kind-Link**, deren Verbindungsvektor (im Kind-Lokalframe) parallel
|
||||
zur **eigenen** Achse des Kind-Links liegt (Toleranz `--child-axis-tol`, default
|
||||
1mm) → invariant gegen dessen noch unbekannten Winkel, daher als Stellvertreter
|
||||
für „zwei Marker am Ziel-Link" nutzbar. Beispiel: Ellbow (Achse X) ← Arm2-Marker
|
||||
144↔148 bzw. 143↔146 (Arm2-Achse Y, ⟂ zu X, beide Paare exakt achsparallel in
|
||||
Arm2s Lokalframe). Wie Primär unabhängig von der Pivot-Position — eine separate
|
||||
„ist die nächste Achse senkrecht"-Prüfung war nicht nötig, das ergibt sich
|
||||
automatisch aus der bestehenden Mindest-Baseline-Prüfung nach der Projektion.
|
||||
3. **Fallback-2** (`TIER_FALLBACK_2`, implementiert) — ein einzelner Marker auf dem
|
||||
Ziel-Link gegen den Gelenk-**Pivot** selbst (Pivot + Achse aus FK der Vorgänger).
|
||||
Einzige Stufe, die mit nur 1 sichtbaren Marker funktioniert — aber zusätzlich
|
||||
abhängig von der Pivot-*Position* (also den geschätzten Vorgänger-*Werten*, nicht
|
||||
nur deren Achsrichtung). Letzter Rückfall, nur falls Fallback-1 auch nichts findet
|
||||
(kein Kind-Link, oder dessen Marker nicht sichtbar/nicht achsparallel).
|
||||
|
||||
→ Code: `scripts/4b_revolute_angle.py` (`estimate_revolute_angle()`); Konstanten
|
||||
`TIER_PRIMARY`/`TIER_FALLBACK_1`/`TIER_FALLBACK_2`/`PIVOT_FALLBACK_ID`; Felder
|
||||
`"method"` (top-level) und `"tier"`/`"link"` (je `per_pair`-Eintrag) im Ergebnis-JSON.
|
||||
|
||||
### Befund 2026-06-16 (Anlass für Fallback-1)
|
||||
|
||||
Im Testlauf `test/homing/20260616_120456` waren am Ellbow nur Marker 129/132 sichtbar,
|
||||
deren Verbindungsvektor exakt parallel zur Ellbow-Achse liegt → Primär-Methode liefert
|
||||
nichts. **Vor** Fallback-1 sprang Fallback-2 (Pivot) ein und meldete `z ≈ -4.33°`
|
||||
(intern konsistent, Exit 0) — eine unabhängige Gegenrechnung (Least-Squares über alle
|
||||
Ellbow- *und* Arm2-Marker, `z`+`a` frei) zeigte aber ihr Minimum bei `z ≈ -38°`, also
|
||||
ca. 35–40° daneben. **Mit** Fallback-1 (Arm2-Marker 144↔148/143↔146) liefert derselbe
|
||||
Lauf jetzt `z ≈ -44.15°` (circular_σ 0.80°) — deutlich näher am Least-Squares-Minimum.
|
||||
Bestätigt auch downstream: die anschließende Arm2-Schätzung (Primär, eigene Marker)
|
||||
hatte mit der alten Fallback-2-Kette einen Ausreißer und `circular_σ 45.8°`; mit der
|
||||
Fallback-1-Kette sind alle vier Arm2-Paare konsistent, `circular_σ 5.1°`.
|
||||
→ Fallback-1 war hier kein „nice to have", sondern ca. 35° Genauigkeitsgewinn.
|
||||
Weitere/künftige Befunde: siehe `Homing_2_improvement.md` (geplant).
|
||||
|
||||
## robot.json-Struktur (Kurzreferenz)
|
||||
|
||||
- `links.<Name>.parent` — Name des Eltern-Links (Kette/Baum)
|
||||
- `links.<Name>.jointToParent` — `{type, axis, origin, rotation, variable}`
|
||||
- `links.<Name>.markers[]` — `{id, position, normal, size}`; `position` ist relativ
|
||||
zum **Pivot** des eigenen Links, im lokalen, noch nicht eigen-rotierten Frame.
|
||||
- FK-Engine: `scripts/robot_fk.py` (`RobotFK.compute()`, `.joint_origin_world()`,
|
||||
`.joint_axis_world()`, `.marker_world()`).
|
||||
|
||||
## Offene Punkte / noch zu dokumentieren
|
||||
|
||||
- [ ] State-JSON-Schema im Detail (`accumulated_state`, `per_pair`, `method`, `circular_std_deg`)
|
||||
- [ ] `--min-baseline` / `--child-axis-tol` Tuning / Auswirkung
|
||||
- [x] Fallback-1 Implementierung (2026-06-16, `scripts/4b_revolute_angle.py`)
|
||||
- [ ] Mehrstufige Rekursion (Enkel-Links) für Fallback-1 — aktuell bewusst nur
|
||||
direkter Kind-Link, siehe Code-Kommentar bei `_child_links()`
|
||||
- [ ] y-Restfehler (~2°) aus `Homing.md` → Offene Punkte
|
||||
|
||||
## Verweise
|
||||
|
||||
- Allgemeiner Ablauf: [`Homing.md`](Homing.md)
|
||||
- Vorheriger Schritt (Kamera/Triangulation): [`Homing_0_Camera.md`](Homing_0_Camera.md)
|
||||
- Nächster Schritt (Verfeinerung nach 4b): [`Homing_5_Pose.md`](Homing_5_Pose.md)
|
||||
- Bekannte Probleme / Ideen: `Homing_2_improvement.md` (geplant)
|
||||
439
doc/Homing_5_Pose.md
Normal file
439
doc/Homing_5_Pose.md
Normal file
@@ -0,0 +1,439 @@
|
||||
# Homing 5 – Pose-Schätzung per Bundle-Adjustment (`hybrid`)
|
||||
|
||||
> Technische Detail-Doku zu [`Homing.md`](Homing.md) — **Verfeinerungsschritt NACH
|
||||
> der 4b-Kette** ([`Homing_1_StepByStep.md`](Homing_1_StepByStep.md)), **nicht**
|
||||
> deren Ersatz: `5_pose_estimation.py` braucht den `accumulated_state` von 4b als
|
||||
> Startwert. Ohne guten Startwert läuft die interne Optimierung mangels eigener
|
||||
> verlässlicher Initialisierung leicht in ein lokales Minimum (siehe „Wichtige
|
||||
> Einschränkung" unten).
|
||||
> Status (2026-06-16): **Code-Hooks umgesetzt und gegen drei echte Homing-Captures
|
||||
> verifiziert** — `--from-state` (Startwert aus 4b, mit Multi-Start-Schutz für alles,
|
||||
> was der Startwert nicht abdeckt), `null` statt `0` für unbeobachtbare Gelenke
|
||||
> (z. B. `Hand`/`Palm`/Finger, aktuell ohne Marker in `robot_1781069752019.json`),
|
||||
> `scipy` in `docker-compose.yaml` ergänzt, sowie der **eine Schalter**
|
||||
> `pose_estimation.fit_origin_link` (siehe eigener Abschnitt unten), der einen
|
||||
> Gelenk-Drehpunkt automatisch mitbestimmt und in `robot.json` übernimmt. **Noch
|
||||
> offen:** Anbindung in `homingOrchestrator.js`/`server.js`/Frontend (siehe
|
||||
> Integrationsschritte — Umfang/Fehlerfall/Robotersteuerung-Politik dafür sind
|
||||
> noch nicht festgelegt).
|
||||
|
||||
---
|
||||
|
||||
## Herkunft
|
||||
|
||||
`scripts/5_pose_estimation.py` ist 1:1 (byte-identisch, per Diff verifiziert)
|
||||
aus dem Schwesterprojekt **`appRobotRendering`** übernommen
|
||||
(`pipeline/pose_estimation.py`, dort Stufe 4 der allgemeinen Pose-Pipeline).
|
||||
Mitgewandert und ebenfalls identisch: `scripts/robot_fk.py`. Dort ist das
|
||||
Verfahren an zehn simulierten Szenen mit bekannter Grundwahrheit validiert
|
||||
(`doc/pipeline.tex` im Rendering-Projekt) — diese Zahlen unten sind **Simulationsergebnisse
|
||||
aus appRobotRendering**, keine Messung an appRobotHoming/echter Hardware.
|
||||
|
||||
`scripts/robot_1781069752019.json` enthält bereits den passenden
|
||||
`pose_estimation`-Konfigurationsblock (identisch zu den Rendering-Defaults:
|
||||
`method: hybrid`, `normal_weight: 100`, `huber_delta_mm: 8.0`, …) — die Datenseite
|
||||
ist also schon vorbereitet, nur die Prozess-Verdrahtung fehlt noch.
|
||||
|
||||
---
|
||||
|
||||
## Einordnung in den Homing-Ablauf
|
||||
|
||||
```
|
||||
1_detect_aruco_observations.py ┐
|
||||
2_estimate_camera_from_observations.py │ = "Board-Pipeline" (Homing_0_Camera.md)
|
||||
3b_corner_marker_poses.py ┘
|
||||
│
|
||||
▼ aruco_marker_poses.json
|
||||
│
|
||||
▼
|
||||
4b_revolute_angle.py × N (sequenziell root→tip, über --from-state verkettet)
|
||||
│ Homing_1_StepByStep.md — liefert pro Gelenk eine Schätzung
|
||||
│ (Primär/Fallback-1/Fallback-2), abhängig von Sichtbarkeit
|
||||
▼
|
||||
accumulated_state {x,y,z,a,b,c,e} ← Startwert, NICHT optional überspringbar
|
||||
│
|
||||
▼
|
||||
5_pose_estimation.py (method=global_ba, accumulated_state als Startwert x0)
|
||||
│ dieses Dokument — EIN gemeinsamer Bündelausgleich über alle 7
|
||||
│ Variablen gleichzeitig, verfeinert/korrigiert den 4b-Zustand global
|
||||
▼
|
||||
robot_state.json { movements: {…}, residual_rms, … }
|
||||
```
|
||||
|
||||
**Wichtig:** `5_pose_estimation.py` ist **kein** Ersatz für die 4b-Kette, sondern
|
||||
ein **Verfeinerungsschritt danach**, der deren `accumulated_state` als Startwert
|
||||
braucht. Lässt man die 4b-Kette weg, fehlt dieser Startwert — die interne
|
||||
Optimierung initialisiert dann faktisch bei `0` für jede Variable, und bei einer
|
||||
beim Einschalten unbekannten Roboterpose ist das ein guter Weg in ein lokales
|
||||
Minimum (Mechanismus s. „Wichtige Einschränkung" unten).
|
||||
|
||||
| Stufe | Eingabe | Aufruf | Ausgabe |
|
||||
|---|---|---|---|
|
||||
| **4b-Kette** (liefert den Startwert) | `aruco_marker_poses.json` + extern geschätztes `x_mm` | N Prozesse, je Link einer, `--from-state` verkettet | `accumulated_state` (flach: `x,y,z,a,b,c,e`) |
|
||||
| **`5_pose_estimation.py`** (verfeinert global) | `aruco_marker_poses.json` **+ `accumulated_state` aus 4b als Startwert** | 1 Prozess | `robot_state.json` → `movements.<var>.{value,unit,observable,confidence,n_markers}` |
|
||||
|
||||
---
|
||||
|
||||
## Wie es funktioniert (kurz)
|
||||
|
||||
Das Skript parametrisiert über **Gelenkvariablen** (nicht Links) und liest pro
|
||||
Marker Position **und gemessene Normale** aus `aruco_marker_poses.json`
|
||||
(3b-Ausgabe). Vier austauschbare Verfahren (`robot.json` → `pose_estimation.method`),
|
||||
`hybrid` ist Standard und kombiniert die letzten beiden:
|
||||
|
||||
1. `sequential_vector` — analytische Winkel aus Marker-Paar-Vektoren (schnell, braucht ≥2 Marker/Gelenk)
|
||||
2. `sequential_fk` — blockweiser nichtlinearer Fit entlang der Kette, vorherige Variablen eingefroren, Multi-Start `{0,60,…,300}°` gegen lokale Minima
|
||||
3. `global_ba` — **einziges** Bündelausgleichsproblem über **alle 7 Variablen gleichzeitig** (`scipy.optimize.least_squares`, Huber-Loss)
|
||||
4. **`hybrid`** = 2 als Startwert → 3 als Verfeinerung
|
||||
|
||||
Die Blockbildung in `analyze_chain()` ist generisch aus der FK-Topologie
|
||||
abgeleitet (keine festen Link-Namen) — passt damit zur Projekt-Konvention
|
||||
„Scripts müssen Szenen/Ketten automatisch erkennen, nichts hartkodieren".
|
||||
Für *dieses* Robot-Modell ergibt sich u. a. der Block `{x, y}`: `Base` (Variable
|
||||
`x`) hat **keine eigenen Marker** (`"markers": []` in `robot_1781069752019.json`)
|
||||
und wird automatisch mit `Arm1` (Variable `y`, 5 Marker) zu einem gemeinsamen
|
||||
Least-Squares-Fit zusammengefasst.
|
||||
|
||||
Jedes Ergebnis kommt mit einer Konfidenz pro Variable (`high/medium/low/none`,
|
||||
abgeleitet aus sichtbaren Markern je Block) — analog zur 4b-Kette, aber pro
|
||||
Block statt pro Einzel-Fallback-Stufe.
|
||||
|
||||
### Wichtige Einschränkung: Startwert und lokale Minima
|
||||
|
||||
**Ursprünglicher Befund (vor dem Code-Hook vom 2026-06-16):** `estimate_pose()`
|
||||
rief für `global_ba`/`hybrid` immer selbst `estimate_sequential_fk()` als
|
||||
„billigen, robusten Init" auf — es gab keinen Parameter, um stattdessen einen
|
||||
extern vorgegebenen Startwert einzuspeisen, obwohl `estimate_global_ba()`
|
||||
selbst intern bereits ein `x0`-Dict entgegennahm (`:272-273`). **Das ist jetzt
|
||||
behoben** (`seed`-Parameter auf `estimate_pose()`/`estimate_sequential_fk()`,
|
||||
CLI `--from-state`) — die folgende Beschreibung des Multi-Start-Mechanismus
|
||||
gilt weiterhin unverändert für **alles, was der Seed nicht abdeckt** (bzw. für
|
||||
den reinen Kaltstart ohne `--from-state`):
|
||||
|
||||
`estimate_sequential_fk()` initialisiert jede nicht geseedete Variable bei
|
||||
`0.0` und rastert den Multi-Start `{0,60,120,180,240,300}°` **nur über die
|
||||
erste Variable eines Blocks** (`bvars[0]`) — und auch das **nur, wenn diese
|
||||
selbst `revolute` ist** (`:348-356`). Für dieses Robotermodell heißt das konkret:
|
||||
|
||||
- Block `{x, y}` (Base markerlos → mit Arm1 zusammengefasst): `bvars[0]` ist
|
||||
`x` (linear) → `lead_type != "revolute"` → **kein** Multi-Start. `y`
|
||||
(Schultergelenk, Arm1) wird in einem einzigen Lauf ab `0°` gefittet.
|
||||
- Block `{b, c, e}` (Hand/Palm markerlos → mit den Fingermarkern zusammengefasst):
|
||||
nur `b` bekommt den 6-Punkte-Raster; `c` und `e` starten in **jedem** der
|
||||
6 Läufe fix bei `0`.
|
||||
- Einzelvariablen-Blöcke wie Ellbow (`{z}`) oder Arm2 (`{a}`) bekommen den
|
||||
vollen Raster auf sich selbst — dort ist das Risiko deutlich kleiner.
|
||||
|
||||
Liegt die echte Pose in `y`, `c` oder `e` weit von `0` entfernt (beim Homing
|
||||
nach dem Einschalten der Normalfall, nicht die Ausnahme), kann schon die
|
||||
`sequential_fk`-Vorstufe in einem falschen lokalen Minimum landen — die
|
||||
anschließende `global_ba`-Verfeinerung poliert dieses falsche Minimum dann nur
|
||||
noch, statt es zu verlassen. Das deckt sich mit dem in der Validierungstabelle
|
||||
unten sichtbaren großen Abstand zwischen Mittelwert (0,253°) und Schlechtestfall
|
||||
(1,568°) bei sonst niedriger Streuung (0,134°) — ein Muster, das zu „meist
|
||||
gut, gelegentlich falsches Minimum" passt.
|
||||
|
||||
**Konsequenz / Status:** `5_pose_estimation.py` sollte in appRobotHoming **nicht
|
||||
kalt** laufen, sondern mit dem `accumulated_state` der 4b-Kette als Startwert.
|
||||
**Umgesetzt:** `--from-state <json>` lädt einen flachen oder
|
||||
`{"accumulated_state": {...}}`-verpackten Zustand; `estimate_sequential_fk()`
|
||||
überspringt nur Blöcke, die **vollständig** im Startwert enthalten sind, und
|
||||
wendet auf alles andere weiterhin seinen normalen Multi-Start an — auch bei
|
||||
einem **unvollständigen** Seed (z. B. nur `x,y` aus einem abgebrochenen
|
||||
4b-Lauf) bleiben `z`/`a` also Multi-Start-geschützt, statt ungeschützt bei `0`
|
||||
zu starten. Verifiziert an drei echten Captures (s. „Validierung an echten
|
||||
appRobotHoming-Daten" unten).
|
||||
|
||||
### Validierung im Rendering-Projekt (Simulation, 10 Posen, bekannte GT)
|
||||
|
||||
| Verfahren | Winkel Ø [°] | Winkel schlechtest. [°] | Position Ø [mm] | Position schlechtest. [mm] |
|
||||
|---|---|---|---|---|
|
||||
| `sequential_vector` | 0,315 | 1,717 | 0,144 | 0,712 |
|
||||
| `sequential_fk` | 0,434 | 1,838 | 0,158 | 0,851 |
|
||||
| `global_ba` | 0,253 | 1,568 | 0,103 | 0,390 |
|
||||
| **`hybrid`** | **0,253** | **1,568** | **0,103** | **0,390** |
|
||||
|
||||
(Quelle: `appRobotRendering/doc/pipeline.tex`, Abschnitt „Validierung und Ergebnisse".)
|
||||
|
||||
### Validierung an echten appRobotHoming-Daten (2026-06-16)
|
||||
|
||||
Drei echte Captures aus `test/homing/` (nicht simuliert; vom Nutzer bereitgestellt,
|
||||
inkl. eines bereits aktualisierten 4b-Laufs):
|
||||
|
||||
| Fixture | 4b kam bis | Arm1-Marker gesehen | Ellbow-Marker gesehen |
|
||||
|---|---|---|---|
|
||||
| `20260616_120456` | Arm1 (Ellbow nicht gespeichert) | 197, 243 | 129, 132 |
|
||||
| `20260616_133151` | Arm2 | 198, 229 | 129, 132 |
|
||||
| `20260616_135403` | Arm2 | 197, 243 | 129, 132, 121 |
|
||||
|
||||
Geprüft für jede Fixture (`python scripts/5_pose_estimation.py … --from-state state_Arm2.json`,
|
||||
bzw. `state_Arm1.json` für die unvollständige erste Fixture):
|
||||
|
||||
- **Kein Crash**, trotz `Hand`/`Palm`/`FingerA`/`FingerB` aktuell ganz ohne
|
||||
Marker in `robot_1781069752019.json` (`"markers": []` an allen vieren) —
|
||||
`b`, `c`, `e` kommen als `confidence:"none"`, `"value": null` heraus, exakt
|
||||
wie gefordert ("Hand als unbekannt stehen lassen").
|
||||
- **Kalt vs. geseedet liefert dieselben Werte** (z. B. `133151`: `x=193.96mm,
|
||||
y=25.74°, z=-28.00°, a=-0.81°` in beiden Fällen) — der Seed verändert das
|
||||
Ergebnis nicht, wenn der Kaltstart bereits im richtigen Minimum lag; er
|
||||
schützt nur die Fälle, in denen das nicht so ist.
|
||||
- **Unvollständiger Seed** (`120456`, nur `x,y` aus `state_Arm1.json`, `z`/`a`
|
||||
fehlen): liefert dieselben Werte wie der volle Kaltstart — und durchläuft
|
||||
jetzt nachweislich den Multi-Start-Pfad für `z`/`a` (Code-Pfad geprüft, nicht
|
||||
nur Zufall des Ergebnisses).
|
||||
- Residuum über alle Marker: **4,3–4,5 mm RMS** (deutlich über der
|
||||
Simulationsvalidierung oben — erwartbar, reale Marker/Kameras sind
|
||||
verrauschter als der appRobotRendering-Renderfehler-Boden; noch keine
|
||||
`huber_delta_mm`/`normal_weight`-Nachjustierung vorgenommen).
|
||||
|
||||
---
|
||||
|
||||
## Vorteile
|
||||
|
||||
- **Bestes/stabilstes Verfahren im Rendering-Benchmark** (s. Tabelle oben) — unter
|
||||
allen vier Methoden der niedrigste Mittel- *und* Worst-Case-Fehler.
|
||||
- **Überbrückt markerlose Gelenke automatisch.** `Hand` (Variable `b`) und `Palm`
|
||||
(`c`) tragen keine eigenen Marker — `global_ba` zieht die Information aus den
|
||||
Fingermarkern *rückwärts* durch die Kette. Die 4b-Kette braucht dafür explizit
|
||||
einen Fallback pro Gelenk; hier passiert es als Nebenprodukt der gemeinsamen
|
||||
Optimierung.
|
||||
- **Fittet `x` und `y` gemeinsam aus denselben Arm1-Markern** (Block `{x,y}`,
|
||||
weil `Base` markerlos ist) — konsistenter als zwei getrennte Schätzungen.
|
||||
Ersetzt `estimateXFromMarkers()` aber **nicht**: dieser Block ist genau einer
|
||||
der beiden, die ohne guten Startwert anfällig für ein lokales Minimum sind
|
||||
(s. „Wichtige Einschränkung" unten) — die gemeinsame Schätzung ist also ein
|
||||
Mehrwert *nach* einer 4b-Vorschätzung, kein Grund, diese zu überspringen.
|
||||
- **Funktioniert mit nur 1 sichtbarem Marker pro Gelenk**, weil das Residuum
|
||||
Position **und** Normale nutzt (Gl. in `residual_vector()`) — die 4b-Primärmethode
|
||||
braucht dafür mindestens 2.
|
||||
- **Ist die automatisierte Form der bereits manuell durchgeführten Gegenrechnung.**
|
||||
Der Befund vom 2026-06-16 in `Homing_1_StepByStep.md` (Ellbow: Fallback-2 lag
|
||||
35–40° neben einer von Hand gerechneten Least-Squares-Kontrolle über Ellbow- *und*
|
||||
Arm2-Marker) ist exakt das, was `global_ba`/`hybrid` automatisch und für **alle**
|
||||
Gelenke gleichzeitig macht. Ein Lauf hätte den Fallback-2-Fehler vermutlich direkt
|
||||
erkennbar gemacht.
|
||||
- **Robuste Verlustfunktion (Huber)** dämpft einzelne Ausreißer-Marker (Fehldetektion,
|
||||
Verdeckung) automatisch, statt dass ein einzelner schlechter Marker das ganze
|
||||
Gelenk verfälscht.
|
||||
- **Multi-Start über mehrere Startwinkel** hilft dort, wo er greift (Blöcke mit
|
||||
genau einer Variable, z. B. Ellbow/`z`, Arm2/`a`) — für Homing wertvoller als
|
||||
für Kalibrierung, weil beim Einschalten die Pose komplett unbekannt ist. Greift
|
||||
aber **nicht** bei den gekoppelten Blöcken `{x,y}` und `{b,c,e}` (s. u.) — genau
|
||||
dort ist ein externer Startwert aus 4b nötig.
|
||||
|
||||
## Nachteile
|
||||
|
||||
- **Kein verlässlicher eigener Kaltstart — Startwert von außen zwingend nötig.**
|
||||
Wie im Abschnitt „Wichtige Einschränkung" hergeleitet: der interne Multi-Start
|
||||
deckt nur Einzelvariablen-Blöcke ab, nicht die gekoppelten Blöcke `{x,y}` und
|
||||
`{b,c,e}`. Allein aufgerufen (ohne `accumulated_state` aus 4b) ist
|
||||
`5_pose_estimation.py` daher beim Homing real gefährdet, in einem lokalen
|
||||
Minimum zu landen, statt die echte Pose zu finden — kein Rand-, sondern ein
|
||||
Kernfall, weil die Pose beim Einschalten grundsätzlich unbekannt ist.
|
||||
- ~~`scipy` fehlt im appRobotHoming-Container~~ — **behoben (2026-06-16):**
|
||||
`docker-compose.yaml` installierte nur `opencv-python-headless numpy`; ohne
|
||||
`scipy` greift `HAVE_SCIPY=False` und `estimate_sequential_fk`/`estimate_global_ba`
|
||||
fallen lautlos auf den Nullzustand zurück (nur eine `[WARN]`-Zeile, kein
|
||||
Fehler-Exit) — ein stiller Fehlmodus. `scipy` ist jetzt in der
|
||||
`pip3 install`-Zeile ergänzt (kein separater Image-Build nötig — `pip3
|
||||
install` läuft laut `command:` bei jedem Containerstart neu). **Noch nicht**
|
||||
auf einem laufenden Container wirksam geprüft — wirkt erst nach dessen
|
||||
nächstem Neustart.
|
||||
- **Zwei nichtlineare Least-Squares-Läufe statt eines geschlossenen Ausdrucks** —
|
||||
langsamer als `sequential_vector` und langsamer als ein einzelner
|
||||
`4b_revolute_angle.py`-Aufruf. Für „schnell, vollautomatisch" (Anspruch aus
|
||||
`Homing.md`) noch nicht auf echter Hardware gemessen.
|
||||
- **Kein progressives Zwischenergebnis.** Die 4b-Kette liefert nach jedem Link
|
||||
ein SSE-`analysis`-Event und aktualisiert den Board-Viewer live
|
||||
(„progressiver Update je erkanntem Gelenk", `Homing.md` → Implementierung).
|
||||
`estimate_pose()` gibt nur den fertigen Endzustand zurück — für dieselbe UX
|
||||
müsste man zusätzlich die internen Zwischenstände von `estimate_sequential_fk()`
|
||||
exponieren.
|
||||
- **Verliert die dokumentierte Fallback-Diagnostik.** `Homing_1_StepByStep.md`
|
||||
protokolliert pro Gelenk, *welche* Stufe gegriffen hat (`method`: primary /
|
||||
fallback_1 / fallback_2). `5_pose_estimation.py` liefert nur eine
|
||||
Block-Konfidenz (`high/medium/low/none`), nicht *welche* Heuristik intern
|
||||
gewirkt hat — weniger Transparenz beim Debuggen einzelner Gelenke.
|
||||
- **Ausgabeformat passt nicht direkt auf `/api/state`.** Der Endpunkt erwartet
|
||||
ein flaches `{x,y,z,a,b,c,e}` (`accumulated_state`, siehe
|
||||
`server/server.js` → `POST /api/homing/send-state`), `5_pose_estimation.py`
|
||||
schreibt verschachtelt (`movements.<var>.value`). Eine kleine Adapterfunktion
|
||||
ist nötig, kein Drop-in-Ersatz.
|
||||
- ~~Unbeobachtbare Gelenke werden als `0.0` ausgegeben, nicht als `null`~~ —
|
||||
**behoben (2026-06-16):** `main()`s Output-Writer schreibt jetzt `"value":
|
||||
null`, wenn `observable:false`, statt der internen `0.0` (die intern bleibt,
|
||||
weil die FK für die *anderen* Gelenke einen Zahlenwert braucht — nur der
|
||||
*Output*-Vertrag ändert sich). Verifiziert an allen drei Fixtures (`Hand`,
|
||||
`Palm`, `e` → `null`). Gilt nur für `5_pose_estimation.py` selbst — der
|
||||
Adapter zu `/api/homing/send-state` (nächster Punkt) muss `null` weiterhin
|
||||
korrekt durchreichen, nicht wieder in `0` zurückverwandeln.
|
||||
- **Noch nicht an echten Kamerabildern/Markern validiert.** Die Zahlen oben sind
|
||||
Simulation aus appRobotRendering (saubere FK-Marker-Positionen, definierter
|
||||
Renderfehler-Rauschboden). Reale Marker-Ungenauigkeiten (s.
|
||||
`Kalibrierung_Marker.md`) und reale Kameranoise könnten andere `huber_delta_mm`/
|
||||
`normal_weight`-Werte als die übernommenen Defaults verlangen.
|
||||
|
||||
## Besonderheiten
|
||||
|
||||
- **Reiner, unveränderter Import-Stand** — momentan git-`??` (untracked), noch
|
||||
nicht in `homingOrchestrator.js`/`server.js` referenziert (nur `4b_revolute_angle.py`
|
||||
ist dort als `SCRIPT_4B` verdrahtet).
|
||||
- **Schema-Kompatibilität zur lokalen `3b_corner_marker_poses.py` bereits
|
||||
geprüft:** Feldnamen `marker_id`, `position_mm`/`position_m`, `normal`,
|
||||
`num_cameras` stimmen 1:1 — `load_observations()` braucht keine Anpassung.
|
||||
- **Namens-Kollision mit `5_camera_z_refine.py`** — zwei Skripte teilen sich das
|
||||
Präfix `5_`. Entspricht der Konvention aus appRobotRendering, wo mehrere
|
||||
Dateien sich ein Stufen-Präfix teilen (z. B. `3_*`, `4_*`); kein Bug, aber beim
|
||||
Lesen der `scripts/`-Liste leicht zu verwechseln.
|
||||
- **Die `pose_estimation.method`-Option erlaubt gezieltes A/B-Testen** ohne
|
||||
Codeänderung: `--method sequential_vector|sequential_fk|global_ba|hybrid` per
|
||||
CLI-Override, oder dauerhaft über `robot_1781069752019.json` →
|
||||
`pose_estimation.method`. Nützlich, um den Effekt des Startwerts zu isolieren:
|
||||
einmal kalt (zeigt das Problem aus „Wichtige Einschränkung"), einmal mit
|
||||
`--from-state` und 4b-Startwert — als Regressionstest für genau diese
|
||||
Einschränkung (beide Aufrufe stehen unter „Aufruf (Stand-alone, zum Testen)").
|
||||
- **`finger_block_joints`/`per_link_method`** stehen schon (leer) in der
|
||||
robot.json — vorbereitete, aber im Skript bisher ungenutzte Erweiterungspunkte
|
||||
aus appRobotRendering.
|
||||
|
||||
---
|
||||
|
||||
## Kalibrier-Switch: Gelenk-Origin (`pose_estimation.fit_origin_link`)
|
||||
|
||||
**Motivation:** `doc/Kalibrierung.md` Schritt [4] bestimmt
|
||||
`links.Arm1.jointToParent.origin[1,2]` (Y/Z des Schultergelenk-Drehpunkts) geometrisch
|
||||
aus einer dedizierten 3-Pose-Aufnahme (Marker-Mittelpunkte, keine Normalen). Diese
|
||||
Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen" — `5_pose_estimation.py` hat mit
|
||||
Position+Normale und einem robusten Least-Squares-Löser bereits die Bausteine, um
|
||||
denselben Drehpunkt aus den ohnehin vorhandenen Homing-Aufnahmen zu verfeinern.
|
||||
|
||||
**Ein Schalter, eine Stelle:** `robot.json` → `pose_estimation.fit_origin_link`
|
||||
(aktuell `"Arm1"`). Wenn gesetzt, gibt `estimate_global_ba()` für diesen Link
|
||||
`jointToParent.origin[1,2]` (Y,Z) als **2 zusätzliche Variablen derselben
|
||||
Optimierung** frei (kein separater Lauf, keine Restore-Logik, keine eigene
|
||||
Funktion) — die Gelenkvariable und der Drehpunkt werden **gemeinsam** bestimmt.
|
||||
Bei Erfolg übernimmt `main()` das Ergebnis automatisch: `patch_robot_json_origin()`
|
||||
schreibt nur die eine `"origin": [...]`-Zeile des Links in `robot.json` zurück
|
||||
(Text-Patch, nicht `json.dump()` — der Rest der handgepflegten, kompakten Datei
|
||||
bleibt unverändert). `null`/Feld weglassen = aus, keine Verhaltensänderung.
|
||||
|
||||
### Befund an echten Daten (drei reale Captures, sequenziell, 2026-06-16)
|
||||
|
||||
| Lauf | Fixture | Origin Y / Z danach | Δ zum Vorlauf |
|
||||
|---|---|---|---|
|
||||
| 1 | `20260616_133151` | 108,28 / 34,81 | +7,18 / −20,39 (ggü. ursprünglich 101,1 / 55,2) |
|
||||
| 2 | `20260616_135403` | 108,84 / 34,89 | +0,57 / +0,08 |
|
||||
| 3 | `20260616_120456` (unvollst. Seed) | 107,42 / 35,33 | −1,43 / +0,45 |
|
||||
|
||||
Drei unabhängige Aufnahmen (unterschiedliche Marker, unterschiedliche Posen) landen
|
||||
im selben Bereich, und die Schritte werden **kleiner statt größer** — spricht für
|
||||
Konvergenz, nicht für Rauschen/Drift. (Für die Doku danach wieder auf den
|
||||
Ursprungswert `101.1, 55.2` zurückgesetzt — die Tabelle zeigt nur den Testlauf.)
|
||||
**Konsequenz des „bei jedem Lauf automatisch":** der Wert wandert mit jeder neuen
|
||||
Aufnahme leicht weiter, statt einmalig fixiert zu werden — gewünscht laut Nutzer,
|
||||
aber gut zu wissen. Schalter auf `null` setzen, um das einzufrieren.
|
||||
|
||||
Ergänzt, ersetzt nicht: `doc/Kalibrierung.md` Schritt [4] (3-Pose-Kreis-Fit, nur
|
||||
Marker-Mittelpunkte) bleibt die unabhängige Gegenmessung. Die Achs**richtung**
|
||||
(`jointToParent.axis`) fitten beide Verfahren nicht.
|
||||
|
||||
---
|
||||
|
||||
## Aufruf (Stand-alone, zum Testen)
|
||||
|
||||
**Empfohlen — mit Startwert aus der 4b-Kette** (z. B. dem letzten vorhandenen
|
||||
`state_*.json`; unvollständig ist ok, fehlende Variablen bleiben Multi-Start-geschützt):
|
||||
|
||||
```bash
|
||||
python scripts/5_pose_estimation.py data/homing/<run>/aruco_marker_poses.json \
|
||||
-robot scripts/robot_1781069752019.json \
|
||||
--from-state data/homing/<run>/state_Arm2.json \
|
||||
-out data/homing/<run>/robot_state.json
|
||||
```
|
||||
|
||||
**Kalt** (kein `--from-state`) — funktioniert weiterhin identisch wie vor diesem
|
||||
Code-Hook, aber ohne den oben beschriebenen Schutz für gekoppelte Blöcke;
|
||||
nützlich, um das Kaltstart-/Lokales-Minimum-Verhalten aus „Wichtige
|
||||
Einschränkung" gezielt zu reproduzieren/regressionszutesten:
|
||||
|
||||
```bash
|
||||
python scripts/5_pose_estimation.py data/homing/<run>/aruco_marker_poses.json \
|
||||
-robot scripts/robot_1781069752019.json
|
||||
# Verfahren erzwingen, z.B. zum gezielten Vergleich einzelner Methoden:
|
||||
python scripts/5_pose_estimation.py data/homing/<run>/aruco_marker_poses.json \
|
||||
-robot scripts/robot_1781069752019.json --method global_ba
|
||||
```
|
||||
|
||||
Gegen die echten Testdaten in `test/homing/*/` ausprobiert — siehe
|
||||
„Validierung an echten appRobotHoming-Daten" oben.
|
||||
|
||||
---
|
||||
|
||||
## Integrationsschritte (Offene Punkte)
|
||||
|
||||
**Erledigt (2026-06-16):**
|
||||
|
||||
- [x] **Architektur entschieden:** 4b-Kette läuft zuerst und liefert den
|
||||
`accumulated_state` als Startwert; `5_pose_estimation.py` läuft danach als
|
||||
globaler Verfeinerungsschritt darüber. Kein Ersatz, keine parallele
|
||||
Alternative — siehe „Wichtige Einschränkung" oben.
|
||||
- [x] **`scipy` in `docker-compose.yaml` ergänzt** (`pip3 install … numpy scipy`).
|
||||
- [x] **Code-Hook `--from-state`:** `load_seed_state()` (akzeptiert flach oder
|
||||
`{accumulated_state:{...}}`) + `estimate_sequential_fk(..., seed=...)`
|
||||
überspringt nur vollständig geseedete Blöcke, alles andere bleibt
|
||||
Multi-Start-geschützt. `estimate_pose(..., seed=...)` reicht das durch.
|
||||
Verifiziert an 3 echten Fixtures (s. „Validierung an echten
|
||||
appRobotHoming-Daten").
|
||||
- [x] **Robustheit gegen fehlende Marker:** `Hand`/`Palm`/`FingerA`/`FingerB`
|
||||
(aktuell `"markers": []`) laufen ohne Crash durch, Output zeigt `null`/
|
||||
`confidence:"none"` statt erfundener `0`. `main()`s Output-Writer mappt
|
||||
`observable:false → value:null` (intern bleibt `0.0` für die FK-Rechnung
|
||||
der anderen Gelenke — nur der Output-Vertrag ändert sich).
|
||||
- [x] **Kalibrier-Switch `pose_estimation.fit_origin_link`** umgesetzt — ein
|
||||
Konfig-Feld, direkt in `estimate_global_ba()` integriert (keine separate
|
||||
Funktion/Report/CLI-Flag mehr), übernimmt das Ergebnis automatisch in
|
||||
`robot.json` (`patch_robot_json_origin()`, Text-Patch). Generisch für jeden
|
||||
Link, aktuell für `Arm1` aktiviert. Details: eigener Abschnitt oben.
|
||||
|
||||
**Noch offen:**
|
||||
|
||||
- [ ] **Adapter** `movements.<var>.value` → flaches `{x,…,e}`-State-Objekt für
|
||||
`POST /api/homing/send-state`; `null` muss `null` bleiben (nicht zurück zu `0`).
|
||||
- [ ] **Anbindung in `homingOrchestrator.js`** (neuer Schritt nach der 4b-Schleife,
|
||||
SSE-Events) — **Umfang/Fehlerfall/Sende-Politik an die Robotersteuerung sind
|
||||
noch nicht festgelegt** (offene Rückfrage vom 2026-06-16, noch unbeantwortet:
|
||||
Minimal-Fix vs. Voll-Integration; Abbruch vs. Fallback bei Fehler; Senden vs.
|
||||
nur Anzeigen). Diese drei Entscheidungen zuerst klären, dann verdrahten.
|
||||
- [ ] **Arm1-Origin-Wert beobachten:** wandert bei jedem Lauf mit `fit_origin_link`
|
||||
leicht weiter (s. Befund-Tabelle oben, wird kleiner/konvergiert bisher). Falls
|
||||
das nicht erwünscht ist: Schalter nach dem Einschwingen auf `null` setzen, oder
|
||||
gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken.
|
||||
- [ ] **`fit_origin_link` in der Kalibrierung-UI sichtbar machen** (`doc/Kalibrierung.md`
|
||||
Schritt [4]) — aktuell nur in `robot.json` umschaltbar; Tab „Arm1 – Y" könnte
|
||||
den aktuellen Wert/letzte Änderung neben Verfahren B anzeigen.
|
||||
- [ ] **Mehrpose-Erweiterung** (mehrere `aruco_marker_poses.json` mit
|
||||
gemeinsamem `origin`, je Pose ein eigener Gelenkwinkel, ein gemeinsamer Solve)
|
||||
— würde die Winkel/Origin-Korrelation aus einer Einzelpose weiter reduzieren,
|
||||
analog zur bestehenden 3-Pose-Aufnahme.
|
||||
- [ ] `huber_delta_mm`/`normal_weight` ggf. gegen reale Marker-Genauigkeit
|
||||
nachjustieren — reales Residuum (2,4–4,5 mm RMS) liegt deutlich über der
|
||||
Simulation; Defaults sind unverändert aus appRobotRendering übernommen.
|
||||
- [ ] Python-Tests (`pytest`) für `load_seed_state()`, den Block-Skip in
|
||||
`estimate_sequential_fk()` und die Origin-Erweiterung in `estimate_global_ba()`
|
||||
— aktuell nur manuell gegen die drei Fixtures verifiziert (s. oben);
|
||||
appRobotHoming hat bisher keine Python-Testinfrastruktur (nur Jest/JS).
|
||||
- [ ] Eintrag in `Homing.md`-Tabelle (Doku-Übersicht) ergänzen, sobald
|
||||
`homingOrchestrator.js` verdrahtet ist.
|
||||
|
||||
---
|
||||
|
||||
## Verweise
|
||||
|
||||
- Allgemeiner Ablauf: [`Homing.md`](Homing.md)
|
||||
- Vorheriger Schritt (Kamera/Triangulation, liefert den gemeinsamen Input):
|
||||
[`Homing_0_Camera.md`](Homing_0_Camera.md)
|
||||
- Vorstufe (4b-Kette, liefert den hier benötigten Startwert):
|
||||
[`Homing_1_StepByStep.md`](Homing_1_StepByStep.md)
|
||||
- Ursprung & Validierung: Projekt **`appRobotRendering`**,
|
||||
`pipeline/pose_estimation.py` + `doc/pipeline.tex` (Abschnitte „Pose-Estimation:
|
||||
Vier Schätzverfahren" und „Validierung und Ergebnisse").
|
||||
365
doc/Homing_5_Pose_MultiPoint_Weighted.md
Normal file
365
doc/Homing_5_Pose_MultiPoint_Weighted.md
Normal file
@@ -0,0 +1,365 @@
|
||||
# Homing 5 – Verbesserungspfade: Mehrpunkt-Residuen & Gewichtung
|
||||
|
||||
> Ergänzt [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche
|
||||
> Verbesserungen, die alle an derselben Stelle ansetzen: *was* ein Marker als
|
||||
> Messung beiträgt und *wie stark* sie zählt.
|
||||
>
|
||||
> **Status (2026-06-25):** **Qualitäts-Gewichtung** (Doc-Punkt 3 / Schritte 1+2)
|
||||
> umgesetzt. **Mehrpunkt-Eckresiduen** (Doc-Punkt 1 / Schritte 3+4) als
|
||||
> Opt-in-Modus `corner_points` implementiert, aber nach gescheitertem
|
||||
> Live-Test wieder **DEAKTIVIERT** — robot.json steht auf `corner_pose`
|
||||
> (bewährter Zustand). Der Code bleibt inaktiv im Repo. Ursache + Vorbedingungen
|
||||
> für eine erneute Aktivierung: Notiz unten. **Einzelkamera-Einbindung**
|
||||
> (Doc-Punkt 2 / Schritte 5+6) weiterhin **offen**.
|
||||
|
||||
---
|
||||
|
||||
## Ausgangslage (aktueller Code)
|
||||
|
||||
Pro Marker fließen heute genau **6 Residuen** in `5_pose_estimation.py`s
|
||||
`residual_vector()` ein: 3× Position (`pos_mm`) + 3× Normale (skaliert mit
|
||||
`normal_weight`, Default 100). Beide kommen aus `aruco_marker_poses.json`,
|
||||
geschrieben von `3b_corner_marker_poses.py`:
|
||||
|
||||
```
|
||||
3b: trianguliert 4 Ecken (DLT, je Ecke eigene Multi-View-Triangulation)
|
||||
→ position_m/mm = Schwerpunkt der 4 Ecken
|
||||
→ normal = SVD-Ebenen-Fit durch die 4 Ecken
|
||||
→ corners_m = die 4 Ecken selbst (steht im JSON, wird aber von
|
||||
5_pose_estimation.py nie gelesen)
|
||||
```
|
||||
|
||||
Alle drei folgenden Punkte hängen an genau dieser Kette.
|
||||
|
||||
---
|
||||
|
||||
## 1. Vier Eckpunkte statt Center+Normal
|
||||
|
||||
**Frage des Nutzers:** „Mit vier Eckpunkten hätten wir weit mehr Statistik,
|
||||
und nicht die zusammengefassten Werte, oder?" — ja.
|
||||
|
||||
**Befund:** `corners_m` steht bereits in `aruco_marker_poses.json`
|
||||
(`3b_corner_marker_poses.py:166`), wird aber von
|
||||
`load_observations()` (`5_pose_estimation.py:110`) nicht gelesen. Center und
|
||||
Normale sind beide bereits verlustbehaftete Zusammenfassungen der 4 Ecken
|
||||
(Mittelwert bzw. SVD-Ebenen-Fit) — der eigentliche Fit in `residual_vector()`
|
||||
sieht nur noch diese zusammengefasste Version, nicht die Rohdaten.
|
||||
|
||||
**Idee:** Pro Marker 4×3=12 Eck-Residuen statt 3+3 Center/Normal-Residuen.
|
||||
Vorteile:
|
||||
- Mehr unabhängige Messpunkte → realistischere Statistik/Unsicherheit.
|
||||
- Eine einzelne schlecht erkannte Ecke (Verdeckung, Blur) verzerrt nicht mehr
|
||||
automatisch Center *und* Normale des ganzen Markers — der robuste
|
||||
Huber-Verlust könnte direkt auf Eck-Ebene greifen statt auf Marker-Ebene.
|
||||
- Lage *und* Orientierung kommen aus denselben 4 Punkten — keine separate,
|
||||
zusätzlich verlustbehaftete Normalen-Schätzung nötig.
|
||||
|
||||
**Nötig dafür:**
|
||||
- `robot_fk.py`: neue Methode (analog `marker_world()`), die die 4 lokalen
|
||||
Eckpunkte eines Markers (aus `marker["size"]` + ArUco-Eckreihenfolge/Spin-
|
||||
Konvention, schon bekannt aus `corner_plane_normal()` und
|
||||
`doc/Kalibrierung_Marker.md`) ins Weltsystem transformiert.
|
||||
- `5_pose_estimation.py`: `load_observations()` um `corners_m` erweitern,
|
||||
`residual_vector()` um einen dritten Modus neben den bestehenden
|
||||
`"corner_pose"`/`"center_point"`-Werten von `marker_observation`.
|
||||
|
||||
**Risiko:** mittel. Die Spin/Eckreihenfolge-Konvention muss exakt stimmen,
|
||||
sonst systematischer Bias statt Verbesserung. Gut gegen Simulations-GT
|
||||
(appRobotRendering) prüfbar, bevor an reale Daten.
|
||||
|
||||
---
|
||||
|
||||
## 2. Einzelkamera-Marker einbeziehen statt verwerfen
|
||||
|
||||
**Frage des Nutzers:** Komplett verworfene 1-Kamera-Marker seien vermutlich
|
||||
schlechter als sie schwach gewichtet einzubeziehen — insbesondere relevant für
|
||||
zukünftige Finger-Marker, wo oft zwei Kameras je einen *anderen* Marker sehen.
|
||||
|
||||
**Befund — zwei Stellen verwerfen heute hart:**
|
||||
- `3b_corner_marker_poses.py:142`: `if len(cam_ids) < args.minCams: continue`
|
||||
(Default `--minCams 2`) — ein 1-Kamera-Marker bekommt **gar keinen Eintrag**
|
||||
in `aruco_marker_poses.json`. Das passiert schon **vor** `5_pose_estimation.py`.
|
||||
- `5_pose_estimation.py`s `load_observations(..., min_cams=2)` filtert
|
||||
zusätzlich — meist wirkungslos, weil 3b den Marker oft schon gar nicht erst
|
||||
schreibt.
|
||||
|
||||
### Reprojektions-Residuum statt Single-View-PnP (bevorzugter Weg)
|
||||
|
||||
**Ursprünglich angenommener Weg — Single-View-PnP — explizit nicht bevorzugt.**
|
||||
`triangulate_multiview()` (3b) ist Multi-View-DLT — mit 1 Kamera unterbestimmt,
|
||||
ein einzelnes Bild kann keinen 3D-Punkt triangulieren. Der naheliegende Ersatz
|
||||
wäre Single-View-PnP (bekannte Kantenlänge `size` als Skalen-Anker) — das hat
|
||||
aber die bekannte **Flip-Ambiguität** (zwei spiegelbildliche Lösungen aus
|
||||
einer Ansicht, vgl. appRobotRendering `scene_reconstruct.py`). Nur als
|
||||
**Fallback** vorgesehen ("besser als nichts"), explizit nicht der Hauptweg.
|
||||
|
||||
**Bevorzugt: Reprojektion in den bestehenden globalen Fit, keine eigene
|
||||
Marker-Pose.** Kamera A ist gegen das Board bereits voll posenbestimmt
|
||||
(Stufe 2: `K`,`D`,`R`,`t` bekannt). Statt für einen 1-Kamera-Marker einen
|
||||
eigenständigen (zwangsläufig mehrdeutigen) 3D-Punkt zu rekonstruieren, geht er
|
||||
direkt als **Bild-Residuum** in denselben globalen Zustand `q` ein:
|
||||
|
||||
```
|
||||
für den Gelenkzustand q (einzige Unbekannte — keine separate Marker-Pose):
|
||||
P_k(q) = T_link(q) · p_k_lokal (4 Eckpunkte via FK)
|
||||
û_k = project(K_A, D_A, R_A, t_A; P_k(q)) (Reprojektion in Kamera A)
|
||||
residual_k = û_k − u_k_beobachtet (Pixel-Differenz je Ecke)
|
||||
```
|
||||
|
||||
Keine Flip-Ambiguität, weil keine unabhängige Marker-Pose gesucht wird — nur
|
||||
`q` (7 Variablen, durch alles andere im Modell schon mitbestimmt). Ein falscher
|
||||
`q` projiziert einfach sichtbar daneben, statt eine zweite gültige Lösung zu
|
||||
erzeugen.
|
||||
|
||||
**„Kette von Triangulations-Optionen" (Nutzeridee):** Geteilte Variablen
|
||||
(z. B. `e` bei FingerA/FingerB) werden in `analyze_chain()` schon heute in
|
||||
**einen** Block zusammengefasst — sind beide Finger-Marker einzeln
|
||||
triangulierbar (je ≥2 Kameras), bestimmen sie `e` schon jetzt gemeinsam. Neu
|
||||
wäre: auch wenn FingerA nur von Kamera 1 und FingerB nur von Kamera 2 gesehen
|
||||
wird (keiner einzeln triangulierbar), könnten zwei Reprojektions-Residuen
|
||||
(beide Funktionen desselben `q`) den geteilten Freiheitsgrad **gemeinsam**
|
||||
einschränken — ohne dass einer der beiden Marker für sich eine vollständige
|
||||
Pose bräuchte. Passt zur vom Nutzer ausdrücklich erlaubten groben Genauigkeit
|
||||
(±10°/±10 mm reicht für die Finger völlig).
|
||||
|
||||
**Bezug zu Finger-Positionen:** `Hand`/`Palm`/`FingerA`/`FingerB` haben aktuell
|
||||
**0 Marker** in `robot.json`. Sobald dort Marker ergänzt werden, dürften sie
|
||||
wegen Größe/Beweglichkeit/Verdeckung oft nur von 1 Kamera gesehen werden — mit
|
||||
der harten ≥2-Kamera-Regel blieben diese Gelenke dann trotz vorhandener Marker
|
||||
oft `confidence:"none"`. Die Teilbaum-Logik in `observability()` hilft nur,
|
||||
wenn irgendein Marker *im Teilbaum* ≥2 Kameras hat — nicht, wenn alle
|
||||
Finger-Marker isoliert nur je 1 Kamera sehen.
|
||||
|
||||
**Nötig dafür (deutlich größer als „Schwellwert ändern"):**
|
||||
- 3b (oder ein neuer Zwischenschritt): 1-Kamera-Beobachtungen nicht verwerfen,
|
||||
sondern mit Kamera-Referenz + rohen 2D-Eckpunkten aufnehmen.
|
||||
- `5_pose_estimation.py`: `load_observations()`/`estimate_pose()` müssten
|
||||
zusätzlich Kamerakalibrierung (`{cam}_camera_pose.json`, NPZ-Intrinsik)
|
||||
einlesen können — heute reicht `aruco_marker_poses.json` allein.
|
||||
- `residual_vector()`: zweiter Residuumstyp (Pixel statt mm) **gemeinsam** mit
|
||||
den bestehenden mm-Residuen optimiert, mit eigenem Gewicht (analog
|
||||
`normal_weight`, aber für „Pixel vs. Millimeter").
|
||||
|
||||
**Risiko:** primär die **Gewichtung zwischen den beiden Residuumstypen** —
|
||||
falsch skaliert, dominiert einer den Fit und verschlechtert sogar die heute
|
||||
schon gut funktionierenden ≥2-Kamera-Marker. Architektonisch größer als
|
||||
Punkt 1/3, aber ohne die Flip-Problematik des ursprünglich angenommenen Wegs.
|
||||
|
||||
---
|
||||
|
||||
## 3. Marker-Qualitäts-Gewichtung (Größe/Schärfe/Distanz/Kontrast)
|
||||
|
||||
**Vom Nutzer bestätigt:** bewusst (provisorisch) entfernt — brachte in
|
||||
Simulationen wenig, Option soll aber offen bleiben.
|
||||
|
||||
**Befund — die Bausteine existieren bereits, sind aber nicht verbunden:**
|
||||
- `1_detect_aruco_observations.py` berechnet pro Detektion bereits:
|
||||
`area_px`, `sharpness` (Laplace-Varianz, `compute_sharpness()`), `contrast`/
|
||||
`dynamic_range` (`compute_contrast()`), `distance_to_border_px`, kombiniert
|
||||
zu einem `confidence`-Score (`compute_confidence()`) — geschrieben ins
|
||||
`quality`-Feld jeder Detektion in `{cam}_aruco_detection.json`.
|
||||
- `robot.json` hat dafür ein fertiges Schema: `observation_weighting`
|
||||
(`distance_weight`, `marker_size_weight`, `view_angle_weight`) und
|
||||
`multiview_calculation` (`size_factor`, `sharpness_factor`, `border_factor`,
|
||||
`homography_factor`, `spin_factor`, `weight_floor`, ...).
|
||||
- **Aber:** Dieses Schema liest nur `3_multiview_bundle_adjustment_v4.py` —
|
||||
ein Skript, das im Homing-Pfad **nicht** läuft (Homing nutzt
|
||||
`3b_corner_marker_poses.py`).
|
||||
- `3b_corner_marker_poses.py`s `load_cameras()` liest aus der Detection-JSON
|
||||
nur `camera_matrix`, `distortion_coefficients`, `image_points_px` — das
|
||||
`quality`-Feld wird **nie gelesen**. Es geht also schon hier verloren, bevor
|
||||
es überhaupt zu `aruco_marker_poses.json` käme.
|
||||
|
||||
**Idee (falls reaktiviert):** Die Qualitätsmaße existieren schon ganz am
|
||||
Anfang der Pipeline — nur durchreichen nötig: 3b liest `quality`/`confidence`
|
||||
aus der Detection-JSON und schreibt einen `weight`-Wert pro Marker in
|
||||
`aruco_marker_poses.json`; `5_pose_estimation.py`s `residual_vector()` skaliert
|
||||
das jeweilige Markerresiduum damit (analog zu `normal_weight`, aber pro Marker
|
||||
statt global für alle).
|
||||
|
||||
**Mögliche Erklärung, warum es in Simulation wenig brachte:** Der
|
||||
appRobotRendering-Renderfehler-Boden (`markerOffsetMaxMm`, `sensorNoise`, …
|
||||
aus `renderingInfo`) ist recht gleichförmig über alle Marker — wenig echte
|
||||
Qualitätsunterschiede zum Gewichten. Bei echten Kameras (Beleuchtung,
|
||||
Entfernung, Bewegungsunschärfe) könnte die Streuung größer und der Effekt
|
||||
dadurch sichtbarer sein — das ist der Grund, die Option offen zu halten.
|
||||
|
||||
**Nötig dafür:** klein und lokal begrenzt — nur 2 Stellen (3b: `quality`
|
||||
durchreichen; `5_pose_estimation.py`: Gewicht im Residuum nutzen).
|
||||
`1_detect_aruco_observations.py` und das `robot.json`-Schema müssen nicht
|
||||
angefasst werden, die liegen schon bereit.
|
||||
|
||||
**Risiko:** niedrig (reines Durchreichen + ein Faktor) — der Aufwand liegt im
|
||||
erneuten Validieren (Simulation + reale Daten), nicht im Code selbst.
|
||||
|
||||
---
|
||||
|
||||
## Zusammenhang der drei Punkte
|
||||
|
||||
Alle drei ändern letztlich dasselbe: was als „eine Messung" zählt und wie
|
||||
stark sie zählt. Sie sind unabhängig umsetzbar, aber am Ende würde man sie
|
||||
zusammenführen wollen:
|
||||
|
||||
```
|
||||
heute: residual = [Δposition, Δnormal × normal_weight] (6 Werte/Marker, nur ≥2-Kamera-Marker)
|
||||
1: residual = [Δcorner_0 .. Δcorner_3] × marker_weight (12 Werte/Marker, weiterhin ≥2-Kamera)
|
||||
2: + Reprojektions-Residuen für 1-Kamera-Marker (neuer Typ, eigene Gewichtung)
|
||||
3: marker_weight zusätzlich nach Quality-Score aus Stufe 1
|
||||
```
|
||||
|
||||
**Geschätzte Reihenfolge nach Aufwand/Risiko** (keine Festlegung, nur
|
||||
Einschätzung): 3 (niedrig, reines Durchreichen) → 1 (mittel, FK-Erweiterung,
|
||||
gut simulationstestbar) → 2 (architektonisch am größten — neuer Residuumstyp
|
||||
+ Kamerakalibrierung als zusätzlicher Input —, aber ohne Flip-Risiko, seit
|
||||
Reprojektion statt PnP der bevorzugte Weg ist).
|
||||
|
||||
---
|
||||
|
||||
## Umsetzungsplan (ToDo)
|
||||
|
||||
Reihenfolge folgt der Risiko-Einschätzung oben: erst risikoarmes Durchreichen,
|
||||
dann die beiden strukturellen Erweiterungen. Jeder Schritt ist einzeln
|
||||
umsetzbar und (wo möglich) einzeln testbar, bevor der nächste beginnt.
|
||||
|
||||
> **Status-Legende:** ✅ erledigt · ⬜ **offen**
|
||||
>
|
||||
> **Punkt-Zuordnung** (Doc-Abschnitt ↔ Chat-Nummerierung): Doc-Punkt 1 „Vier
|
||||
> Eckpunkte" = Schritte 3+4 · Doc-Punkt 2 „Einzelkamera" = Schritte 5+6 ·
|
||||
> Doc-Punkt 3 „Qualitäts-Gewichtung" = Schritte 1+2 (erledigt).
|
||||
|
||||
| # | Status | Schritt | Testbar an | Bricht Bestehendes? |
|
||||
|---|---|---|---|---|
|
||||
| 1 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `quality`/`confidence` aus `{cam}_aruco_detection.json` bis nach `aruco_marker_poses.json` durchreichen (3b liest es, schreibt ein neues `weight`-Feld pro Marker) | Diff der Ausgabedatei: nur das neue Feld ist zusätzlich da, alles andere (Position, Normale, …) identisch zu vorher — reiner Additivitätstest | **Nein.** Rein additives Feld, kein Pflichtfeld, alte Leser ignorieren es |
|
||||
| 2 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `residual_vector()` nutzt das neue Gewicht, hinter einem Schalter (`pose_estimation.use_marker_weight`, Default `false`) | A/B-Vergleich auf den appRobotRendering-Simulationsszenen (mit/ohne Schalter) gegen bekannte Grundwahrheit — genau der Test, der laut Nutzer beim ersten Versuch wenig brachte, jetzt wiederholbar | **Nein bei Default aus.** Mit `true`: Ergebnisse ändern sich gewollt — muss vor Produktiv-Default separat validiert werden |
|
||||
| 3 | ✅ erledigt (2026-06-25) | **(Doc-Punkt 1)** `robot_fk.py`: neue Methode liefert die 4 lokalen Eckpunkte eines Markers im Weltsystem (Baustein, noch ohne Anbindung) | Isoliert testbar, ganz ohne `5_pose_estimation.py`: gegen die wahren Eckpositionen aus `render_*.json` (Simulation liefert das schon) | **Nein.** Neue, bisher von niemandem aufgerufene Methode |
|
||||
| 4 | 🔴 Code da, aber DEAKTIVIERT (live gescheitert 2026-06-25 → robot.json zurück auf `corner_pose`) | **(Doc-Punkt 1)** Neuer `marker_observation`-Modus (z. B. `"corner_points"`) nutzt die 12 Eck-Residuen statt 6 Center/Normal-Residuen | Direkter Vorher/Nachher-Vergleich gegen dieselbe Validierungstabelle wie in `Homing_5_Pose.md` (10 Simulationsposen, bekannte GT) | **Nein als Opt-in** (Default bleibt `"corner_pose"`). Tuning-Punkt: `huber_delta_mm` ist auf die heutige Residuumsgröße kalibriert — mit 12 statt 6 Werten/Marker verschiebt sich die RMS-Größenordnung, müsste neu eingeordnet werden |
|
||||
| 5 | ⬜ **offen** (Vorarbeit/Guards ✅) | **(Doc-Punkt 2)** 3b nimmt 1-Kamera-Beobachtungen mit auf (rohe 2D-Ecken + Kamera-Referenz), statt sie zu verwerfen | Output-Diff: nur neue Einträge für vorher fehlende Marker, bestehende ≥2-Kamera-Einträge unverändert | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird |
|
||||
| 6 | ⬜ **offen** | **(Doc-Punkt 2)** `residual_vector()` um Reprojektions-Residuen erweitert; `load_observations()`/`estimate_pose()` lesen zusätzlich Kamerakalibrierung | Zuerst an Simulationsszenen, bei denen gut beobachtete Marker künstlich auf „nur 1 Kamera" reduziert werden (GT bekannt) — saubere Kontrolle, ob das Residuum tatsächlich hilft, bevor reale Finger-Marker überhaupt existieren | **Nein strukturell** (additiver Residuumstyp), aber Regressionsrisiko durch falsche mm/px-Gewichtung — vor Produktiv-Default gegen die bestehende Validierungstabelle gegenprüfen (wie Schritt 4) |
|
||||
| 7 | ⬜ **offen** | Zusammenführen: ein gemeinsames Gewichtsschema (Quality × Kamera-Anzahl × Residuumstyp) statt drei separater Schalter | End-to-End gegen Simulationsbenchmark **und** die drei realen Fixtures aus `Homing_5_Pose.md` | **Nein**, wenn alle Vorstufen additive Defaults hatten |
|
||||
|
||||
### Recherche zu Schritt 5: wer liest `aruco_marker_poses.json`? (2026-06-16)
|
||||
|
||||
Alle Dateien mit Referenz auf `aruco_marker_poses` im Projekt durchsucht und
|
||||
geprüft, ob sie `position_mm` (o. ä.) ungeschützt voraussetzen oder schon einen
|
||||
Guard haben:
|
||||
|
||||
| Datei | Nutzung | Wenn `position_mm` fehlt | Guard schon da? |
|
||||
|---|---|---|---|
|
||||
| `public/yAxisCompute.js:109-111` | Y-Rotationsachse Base↔Arm1 (Kalibrierung [4], Kreisfit über 3 Posen) | Guard eingefügt (Z. 109–114): `Array.isArray()`-Check auf alle drei Posen, fehlende landen im `skipped`-Log statt Crash | ✅ Ja (2026-06-16) |
|
||||
| `public/boardViewer.html` | X-Achsen-Richtung (Kalibrierung [3]) **und** Y-Achsen-Viewer **und** allgemeiner Pos-A/B/C-Vergleich | `hasXYZ()`-Helper (Z. 220–226) + Pre-Filterung der `_*FremdMarkers`-Arrays beim Laden (Z. 1069/1107/1140); direkte Zugriffe in `buildCompareLines()` (Z. 892, 915–916) sicher, weil nur pre-gefilterte Marker in den Arrays stehen | ✅ Ja (2026-06-16) |
|
||||
| `public/homing.js:96` | Homing-Marker-Tabelle | Keiner — `m.position_mm ?? [null,null,null]` | ✅ Ja |
|
||||
| `server/editRobot.js` → `assignByZRange()` | Marker-Z-Bereich-Zuordnung (Kalibrierung „Board"-Tab) | Keiner — `Array.isArray(emPos)`-Check, sonst `continue` | ✅ Ja |
|
||||
| `server/editRobot.js` → `alignSetToMeasured()` | Set-Ausrichtung (Kabsch-Fit) | Keiner — Marker ohne `position_mm` werden beim Aufbau der Messwert-Map einfach ausgelassen | ✅ Ja |
|
||||
| `server/editRobot.js` → `assignMarkerId()` | Einzelnen Marker manuell per ID zuordnen | Guard eingefügt (vor Z. 379): `Array.isArray(em.position_mm)`-Check — fehlende Position gibt klare Fehlermeldung statt Crash | ✅ Ja (2026-06-16) |
|
||||
| `scripts/4_yAxis_rotation_reconstruction.py` | Python-Variante der Y-Achsen-Rekonstruktion (offline, parallel zu `yAxisCompute.js`) | Guard eingefügt (Z. 165–174): expliziter `None`-Check statt `.get(..., [0,0,0])` — fehlende Messung landet mit klarem Grund im `skipped`-Log | ✅ Ja (2026-06-16) |
|
||||
| `scripts/9_evaluateMarker.py` | Offline-Benchmark gegen Simulations-GT, **nicht** im Live-Homing-Pfad | **Crash** — `o["position_m"]` ohne `.get()` | ❌ Nein, aber kein Produktionscode |
|
||||
| `public/client.js` | Nur CSV-Anzeige/Zahlenformat | Keine Berechnung, nur Darstellung | n/a |
|
||||
|
||||
**Bestätigt deine Vermutung, aber breiter als gedacht:** Es betrifft tatsächlich
|
||||
nur die **X-Achsen-** und **Y-Rotationsachsen-Kalibrierung** (Schritt [3]/[4]
|
||||
in `Kalibrierung.md`) plus den Offline-Benchmark — aber **nicht nur ein
|
||||
Filter an einer Stelle**, sondern `yAxisCompute.js` **und** mehrere Stellen in
|
||||
`boardViewer.html` (das Viewer-File bedient beide Kalibrierschritte). Die
|
||||
Homing-Seite selbst (`editRobot.js`, `homing.js`) ist bereits robust.
|
||||
|
||||
**Guards umgesetzt (2026-06-16) — alle relevanten Stellen:**
|
||||
- `yAxisCompute.js` (Z. 109–114): `Array.isArray()`-Check, fehlende landen im `skipped`-Log.
|
||||
- `boardViewer.html`: `hasXYZ()`-Helper (Z. 220–226) + Pre-Filterung der `_*FremdMarkers`-Arrays; Viewer in allen Situationen getestet und stabil.
|
||||
- `4_yAxis_rotation_reconstruction.py` (Z. 165–174): expliziter `None`-Check ersetzt irreführendes `.get(..., [0,0,0])`; fehlende Messung landet mit klarem Grund im `skipped`-Log.
|
||||
- `editRobot.js` → `assignMarkerId()` (vor Z. 379): `Array.isArray(em.position_mm)`-Check gibt klare Fehlermeldung zurück statt Crash.
|
||||
|
||||
Alle anderen Konsumenten (`homing.js`, `editRobot.js` → `assignByZRange`/`alignSetToMeasured`, `scripts/9_evaluateMarker.py`) waren schon robust oder sind Offline-Benchmark-Code ohne Produktionsrelevanz.
|
||||
|
||||
### Umsetzung Schritt 3+4 (Doc-Punkt 1) — Befunde (2026-06-25)
|
||||
|
||||
- **Schritt 3** (`robot_fk.py`): `marker_corners_local/_world` + `all_markers_world`
|
||||
liefern jetzt `corners_world` (4×3, in `corners_m`-Reihenfolge). Orientierung =
|
||||
Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale (exakt wie
|
||||
boardViewer.html). Verifiziert in `test/test_robot_fk_corners.py`:
|
||||
Selbst-Konsistenz (Center/Kanten/planar/Normalen-Rückgewinnung) **und** gegen
|
||||
echte triangulierte Roboter-Ecken am Seed-Pose (~1 mm RMS, Identitäts-Paarung
|
||||
schlägt jede Umordnung). Eckreihenfolge = `(+h,+h),(+h,-h),(-h,-h),(-h,+h)`
|
||||
(= boardViewer-Zeiger (1,1,0)).
|
||||
- **Wichtiger Konventions-Befund:** Die `spin`-Werte sind nur für die
|
||||
**Roboter-Marker** kalibriert/visuell verifiziert, **nicht** für die Board/
|
||||
Rail-Marker (Set A0/rail, alle auf dem Root-Link `Board`). Deren Eckreihenfolge
|
||||
liegt ~90° daneben. Lösung: `corner_points` nutzt 12 Eck-Residuen nur für
|
||||
Roboter-Links (`corner_point_links`, Default = alle Nicht-Root-Links) und 1
|
||||
Center-Residuum für die Board/Rail-Marker. Da `Board` Root ist (Residuum
|
||||
konstant bzgl. der Gelenke), kostet das nichts an Information.
|
||||
- **Datenbefund (nicht Code):** 6 Marker des Board-Sets **A0 sind auf `Arm1`
|
||||
fehlzugeordnet** (`[55,56,57,77,78,99]`), ~230 mm neben dem Modell. Sie
|
||||
destabilisieren `corner_points` (12 statt 3 Residuen → falsches Minimum) und
|
||||
ziehen auch `corner_pose` leicht. Auf bereinigten Markern konvergiert
|
||||
`corner_points` ≈ `corner_pose`. → Marker-Zuordnung korrigieren (separate
|
||||
Kalibrier-Aufgabe).
|
||||
- **DEAKTIVIERT (2026-06-25) — Aktivierung am echten Roboter gescheitert,
|
||||
zurückgedreht.** robot.json steht wieder auf `marker_observation:
|
||||
"corner_pose"` (der bewährte Zustand). Der `corner_points`-Code bleibt im
|
||||
Repo, ist aber **inaktiv** (opt-in).
|
||||
|
||||
**Was probiert wurde:** `corner_points` mit `corner_point_links:
|
||||
["Hand","Palm","FingerA","FingerB"]` scharfgeschaltet (nur Hand/Finger über
|
||||
Ecken, Arme/Board unverändert). Am echten Lauf (data/homing/20260625_175916)
|
||||
kippte die Hand (`b` −52° → +62°) und `x` wanderte (160 → 110 mm).
|
||||
|
||||
**Belegte Ursache (Gegenprobe an denselben Daten):**
|
||||
- `corner_pose` → gutes Ergebnis (`b`≈−52, `x`≈160); `corner_points` → das
|
||||
schlechte. Also eindeutig der Modus.
|
||||
- Die **Eck-Konvention ist NICHT der Fehler**: 2 der 3 Finger-Marker passen am
|
||||
guten Pose exakt (FingerB #178/#179, ~2–3 mm, korrekte Eck-Paarung fwd+r0).
|
||||
- **Eigentliche Ursache: ein einzelner schlechter Marker.** FingerA **#147**
|
||||
liegt **132 mm neben dem Modell** (Position/Zuordnung in robot.json noch
|
||||
provisorisch, vgl. Commits „Finger1 Marker"/„zweiter Finger – verdreht").
|
||||
Im Eck-Modus liefert ein Marker **12 statt 3 Residuen** → ein einzelner
|
||||
Ausreißer hat ~4× Zugkraft und reißt `global_ba` ins falsche Minimum.
|
||||
`corner_pose` (3 Residuen) dämpft ihn per Huber weg.
|
||||
|
||||
**Damit `corner_points` je produktiv taugt, fehlen ZWEI Dinge:**
|
||||
1. **Daten:** FingerA #147 sauber einmessen / Zuordnung prüfen.
|
||||
2. **Code-Robustheit:** Ausreißer-Schutz im Eck-Modus (grob daneben liegende
|
||||
Marker pro Marker auf Center zurückfallen lassen oder verwerfen), sonst
|
||||
kippt jede reale Aufnahme mit *einem* schlechten Marker. Erst danach –
|
||||
gegen GT (Simulation oder Finger-Capture bekannter Pose), **nicht** live –
|
||||
erneut testen. CLI zum Vergleichen: `--marker-observation corner_points`.
|
||||
- **Offen (Schritt 4 Tuning):** `huber_delta_mm` ist auf 6 Residuen/Marker
|
||||
kalibriert; mit 12 verschiebt sich die RMS-Größenordnung. Sauberes A/B + Tuning
|
||||
der Hand/Finger-Ecken gegen appRobotRendering-Simulations-GT steht aus (hier
|
||||
fehlten Finger-Marker in den Captures). CLI: `--marker-observation corner_pose`
|
||||
schaltet zum Vergleich zurück.
|
||||
|
||||
## Offene Punkte
|
||||
|
||||
- [ ] Keiner der drei Punkte/Schritte ist priorisiert/entschieden — reine Optionen.
|
||||
- [x] Schritt 5: Konsumenten-Recherche erledigt (Tabelle oben) — vor Schritt 5
|
||||
müssen mindestens `yAxisCompute.js` (1 Stelle) und `boardViewer.html`
|
||||
(≥9 Stellen) einen Guard bekommen (fehlende Marker überspringen statt
|
||||
crashen), sonst brechen X-/Y-Achsen-Kalibrierung beim nächsten Lauf mit
|
||||
1-Kamera-Markern.
|
||||
- [x] Guards für Schritt 5 umgesetzt (2026-06-16): alle vier offenen Stellen
|
||||
(`yAxisCompute.js`, `boardViewer.html`, `4_yAxis_rotation_reconstruction.py`,
|
||||
`editRobot.js → assignMarkerId`) schützen nun gegen fehlende `position_mm`.
|
||||
Schritt 5 selbst (1-Kamera-Marker in 3b aufnehmen) ist noch offen.
|
||||
- [x] Schritt 1 (Punkt 3) umgesetzt (2026-06-17): `3b_corner_marker_poses.py`
|
||||
liest `confidence` aus der Detection-JSON pro Kamera und schreibt
|
||||
`weight` (Mittelwert über alle beteiligten Kameras) als neues Feld in
|
||||
`aruco_marker_poses.json`. Alles andere identisch zu vorher — rein additiv.
|
||||
- [x] Schritt 2 (Punkt 3) umgesetzt (2026-06-17): `5_pose_estimation.py`
|
||||
liest `weight` aus `aruco_marker_poses.json` in `load_observations()` und
|
||||
wendet es in `residual_vector()` an, gesteuert durch
|
||||
`pose_estimation.use_marker_weight` (Default `false`). Kein Verhalten bei
|
||||
Default; aktivierbar sobald Simulationsvalidierung erfolgt.
|
||||
- [ ] Für Schritt 3/4: Eckreihenfolge/Spin-Konvention zuerst exakt verifizieren,
|
||||
bevor Residuen darauf aufbauen.
|
||||
- [ ] Für Schritt 2: Simulationsvalidierung (A/B-Vergleich mit/ohne
|
||||
`use_marker_weight`) vor Umstellung des Defaults auf `true`.
|
||||
|
||||
## Verweise
|
||||
|
||||
- [`Homing_5_Pose.md`](Homing_5_Pose.md) — Hauptdokument zu `5_pose_estimation.py`
|
||||
- `scripts/1_detect_aruco_observations.py` — Qualitätsmaße je Detektion
|
||||
- `scripts/3b_corner_marker_poses.py` — Triangulation, Center/Normal-Aggregation,
|
||||
≥2-Kamera-Filter
|
||||
- `scripts/3_multiview_bundle_adjustment_v4.py` — einziger Ort, der das
|
||||
bestehende Gewichtungsschema (`observation_weighting`/`multiview_calculation`)
|
||||
aktuell liest (nicht im Homing-Pfad)
|
||||
191
doc/Homing_8_appRobotDriver.md
Normal file
191
doc/Homing_8_appRobotDriver.md
Normal file
@@ -0,0 +1,191 @@
|
||||
# Homing 8 – Übergabe an appRobotDriver (G92-Konvention)
|
||||
|
||||
> Technische Doku zur Konvertierung des Homing-Ergebnisses
|
||||
> (FK-State aus [`4b_revolute_angle.py`](../scripts/4b_revolute_angle.py) /
|
||||
> [`5_pose_estimation.py`](../scripts/5_pose_estimation.py))
|
||||
> in die G92-Konvention von **appRobotDriver**.
|
||||
>
|
||||
> Umgesetzt in [`server/server.js`](../server/server.js) → Funktion `fkStateToDriverG92()`,
|
||||
> aufgerufen im `POST /api/homing/send-state`-Handler.
|
||||
>
|
||||
> Vollständige Driver-Konvention: [`appRobotDriver/doc/Info_G92.md`](../../appRobotDriver/doc/Info_G92.md).
|
||||
|
||||
---
|
||||
|
||||
## Das Problem: zwei verschiedene Winkel-Konventionen
|
||||
|
||||
Die Homing-Pipeline (4b / 5_pose_estimation) liefert Gelenkwinkel im
|
||||
**FK-Koordinatensystem von `robot.json`** (forward kinematics, Blender-Hierarchie).
|
||||
Der appRobotDriver erwartet in `G92` eine **eigene Konvention**, die sich an
|
||||
physischen Konfigurationen und Motor-Nullpunkten orientiert — nicht an der FK-Nulllage.
|
||||
|
||||
Für X (Schiene), Y (Oberarm), A (Unterarm-Dreher) stimmen die Konventionen überein.
|
||||
Für **B, C und Z** gibt es definierte Verschiebungen, die vor dem Senden umgerechnet
|
||||
werden müssen.
|
||||
|
||||
---
|
||||
|
||||
## Befehlsformat G92
|
||||
|
||||
```
|
||||
G92 X<mm> Y<°> Z<°> A<°> B<°> C<°> E<mm>
|
||||
```
|
||||
|
||||
Der Befehl setzt am Driver die Motorposition **ohne Bewegung** (intern M92 = Homing).
|
||||
Fehlende Achsen werden weggelassen; der Driver lässt sie unverändert.
|
||||
|
||||
---
|
||||
|
||||
## Umrechnungstabelle FK → Driver
|
||||
|
||||
| Achse | FK-State (Homing) | Driver G92 erwartet | Umrechnung |
|
||||
|-------|--------------------------------------------|--------------------------------------------|--------------------------|
|
||||
| X | xMotor in mm | xMotor in mm | — (identisch) |
|
||||
| Y | Oberarm-Winkel α, absolut (0=waagerecht) | α absolut (0=waagerecht, 90=hoch) | — (identisch) |
|
||||
| Z | Ellbogen-Knick **relativ** zu Arm1 | Unterarm-Winkel β **absolut** (wie Y) | `Z = Y + z_relativ` |
|
||||
| A | Unterarm-Dreher (Arm2-Rotation um -y) | Unterarm-Dreher (Roll um Unterarm-Achse) | — (identisch) |
|
||||
| B | FK `b=0` = gerade Hand (kein Knick) | `B=180°` = gerade Hand, `B=0°` = zurück | `B = 180 − b` |
|
||||
| C | FK `c=0` = neutraler Palm-Roll | `C=90°` = neutral (ψ=0°) | `C = c + 90` |
|
||||
| E | Finger-Öffnung in mm (rein geometrisch) | Finger-Öffnung in mm (Sehnen-Kopplung im Driver) | — (identisch) |
|
||||
|
||||
---
|
||||
|
||||
## Detailerklärung je Achse
|
||||
|
||||
### Z — Ellbogen: relativ vs. absolut
|
||||
|
||||
`4b_revolute_angle.py` misst den Ellbogen-Winkel **relativ zu Arm1** (FK-Variable z =
|
||||
Rotation des Ellbow-Gelenks gegenüber der Arm1-Nulllage). Der Driver interpretiert Z
|
||||
dagegen als **absoluten** Unterarm-Winkel zur Horizontalen — genauso wie Y den
|
||||
Oberarm beschreibt.
|
||||
|
||||
```
|
||||
Z_Driver = Y_FK + z_relativ_FK
|
||||
```
|
||||
|
||||
Intern rechnet der Driver für die FluidNC-Weiterleitung wieder zurück:
|
||||
`FluidNC-z = (β − α) × D`, d.h. relativer Motor-Winkel.
|
||||
|
||||
> **Typische Falle:** z_relativ direkt als Z senden. Bei kleinen Y-Winkeln (Y ≈ 0) ist
|
||||
> der Fehler kaum merklich; bei Y = 86° beträgt er 86°.
|
||||
|
||||
---
|
||||
|
||||
### B — Hand-Knick: 180°-Dreher
|
||||
|
||||
Im `robot.json`:
|
||||
```json
|
||||
"Hand": {
|
||||
"jointToParent": { "axis": [1, 0, 0], "rotation": [0, 0, 0], "variable": "b" }
|
||||
}
|
||||
```
|
||||
|
||||
Die FK-Nulllage (`b = 0`) bedeutet: Hand verlängert Arm2 geradeaus — kein Knick.
|
||||
Der Driver definiert dagegen:
|
||||
|
||||
| B (G92) | physischer Knick Unterarm↔Hand |
|
||||
|---------|-------------------------------|
|
||||
| 0° | 180° (Hand voll zurückgeklappt) |
|
||||
| 90° | 90° (Hand ⊥ Unterarm) |
|
||||
| 180° | 0° (Hand gerade) |
|
||||
|
||||
Ohne Umrechnung wird `b ≈ 0` (gerade Hand) als `B = 0°` gesendet → Driver klappt
|
||||
die Hand voll zurück → „rückwärts haltende Hand-Stellung".
|
||||
|
||||
```
|
||||
B_Driver = 180 − b_FK
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### C — Palm-Roll: 90°-Offset
|
||||
|
||||
Im `robot.json`:
|
||||
```json
|
||||
"Palm": {
|
||||
"jointToParent": { "axis": [0, -1, 0], "rotation": [0, 0, 0], "variable": "c" }
|
||||
}
|
||||
```
|
||||
|
||||
Die FK-Nulllage (`c = 0`) entspricht keiner Rotation des Palm-Gelenks = neutraler Roll.
|
||||
Der Driver definiert `C = 90°` als neutral (ψ = 0°):
|
||||
|
||||
| C (G92) | Hand-Roll ψ |
|
||||
|---------|-------------|
|
||||
| 0° | −90° |
|
||||
| 90° | 0° (neutral) |
|
||||
| 180° | +90° |
|
||||
|
||||
```
|
||||
C_Driver = c_FK + 90
|
||||
```
|
||||
|
||||
> **Vorzeichen prüfen:** Falls nach dem Fix die Palm-Rotation spiegelverkehrt erscheint,
|
||||
> lautet die Formel `C = 90 − c_FK` (Vorzeichen des physischen Motors umgekehrt).
|
||||
> Hängt von der Einbaurichtung des Palm-Servos ab.
|
||||
|
||||
---
|
||||
|
||||
### E — Greifer: Sehnen-Kopplung
|
||||
|
||||
E wird als reine Finger-Öffnung in mm übergeben (FK und Driver identisch — keine
|
||||
Winkelumrechnung). Der Driver berechnet den tatsächlichen Motor-Wert intern:
|
||||
|
||||
```
|
||||
eMotor = E − b − c (b, c in Radiant!)
|
||||
```
|
||||
|
||||
Die Sehnen-Kopplung kompensiert, dass Handgelenk-Knick (B) und Palm-Roll (C)
|
||||
an der Greifer-Sehne ziehen. Sind B und C korrekt gesetzt, stimmt die Kompensation
|
||||
automatisch — E selbst braucht nicht angepasst zu werden.
|
||||
|
||||
---
|
||||
|
||||
## Implementierung
|
||||
|
||||
**`server/server.js`** — Funktion `fkStateToDriverG92()`:
|
||||
|
||||
```js
|
||||
function fkStateToDriverG92(s) {
|
||||
const d = { ...s };
|
||||
if (d.b != null) d.b = 180 - d.b; // Hand-Knick: 180°-Dreher
|
||||
if (d.c != null) d.c = d.c + 90; // Palm-Roll: +90° Offset
|
||||
if (d.z != null && d.y != null) d.z = d.y + d.z; // Ellbogen: relativ → absolut
|
||||
return d;
|
||||
}
|
||||
```
|
||||
|
||||
Aufruf im `POST /api/homing/send-state`-Handler:
|
||||
|
||||
```js
|
||||
const gcode = buildG92(fkStateToDriverG92(state));
|
||||
```
|
||||
|
||||
Null-Werte (unbeobachtbare Achsen) bleiben null → werden von `buildG92` weggelassen →
|
||||
Driver lässt die entsprechenden Achsen unverändert.
|
||||
|
||||
---
|
||||
|
||||
## Kontext: Woher kommen die FK-Werte?
|
||||
|
||||
```
|
||||
4b_revolute_angle.py × N → accumulated_state {x, y, z, a, b}
|
||||
(Hand/b schlägt fehl, wenn keine Marker am Hand-Link)
|
||||
↓ (falls Hand/b fehlt)
|
||||
5_pose_estimation.py → movements {x,y,z,a,b,c,e} als FK-Winkel (Bundle-Adjustment)
|
||||
↓
|
||||
homingOrchestrator.js → finalState (flaches {x,y,z,a,b,c,e})
|
||||
↓
|
||||
POST /api/homing/send-state
|
||||
↓
|
||||
fkStateToDriverG92() → Konvertierung FK → Driver
|
||||
↓
|
||||
buildG92() → "G92 X… Y… Z… A… B… C… E…"
|
||||
↓
|
||||
sendGcode() → appRobotDriver (WebSocket)
|
||||
```
|
||||
|
||||
Im aktuellen `robot_1781069752019.json` hat der Hand-Link **keine eigenen Marker**;
|
||||
der b-Wert kommt daher immer aus `5_pose_estimation.py` (Bundle-Adjustment über
|
||||
die FingerA/FingerB-Marker, die durch die kinematische Kette Hand→Palm→Finger
|
||||
indirekt b einschränken).
|
||||
@@ -102,6 +102,23 @@ befestigt. Diese werden in `links.Base.markers` eingetragen.
|
||||
- `links.Arm1.jointToParent.origin[1]` (Y) und `[2]` (Z) in `robot.json`
|
||||
- Optional: `links.Base.markers` ergänzt
|
||||
|
||||
### Alternative/Ergänzung — `pose_estimation.fit_origin_link` (`5_pose_estimation.py`)
|
||||
|
||||
🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Befund und
|
||||
Vergleich zu Verfahren B: [`doc/Homing_5_Pose.md`](Homing_5_Pose.md) (Abschnitt
|
||||
„Kalibrier-Switch: Gelenk-Origin").
|
||||
|
||||
Ein Schalter in `robot.json` (`pose_estimation.fit_origin_link: "Arm1"`, aktuell
|
||||
gesetzt): bestimmt `origin[1,2]` (Y,Z) **zusammen mit** der Gelenkvariable in
|
||||
derselben Pose-Schätzung — aus einer einzelnen vorhandenen Homing-Aufnahme
|
||||
(Position + gemessene Normale aller Arm1-Marker), keine eigene Mess-Session
|
||||
nötig. Bei Erfolg **automatisch übernommen**: jeder Lauf schreibt den neuen
|
||||
Wert direkt in `robot.json` zurück (wie der „Joint-Origin Y/Z übernehmen"-Button,
|
||||
nur automatisch statt per Klick). Auf drei realen Captures ergab sich eine
|
||||
konsistente, sich einschwingende Korrektur von ca. **+6 bis +7 mm (Y) / −19 bis
|
||||
−20 mm (Z)** gegenüber dem ursprünglichen Wert — bisher **nicht** gegen eine
|
||||
frische Messung mit Verfahren B gegengeprüft.
|
||||
|
||||
---
|
||||
|
||||
### Mathematik: Bestimmung der Rotationsachse
|
||||
|
||||
@@ -1,14 +1,25 @@
|
||||
# robot.json – Zugriff via appRobotDriver
|
||||
|
||||
> Stand: 2026-06-15
|
||||
> Beschreibt die geplante Umstellung: robot.json kommt vom appRobotDriver, nicht
|
||||
> mehr aus einer lokalen Datei.
|
||||
> **Status: umgesetzt** (2026-06-17) — `server/robotConfig.js` ist aktiv.
|
||||
> Dieses Dokument beschreibt Entwurf und Implementierung. Der Implementierungsplan
|
||||
> (Schritte 1–3) ist vollständig abgearbeitet.
|
||||
|
||||
---
|
||||
|
||||
## Ist-Zustand
|
||||
## Verhalten je Env-Variable
|
||||
|
||||
`appRobotHoming` liest und schreibt die Roboter-Konfiguration direkt aus einer
|
||||
| Variable | nicht gesetzt | gesetzt |
|
||||
|----------|--------------|---------|
|
||||
| `ROBOT_URL` | Kein Driver-Kontakt; alle Lese-/Schreibvorgänge direkt auf die lokale Datei | `fetchRobot()` liest von `GET {ROBOT_URL}/api/robot/config`; `pushRobot()` schreibt nach `POST {ROBOT_URL}/api/robot/config` |
|
||||
| `ROBOT_JSON` | Standardpfad `scripts/robot_1781069752019.json` | Angegebener Pfad wird als lokale Cache-Datei verwendet |
|
||||
|
||||
Beide Variablen nicht gesetzt = **reiner Lokal-Modus**, identisch zum Verhalten vor dem Umbau.
|
||||
|
||||
---
|
||||
|
||||
## Ehemaliger Ist-Zustand (vor 2026-06-17)
|
||||
|
||||
`appRobotHoming` las und schrieb die Roboter-Konfiguration direkt aus einer
|
||||
lokalen Datei:
|
||||
|
||||
```
|
||||
@@ -16,8 +27,8 @@ ROBOT_JSON = process.env.ROBOT_JSON
|
||||
|| 'scripts/robot_1781069752019.json'
|
||||
```
|
||||
|
||||
Die Python-Skripte erhalten den Dateipfad als CLI-Argument (`-robot`, `--robot`).
|
||||
Alle Kalibrierungs-Endpoints schreiben ebenfalls in diese Datei.
|
||||
Die Python-Skripte erhielten den Dateipfad als CLI-Argument (`-robot`, `--robot`).
|
||||
Alle Kalibrierungs-Endpoints schrieben ebenfalls in diese Datei.
|
||||
|
||||
**Problem:** Der appRobotDriver besitzt die maßgebliche Konfiguration — nicht das
|
||||
Homing-System. Nach einem Neustart könnten Konfiguration und Driver auseinanderlaufen.
|
||||
@@ -247,9 +258,13 @@ try {
|
||||
|
||||
---
|
||||
|
||||
## Status: Umgesetzt (2026-06-17)
|
||||
|
||||
`server/robotConfig.js` erstellt. `server/editRobot.js` und `server/server.js` angepasst.
|
||||
|
||||
## Offene Fragen
|
||||
|
||||
- [ ] Genaue Endpoints des appRobotDriver für GET / POST robot.json bestätigen
|
||||
- [ ] Genaue Endpoints des appRobotDriver für GET / POST robot.json bestätigen (aktuell: `/api/robot/config`)
|
||||
- [ ] Soll der Driver eine Versions-/Konflikterkennung haben (z.B. ETag / `updatedAt`)?
|
||||
- [ ] Soll `pushRobot()` bei Driver-Fehler still auf lokal-only zurückfallen, oder hard fail?
|
||||
- [ ] `pushRobot()` bei Driver-Fehler: aktuell hard fail → Kalibrierungs-Endpoint antwortet 502
|
||||
- [ ] Authentifizierung zwischen appRobotHoming und appRobotDriver nötig?
|
||||
|
||||
467
doc/homingAPI.md
Normal file
467
doc/homingAPI.md
Normal file
@@ -0,0 +1,467 @@
|
||||
# Homing Offline-API
|
||||
|
||||
> **Status: implementiert** (2026-06-18).
|
||||
> `POST /api/homing/run-offline` ist aktiv in `server/server.js`.
|
||||
> Abhängigkeit: `multer` (npm-Package, bereits installiert).
|
||||
>
|
||||
> Gedacht für die **Simulations-Pipeline** (`appRobotRendering`), die synthetische oder
|
||||
> aufgezeichnete Bilder liefert und die aktuelle Pose-Erkennung von appRobotHoming nutzen
|
||||
> möchte — ohne den Umweg über echte Kameras und den WebCam-Service.
|
||||
|
||||
---
|
||||
|
||||
## Motivation
|
||||
|
||||
Die Live-Homing-Pipeline (`POST /api/homing/run`) setzt voraus, dass `WEBCAM_URL` auf
|
||||
einen laufenden WebCam-Service zeigt, der Bilder auf Abruf liefert. Das ist für zwei
|
||||
Szenarien unpraktisch:
|
||||
|
||||
1. **Simulations-Validierung** — `appRobotRendering` rendert synthetische Bilder zu
|
||||
bekannten Gelenkwinkeln und will prüfen, wie gut die aktuelle Pose-Erkennung die
|
||||
Winkel zurückrechnet. Die Pipeline in `appRobotRendering` liegt lokal und braucht
|
||||
keine Live-Kamera.
|
||||
|
||||
2. **Offline-Replay** — früher aufgenommene Bildsätze sollen mit dem *aktuellen*
|
||||
Stand der Algorithmen neu ausgewertet werden (z. B. nach Verbesserungen an
|
||||
`4b_revolute_angle.py` oder `5_pose_estimation.py`).
|
||||
|
||||
In beiden Fällen sind Bilder und Kalibrierungsdaten bereits vorhanden. Die API soll
|
||||
diese entgegennehmen, die Pipeline identisch zum Live-Modus durchlaufen und die
|
||||
Ergebnisdateien zurückgeben.
|
||||
|
||||
---
|
||||
|
||||
## Abgrenzung zum Live-Modus
|
||||
|
||||
| Aspekt | Live (`/api/homing/run`) | Offline (diese API) |
|
||||
|---|---|---|
|
||||
| Bilder | WebCam-Service liefert auf Abruf | Caller liefert im Request |
|
||||
| NPZ | Server sucht neueste Session in `data/calibration/` | Caller liefert im Request |
|
||||
| `robot.json` | Server nutzt `robotConfig.js` (Driver oder lokale Cache-Datei) | Caller liefert im Request |
|
||||
| Pipeline (1→2→3b→4b→5) | identisch | identisch |
|
||||
| Antwort | SSE-Stream während Lauf | Synchrones JSON mit allen Ergebnisdateien |
|
||||
| Zweck | Produktions-Homing | Simulation / Replay |
|
||||
|
||||
Die Pipeline-Skripte (`1_detect_aruco_observations.py` bis `5_pose_estimation.py`)
|
||||
werden **unverändert** aufgerufen — die Offline-API ist nur eine andere Eingangsschicht.
|
||||
Neue Algorithmen erscheinen automatisch in beiden Pfaden.
|
||||
|
||||
---
|
||||
|
||||
## API-Beschreibung
|
||||
|
||||
### `POST /api/homing/run-offline`
|
||||
|
||||
**Content-Type:** `multipart/form-data`
|
||||
|
||||
#### Felder
|
||||
|
||||
| Feld | Typ | Pflicht | Beschreibung |
|
||||
|---|---|---|---|
|
||||
| `images` | Datei(en), `image/jpeg` | ja | Ein oder mehrere JPEG-Bilder. Dateiname **muss** `{cameraId}.jpg` sein (z. B. `cam0.jpg`, `cam1.jpg`). |
|
||||
| `calibrations` | Datei(en), `application/octet-stream` | ja | Je eine `.npz`-Datei pro Kamera. Dateiname **muss** `{cameraId}_calibration.npz` sein (z. B. `cam0_calibration.npz`). |
|
||||
| `robot` | Datei, `application/json` | ja | `robot.json` für diesen Lauf. Wird nur für diesen Aufruf verwendet, nicht dauerhaft gespeichert. |
|
||||
| `refSet` | Text | nein | Optionaler Referenz-Set-Name für Script 2 (`--refSet`), z. B. `A0`. |
|
||||
|
||||
Das Pairing `{cameraId}.jpg` ↔ `{cameraId}_calibration.npz` erfolgt rein über den
|
||||
Dateinamen-Prefix vor dem ersten `_` bzw. vor `.jpg`. Kamera-IDs ohne passende NPZ
|
||||
werden wie im Live-Modus übersprungen (Log-Eintrag, kein Fehler).
|
||||
|
||||
#### Antwort (Erfolg): `200 OK`
|
||||
|
||||
```json
|
||||
{
|
||||
"ok": true,
|
||||
"runDir": "20260616_183042",
|
||||
"state": {
|
||||
"x": 312.4,
|
||||
"y": 44.8,
|
||||
"z": -12.1,
|
||||
"a": 7.3,
|
||||
"b": 0.0,
|
||||
"c": null,
|
||||
"e": null
|
||||
},
|
||||
"files": {
|
||||
"aruco_marker_poses.json": { /* Inhalt */ },
|
||||
"robot_state.json": { /* Inhalt */ },
|
||||
"state_Arm1.json": { /* Inhalt */ },
|
||||
"state_Ellbow.json": { /* Inhalt */ },
|
||||
"state_Arm2.json": { /* Inhalt */ },
|
||||
"state_Hand.json": { /* Inhalt */ },
|
||||
"cam0_aruco_detection.json": { /* Inhalt */ },
|
||||
"cam0_camera_pose.json": { /* Inhalt */ },
|
||||
"cam1_aruco_detection.json": { /* Inhalt */ },
|
||||
"cam1_camera_pose.json": { /* Inhalt */ }
|
||||
},
|
||||
"log": [
|
||||
"▶ Homing-Run: 20260616_183042",
|
||||
"▶ cam0: 14 Marker erkannt",
|
||||
"…"
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
`state` entspricht dem `accumulated_state` aus der 4b-Kette (wenn erfolgreich) oder
|
||||
den `movements`-Werten aus `robot_state.json` (wenn 4b abbricht und 5_pose_estimation
|
||||
einspringt). `null`-Werte bedeuten: Gelenk nicht beobachtbar (wie im Live-Modus).
|
||||
|
||||
`files` enthält alle JSON-Ausgabedateien des Laufs als geparste Objekte — kein Base64,
|
||||
da es sich ausschliesslich um JSON handelt.
|
||||
|
||||
#### Antwort (Fehler)
|
||||
|
||||
| Code | Bedeutung |
|
||||
|---|---|
|
||||
| `400` | Pflichtfelder fehlen, Dateinamen-Convention verletzt, robot.json ungültig |
|
||||
| `422` | Pipeline abgebrochen: Script 3b konnte `aruco_marker_poses.json` nicht erzeugen (zu wenige Kameras o. ä.) |
|
||||
| `500` | Unerwarteter Server-Fehler (Python nicht gefunden, Datei-I/O-Fehler, …) |
|
||||
|
||||
Fehlerresponse immer `{ "error": "…", "log": ["…"] }`.
|
||||
|
||||
---
|
||||
|
||||
## Aufruf-Beispiele
|
||||
|
||||
### curl
|
||||
|
||||
```bash
|
||||
curl -X POST https://thinkcentre.local:2093/api/homing/run-offline \
|
||||
-F "images=@cam0.jpg" \
|
||||
-F "images=@cam1.jpg" \
|
||||
-F "images=@cam2.jpg" \
|
||||
-F "calibrations=@cam0_calibration.npz" \
|
||||
-F "calibrations=@cam1_calibration.npz" \
|
||||
-F "calibrations=@cam2_calibration.npz" \
|
||||
-F "robot=@robot_1781069752019.json;type=application/json" \
|
||||
-F "refSet=A0"
|
||||
```
|
||||
|
||||
Ohne `refSet` (alle Board-Marker als Referenz):
|
||||
```bash
|
||||
curl -X POST https://thinkcentre.local:2093/api/homing/run-offline \
|
||||
-F "images=@cam0.jpg" -F "images=@cam1.jpg" -F "images=@cam2.jpg" \
|
||||
-F "calibrations=@cam0_calibration.npz" -F "calibrations=@cam1_calibration.npz" \
|
||||
-F "calibrations=@cam2_calibration.npz" \
|
||||
-F "robot=@robot_1781069752019.json;type=application/json"
|
||||
```
|
||||
|
||||
### JavaScript / fetch (Browser oder Node)
|
||||
|
||||
```javascript
|
||||
const formData = new FormData();
|
||||
|
||||
// Bilder – Dateiname MUSS {cameraId}.jpg sein
|
||||
formData.append('images', cam0Blob, 'cam0.jpg');
|
||||
formData.append('images', cam1Blob, 'cam1.jpg');
|
||||
formData.append('images', cam2Blob, 'cam2.jpg');
|
||||
|
||||
// Kalibrierungen – Dateiname MUSS {cameraId}_calibration.npz sein
|
||||
formData.append('calibrations', cam0NpzBlob, 'cam0_calibration.npz');
|
||||
formData.append('calibrations', cam1NpzBlob, 'cam1_calibration.npz');
|
||||
formData.append('calibrations', cam2NpzBlob, 'cam2_calibration.npz');
|
||||
|
||||
// robot.json
|
||||
formData.append('robot', new Blob([JSON.stringify(robotJson)], { type: 'application/json' }), 'robot.json');
|
||||
|
||||
// optional
|
||||
formData.append('refSet', 'A0');
|
||||
|
||||
const res = await fetch('/api/homing/run-offline', { method: 'POST', body: formData });
|
||||
const data = await res.json();
|
||||
// data.state → { x, y, z, a, b, c, e }
|
||||
// data.files → { "aruco_marker_poses.json": {...}, "state_Arm1.json": {...}, … }
|
||||
// data.log → ["▶ Kameras: cam0, cam1, cam2", …]
|
||||
// data.runDir → "20260618_143022"
|
||||
```
|
||||
|
||||
### Python (requests)
|
||||
|
||||
```python
|
||||
import requests
|
||||
|
||||
url = 'https://thinkcentre.local:2093/api/homing/run-offline'
|
||||
|
||||
files = [
|
||||
('images', ('cam0.jpg', open('cam0.jpg', 'rb'), 'image/jpeg')),
|
||||
('images', ('cam1.jpg', open('cam1.jpg', 'rb'), 'image/jpeg')),
|
||||
('images', ('cam2.jpg', open('cam2.jpg', 'rb'), 'image/jpeg')),
|
||||
('calibrations', ('cam0_calibration.npz', open('cam0_calibration.npz', 'rb'), 'application/octet-stream')),
|
||||
('calibrations', ('cam1_calibration.npz', open('cam1_calibration.npz', 'rb'), 'application/octet-stream')),
|
||||
('calibrations', ('cam2_calibration.npz', open('cam2_calibration.npz', 'rb'), 'application/octet-stream')),
|
||||
('robot', ('robot.json', open('robot.json', 'rb'), 'application/json')),
|
||||
]
|
||||
data = {'refSet': 'A0'} # optional
|
||||
|
||||
resp = requests.post(url, files=files, data=data, verify=False, timeout=120)
|
||||
result = resp.json()
|
||||
print(result['state']) # {'x': 312.4, 'y': 44.8, 'z': -12.1, ...}
|
||||
```
|
||||
|
||||
### Wichtige Regeln für Dateinamen
|
||||
|
||||
| Feld | Pflichtformat | Beispiel |
|
||||
|------|--------------|---------|
|
||||
| `images` | `{cameraId}.jpg` | `cam0.jpg`, `cam1.jpg` |
|
||||
| `calibrations` | `{cameraId}_calibration.npz` | `cam0_calibration.npz` |
|
||||
| `robot` | beliebig (wird intern als `robot_run.json` gespeichert) | `robot.json` |
|
||||
|
||||
Das Pairing Bild ↔ NPZ erfolgt rein über den `cameraId`-Prefix.
|
||||
Eine Kamera ohne passende NPZ wird übersprungen (kein Fehler, aber Log-Eintrag).
|
||||
|
||||
### HTTP-Statuscodes
|
||||
|
||||
| Code | Bedeutung |
|
||||
|------|-----------|
|
||||
| `200` | Erfolg: `{ ok: true, runDir, state, files, log }` |
|
||||
| `400` | Pflichtfelder fehlen oder `robot.json` ist kein valides JSON |
|
||||
| `422` | Pipeline abgebrochen: `aruco_marker_poses.json` nicht erzeugt (< 2 Kamera-Posen) |
|
||||
| `500` | Homing fehlgeschlagen (4b-Kette + Fallback gescheitert) |
|
||||
|
||||
### Timeout-Hinweis
|
||||
|
||||
Die Pipeline kann 20–60 s dauern. Client-Timeout auf **≥ 120 s** setzen
|
||||
(curl: `--max-time 120`, requests: `timeout=120`, nginx/Caddy: `proxy_read_timeout 120s`).
|
||||
|
||||
---
|
||||
|
||||
## Datenfluss (detailliert)
|
||||
|
||||
```
|
||||
multipart/form-data
|
||||
images: cam0.jpg, cam1.jpg
|
||||
calibrations: cam0_calibration.npz, cam1_calibration.npz
|
||||
robot: robot_xxx.json
|
||||
│
|
||||
▼
|
||||
Server: Temp-Verzeichnis anlegen data/homing-offline/{timestamp}/
|
||||
→ Bilder speichern: cam0.jpg, cam1.jpg
|
||||
→ NPZ speichern: cam0_calibration.npz, cam1_calibration.npz
|
||||
→ robot.json speichern: robot_run.json (nur für diesen Lauf)
|
||||
│
|
||||
▼ für jede Kamera:
|
||||
1_detect_aruco_observations.py
|
||||
-i cam0.jpg -npz cam0_calibration.npz -robot robot_run.json
|
||||
-cameraId cam0 -outDir {runDir}
|
||||
→ cam0_aruco_detection.json
|
||||
|
||||
2_estimate_camera_from_observations.py
|
||||
-i cam0_aruco_detection.json -robot robot_run.json
|
||||
-outDir {runDir} [--refSet A0]
|
||||
→ cam0_camera_pose.json
|
||||
│
|
||||
▼ nach allen Kameras:
|
||||
3b_corner_marker_poses.py
|
||||
--evalDir {runDir} --robot robot_run.json
|
||||
→ aruco_marker_poses.json
|
||||
│
|
||||
▼ X-Position schätzen (homingXEstimate)
|
||||
x_mm ← estimateXFromMarkers(aruco_marker_poses.json, robot_run.json)
|
||||
│
|
||||
▼ 4b-Kette sequenziell Arm1→Ellbow→Arm2→Hand:
|
||||
4b_revolute_angle.py
|
||||
--robot robot_run.json --aruco aruco_marker_poses.json
|
||||
--link Arm1 --x-mm {x_mm} --output state_Arm1.json
|
||||
4b_revolute_angle.py --link Ellbow --from-state state_Arm1.json …
|
||||
…
|
||||
→ state_Arm1.json, state_Ellbow.json, state_Arm2.json, state_Hand.json
|
||||
│
|
||||
▼ (falls 4b-Kette vollständig)
|
||||
accumulated_state → state in der Antwort
|
||||
│
|
||||
▼ (Verfeinerung / Fallback bei abgebrochener 4b-Kette)
|
||||
5_pose_estimation.py
|
||||
aruco_marker_poses.json -robot robot_run.json
|
||||
--from-state state_Hand.json -out robot_state.json
|
||||
→ robot_state.json
|
||||
│
|
||||
▼
|
||||
Antwort: { ok, state, files: { alle JSON-Dateien }, log }
|
||||
```
|
||||
|
||||
Der Ablauf ist **identisch** zu `runHoming()` in `homingOrchestrator.js`, mit dem
|
||||
einzigen Unterschied, dass `runBoardPipeline()` die Bilder nicht vom WebCam-Service
|
||||
holt, sondern aus dem vorab befüllten `{runDir}`.
|
||||
|
||||
---
|
||||
|
||||
## Umsetzungsplan (abgeschlossen 2026-06-18)
|
||||
|
||||
### Schritt 1 — `runBoardPipelineOffline(runDir, send, opts)` (Kern)
|
||||
|
||||
**Was:** Neue Funktion in `server/server.js` oder `server/homingOrchestrator.js`, analog
|
||||
zu `runBoardPipeline()`. Unterschied: statt `WEBCAM_URL` und `findLatestNpzForCamera()`
|
||||
leitet sie die bereits-im-Verzeichnis-liegenden Dateien direkt weiter.
|
||||
|
||||
**Konkret:** Der einzige geänderte Teil in `runBoardPipeline()` ist die
|
||||
Kamera-Schleife — statt Snapshot + NPZ-Suche wird einfach geprüft, ob
|
||||
`{camId}.jpg` und `{camId}_calibration.npz` im `runDir` existieren. Danach
|
||||
rufts Script 1 und 2 identisch auf.
|
||||
|
||||
**Risiko:** keines — die Script-Aufrufe selbst bleiben unverändert. Nur der Pfad zur
|
||||
NPZ ändert sich von `data/calibration/{session}/` zu `{runDir}/`.
|
||||
|
||||
**Testbar:** Einzel-Test mit einer Kamera, geprüft, ob `cam0_aruco_detection.json`
|
||||
korrekt erzeugt wird.
|
||||
|
||||
---
|
||||
|
||||
### Schritt 2 — Multipart-Upload-Handling
|
||||
|
||||
**Was:** Parsing von `multipart/form-data` für den neuen Endpoint. Express verarbeitet
|
||||
`application/json` und `urlencoded` nativ, aber nicht `multipart`. Benötigt entweder:
|
||||
|
||||
- **Option A** — `multer` (npm-Package), etabliert, 0 Boilerplate
|
||||
- **Option B** — manuell mit dem Node `busboy`-Parser (bereits in Node 18+, kein
|
||||
Extra-Package)
|
||||
|
||||
Empfehlung: `multer` — es ist bereits `multer` in vielen Express-Projekten Standard,
|
||||
klar dokumentiert, und `diskStorage` legt Dateien direkt in `{runDir}` ab. Vermeidet
|
||||
Buffer-Accumulation für grosse NPZ-Dateien.
|
||||
|
||||
**Risiko:** Dateinamen-Sanitising — Multer übergibt den Originaldateinamen. Vor dem
|
||||
Speichern: `path.basename()` und nur Zeichen `[a-zA-Z0-9_.-]` zulassen, sonst 400.
|
||||
|
||||
**Testbar:** curl-Upload-Test mit zwei kleinen Dummy-Dateien, geprüft, ob sie im
|
||||
richtigen Verzeichnis landen.
|
||||
|
||||
---
|
||||
|
||||
### Schritt 3 — `runHomingOffline()` Orchestrator
|
||||
|
||||
**Was:** Analoge Funktion zu `runHoming()` in `homingOrchestrator.js`, jedoch:
|
||||
- kein SSE-Stream, sondern Log-Akkumulation in ein Array
|
||||
- `runBoardPipelineOffline()` statt `runBoardPipeline()`
|
||||
- `robotJsonPath` zeigt auf die hochgeladene, temporäre `robot_run.json`
|
||||
- Rückgabewert: `{ state, files, log }` statt SSE-Events
|
||||
|
||||
Die 4b-Kette und der 5_pose-Aufruf bleiben **unverändert** — gleiche Args, gleiche
|
||||
Exit-Code-Logik, gleiche `accumulated_state`-Extraktion.
|
||||
|
||||
**Risiko:** 5_pose_estimation.py braucht `scipy`. In der lokalen Entwicklungsumgebung
|
||||
muss `scipy` im Python-Umfeld vorhanden sein (in `docker-compose.yaml` ist es bereits
|
||||
eingetragen, lokal muss `pip install scipy` geprüft werden).
|
||||
|
||||
---
|
||||
|
||||
### Schritt 4 — Endpoint `POST /api/homing/run-offline`
|
||||
|
||||
**Was:** Express-Route in `server.js`:
|
||||
1. Multer-Middleware: Dateien in Temp-Verzeichnis
|
||||
2. Validierung: mindestens 1 Kamera mit passendem `.jpg` + `.npz`-Pair; `robot`-Feld vorhanden
|
||||
3. `runHomingOffline()` aufrufen
|
||||
4. Alle JSON-Dateien aus `{runDir}` einlesen und in `files`-Objekt verpacken
|
||||
5. Temp-Verzeichnis aufräumen (oder unter `data/homing-offline/` für Replay behalten?)
|
||||
|
||||
**Aufräumen vs. Behalten:** Empfehlung — Verzeichnis behalten, wie bei Live-Homing-Runs.
|
||||
So ist Replay/Debug möglich. Ein periodischer Aufräum-Cron ist eine separate Aufgabe.
|
||||
|
||||
**Risiko:** Timeouts — bei vielen Kameras oder langsamer Maschine kann die Pipeline
|
||||
20–60 Sekunden dauern. Express hat kein Default-Timeout, aber ein vorgeschalteter
|
||||
Reverse-Proxy (nginx, Caddy) schon. In der Doku festhalten: Client soll Timeout ≥ 120 s
|
||||
setzen. Alternativ: SSE-Variante (gleiche Daten, aber inkrementell gestreamt).
|
||||
|
||||
---
|
||||
|
||||
### Schritt 5 — Aufräumen temporärer `robot.json`
|
||||
|
||||
**Was:** Die hochgeladene `robot.json` wird nur für diesen Lauf im `{runDir}` als
|
||||
`robot_run.json` gespeichert. Sie liegt **nicht** an der Stelle von `robotCachePath` (aus `robotConfig.js`)
|
||||
und wird daher nie vom Live-Modus versehentlich gelesen. Kein Konflikt.
|
||||
|
||||
**Risiko:** keines — rein additive Datei in einem neuen Verzeichnis.
|
||||
|
||||
---
|
||||
|
||||
### Testplan
|
||||
|
||||
| Test | Was wird geprüft | Werkzeug |
|
||||
|---|---|---|
|
||||
| **Smoke-Test Upload** | Endpoint antwortet 200, `runDir` im Response vorhanden | curl mit zwei Dummy-JPEGs + NPZs + robot.json |
|
||||
| **Kamera-Pairing** | `.jpg` ohne passende NPZ wird übersprungen (kein 500) | curl mit fehlendem NPZ |
|
||||
| **Dateinamen-Sanitising** | `../../../evil.npz` → 400 | curl mit bösem Dateinamen |
|
||||
| **Simulations-Roundtrip** | `appRobotRendering` rendert 10 Posen mit bekannten GT-Winkeln, API gibt `state` zurück, Abweichung < Toleranz | automatisiert aus `appRobotRendering`-Pipeline |
|
||||
| **Identität Live vs. Offline** | Denselben Bildsatz einmal per Live-Run und einmal per Offline-API auswerten → `state`-Differenz ≈ 0 | manuell mit aufgezeichnetem Homing-Run |
|
||||
| **Fallback-Pfad** | Script 3b schlägt fehl (< 2 Kameras) → 422 mit log | curl mit nur einem Bild |
|
||||
| **Timeout-Robustheit** | 4b bricht ab → 5_pose_estimation greift ein, 200 wird trotzdem zurückgegeben | simulierter Abbruch durch fehlende Marker |
|
||||
|
||||
---
|
||||
|
||||
## Bekannte Risiken und Probleme
|
||||
|
||||
### Dateinamen-Convention ist implizit
|
||||
|
||||
Die Kamera-ID wird aus dem Dateinamen abgeleitet — kein explizites Metadaten-Feld.
|
||||
Das funktioniert solange die Namenskonvention (`{cameraId}.jpg` / `{cameraId}_calibration.npz`)
|
||||
eingehalten wird. Abweichungen (z. B. `frame_cam0_001.jpg`) führen zu einem ungematchten
|
||||
Bild, das still übersprungen wird — kein Fehler, aber unerwartetes Verhalten.
|
||||
|
||||
**Mitigation:** Explizite Validierung und Fehlermeldung wenn kein Match gefunden wird.
|
||||
Alternativ: Metadaten-Feld `cameraMappings: {"cam0": {"image": "frame_cam0_001.jpg", "npz": "cam0_calibration.npz"}}`.
|
||||
Für den Simulations-Use-Case ist die einfache Konvention ausreichend.
|
||||
|
||||
### robot.json-Versionskonflikt
|
||||
|
||||
Simulation und Live-Homing teilen sich `robot.json` konzeptionell, aber die Datei
|
||||
entwickelt sich weiter (Kalibrierung, neue Marker, geänderte Positionen). Eine veraltete
|
||||
`robot.json` aus `appRobotRendering` kann zu systematisch falschen Posen führen, die
|
||||
schwer von Algorithmus-Fehlern zu unterscheiden sind.
|
||||
|
||||
**Mitigation:** Im Simulations-Roundtrip-Test die `robot.json`-Version (z. B. Timestamp
|
||||
im Dateinamen) protokollieren und mit dem API-Response abgleichen.
|
||||
|
||||
### Gleichzeitige Anfragen
|
||||
|
||||
Mehrere Offline-Runs gleichzeitig schreiben in separate `{timestamp}`-Verzeichnisse —
|
||||
kein Konflikt. Aber: Python-Subprozesse multiplizieren sich. Bei parallelen Requests aus
|
||||
der Simulations-Pipeline könnte die CPU-Last (scipy + ArUco) den Server blockieren.
|
||||
|
||||
**Mitigation (optional):** Request-Queue mit max. 1–2 parallelen Runs. Für den
|
||||
Simulations-Use-Case wird sequenzieller Aufruf empfohlen.
|
||||
|
||||
### scipy-Abhängigkeit auf Deployment-Maschine
|
||||
|
||||
`5_pose_estimation.py` braucht `scipy`. In `docker-compose.yaml` ist es vorhanden.
|
||||
Lokal (Entwicklung ohne Docker) muss `pip install scipy` sichergestellt sein, sonst
|
||||
schlägt Schritt 5 stumm fehl (Exit 1, aber Schritt 4 hat bereits ein Ergebnis).
|
||||
|
||||
### Festplattenverbrauch
|
||||
|
||||
Jeder Offline-Run erzeugt ein Verzeichnis mit JPEGs + NPZs + ca. 10 JSON-Dateien.
|
||||
Bei intensiver Simulations-Nutzung (100 Posen/Tag) kann das summieren.
|
||||
|
||||
**Mitigation:** TTL-basiertes Aufräumen als gelegentliche Wartungsaufgabe (kein Teil
|
||||
dieser Implementierung).
|
||||
|
||||
---
|
||||
|
||||
## Offene Entscheidungen (getroffen)
|
||||
|
||||
- [x] `multer` (v2.2.0) — installiert, `diskStorage` schreibt direkt in `{runDir}`
|
||||
- [x] Offline-Runs in `data/homing-offline/` (eigenes Verzeichnis, nicht im boardViewer)
|
||||
- [x] Synchrone Antwort — Pipeline läuft durch, dann JSON; Client-Timeout ≥ 120 s empfohlen
|
||||
- [x] `5_pose_estimation.py` nur als Fallback (identisch zum Live-Modus)
|
||||
|
||||
---
|
||||
|
||||
## Abhängigkeiten und Voraussetzungen
|
||||
|
||||
Vor der Implementierung müssen vorhanden sein:
|
||||
|
||||
- `multer` installiert (oder busboy-basierte Alternative entschieden)
|
||||
- `scipy` im Python-Environment verfügbar (lokal + Docker)
|
||||
- `scripts/5_pose_estimation.py` + `scripts/robot_fk.py` im Repo (beide vorhanden, ✅)
|
||||
- `homingOrchestrator.js` für Orientierung (vorhanden, ✅)
|
||||
- Test-Bilder + Test-NPZs für automatisierten Smoke-Test (aus `test/homing/` oder
|
||||
`test/y-axis-finder-examples/`; die NPZs müssen dazu noch als Testfixtures bereitgestellt werden)
|
||||
|
||||
---
|
||||
|
||||
## Verweise
|
||||
|
||||
- [`Homing.md`](Homing.md) — Gesamtüberblick Homing-Ablauf
|
||||
- [`Homing_0_Camera.md`](Homing_0_Camera.md) — Schritte 1–3b (Board-Pipeline)
|
||||
- [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md) — 4b-Kette (Gelenkwinkel-Schätzung)
|
||||
- [`Homing_5_Pose.md`](Homing_5_Pose.md) — 5_pose_estimation.py (Bundle-Adjustment)
|
||||
- `server/server.js` — bestehende Endpoints (`/api/homing/run`, `runBoardPipeline()`)
|
||||
- `server/homingOrchestrator.js` — `runHoming()`, Vorlage für `runHomingOffline()`
|
||||
232
doc/multilingual.md
Normal file
232
doc/multilingual.md
Normal file
@@ -0,0 +1,232 @@
|
||||
# Mehrsprachigkeit (DE/EN) – Vorschlag
|
||||
|
||||
## Ausgangslage
|
||||
|
||||
Die App ist heute komplett deutschsprachig, und zwar an drei verschiedenen Stellen, die unterschiedlich behandelt werden müssen:
|
||||
|
||||
1. **Statisches HTML-Markup** – Labels, Überschriften, `placeholder`/`title`-Attribute direkt in
|
||||
`public/*.html` (`index.html`, `homing.html`, `calibration*.html`, …).
|
||||
Beispiel: `<h2>Aktionen</h2>`, `<button title="Erst Homing ausführen">`.
|
||||
2. **Dynamisch erzeugte Strings in Client-JS** – Template-Literals in `public/client.js`,
|
||||
`public/homing.js`, `public/calibration.js` etc., z. B.
|
||||
`txt.textContent = text || \`Schritt ${step} / ${total}\`;` oder
|
||||
`result.innerHTML = '<span>⚠ Bitte Set auswählen, das verschoben werden soll.</span>'`.
|
||||
3. **Server-seitige Strings**, die als JSON an den Client durchgereicht und dort angezeigt werden,
|
||||
z. B. `res.status(400).json({ error: '"y" und "z" müssen Zahlen sein.' })` in
|
||||
[server/server.js](../server/server.js).
|
||||
|
||||
Es gibt kein Build-Tool (kein Webpack/Vite/React) – alles ist Vanilla HTML/JS, das Express
|
||||
als statische Dateien ausliefert ([server/server.js](../server/server.js)). Jede Lösung sollte
|
||||
also **ohne Build-Schritt** funktionieren und sich inkrementell einführen lassen (nicht „big bang“,
|
||||
sondern Seite für Seite).
|
||||
|
||||
## Zielbild
|
||||
|
||||
- Sprache wird primär aus `navigator.language` (Browser-Einstellung) bestimmt, mit manuellem
|
||||
Override (Dropdown/Flag-Icon in der Topbar) und Persistenz in `localStorage`.
|
||||
- Fallback-Sprache ist Deutsch (aktueller Stand), zweite Sprache Englisch.
|
||||
- Übersetzungen liegen in einfachen JSON-Dateien, kein zusätzliches npm-Paket nötig
|
||||
(es reicht ein paar Dutzend Zeilen eigener Lade-/Ersetzungslogik).
|
||||
- Server-Fehlermeldungen werden nicht als fertiger Text geschickt, sondern als
|
||||
**Fehlercode**, den der Client anhand der aktuellen Sprache übersetzt.
|
||||
|
||||
## 1. Struktur der Sprachdateien
|
||||
|
||||
```
|
||||
public/
|
||||
i18n/
|
||||
de.json
|
||||
en.json
|
||||
i18n.js <- kleine Lade-/Helper-Bibliothek, von jeder Seite eingebunden
|
||||
```
|
||||
|
||||
`de.json` (Auszug, Keys gruppiert nach Seite/Bereich, „flach mit Punktnotation“ ist für diese
|
||||
Größenordnung einfacher zu pflegen als tief verschachteltes JSON):
|
||||
|
||||
```json
|
||||
{
|
||||
"common.back": "← Zurück",
|
||||
"common.error": "Fehler",
|
||||
"index.actions.title": "Aktionen",
|
||||
"index.actions.runHoming": "📷 Foto & Homing berechnen",
|
||||
"index.actions.sendToRobot": "✅ An Roboter senden",
|
||||
"index.actions.sendDisabledTitle": "Erst Homing ausführen",
|
||||
"index.status.idle": "○ Warte",
|
||||
"index.status.running": "● Läuft …",
|
||||
"calibration.move.selectSetWarning": "⚠ Bitte Set auswählen, das verschoben werden soll.",
|
||||
"calibration.move.sameSetWarning": "⚠ \"Bleibt\" und \"verschoben\" dürfen nicht dasselbe Set sein.",
|
||||
"server.error.yzMustBeNumbers": "\"y\" und \"z\" müssen Zahlen sein.",
|
||||
"server.error.invalidAxisPayload": "Ungültige Nutzlast: axis.dir und axis.referencePoint erwartet"
|
||||
}
|
||||
```
|
||||
|
||||
`en.json` hat exakt dieselben Keys mit englischen Werten. Das ist die einzige Stelle, an der eine
|
||||
fehlende Übersetzung sofort auffällt: Key in `de.json` ohne Pendant in `en.json` → Fallback auf
|
||||
Deutsch (siehe unten), kein Crash.
|
||||
|
||||
**Warum nicht ein JSON pro HTML-Seite?** Weil viele Strings (Status-Badges, Fehlermeldungen,
|
||||
Buttons wie „Zurück“) seitenübergreifend wiederverwendet werden. Eine flache `common.*`-Gruppe plus
|
||||
eine Gruppe pro Seite (`index.*`, `homing.*`, `calibration.*`) hält das überschaubar, ohne dass man
|
||||
Strings doppelt pflegt.
|
||||
|
||||
## 2. Statisches HTML markieren
|
||||
|
||||
Jedes übersetzbare Element bekommt ein `data-i18n`-Attribut mit dem Key. Für Attribute (z. B.
|
||||
`title`, `placeholder`) gibt es ein zusätzliches `data-i18n-attr`:
|
||||
|
||||
```html
|
||||
<h2 data-i18n="index.actions.title">Aktionen</h2>
|
||||
|
||||
<button id="btn-homing-send" disabled
|
||||
data-i18n="index.actions.sendToRobot"
|
||||
data-i18n-attr-title="index.actions.sendDisabledTitle"
|
||||
title="Erst Homing ausführen">
|
||||
✅ An Roboter senden
|
||||
</button>
|
||||
```
|
||||
|
||||
Wichtig: Der **deutsche Text bleibt im HTML stehen** (als Fallback/Default und damit die Seite ohne
|
||||
JS oder bei Ladefehler nicht leer ist). `i18n.js` ersetzt den Inhalt nur, wenn die Zielsprache nicht
|
||||
Deutsch ist bzw. wenn ein Override gesetzt wurde. Das macht die Migration risikoarm: Man kann Seite
|
||||
für Seite Attribute ergänzen, ohne dass etwas kaputtgeht, falls eine Seite noch nicht migriert ist.
|
||||
|
||||
## 3. `i18n.js` – minimale Laufzeit-Bibliothek
|
||||
|
||||
Kernfunktionen, ca. 60–80 Zeilen, kein Tooling nötig:
|
||||
|
||||
```js
|
||||
// public/i18n.js
|
||||
const SUPPORTED = ['de', 'en'];
|
||||
const FALLBACK = 'de';
|
||||
|
||||
function detectLang() {
|
||||
const stored = localStorage.getItem('lang');
|
||||
if (stored && SUPPORTED.includes(stored)) return stored;
|
||||
const nav = (navigator.language || FALLBACK).slice(0, 2).toLowerCase();
|
||||
return SUPPORTED.includes(nav) ? nav : FALLBACK;
|
||||
}
|
||||
|
||||
let dict = {};
|
||||
let fallbackDict = {};
|
||||
|
||||
export async function initI18n() {
|
||||
const lang = detectLang();
|
||||
[dict, fallbackDict] = await Promise.all([
|
||||
fetch(`/i18n/${lang}.json`).then(r => r.json()),
|
||||
lang === FALLBACK ? Promise.resolve({}) : fetch(`/i18n/${FALLBACK}.json`).then(r => r.json()),
|
||||
]);
|
||||
document.documentElement.lang = lang;
|
||||
applyToDom();
|
||||
return lang;
|
||||
}
|
||||
|
||||
export function t(key, vars) {
|
||||
let str = dict[key] ?? fallbackDict[key] ?? key; // Key selbst als letzter Fallback -> sichtbar im UI statt "undefined"
|
||||
if (vars) for (const [k, v] of Object.entries(vars)) str = str.replaceAll(`{${k}}`, v);
|
||||
return str;
|
||||
}
|
||||
|
||||
function applyToDom(root = document) {
|
||||
root.querySelectorAll('[data-i18n]').forEach(el => { el.textContent = t(el.dataset.i18n); });
|
||||
root.querySelectorAll('[data-i18n-attr-title]').forEach(el => { el.title = t(el.dataset.i18nAttrTitle); });
|
||||
root.querySelectorAll('[data-i18n-attr-placeholder]').forEach(el => { el.placeholder = t(el.dataset.i18nAttrPlaceholder); });
|
||||
}
|
||||
|
||||
export async function setLang(lang) {
|
||||
localStorage.setItem('lang', lang);
|
||||
location.reload(); // einfach & robust, kein dynamisches Re-Rendering nötig bei dieser App-Größe
|
||||
}
|
||||
```
|
||||
|
||||
Einbindung pro Seite (vor dem bestehenden Seiten-Script):
|
||||
|
||||
```html
|
||||
<script type="module" src="/i18n.js"></script>
|
||||
<script type="module">
|
||||
import { initI18n } from '/i18n.js';
|
||||
await initI18n();
|
||||
</script>
|
||||
<script src="/client.js"></script>
|
||||
```
|
||||
|
||||
Da `client.js`/`homing.js`/`calibration.js` aktuell keine ES-Module sind, reicht es, `initI18n()`
|
||||
in einem kleinen Inline-`<script type="module">` vor dem normalen `<script>`-Tag auszuführen –
|
||||
kein Umbau der bestehenden Dateien zu Modulen nötig (DOM ist zu diesem Zeitpunkt geparst, weil
|
||||
Module standardmäßig wie `defer` laufen).
|
||||
|
||||
## 4. Dynamische Strings in Client-JS
|
||||
|
||||
Für Strings, die per JS erzeugt werden (Status-Badges, Fehlermeldungen, generierte Tabellen),
|
||||
wird `t()` global verfügbar gemacht (z. B. `window.t = t;` am Ende von `i18n.js`) und an den
|
||||
Stellen, an denen heute literal Deutsch steht, durch einen Key-Aufruf ersetzt:
|
||||
|
||||
```js
|
||||
// vorher (public/homing.js)
|
||||
if (txt) txt.textContent = text || `Schritt ${step} / ${total}`;
|
||||
|
||||
// nachher
|
||||
if (txt) txt.textContent = text || t('homing.progress.step', { step, total });
|
||||
// "homing.progress.step": "Schritt {step} / {total}" / "Step {step} of {total}"
|
||||
```
|
||||
|
||||
```js
|
||||
// vorher (public/calibration.js)
|
||||
result.innerHTML = '<span style="color:#f87171">⚠ Bitte Set auswählen, das verschoben werden soll.</span>';
|
||||
|
||||
// nachher
|
||||
result.innerHTML = `<span style="color:#f87171">⚠ ${t('calibration.move.selectSetWarning')}</span>`;
|
||||
```
|
||||
|
||||
Das ist die aufwändigste Stelle (laut grep aktuell **deutlich über 1000 Treffer** für deutsche
|
||||
Wortfragmente über alle `public/*.js`-Dateien), aber rein mechanisch und gut inkrementell machbar:
|
||||
eine Datei nach der anderen, jeweils committed und getestet.
|
||||
|
||||
## 5. Server-seitige Strings (Fehlermeldungen)
|
||||
|
||||
`server/server.js` schickt aktuell fertige deutsche Sätze im JSON zurück, z. B.
|
||||
|
||||
```js
|
||||
return res.status(400).json({ error: '"y" und "z" müssen Zahlen sein.' });
|
||||
```
|
||||
|
||||
Diese Strings landen direkt im UI (`infoEl.textContent = data.error` o. ä.) – der Server kann aber
|
||||
die Browsersprache des Clients nicht zuverlässig kennen (und sollte sie auch nicht raten müssen).
|
||||
Saubere Lösung: Server schickt einen **Code**, Client übersetzt:
|
||||
|
||||
```js
|
||||
// server/server.js
|
||||
return res.status(400).json({ errorCode: 'yzMustBeNumbers' });
|
||||
```
|
||||
|
||||
```js
|
||||
// Client, z. B. client.js / homing.js
|
||||
const msg = data.errorCode ? t(`server.error.${data.errorCode}`) : (data.error ?? t('common.unknownError'));
|
||||
```
|
||||
|
||||
Migration ist optional und kann zuletzt erfolgen – bis dahin einfach `error` (deutsch) weiter
|
||||
durchreichen und im Client mit `data.error ?? t(...)` abfangen, damit nichts bricht, während man
|
||||
Stück für Stück auf `errorCode` umstellt.
|
||||
|
||||
## 6. Sprachumschalter in der UI
|
||||
|
||||
Kleines Dropdown/Flag-Toggle in der Topbar (`calib-topbar` existiert schon in mehreren Seiten),
|
||||
das `setLang('en')` / `setLang('de')` aufruft. Reicht als erster Schritt; ein automatisches
|
||||
Re-Rendering ohne Reload ist bei dieser Seitenanzahl unnötiger Aufwand.
|
||||
|
||||
## 7. Reihenfolge der Umsetzung (Vorschlag)
|
||||
|
||||
1. `public/i18n/de.json` aus den **aktuellen** deutschen Strings extrahieren (1:1, keine
|
||||
Textänderung) + `i18n.js` Lib bauen. Keine sichtbare Änderung im Verhalten.
|
||||
2. `public/i18n/en.json` befüllen (Übersetzung).
|
||||
3. Seite für Seite umstellen, anzufangen bei `index.html` + `client.js` (am meisten genutzt),
|
||||
danach `homing.html`/`homing.js`, danach `calibration*.html`/`calibration.js`.
|
||||
4. Sprachumschalter in der Topbar ergänzen.
|
||||
5. Server-Fehlermeldungen optional zuletzt auf `errorCode` umstellen.
|
||||
|
||||
## Warum keine Library wie i18next?
|
||||
|
||||
Für die Größe dieser App (kein Build-Schritt, ~5 HTML-Seiten, kein Framework) wäre i18next/
|
||||
FormatJS deutlich mehr Gewicht (zusätzliche Dependency, Build-Integration, ICU-Syntax) als Nutzen.
|
||||
Die ~80 Zeilen oben decken Pluralisierung zwar nicht ab, aber dafür gibt es in der App aktuell
|
||||
keinen Bedarf (keine zählerabhängigen Sätze wie „1 Marker“ vs. „3 Marker“ – falls das später
|
||||
gebraucht wird, reicht eine simple `count === 1 ? key + '.one' : key + '.many'`-Konvention).
|
||||
@@ -11,13 +11,22 @@ services:
|
||||
- PYTHON_BIN=python3
|
||||
- WEBCAM_URL=http://host.docker.internal:8444
|
||||
- BODYTRACKER_URL=http://host.docker.internal:8446
|
||||
# Driver-WebSocket (Plain-Text-G-Code, self-signed). Homing sendet G92
|
||||
# hierhin (= Motorposition setzen ohne Bewegung).
|
||||
# WICHTIG: Der Input-WS lauscht container-intern auf 2095 (startRobot.js:
|
||||
# PORT||2095) und ist NICHT per host-port veröffentlicht. Die Driver-Ports
|
||||
# 2081 (Node --inspect) und 2098 (Info/Status) sind NICHT dieser WS.
|
||||
# Variante A (robust): beide Container im selben Netz (approbots), per Name:
|
||||
- DRIVER_WS_URL=wss://appRobot_Driver:2095
|
||||
# Variante B (über Host): im Driver `- "2095:2095"` veröffentlichen, dann
|
||||
# DRIVER_WS_URL=wss://host.docker.internal:2095
|
||||
extra_hosts:
|
||||
# Macht host.docker.internal auf Linux verfügbar (Standard auf macOS/Windows)
|
||||
- "host.docker.internal:host-gateway"
|
||||
ports:
|
||||
- "2093:2093"
|
||||
command: >
|
||||
/bin/bash -lc "apt-get update -qq && apt-get install -y --no-install-recommends python3-pip && pip3 install --quiet --no-cache-dir opencv-python-headless numpy && npm ci || npm install && node server/server.js"
|
||||
/bin/bash -lc "apt-get update -qq && apt-get install -y --no-install-recommends python3-pip && pip3 install --quiet --no-cache-dir opencv-python-headless numpy scipy && npm ci || npm install && node server/server.js"
|
||||
networks:
|
||||
- approbots
|
||||
restart: unless-stopped
|
||||
|
||||
98
package-lock.json
generated
98
package-lock.json
generated
@@ -9,7 +9,8 @@
|
||||
"version": "0.1.0",
|
||||
"dependencies": {
|
||||
"dotenv": "^16.4.5",
|
||||
"express": "^4.19.2"
|
||||
"express": "^4.19.2",
|
||||
"multer": "^2.2.0"
|
||||
},
|
||||
"devDependencies": {
|
||||
"jest": "^29.7.0",
|
||||
@@ -1256,6 +1257,12 @@
|
||||
"node": ">= 8"
|
||||
}
|
||||
},
|
||||
"node_modules/append-field": {
|
||||
"version": "1.0.0",
|
||||
"resolved": "https://registry.npmjs.org/append-field/-/append-field-1.0.0.tgz",
|
||||
"integrity": "sha512-klpgFSWLW1ZEs8svjfb7g4qWY0YS5imI82dTg+QahUvJ8YqAY0P10Uk8tTyh9ZGuYEZEMaeJYCF5BFuX552hsw==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/argparse": {
|
||||
"version": "1.0.10",
|
||||
"resolved": "https://registry.npmjs.org/argparse/-/argparse-1.0.10.tgz",
|
||||
@@ -1524,9 +1531,19 @@
|
||||
"version": "1.1.2",
|
||||
"resolved": "https://registry.npmjs.org/buffer-from/-/buffer-from-1.1.2.tgz",
|
||||
"integrity": "sha512-E+XQCRwSbaaiChtv6k6Dwgc+bx+Bs6vuKJHHl5kox/BaKbhiXzqQOwK4cO22yElGp2OCmjwVhT3HmxgyPGnJfQ==",
|
||||
"dev": true,
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/busboy": {
|
||||
"version": "1.6.0",
|
||||
"resolved": "https://registry.npmjs.org/busboy/-/busboy-1.6.0.tgz",
|
||||
"integrity": "sha512-8SFQbg/0hQ9xy3UNTB0YEnsNBbWfhf7RtnzpL7TkBiTBRfrQ9Fxcnz7VJsleJpyp6rVLvXiuORqjlHi5q+PYuA==",
|
||||
"dependencies": {
|
||||
"streamsearch": "^1.1.0"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">=10.16.0"
|
||||
}
|
||||
},
|
||||
"node_modules/bytes": {
|
||||
"version": "3.1.2",
|
||||
"resolved": "https://registry.npmjs.org/bytes/-/bytes-3.1.2.tgz",
|
||||
@@ -1754,6 +1771,21 @@
|
||||
"dev": true,
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/concat-stream": {
|
||||
"version": "2.0.0",
|
||||
"resolved": "https://registry.npmjs.org/concat-stream/-/concat-stream-2.0.0.tgz",
|
||||
"integrity": "sha512-MWufYdFw53ccGjCA+Ol7XJYpAlW6/prSMzuPOTRnJGcGzuhLn4Scrz7qf6o8bROZ514ltazcIFJZevcfbo0x7A==",
|
||||
"engines": [
|
||||
"node >= 6.0"
|
||||
],
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"buffer-from": "^1.0.0",
|
||||
"inherits": "^2.0.3",
|
||||
"readable-stream": "^3.0.2",
|
||||
"typedarray": "^0.0.6"
|
||||
}
|
||||
},
|
||||
"node_modules/content-disposition": {
|
||||
"version": "0.5.4",
|
||||
"resolved": "https://registry.npmjs.org/content-disposition/-/content-disposition-0.5.4.tgz",
|
||||
@@ -3978,6 +4010,25 @@
|
||||
"integrity": "sha512-Tpp60P6IUJDTuOq/5Z8cdskzJujfwqfOTkrwIwj7IRISpnkJnT6SyJ4PCPnGMoFjC9ddhal5KVIYtAt97ix05A==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/multer": {
|
||||
"version": "2.2.0",
|
||||
"resolved": "https://registry.npmjs.org/multer/-/multer-2.2.0.tgz",
|
||||
"integrity": "sha512-6rdyFg2kLrMh9Jee7/BMPuV9lEAd7lLW2YUpF9/YxR7njyoUwwQ0ZPh3TaIY50Sw6vlyD2HW3wGOkTS4P79xrQ==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"append-field": "^1.0.0",
|
||||
"busboy": "^1.6.0",
|
||||
"concat-stream": "^2.0.0",
|
||||
"type-is": "^1.6.18"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">= 10.16.0"
|
||||
},
|
||||
"funding": {
|
||||
"type": "opencollective",
|
||||
"url": "https://opencollective.com/express"
|
||||
}
|
||||
},
|
||||
"node_modules/natural-compare": {
|
||||
"version": "1.4.0",
|
||||
"resolved": "https://registry.npmjs.org/natural-compare/-/natural-compare-1.4.0.tgz",
|
||||
@@ -4554,6 +4605,20 @@
|
||||
"dev": true,
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/readable-stream": {
|
||||
"version": "3.6.2",
|
||||
"resolved": "https://registry.npmjs.org/readable-stream/-/readable-stream-3.6.2.tgz",
|
||||
"integrity": "sha512-9u/sniCrY3D5WdsERHzHE4G2YCXqoG5FTHUiCC4SIbr6XcLZBY05ya9EKjYek9O5xOAwjGq+1JdGBAS7Q9ScoA==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"inherits": "^2.0.3",
|
||||
"string_decoder": "^1.1.1",
|
||||
"util-deprecate": "^1.0.1"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">= 6"
|
||||
}
|
||||
},
|
||||
"node_modules/readdirp": {
|
||||
"version": "3.6.0",
|
||||
"resolved": "https://registry.npmjs.org/readdirp/-/readdirp-3.6.0.tgz",
|
||||
@@ -4934,6 +4999,23 @@
|
||||
"node": ">= 0.8"
|
||||
}
|
||||
},
|
||||
"node_modules/streamsearch": {
|
||||
"version": "1.1.0",
|
||||
"resolved": "https://registry.npmjs.org/streamsearch/-/streamsearch-1.1.0.tgz",
|
||||
"integrity": "sha512-Mcc5wHehp9aXz1ax6bZUyY5afg9u2rv5cqQI3mRrYkGC8rW2hM02jWuwjtL++LS5qinSyhj2QfLyNsuc+VsExg==",
|
||||
"engines": {
|
||||
"node": ">=10.0.0"
|
||||
}
|
||||
},
|
||||
"node_modules/string_decoder": {
|
||||
"version": "1.3.0",
|
||||
"resolved": "https://registry.npmjs.org/string_decoder/-/string_decoder-1.3.0.tgz",
|
||||
"integrity": "sha512-hkRX8U1WjJFd8LsDJ2yQ/wWWxaopEsABU1XfkM8A+j0+85JAGppt16cr1Whg6KIbb4okU6Mql6BOj+uup/wKeA==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"safe-buffer": "~5.2.0"
|
||||
}
|
||||
},
|
||||
"node_modules/string-length": {
|
||||
"version": "4.0.2",
|
||||
"resolved": "https://registry.npmjs.org/string-length/-/string-length-4.0.2.tgz",
|
||||
@@ -5161,6 +5243,12 @@
|
||||
"node": ">= 0.6"
|
||||
}
|
||||
},
|
||||
"node_modules/typedarray": {
|
||||
"version": "0.0.6",
|
||||
"resolved": "https://registry.npmjs.org/typedarray/-/typedarray-0.0.6.tgz",
|
||||
"integrity": "sha512-/aCDEGatGvZ2BIk+HmLf4ifCJFwvKFNb9/JeZPMulfgFracn9QFcAf5GO8B/mweUjSoblS5In0cWhqpfs/5PQA==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/undefsafe": {
|
||||
"version": "2.0.5",
|
||||
"resolved": "https://registry.npmjs.org/undefsafe/-/undefsafe-2.0.5.tgz",
|
||||
@@ -5236,6 +5324,12 @@
|
||||
"requires-port": "^1.0.0"
|
||||
}
|
||||
},
|
||||
"node_modules/util-deprecate": {
|
||||
"version": "1.0.2",
|
||||
"resolved": "https://registry.npmjs.org/util-deprecate/-/util-deprecate-1.0.2.tgz",
|
||||
"integrity": "sha512-EPD5q1uXyFxJpCrLnCc1nHnq3gOa6DZBocAIiI2TaSCA7VCJ1UJDMagCzIkXNsUYfD1daK//LTEQ8xiIbrHtcw==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/utils-merge": {
|
||||
"version": "1.0.1",
|
||||
"resolved": "https://registry.npmjs.org/utils-merge/-/utils-merge-1.0.1.tgz",
|
||||
|
||||
@@ -15,7 +15,9 @@
|
||||
},
|
||||
"dependencies": {
|
||||
"dotenv": "^16.4.5",
|
||||
"express": "^4.19.2"
|
||||
"express": "^4.19.2",
|
||||
"multer": "^2.2.0",
|
||||
"ws": "^8.20.0"
|
||||
},
|
||||
"devDependencies": {
|
||||
"jest": "^29.7.0",
|
||||
|
||||
@@ -216,6 +216,16 @@ function r2v(rx, ry, rz) { return new THREE.Vector3(rx * S, rz * S, -ry * S
|
||||
function r2vArr([rx, ry, rz]) { return r2v(rx, ry, rz); }
|
||||
function r2dir([dx, dy, dz]) { return new THREE.Vector3(dx, dz, -dy).normalize(); }
|
||||
|
||||
/**
|
||||
* True wenn `arr` ein nutzbares [x,y,z] ist (z. B. position_mm). Marker, die
|
||||
* 3b nicht triangulieren konnte (z. B. nur 1 Kamera), haben dieses Feld nicht
|
||||
* — solche Marker werden an allen Stellen, die diese Funktion nutzen, einfach
|
||||
* ignoriert statt einen Crash auszulösen.
|
||||
*/
|
||||
function hasXYZ(arr) {
|
||||
return Array.isArray(arr) && arr.length >= 3 && arr.slice(0, 3).every(Number.isFinite);
|
||||
}
|
||||
|
||||
// ── Renderer ──────────────────────────────────────────────────────────────────
|
||||
const canvas = document.getElementById('cv');
|
||||
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||||
@@ -322,7 +332,16 @@ function buildSkeletonFK(robot, angles) {
|
||||
const skel = link.skeleton;
|
||||
if (skel?.from && skel?.to) {
|
||||
const [fx, fy, fz] = skel.from;
|
||||
const [tx, ty, tz] = skel.to;
|
||||
// Endpunkt folgt dem (kalibrierten) jointToParent.origin des einzigen
|
||||
// Kind-Links, statt der statischen skeleton.to-Koordinate. Dadurch wandert
|
||||
// z.B. die Basis-Linie mit, wenn der Arm1-Pivot in Y/Z kalibriert wurde.
|
||||
let toCoord = skel.to;
|
||||
const childNames = order.filter(cn => links[cn]?.parent === linkName);
|
||||
if (childNames.length === 1) {
|
||||
const childOrigin = links[childNames[0]]?.jointToParent?.origin;
|
||||
if (Array.isArray(childOrigin) && childOrigin.length >= 3) toCoord = childOrigin;
|
||||
}
|
||||
const [tx, ty, tz] = toCoord;
|
||||
const fromW = new THREE.Vector3(fx * S, fz * S, -fy * S).applyMatrix4(childFrame);
|
||||
const toW = new THREE.Vector3(tx * S, tz * S, -ty * S).applyMatrix4(childFrame);
|
||||
|
||||
@@ -347,22 +366,35 @@ function buildSkeletonFK(robot, angles) {
|
||||
const posWorld = new THREE.Vector3(lx * S, lz * S, -ly * S).applyMatrix4(childFrame);
|
||||
const markerSizeM = (m.size ?? 25) * S;
|
||||
const [nx, ny, nz] = m.normal ?? [0, 0, 1];
|
||||
const normalW = new THREE.Vector3(nx, nz, -ny).transformDirection(childFrame).normalize();
|
||||
|
||||
// P1: Quadrat mit spin-Rotation (um die Marker-Normale in Welt-Koordinaten)
|
||||
const markerMesh = makeMarkerSquareOriented(posWorld, normalW, markerSizeM, col);
|
||||
// Marker-Orientierung ZUERST im lokalen ROBOT-Frame bauen (rohe Normale:
|
||||
// Minimal-Rotation [0,0,1]→Normale + Spin um die Normale), DANN über qView
|
||||
// in three.js-Achsen und mit qFrame in die Welt drehen. Würde man die
|
||||
// Normale wie früher schon VOR der Minimal-Rotation nach three.js drehen
|
||||
// (nx,nz,-ny), verdreht eine schräg liegende Normale (z.B. [-1,0,1]) das
|
||||
// Quadrat zusätzlich um ihren Azimut (~45°) um die eigene Achse; der Spin
|
||||
// kann das nicht kompensieren. Der Link-Roll um die Normale bleibt
|
||||
// erhalten, weil qFrame zuletzt wirkt (z.B. Marker 197: normal [-1,0,0] ∥
|
||||
// Arm1-Achse). Gegen triangulierte Ecken geprüft (Capture 20260616_133151,
|
||||
// Marker 146): diese Reihenfolge 0.8°, die alte 45.5°.
|
||||
const nRobot = new THREE.Vector3(nx, ny, nz).normalize(); // rohe robot-Normale
|
||||
const spinRad = ((m.spin ?? 0) * Math.PI) / 180;
|
||||
if (Math.abs(spinRad) > 1e-6) {
|
||||
markerMesh.quaternion.premultiply(
|
||||
new THREE.Quaternion().setFromAxisAngle(normalW, spinRad)
|
||||
);
|
||||
}
|
||||
const qNormalLoc = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nRobot);
|
||||
const qSpinLoc = new THREE.Quaternion().setFromAxisAngle(nRobot, spinRad);
|
||||
const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2); // robot→three.js
|
||||
const qFrame = new THREE.Quaternion().setFromRotationMatrix(childFrame);
|
||||
const qMarkerW = qFrame.clone().multiply(qView).multiply(qSpinLoc.multiply(qNormalLoc)); // lokal → three.js → Welt
|
||||
const normalW = new THREE.Vector3(nx, nz, -ny).applyQuaternion(qFrame).normalize();
|
||||
|
||||
// P1: orientiertes Quadrat (Normale + Roll + Spin in einem Quaternion).
|
||||
// PlaneGeometry hat nativ die +Z-Normale, qMarkerW dreht +Z auf die
|
||||
// Marker-Normale inkl. Link-Roll und Spin.
|
||||
const markerMesh = makeMarkerSquareQuat(posWorld, qMarkerW, markerSizeM, col);
|
||||
gArmMarkers.add(markerMesh);
|
||||
gArmMarkers.add(makeSphere(posWorld, 0.0006, col));
|
||||
|
||||
// P3b (Modell-Seite): Orientierungszeiger zur Ecke 0 (top-left bei spin=0)
|
||||
// markerMesh.quaternion kodiert bereits Q_normal ∘ Q_spin
|
||||
const ptrDir = new THREE.Vector3(1, 1, 0).normalize().applyQuaternion(markerMesh.quaternion);
|
||||
const ptrDir = new THREE.Vector3(1, 1, 0).normalize().applyQuaternion(qMarkerW);
|
||||
const corner0W = posWorld.clone().add(ptrDir.multiplyScalar(markerSizeM * Math.SQRT1_2));
|
||||
gArmMarkers.add(makeLine(posWorld, corner0W, col, 0.9));
|
||||
gArmMarkers.add(makeSphere(corner0W, 0.0008, col));
|
||||
@@ -385,6 +417,7 @@ function buildSkeletonFK(robot, angles) {
|
||||
}
|
||||
|
||||
for (const obs of _measuredMarkers.markers) {
|
||||
if (!hasXYZ(obs.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
|
||||
const obsLink = (obs.link && obs.link !== 'Board')
|
||||
? obs.link
|
||||
: markerIdToLink[obs.marker_id];
|
||||
@@ -454,15 +487,15 @@ function makeMarkerSquare(pos, size, color) {
|
||||
return m;
|
||||
}
|
||||
|
||||
function makeMarkerSquareOriented(pos, normalVec, size, color) {
|
||||
// Quadrat mit voll vorgegebener Orientierung (Quaternion). PlaneGeometry hat
|
||||
// nativ die +Z-Normale; das Quaternion dreht den Marker komplett (Normale, Roll,
|
||||
// Spin) – nötig, damit der Roll bei achs-paralleler Normale nicht verloren geht.
|
||||
function makeMarkerSquareQuat(pos, quat, size, color) {
|
||||
const geo = new THREE.PlaneGeometry(size, size);
|
||||
const mat = new THREE.MeshPhongMaterial({ color, side: THREE.DoubleSide, transparent: true, opacity: 0.85 });
|
||||
const mesh = new THREE.Mesh(geo, mat);
|
||||
mesh.position.copy(pos);
|
||||
const n = normalVec.clone().normalize();
|
||||
if (n.lengthSq() > 1e-9) {
|
||||
mesh.quaternion.setFromUnitVectors(new THREE.Vector3(0, 0, 1), n);
|
||||
}
|
||||
mesh.quaternion.copy(quat);
|
||||
return mesh;
|
||||
}
|
||||
|
||||
@@ -584,6 +617,7 @@ function buildScene(data) {
|
||||
);
|
||||
|
||||
for (const m of a0markers) {
|
||||
if (!hasXYZ(m.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
|
||||
nTriangulated++;
|
||||
// Kein künstlicher Offset – Kugelmittelpunkt exakt an triangulierter Position
|
||||
const mpos = r2vArr(m.position_mm);
|
||||
@@ -607,6 +641,7 @@ function buildScene(data) {
|
||||
!boardMarkers.some(bm => bm.id === m.marker_id)
|
||||
);
|
||||
for (const m of unknownTriangulated) {
|
||||
if (!hasXYZ(m.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
|
||||
nUnknown++;
|
||||
const mpos = r2vArr(m.position_mm);
|
||||
gMeasured.add(makeSphere(mpos, 0.0055, 0x3b82f6));
|
||||
@@ -715,9 +750,9 @@ function buildTable(data) {
|
||||
let dist = null, dz = null, edge = null;
|
||||
let state = 'none'; // 'tri', '1cam', 'unk'
|
||||
|
||||
if (meas) {
|
||||
if (meas && hasXYZ(meas.position_mm)) {
|
||||
[x, y, z] = meas.position_mm;
|
||||
[nx, ny, nz] = meas.normal;
|
||||
if (Array.isArray(meas.normal)) [nx, ny, nz] = meas.normal;
|
||||
edge = meas.edge_length_mm;
|
||||
state = model ? 'tri' : 'unk';
|
||||
|
||||
@@ -727,7 +762,10 @@ function buildTable(data) {
|
||||
dist = Math.sqrt(dx*dx + dy*dy + ddz*ddz);
|
||||
dz = ddz;
|
||||
}
|
||||
} else if (cameras.length > 0) {
|
||||
} else if (cameras.length > 0 || meas) {
|
||||
// meas ohne position_mm (z.B. 1-Kamera-Marker, noch nicht trianguliert)
|
||||
// zählt wie "gesehen, aber nicht trianguliert" — gleicher Stand wie
|
||||
// cameras.length>0, nur eben aus 3b statt aus den rohen Detektionen.
|
||||
state = '1cam';
|
||||
}
|
||||
|
||||
@@ -1031,7 +1069,8 @@ async function loadData(specificRunDir = null) {
|
||||
buildTable(data);
|
||||
// Fremd-Marker für Verbindungslinien merken (Marker, die nicht in Board-Link stehen)
|
||||
const bIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
|
||||
_primaryFremdMarkers = (data.measuredMarkers?.markers ?? []).filter(m => !bIds.has(m.marker_id));
|
||||
_primaryFremdMarkers = (data.measuredMarkers?.markers ?? [])
|
||||
.filter(m => !bIds.has(m.marker_id) && hasXYZ(m.position_mm));
|
||||
const measTotal = data.measuredMarkers?.markers?.length ?? 0;
|
||||
vlog(`Basis: run=${data.runDir} gesamt=${measTotal} fremd=${_primaryFremdMarkers.length} boardIDs=${bIds.size}` +
|
||||
(_primaryFremdMarkers.length ? ` (${_primaryFremdMarkers.map(m => m.marker_id).join(' ')})` : ''));
|
||||
@@ -1069,7 +1108,7 @@ async function loadCompareData() {
|
||||
// Board-Marker-IDs aus Robot.json dieses Runs
|
||||
const boardIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
|
||||
for (const m of markers) {
|
||||
if (!boardIds.has(m.marker_id)) {
|
||||
if (!boardIds.has(m.marker_id) && hasXYZ(m.position_mm)) {
|
||||
_compareFremdMarkers.push(m); // für Linien
|
||||
gCompare.add(makeSphere(r2vArr(m.position_mm), 0.006, 0xf97316)); // orange Kugel
|
||||
}
|
||||
@@ -1102,7 +1141,7 @@ async function loadPositionC() {
|
||||
const markers = data.measuredMarkers?.markers ?? [];
|
||||
const boardIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
|
||||
for (const m of markers) {
|
||||
if (!boardIds.has(m.marker_id)) {
|
||||
if (!boardIds.has(m.marker_id) && hasXYZ(m.position_mm)) {
|
||||
_positionCFremdMarkers.push(m);
|
||||
gPositionC.add(makeSphere(r2vArr(m.position_mm), 0.006, 0x22d3ee)); // cyan
|
||||
}
|
||||
|
||||
@@ -346,11 +346,17 @@ function setHomingProgress(step, total, text) {
|
||||
if (txt) txt.textContent = text || `Schritt ${step} / ${total}`;
|
||||
}
|
||||
|
||||
// Schreibt das G92-Kommando ins Eingabefeld — nur die tatsächlich bestimmten
|
||||
// Achsen, identisch zu dem, was "An Roboter senden" via server/buildG92.cjs
|
||||
// sendet (fehlende/unbeobachtbare Achsen werden weggelassen, nicht 0-gefüllt).
|
||||
function writePartialGCode(state) {
|
||||
const axisMap = { x: 'X', y: 'Y', z: 'Z', a: 'A', b: 'B', c: 'C', e: 'E' };
|
||||
const parts = [];
|
||||
for (const [key, axis] of Object.entries(axisMap)) {
|
||||
if (state[key] != null) parts.push(`${axis}${Number(state[key]).toFixed(2)}`);
|
||||
const num = Number(state[key]);
|
||||
if (state[key] != null && Number.isFinite(num)) {
|
||||
parts.push(`${axis}${num.toFixed(2)}`);
|
||||
}
|
||||
}
|
||||
if (!parts.length) return;
|
||||
const el = document.getElementById('gcodePayload');
|
||||
@@ -549,6 +555,10 @@ async function runHoming() {
|
||||
if (evt.state) {
|
||||
_homingState = evt.state;
|
||||
showHomingResult(evt.state);
|
||||
// Finales G92 ins Feld — auch wenn der Lauf über den Fallback
|
||||
// (5_pose_estimation → analysis 'robot_state' statt 'state_*')
|
||||
// lief und progressiv kein G92 geschrieben wurde.
|
||||
writePartialGCode(evt.state);
|
||||
if (btnSend) {
|
||||
btnSend.disabled = false;
|
||||
btnSend.style.opacity = '';
|
||||
@@ -593,7 +603,8 @@ async function sendHomingToRobot() {
|
||||
});
|
||||
const data = await res.json();
|
||||
if (res.ok) {
|
||||
appendLog('✅ State erfolgreich an Roboter gesendet');
|
||||
appendLog(`✅ An Roboter gesendet: ${data.gcode ?? ''}`);
|
||||
if (data.note) appendLog(`ℹ ${data.note}`);
|
||||
setHomingStatus('✓ Gesendet', 'done');
|
||||
} else {
|
||||
appendLog(`❌ Fehler beim Senden: ${data.error ?? JSON.stringify(data)}`);
|
||||
@@ -605,6 +616,23 @@ async function sendHomingToRobot() {
|
||||
}
|
||||
}
|
||||
|
||||
// Transport für die G-Code-/Befehl-Buttons (data-cmd). Schickt eine rohe
|
||||
// Zeile über das Backend an den Driver-WebSocket (POST /api/robot/gcode).
|
||||
// Liegt ein Payload-Feld vor (z.B. das G92 aus #gcodePayload), wird dessen
|
||||
// Inhalt gesendet, sonst der cmd-Name selbst. Ersetzt den toten WSS-Altpfad.
|
||||
window.sendCommand = async function (cmd, payload) {
|
||||
const line = (payload && payload.trim()) ? payload.trim() : String(cmd ?? '').trim();
|
||||
if (!line) throw new Error('Leere Befehlszeile');
|
||||
const res = await fetch('/api/robot/gcode', {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify({ line }),
|
||||
});
|
||||
const data = await res.json().catch(() => ({}));
|
||||
if (!res.ok) throw new Error(data.error ?? `HTTP ${res.status}`);
|
||||
return data;
|
||||
};
|
||||
|
||||
async function onCommandClick(btn) {
|
||||
const cmd = btn.dataset.cmd;
|
||||
const payloadSelector = btn.dataset.payload;
|
||||
|
||||
@@ -65,24 +65,6 @@
|
||||
<textarea id="analysis-log" readonly></textarea>
|
||||
</div>
|
||||
|
||||
<!-- RESULT RAW JSON -->
|
||||
<div class="section half">
|
||||
<h2>Result – Raw JSON</h2>
|
||||
<div class="panel">
|
||||
<label>Raw JSON</label>
|
||||
<textarea id="result-json" readonly></textarea>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- RESULT TREE VIEW -->
|
||||
<div class="section half">
|
||||
<h2>Result – Tree View</h2>
|
||||
<div class="panel">
|
||||
<label>Tree View</label>
|
||||
<div id="result-tree"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- BOARD-VIEWER -->
|
||||
<div class="section full">
|
||||
<h2>Board-Viewer</h2>
|
||||
|
||||
@@ -483,39 +483,23 @@ function transformDirByT(T, dir) {
|
||||
];
|
||||
}
|
||||
|
||||
function makeMarkerSquare(pos, normal, size, color) {
|
||||
// Marker-Quadrat mit vorab berechneter Orientierung (Quaternion). Die
|
||||
// BoxGeometry ist dünn in lokal-Z, ihre Normale ist also lokal +Z — quat
|
||||
// dreht +Z auf die Marker-Normale inkl. Link-Rotation und Spin.
|
||||
//
|
||||
// Die Orientierung MUSS im lokalen Link-Frame gebaut und erst danach in die
|
||||
// Szene gedreht werden (siehe Aufrufer). Würde man wie früher
|
||||
// setFromUnitVectors([0,0,1], welt_normale) NACH dem robot→three.js-
|
||||
// Achsentausch anwenden, verdreht eine schräg liegende Normale (z.B.
|
||||
// [-1,0,1]) das Quadrat zusätzlich um ihren Azimut (~45°) um die eigene
|
||||
// Achse, und der Spin fehlt ganz. Gegen triangulierte Ecken geprüft
|
||||
// (Capture 20260616_133151, Marker 146): lokale Variante 0.8°, alte 45.5°.
|
||||
function makeMarkerSquareQuat(pos, quat, size, color) {
|
||||
const geo = new THREE.BoxGeometry(size, size, size * 0.1);
|
||||
const mat = new THREE.MeshPhongMaterial({
|
||||
color,
|
||||
shininess: 40
|
||||
});
|
||||
|
||||
const mat = new THREE.MeshPhongMaterial({ color, shininess: 40 });
|
||||
const m = new THREE.Mesh(geo, mat);
|
||||
m.position.copy(pos);
|
||||
|
||||
// Fallback falls keine gültige Normale vorhanden
|
||||
let nx = 0, ny = 0, nz = 1;
|
||||
|
||||
if (Array.isArray(normal) && normal.length >= 3) {
|
||||
nx = Number(normal[0]) || 0;
|
||||
ny = Number(normal[1]) || 0;
|
||||
nz = Number(normal[2]) || 1;
|
||||
} else if (normal instanceof THREE.Vector3) {
|
||||
nx = normal.x;
|
||||
ny = normal.y;
|
||||
nz = normal.z;
|
||||
}
|
||||
|
||||
const n = new THREE.Vector3(nx, ny, nz);
|
||||
|
||||
if (n.lengthSq() > 1e-12) {
|
||||
n.normalize();
|
||||
m.quaternion.setFromUnitVectors(
|
||||
new THREE.Vector3(0, 0, 1),
|
||||
n
|
||||
);
|
||||
}
|
||||
|
||||
m.quaternion.copy(quat);
|
||||
return m;
|
||||
}
|
||||
|
||||
@@ -837,8 +821,19 @@ function rebuild() {
|
||||
// ── model markers + normals ──
|
||||
const modelPositions = {};
|
||||
const modelNormals = {};
|
||||
// robot→three.js view rotation (x,y,z)->(x,z,-y) == Rot_x(-90°). Applied LAST,
|
||||
// so the marker orientation can be built in the robot/link frame first.
|
||||
const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2);
|
||||
for (const [lname, ld] of Object.entries(links)) {
|
||||
const col = linkColor(lname);
|
||||
// link rotation in the robot frame (from FK), as a quaternion
|
||||
const Tl = T[lname] || I4();
|
||||
const qLink = new THREE.Quaternion().setFromRotationMatrix(new THREE.Matrix4().set(
|
||||
Tl[0], Tl[1], Tl[2], 0,
|
||||
Tl[4], Tl[5], Tl[6], 0,
|
||||
Tl[8], Tl[9], Tl[10], 0,
|
||||
0, 0, 0, 1
|
||||
));
|
||||
for (const m of (ld.markers||[])) {
|
||||
if (!m.position) continue;
|
||||
const mid = m.id;
|
||||
@@ -849,8 +844,16 @@ function rebuild() {
|
||||
modelPositions[mid] = wp;
|
||||
modelNormals[mid] = nWorld;
|
||||
|
||||
const sq = makeMarkerSquare(r2vArr(wp), r2vDir(...nWorld), 0.022, col);
|
||||
gModel.add(sq);
|
||||
// Orientierung ZUERST im lokalen Link-Frame (robot): Minimal-Rotation
|
||||
// [0,0,1]→Normale, dann Spin um diese Normale; DANN qLink (Link-Drehung)
|
||||
// und qView (in die Szene). So bleibt der Roll des Links um die Normale
|
||||
// erhalten und der Spin-Azimut-Twist entfällt (siehe makeMarkerSquareQuat).
|
||||
const nLR = new THREE.Vector3(nLocal[0], nLocal[1], nLocal[2]).normalize();
|
||||
const spinRad = ((m.spin ?? 0) * Math.PI) / 180;
|
||||
const qNormal = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nLR);
|
||||
const qSpin = new THREE.Quaternion().setFromAxisAngle(nLR, spinRad);
|
||||
const qMarker = qView.clone().multiply(qLink).multiply(qSpin.multiply(qNormal));
|
||||
gModel.add(makeMarkerSquareQuat(r2vArr(wp), qMarker, 0.022, col));
|
||||
|
||||
// normal arrow (length = half a marker size = ~12.5mm → 0.0125m)
|
||||
const arr = makeNormalArrow(r2vArr(wp), nWorld, 0.018, col);
|
||||
|
||||
@@ -106,6 +106,13 @@
|
||||
const mc = mapC.get(id);
|
||||
if (!mb || !mc) continue;
|
||||
|
||||
// Fehlende position_mm (z.B. Einzelkamera-Marker, von 3b nicht
|
||||
// trianguliert) ignorieren statt crashen — Marker bleibt im skipped-Log.
|
||||
if (!Array.isArray(ma.position_mm) || !Array.isArray(mb.position_mm) || !Array.isArray(mc.position_mm)) {
|
||||
skipped.push({ id, reason: 'fehlende position_mm (z.B. Einzelkamera-Marker, nicht trianguliert)' });
|
||||
continue;
|
||||
}
|
||||
|
||||
const P1 = ma.position_mm.map(Number);
|
||||
const P2 = mb.position_mm.map(Number);
|
||||
const P3 = mc.position_mm.map(Number);
|
||||
|
||||
@@ -59,11 +59,14 @@ def load_cameras(eval_dir: str) -> Dict[str, dict]:
|
||||
R = np.array(w2c["rotation_matrix"], dtype=float).reshape(3, 3)
|
||||
t = np.array(w2c["translation_m"], dtype=float).reshape(3)
|
||||
markers: Dict[int, np.ndarray] = {}
|
||||
confidence: Dict[int, float] = {}
|
||||
for d in det.get("detections", []):
|
||||
pts = d.get("image_points_px")
|
||||
if pts is not None:
|
||||
markers[int(d["marker_id"])] = np.array(pts, dtype=float).reshape(4, 2)
|
||||
cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers)
|
||||
mid = int(d["marker_id"])
|
||||
markers[mid] = np.array(pts, dtype=float).reshape(4, 2)
|
||||
confidence[mid] = float(d.get("confidence", 1.0))
|
||||
cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers, confidence=confidence)
|
||||
return cams
|
||||
|
||||
|
||||
@@ -156,6 +159,9 @@ def main() -> None:
|
||||
normal, center = corner_plane_normal(corners3d)
|
||||
edge_mm = float(np.mean([np.linalg.norm(corners3d[(i + 1) % 4] - corners3d[i]) for i in range(4)]) * 1000.0)
|
||||
|
||||
confidences = [cams[c]["confidence"].get(mid, 1.0) for c in cam_ids]
|
||||
weight = float(np.mean(confidences))
|
||||
|
||||
markers_out.append({
|
||||
"marker_id": int(mid),
|
||||
"link": marker_info.get(mid, {}).get("link", "unknown"),
|
||||
@@ -166,6 +172,7 @@ def main() -> None:
|
||||
"corners_m": [[float(v) for v in c] for c in corners3d],
|
||||
"num_cameras": len(cam_ids),
|
||||
"edge_length_mm": edge_mm,
|
||||
"weight": round(weight, 4),
|
||||
})
|
||||
|
||||
# camera poses in world (for viewer frusta): centre C = -R^T t, view axis = R[2]
|
||||
|
||||
@@ -161,10 +161,20 @@ def compute_rotation_axis(
|
||||
for mid in common_ids:
|
||||
# ── Mindest-Bewegungs-Filter ───────────────────────────────────────────
|
||||
# Marker die sich kaum bewegen liefern degenerate Umkreismittelpunkte.
|
||||
# Wir vergleichen die Zentren (position_mm) der drei Messungen.
|
||||
cA = np.array(mA[mid].get('position_mm', [0, 0, 0]), dtype=float)
|
||||
cB = np.array(mB[mid].get('position_mm', [0, 0, 0]), dtype=float)
|
||||
cC = np.array(mC[mid].get('position_mm', [0, 0, 0]), dtype=float)
|
||||
# Wir vergleichen die Zentren der drei Messungen.
|
||||
# Fehlt position_mm in einer Messung (z.B. Einzelkamera-Marker) → überspringen.
|
||||
cA_raw = mA[mid].get('position_mm')
|
||||
cB_raw = mB[mid].get('position_mm')
|
||||
cC_raw = mC[mid].get('position_mm')
|
||||
if cA_raw is None or cB_raw is None or cC_raw is None:
|
||||
skipped.append({
|
||||
'marker_id': mid,
|
||||
'reason': 'fehlende position_mm in mindestens einer Messung (z.B. Einzelkamera-Marker)',
|
||||
})
|
||||
continue
|
||||
cA = np.array(cA_raw, dtype=float)
|
||||
cB = np.array(cB_raw, dtype=float)
|
||||
cC = np.array(cC_raw, dtype=float)
|
||||
max_movement = max(
|
||||
np.linalg.norm(cB - cA),
|
||||
np.linalg.norm(cC - cB),
|
||||
|
||||
@@ -5,11 +5,19 @@
|
||||
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:
|
||||
this script estimates the rotation angle using one of three methods, tried
|
||||
in order — each next TIER is a pure fallback, only used when the previous
|
||||
one found NOT A SINGLE usable (non axis-degenerate) pair:
|
||||
|
||||
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)
|
||||
TIER 0 — PRIMARY: for every PAIR (m1, m2) of markers belonging to the
|
||||
target link itself:
|
||||
v_model = spoke_world(m2) - spoke_world(m1) (model, world-oriented)
|
||||
v_obs = world_pos_m2 - world_pos_m1 (observed, world frame)
|
||||
|
||||
where spoke_world(m) rotates the marker's local `position` (from
|
||||
robot.json) through the already-known PARENT joints via FK, so it is
|
||||
expressed in world orientation at this joint's own angle = 0 — exactly
|
||||
the frame v_obs lives in.
|
||||
|
||||
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.
|
||||
@@ -20,6 +28,29 @@ this script estimates the rotation angle using the pairwise-vector method:
|
||||
|
||||
Pair weights = baseline_model × baseline_obs (longer baselines → more reliable).
|
||||
|
||||
TIER 1 — FALLBACK-1 (child-axis): only entered when TIER 0 has nothing.
|
||||
Uses a PAIR of markers on the DIRECT CHILD link instead of the target
|
||||
link, picking only pairs whose LOCAL connecting vector is (nearly)
|
||||
parallel to the CHILD's OWN joint axis. A rotation about an axis never
|
||||
moves a vector parallel to that very axis, so such a pair is invariant
|
||||
to the child's own (still-unknown) rotation and transforms purely under
|
||||
the chain up to and including the TARGET joint — exactly like a TIER-0
|
||||
pair, just sourced one link further down. Like TIER 0 (and unlike
|
||||
TIER 2), this only needs the axis DIRECTION to be correct, not the
|
||||
pivot's position, so it is preferred over TIER 2 whenever available.
|
||||
Example: Ellbow (axis X) ← Arm2 markers 144/148 or 143/146 (Arm2's own
|
||||
axis Y, ⟂ to X, both pairs exactly axis-aligned in Arm2's local frame).
|
||||
|
||||
TIER 2 — FALLBACK-2 (pivot): only entered when TIER 1 ALSO has nothing
|
||||
(e.g. no markers visible at all besides one on the target link itself,
|
||||
or no child link exists). The joint PIVOT itself stands in for a
|
||||
missing second marker, i.e. the "pair" becomes (pivot, m1). This needs
|
||||
only ONE matched marker on the target link, but — unlike TIER 0/1 —
|
||||
its accuracy additionally depends on the already-estimated PARENT joint
|
||||
*values* being correct (not just their axis direction), since the
|
||||
pivot's world position comes from FK. See `PIVOT_FALLBACK_ID` /
|
||||
`TIER_*` / `tier_used` in the code.
|
||||
|
||||
How to use sequentially
|
||||
-----------------------
|
||||
Run 4b once per revolute joint, from root to tip:
|
||||
@@ -36,6 +67,7 @@ Output JSON
|
||||
{
|
||||
"link": "Arm1",
|
||||
"joint": "y",
|
||||
"method": "primary", // or "fallback_1_child_axis" / "fallback_2_pivot" — see TIERs above
|
||||
"mean_angle_deg": 86.3,
|
||||
"circular_std_deg": 0.7,
|
||||
"num_pairs": 6,
|
||||
@@ -62,6 +94,20 @@ from robot_fk import RobotFK
|
||||
|
||||
STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e")
|
||||
|
||||
# Sentinel "marker id" used in `per_pair` reports for the joint pivot.
|
||||
# Only ever appears in TIER_FALLBACK_2 entries (pivot vs. a single marker)
|
||||
# — see the TIER_FALLBACK_2 block inside `estimate_revolute_angle()` below.
|
||||
PIVOT_FALLBACK_ID = -1
|
||||
|
||||
# Tier labels — reported in `per_pair[].tier` and the top-level `method`
|
||||
# field, so it's always traceable which method actually produced a given
|
||||
# estimate. Tried in this order; each next one is a pure fallback (see
|
||||
# module docstring above for what each tier means and why it's ordered
|
||||
# this way).
|
||||
TIER_PRIMARY = "primary" # pair of markers on the target link itself
|
||||
TIER_FALLBACK_1 = "fallback_1_child_axis" # pair on a CHILD link, aligned with the child's OWN axis
|
||||
TIER_FALLBACK_2 = "fallback_2_pivot" # single marker on the target link vs. the joint pivot
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# I/O
|
||||
@@ -130,6 +176,112 @@ def _circular_mean_deg(angles_rad: np.ndarray,
|
||||
return math.degrees(mean), c_var, math.degrees(c_std)
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Shared spoke/pair math
|
||||
# (used by BOTH the primary marker-pair method and the pivot FALLBACK)
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _model_spoke_world(fk: RobotFK,
|
||||
zero_transforms: Dict[str, np.ndarray],
|
||||
link_name: str,
|
||||
origin_world: np.ndarray,
|
||||
local_pos: np.ndarray) -> np.ndarray:
|
||||
"""
|
||||
Vector from the joint PIVOT to a marker, in WORLD ORIENTATION, as it
|
||||
would be if this joint's own angle were 0 (only the already-known
|
||||
PARENT rotations applied).
|
||||
|
||||
This is what `v_model` must be expressed as before comparing it to
|
||||
`axis_world` / an observed vector: the raw `position` field from
|
||||
robot.json lives in the link's own pre-rotation local frame and is
|
||||
NOT yet rotated into world orientation by the parent chain. Skipping
|
||||
this rotation silently biases the estimated angle by (roughly) the
|
||||
parent joint's own angle — invisible whenever the parent chain has
|
||||
no rotation yet (e.g. Arm1, whose parent 'Base' is purely linear),
|
||||
but wrong for anything further down the chain (Ellbow, Arm2, Hand …).
|
||||
"""
|
||||
p0 = fk.marker_world(zero_transforms, link_name, local_pos)
|
||||
return p0 - origin_world
|
||||
|
||||
|
||||
def _pair_estimate(v_model: np.ndarray,
|
||||
v_obs: np.ndarray,
|
||||
axis_world: np.ndarray,
|
||||
marker_ids: Tuple[int, int],
|
||||
min_baseline_mm: float,
|
||||
tier: str,
|
||||
source_link: str) -> Tuple[Optional[float], Optional[float], dict]:
|
||||
"""
|
||||
Project model/observed vectors perpendicular to the joint axis and
|
||||
derive one angle estimate from them. Returns (angle_rad, weight,
|
||||
per_pair_entry) — angle_rad/weight are None when skipped (baseline
|
||||
too short).
|
||||
|
||||
`tier` (one of the TIER_* constants) and `source_link` (the link the
|
||||
two marker_ids actually belong to — may differ from the target link
|
||||
for TIER_FALLBACK_1) are purely descriptive, so callers/reports can
|
||||
always tell where a given estimate came from.
|
||||
"""
|
||||
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:
|
||||
return None, None, {
|
||||
"marker_ids": list(marker_ids),
|
||||
"link": source_link,
|
||||
"tier": tier,
|
||||
"skipped": True,
|
||||
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}",
|
||||
}
|
||||
|
||||
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
|
||||
weight = bl_model * bl_obs
|
||||
entry = {
|
||||
"marker_ids": list(marker_ids),
|
||||
"link": source_link,
|
||||
"tier": tier,
|
||||
"skipped": False,
|
||||
"angle_deg": math.degrees(angle),
|
||||
"baseline_model_mm": bl_model,
|
||||
"baseline_obs_mm": bl_obs,
|
||||
"weight": weight,
|
||||
}
|
||||
return angle, weight, entry
|
||||
|
||||
|
||||
def _child_links(fk: RobotFK, link_name: str) -> List[str]:
|
||||
"""Direct children of `link_name` in the kinematic tree (robot.json `parent` field)."""
|
||||
return [n for n, d in fk.links.items() if d.get("parent") == link_name]
|
||||
|
||||
|
||||
def _axis_aligned_pairs(local_positions: Dict[int, np.ndarray],
|
||||
own_axis_local: np.ndarray,
|
||||
tol_mm: float) -> List[Tuple[int, int]]:
|
||||
"""
|
||||
Among marker pairs on a CHILD link, return those whose LOCAL connecting
|
||||
vector is (nearly) parallel to the CHILD's OWN joint axis — i.e. the
|
||||
component perpendicular to that axis is within `tol_mm` of zero.
|
||||
|
||||
Such a pair is invariant to the child's own (still-unknown) rotation
|
||||
(a rotation about an axis never moves a vector parallel to that same
|
||||
axis), which is exactly what TIER_FALLBACK_1 relies on. Pairs that
|
||||
fail this check are skipped here — using them would silently mix in
|
||||
the child's unknown rotation and bias the result (see module
|
||||
docstring / TIER 1).
|
||||
"""
|
||||
a_hat = own_axis_local / (np.linalg.norm(own_axis_local) + 1e-15)
|
||||
good: List[Tuple[int, int]] = []
|
||||
for id1, id2 in combinations(sorted(local_positions.keys()), 2):
|
||||
v_local = local_positions[id2] - local_positions[id1]
|
||||
v_radial = v_local - np.dot(v_local, a_hat) * a_hat
|
||||
if float(np.linalg.norm(v_radial)) <= tol_mm:
|
||||
good.append((id1, id2))
|
||||
return good
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Core estimator (generic — works for any revolute joint)
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
@@ -140,6 +292,7 @@ def estimate_revolute_angle(
|
||||
link_name: str,
|
||||
known_state: Dict[str, float],
|
||||
min_baseline_mm: float = 15.0,
|
||||
child_axis_tol_mm: float = 1.0,
|
||||
verbose: bool = True,
|
||||
) -> dict:
|
||||
"""
|
||||
@@ -152,7 +305,10 @@ def estimate_revolute_angle(
|
||||
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
|
||||
min_baseline_mm : skip pairs with model or observed baseline shorter than this
|
||||
child_axis_tol_mm : TIER_FALLBACK_1 only — max perpendicular component (mm)
|
||||
a child-link marker pair may have relative to the
|
||||
child's OWN axis to still count as "axis-aligned"
|
||||
verbose : print report
|
||||
|
||||
Returns
|
||||
@@ -179,8 +335,9 @@ def estimate_revolute_angle(
|
||||
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)
|
||||
origin_world = fk.joint_origin_world(link_name, zero_state)
|
||||
axis_world = fk.joint_axis_world(link_name, zero_state)
|
||||
zero_transforms = fk.compute(zero_state) # needed to rotate model spokes into world orientation
|
||||
|
||||
# ── collect matched markers ───────────────────────────────
|
||||
model_local: Dict[int, np.ndarray] = {}
|
||||
@@ -192,15 +349,18 @@ def estimate_revolute_angle(
|
||||
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())}"),
|
||||
}
|
||||
# No early return here even if `matched` is empty: TIER_FALLBACK_1
|
||||
# below needs zero markers on the TARGET link itself — only on its
|
||||
# child. Whether ANY tier found anything is checked once, at the end.
|
||||
|
||||
# ── pairwise estimation ───────────────────────────────────
|
||||
def _spoke(local_pos: np.ndarray) -> np.ndarray:
|
||||
return _model_spoke_world(fk, zero_transforms, link_name, origin_world, local_pos)
|
||||
|
||||
# ── TIER 0 — PRIMARY: marker-to-marker pairs within this link ──
|
||||
# Preferred whenever ≥2 markers with a usable (non axis-parallel)
|
||||
# baseline are visible. Only the AXIS DIRECTION needs to be correct
|
||||
# for this — not the pivot's position — so it is the more robust
|
||||
# source of truth and is always tried first.
|
||||
ids = sorted(matched.keys())
|
||||
angle_rad_list: List[float] = []
|
||||
weight_list: List[float] = []
|
||||
@@ -210,42 +370,102 @@ def estimate_revolute_angle(
|
||||
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 = _spoke(l2) - _spoke(l1) # model, world-oriented
|
||||
v_obs = o2 - o1 # observed, world frame
|
||||
|
||||
v_model_perp = _project_perp(v_model, axis_world)
|
||||
v_obs_perp = _project_perp(v_obs, axis_world)
|
||||
angle, weight, entry = _pair_estimate(
|
||||
v_model, v_obs, axis_world, (id1, id2), min_baseline_mm,
|
||||
tier=TIER_PRIMARY, source_link=link_name)
|
||||
per_pair.append(entry)
|
||||
if angle is not None:
|
||||
angle_rad_list.append(angle)
|
||||
weight_list.append(weight)
|
||||
|
||||
bl_model = float(np.linalg.norm(v_model_perp))
|
||||
bl_obs = float(np.linalg.norm(v_obs_perp))
|
||||
tier_used = TIER_PRIMARY
|
||||
children_tried: List[str] = [] # for the diagnostic message if everything fails
|
||||
|
||||
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
|
||||
# ── TIER 1 — FALLBACK-1: axis-aligned pair on a CHILD link ────
|
||||
# Only entered when TIER 0 produced NOT A SINGLE usable pair. Looks
|
||||
# at every DIRECT child of this link and picks marker pairs whose
|
||||
# local vector is parallel to the CHILD's OWN axis (see
|
||||
# `_axis_aligned_pairs()`) — those are invariant to the child's own
|
||||
# still-unknown rotation, so they can stand in for a TIER-0 pair.
|
||||
# Like TIER 0, this needs only the axis DIRECTION, not the pivot's
|
||||
# position, so it is preferred over TIER 2.
|
||||
if not angle_rad_list:
|
||||
tier_used = TIER_FALLBACK_1
|
||||
children_tried = _child_links(fk, link_name)
|
||||
|
||||
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
|
||||
w = bl_model * bl_obs
|
||||
for child_name in children_tried:
|
||||
child = links[child_name]
|
||||
child_ji = child.get("jointToParent", {}) or {}
|
||||
child_axis_local = np.asarray(child_ji.get("axis", [1, 0, 0]), dtype=float)
|
||||
|
||||
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,
|
||||
})
|
||||
child_model_local: Dict[int, np.ndarray] = {}
|
||||
for m in child.get("markers", []):
|
||||
mid = int(m.get("id", -1))
|
||||
if mid >= 0 and "position" in m:
|
||||
child_model_local[mid] = np.array(m["position"], dtype=float)
|
||||
|
||||
child_matched = {mid: (child_model_local[mid], observed_mm[mid])
|
||||
for mid in child_model_local if mid in observed_mm}
|
||||
if len(child_matched) < 2:
|
||||
continue
|
||||
|
||||
aligned_pairs = _axis_aligned_pairs(
|
||||
{mid: l for mid, (l, _o) in child_matched.items()},
|
||||
child_axis_local, child_axis_tol_mm)
|
||||
|
||||
for id1, id2 in aligned_pairs:
|
||||
l1, o1 = child_matched[id1]
|
||||
l2, o2 = child_matched[id2]
|
||||
|
||||
v_model = (_model_spoke_world(fk, zero_transforms, child_name, origin_world, l2)
|
||||
- _model_spoke_world(fk, zero_transforms, child_name, origin_world, l1))
|
||||
v_obs = o2 - o1
|
||||
|
||||
angle, weight, entry = _pair_estimate(
|
||||
v_model, v_obs, axis_world, (id1, id2), min_baseline_mm,
|
||||
tier=TIER_FALLBACK_1, source_link=child_name)
|
||||
per_pair.append(entry)
|
||||
if angle is not None:
|
||||
angle_rad_list.append(angle)
|
||||
weight_list.append(weight)
|
||||
|
||||
# ── TIER 2 — FALLBACK-2: pivot + single marker on the target link ──
|
||||
# Only entered when TIER 1 ALSO produced nothing (e.g. no child
|
||||
# link, or its markers aren't visible/aligned either). Each
|
||||
# matched marker on the TARGET link is paired with the joint
|
||||
# PIVOT instead of another marker, using the rotation axis
|
||||
# already known from the predecessor joints. This is the last
|
||||
# resort: unlike TIER 0/1 it additionally relies on the
|
||||
# predecessor joints' *values* (not just their axis direction)
|
||||
# being accurate, since the pivot's world position comes from FK
|
||||
# rather than being observed directly.
|
||||
if not angle_rad_list:
|
||||
tier_used = TIER_FALLBACK_2
|
||||
for mid in ids:
|
||||
l, o = matched[mid]
|
||||
v_model = _spoke(l) # pivot → marker, model, world-oriented
|
||||
v_obs = o - origin_world # pivot → marker, observed
|
||||
|
||||
angle, weight, entry = _pair_estimate(
|
||||
v_model, v_obs, axis_world,
|
||||
(PIVOT_FALLBACK_ID, mid), min_baseline_mm,
|
||||
tier=TIER_FALLBACK_2, source_link=link_name)
|
||||
per_pair.append(entry)
|
||||
if angle is not None:
|
||||
angle_rad_list.append(angle)
|
||||
weight_list.append(weight)
|
||||
|
||||
if not angle_rad_list:
|
||||
return {
|
||||
"status": "failed",
|
||||
"reason": "All pairs below min_baseline_mm. "
|
||||
"Try --min-baseline 5 or check step-3 output.",
|
||||
"reason": (f"No usable pair at any tier: primary ({len(matched)} "
|
||||
f"marker(s) on '{link_name}'), fallback-1 (children "
|
||||
f"tried: {children_tried or 'none'}), fallback-2 "
|
||||
f"(pivot, same {len(matched)} marker(s)). Try "
|
||||
f"--min-baseline / --child-axis-tol, or check step-3 output."),
|
||||
}
|
||||
|
||||
mean_deg, c_var, c_std_deg = _circular_mean_deg(
|
||||
@@ -258,16 +478,30 @@ def estimate_revolute_angle(
|
||||
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)))}")
|
||||
if tier_used == TIER_FALLBACK_1:
|
||||
print(f" [FALLBACK-1] No usable same-link pair — estimating from "
|
||||
f"axis-aligned marker pair(s) on child link(s) "
|
||||
f"{children_tried} instead.")
|
||||
elif tier_used == TIER_FALLBACK_2:
|
||||
print(f" [FALLBACK-2] No usable pair on this link or its children — "
|
||||
f"estimating from pivot + predecessor axis instead "
|
||||
f"(single-marker spokes).")
|
||||
print(f" Pairs used: {len(angle_rad_list)} / {len(per_pair)}")
|
||||
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:
|
||||
id0, id1_ = pp["marker_ids"]
|
||||
m0 = "PIVOT" if id0 == PIVOT_FALLBACK_ID else f"M{id0}"
|
||||
m1 = "PIVOT" if id1_ == PIVOT_FALLBACK_ID else f"M{id1_}"
|
||||
link_prefix = f"{pp['link']}:" if pp["link"] != link_name else ""
|
||||
tag = {TIER_PRIMARY: "", TIER_FALLBACK_1: " [fallback-1]",
|
||||
TIER_FALLBACK_2: " [fallback-2]"}.get(pp.get("tier"), "")
|
||||
if pp["skipped"]:
|
||||
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: SKIPPED – {pp['reason']}")
|
||||
print(f" {link_prefix}{m0}↔{link_prefix}{m1}{tag}: SKIPPED – {pp['reason']}")
|
||||
else:
|
||||
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: "
|
||||
print(f" {link_prefix}{m0}↔{link_prefix}{m1}{tag}: "
|
||||
f"{pp['angle_deg']:+7.2f}° "
|
||||
f"bl_model={pp['baseline_model_mm']:.1f} "
|
||||
f"bl_obs={pp['baseline_obs_mm']:.1f}")
|
||||
@@ -280,6 +514,7 @@ def estimate_revolute_angle(
|
||||
"status": "ok",
|
||||
"link": link_name,
|
||||
"joint": var,
|
||||
"method": tier_used,
|
||||
"joint_origin_world_mm": origin_world.tolist(),
|
||||
"joint_axis_world": axis_world.tolist(),
|
||||
"mean_angle_deg": mean_deg,
|
||||
@@ -316,6 +551,10 @@ def main() -> int:
|
||||
|
||||
p.add_argument("--min-baseline", type=float, default=15.0,
|
||||
help="Skip pairs with perpendicular baseline < this (mm)")
|
||||
p.add_argument("--child-axis-tol", type=float, default=1.0,
|
||||
help="FALLBACK-1 only: max perpendicular component (mm) a "
|
||||
"child-link marker pair may have relative to the "
|
||||
"child's own axis to still count as axis-aligned")
|
||||
p.add_argument("--output", type=Path, default=None,
|
||||
help="Save result JSON (readable by next 4b as --from-state)")
|
||||
args = p.parse_args()
|
||||
@@ -343,7 +582,8 @@ def main() -> int:
|
||||
observed_mm = observed_mm,
|
||||
link_name = args.link,
|
||||
known_state = known_state,
|
||||
min_baseline_mm = args.min_baseline,
|
||||
min_baseline_mm = args.min_baseline,
|
||||
child_axis_tol_mm = args.child_axis_tol,
|
||||
verbose = True,
|
||||
)
|
||||
|
||||
|
||||
815
scripts/5_pose_estimation.py
Normal file
815
scripts/5_pose_estimation.py
Normal file
@@ -0,0 +1,815 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
pose_estimation.py
|
||||
==================
|
||||
Estimate robot joint angles (x, y, z, a, b, c, e) from triangulated marker
|
||||
poses, using the kinematic model in robot.json (via robot_fk.py).
|
||||
|
||||
Design
|
||||
------
|
||||
The estimator is parametrised over JOINT VARIABLES, not links. This handles the
|
||||
tricky cases of this robot family generically:
|
||||
* Links with zero own markers (Base/x, Hand/b, Palm/c) — their variable is
|
||||
observable only through descendant markers.
|
||||
* A variable shared by several links (FingerA & FingerB share 'e').
|
||||
* Occluded middle links — global BA reconstructs them from the fingers.
|
||||
|
||||
Four switchable methods (robot.json -> pose_estimation.method):
|
||||
sequential_vector : analytic per joint from marker-pair / normal vectors (fast)
|
||||
sequential_fk : block-wise least squares along the chain (robust, 1 marker ok)
|
||||
global_ba : all variables jointly, position + normal residuals, robust loss
|
||||
hybrid : sequential_fk init -> global_ba refine (default, most stable)
|
||||
|
||||
Observation input (robot.json -> pose_estimation.marker_observation):
|
||||
"corner_pose" (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker
|
||||
"corner_points" -> aruco_marker_poses.json: 12 corner residuals for
|
||||
robot-link markers (4 triangulated corners vs FK
|
||||
corners; no separate normal), 1 center residual for
|
||||
root-link (Board: floor/rail) markers whose spin is
|
||||
uncalibrated. Robust loss acts per corner. Opt-in;
|
||||
needs `corners_m`. Links via corner_point_links.
|
||||
"center_point" -> aruco_positions_*.json: position only
|
||||
|
||||
Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
|
||||
--from-state <json> seed/init state (flat {var: value}, or the
|
||||
{"accumulated_state": {...}} shape written by
|
||||
4b_revolute_angle.py) used as x0 for
|
||||
global_ba/hybrid instead of the internal
|
||||
estimate_sequential_fk() cold start. Missing
|
||||
variables default to 0 and are estimated/flagged
|
||||
normally. Without --from-state, behaviour is
|
||||
unchanged (internal cold start, as before).
|
||||
robot.json -> pose_estimation.fit_origin_link = "Arm1" one switch: the
|
||||
named link's jointToParent.origin Y/Z is fit
|
||||
TOGETHER WITH the normal pose in the same
|
||||
global_ba solve (complements the geometric
|
||||
multi-pose method in doc/Kalibrierung.md
|
||||
Schritt [4]) and the result is adopted
|
||||
automatically -- written back into robot.json
|
||||
(surgical text patch, see patch_robot_json_origin()).
|
||||
Off by default (key absent/null).
|
||||
|
||||
Unobservable joints (confidence "none") are written as value=null in the
|
||||
output JSON — never a fabricated 0 (see movements.<var>.observable).
|
||||
|
||||
Both the engine (estimate_pose) and a CLI (main) live here.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
import time
|
||||
from collections import defaultdict
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).parent))
|
||||
from robot_fk import RobotFK, STATE_KEYS # noqa: E402
|
||||
|
||||
try:
|
||||
from scipy.optimize import least_squares
|
||||
HAVE_SCIPY = True
|
||||
except ImportError:
|
||||
HAVE_SCIPY = False
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# Config
|
||||
# ==================================================================
|
||||
|
||||
DEFAULT_CFG: Dict[str, Any] = {
|
||||
"method": "hybrid",
|
||||
"marker_observation": "corner_pose",
|
||||
"use_normals": True,
|
||||
"normal_weight": 100.0,
|
||||
"use_marker_weight": False,
|
||||
"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": {},
|
||||
# Nur im marker_observation="corner_points"-Modus relevant: welche Links die
|
||||
# 4 Eck-Residuen nutzen. None/absent = alle Nicht-Root-Links (= der Roboter);
|
||||
# der Root-Link (Board mit Boden-/Rail-Markern) nutzt ein Center-Residuum.
|
||||
# Hintergrund: nur die Roboter-Marker-Spins sind kalibriert/verifiziert; die
|
||||
# Board/Rail-Spins nicht — deren Eckreihenfolge wäre unzuverlässig. Board ist
|
||||
# zudem Root (Residuum konstant bzgl. der Gelenke). Explizite Liste möglich,
|
||||
# z.B. ["Arm1","Ellbow","Arm2","Hand","Palm","FingerA","FingerB"].
|
||||
"corner_point_links": None,
|
||||
# One switch: if set to a link name (e.g. "Arm1"), that link's
|
||||
# jointToParent.origin Y/Z is fit together with the normal pose (same
|
||||
# global_ba solve) and the result is written back into robot.json
|
||||
# automatically. None/absent = off, no behaviour change. See
|
||||
# doc/Homing_5_Pose.md "Kalibrier-Switch".
|
||||
"fit_origin_link": None,
|
||||
}
|
||||
|
||||
|
||||
def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]:
|
||||
cfg = dict(DEFAULT_CFG)
|
||||
cfg.update(robot_data.get("pose_estimation", {}) or {})
|
||||
return cfg
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# Observations
|
||||
# ==================================================================
|
||||
|
||||
def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Load marker observations. Accepts aruco_marker_poses.json (with measured
|
||||
normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json
|
||||
(position only).
|
||||
Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, corners_mm:(4,3)|None,
|
||||
link:str, n_cams:int, weight:float}
|
||||
corners_mm (aus `corners_m`, m→mm) speist den marker_observation=
|
||||
"corner_points"-Modus in residual_vector().
|
||||
"""
|
||||
data = json.load(open(path, "r", encoding="utf-8"))
|
||||
out: Dict[int, Dict[str, Any]] = {}
|
||||
for m in data.get("markers", []):
|
||||
mid = int(m.get("marker_id", m.get("id", -1)))
|
||||
if mid < 0:
|
||||
continue
|
||||
n_cams = int(m.get("num_cameras", 99))
|
||||
if n_cams < min_cams:
|
||||
continue
|
||||
if "position_mm" in m:
|
||||
pos = np.array(m["position_mm"], dtype=float)
|
||||
elif "position_m" in m:
|
||||
pos = np.array(m["position_m"], dtype=float) * 1000.0
|
||||
else:
|
||||
continue
|
||||
nrm = None
|
||||
if use_normals and m.get("normal") is not None:
|
||||
nv = np.array(m["normal"], dtype=float)
|
||||
nn = np.linalg.norm(nv)
|
||||
if nn > 1e-9:
|
||||
nrm = nv / nn
|
||||
corners_mm = None
|
||||
cm = m.get("corners_m")
|
||||
if cm is not None:
|
||||
arr = np.array(cm, dtype=float)
|
||||
if arr.shape == (4, 3):
|
||||
corners_mm = arr * 1000.0
|
||||
out[mid] = {"pos_mm": pos, "normal": nrm, "corners_mm": corners_mm,
|
||||
"link": m.get("link", "?"), "n_cams": n_cams,
|
||||
"weight": float(m.get("weight", 1.0))}
|
||||
return out
|
||||
|
||||
|
||||
def load_seed_state(path: str) -> Dict[str, float]:
|
||||
"""
|
||||
Load a partial/full joint state to use as an optimisation seed (--from-state).
|
||||
|
||||
Accepts either a flat {variable: value} dict, or the
|
||||
{"accumulated_state": {...}, ...} wrapper written by 4b_revolute_angle.py —
|
||||
same unwrap rule as server/homingOrchestrator.js
|
||||
(`stateData.accumulated_state ?? stateData`), so 4b's output files can be
|
||||
passed in directly. Unknown keys are ignored; missing STATE_KEYS are simply
|
||||
absent from the returned dict (caller defaults them, e.g. to 0.0).
|
||||
"""
|
||||
data = json.load(open(path, "r", encoding="utf-8"))
|
||||
raw = data.get("accumulated_state", data) if isinstance(data, dict) else {}
|
||||
return {k: float(v) for k, v in raw.items() if k in STATE_KEYS and v is not None}
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# Kinematic chain analysis
|
||||
# ==================================================================
|
||||
|
||||
def analyze_chain(fk: RobotFK) -> Dict[str, Any]:
|
||||
"""
|
||||
Derive, generically from the FK topology:
|
||||
ordered_vars : movable joint variables, root->tip order, de-duplicated
|
||||
var_links : variable -> list of links it drives
|
||||
link_markers : link -> [model marker ids]
|
||||
blocks : sequential estimation blocks; each block groups the
|
||||
zero-marker ancestor variables with the next marker-
|
||||
bearing joint variable, estimated from that link's own
|
||||
markers (+ siblings sharing the same variable).
|
||||
"""
|
||||
links = fk.links
|
||||
topo = fk._topo
|
||||
|
||||
link_markers: Dict[str, List[int]] = {}
|
||||
for ln, ld in links.items():
|
||||
ids = []
|
||||
for mk in ld.get("markers", []) or []:
|
||||
if "id" in mk and "position" in mk:
|
||||
ids.append(int(mk["id"]))
|
||||
link_markers[ln] = ids
|
||||
|
||||
link_var: Dict[str, str] = {}
|
||||
for ln, ld in links.items():
|
||||
j = ld.get("jointToParent", {}) or {}
|
||||
if str(j.get("type", "")).lower() in ("revolute", "linear"):
|
||||
v = str(j.get("variable", "")).lower()
|
||||
if v:
|
||||
link_var[ln] = v
|
||||
|
||||
var_type: Dict[str, str] = {}
|
||||
var_links: Dict[str, List[str]] = defaultdict(list)
|
||||
for ln, v in link_var.items():
|
||||
var_links[v].append(ln)
|
||||
var_type[v] = str(links[ln].get("jointToParent", {}).get("type", "")).lower()
|
||||
|
||||
ordered_vars: List[str] = []
|
||||
for ln in topo:
|
||||
if ln in link_var and link_var[ln] not in ordered_vars:
|
||||
ordered_vars.append(link_var[ln])
|
||||
|
||||
# ---- build blocks ----
|
||||
blocks: List[Dict[str, Any]] = []
|
||||
var_block: Dict[str, int] = {}
|
||||
pending: List[str] = []
|
||||
for ln in topo:
|
||||
if ln not in link_var:
|
||||
continue
|
||||
v = link_var[ln]
|
||||
own = link_markers.get(ln, [])
|
||||
if v in var_block:
|
||||
# shared variable already in a block -> add this link's markers there
|
||||
if own:
|
||||
blocks[var_block[v]]["markers"].extend(own)
|
||||
continue
|
||||
if own:
|
||||
bvars = []
|
||||
for x in pending + [v]:
|
||||
if x not in bvars and x not in var_block:
|
||||
bvars.append(x)
|
||||
blocks.append({"vars": bvars, "markers": list(own), "anchor": ln})
|
||||
for x in bvars:
|
||||
var_block[x] = len(blocks) - 1
|
||||
pending = []
|
||||
else:
|
||||
if v not in pending:
|
||||
pending.append(v)
|
||||
if pending:
|
||||
blocks.append({"vars": pending, "markers": [], "anchor": None})
|
||||
for x in pending:
|
||||
var_block[x] = len(blocks) - 1
|
||||
|
||||
# subtree_markers[L] = L's own markers + all descendants' markers. Lets
|
||||
# observability() credit a block whose own link saw nothing this capture
|
||||
# but whose CHILD link did (e.g. Ellbow has no visible markers, but Arm2's
|
||||
# markers still constrain z through the chain — same idea as 4b's
|
||||
# Fallback-1, just for confidence reporting here, not for the fit itself).
|
||||
children: Dict[str, List[str]] = defaultdict(list)
|
||||
for ln, ld in links.items():
|
||||
p = ld.get("parent")
|
||||
if p:
|
||||
children[p].append(ln)
|
||||
subtree_markers: Dict[str, List[int]] = {}
|
||||
for ln in reversed(topo):
|
||||
ids = list(link_markers.get(ln, []))
|
||||
for c in children.get(ln, []):
|
||||
ids.extend(subtree_markers.get(c, []))
|
||||
subtree_markers[ln] = ids
|
||||
|
||||
return {
|
||||
"ordered_vars": ordered_vars,
|
||||
"var_type": var_type,
|
||||
"var_links": dict(var_links),
|
||||
"link_markers": link_markers,
|
||||
"subtree_markers": subtree_markers,
|
||||
"blocks": blocks,
|
||||
}
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# Residuals
|
||||
# ==================================================================
|
||||
|
||||
def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, np.ndarray]]:
|
||||
T = fk.compute(state)
|
||||
return fk.all_markers_world(T) # mid -> {world_mm, normal_world, link, local_mm}
|
||||
|
||||
|
||||
def _resolve_corner_links(fk: RobotFK, cfg: Dict[str, Any]) -> set:
|
||||
"""
|
||||
Welche Links im "corner_points"-Modus die 4 Eck-Residuen nutzen.
|
||||
Explizite Liste in cfg["corner_point_links"], sonst alle Nicht-Root-Links
|
||||
(der Root-Link Board trägt die unkalibrierten Boden-/Rail-Marker und nutzt
|
||||
ein Center-Residuum).
|
||||
"""
|
||||
explicit = cfg.get("corner_point_links")
|
||||
if isinstance(explicit, list) and explicit:
|
||||
return set(explicit)
|
||||
links = fk.links
|
||||
roots = {ln for ln, ld in links.items()
|
||||
if not ld.get("parent") or ld.get("parent") not in links}
|
||||
return set(links.keys()) - roots
|
||||
|
||||
|
||||
def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]],
|
||||
marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray:
|
||||
"""
|
||||
Residuen über die gegebenen Marker. Modus via pose_estimation.marker_observation:
|
||||
|
||||
"corner_pose" (Default): 3 Position (mm) + optional 3 Normale
|
||||
(×normal_weight) je Marker — wie bisher.
|
||||
"corner_points": 12 Eck-Residuen (4 Ecken × xyz, mm) NUR für Marker
|
||||
auf den `corner_point_links` (z.B. Hand/Finger),
|
||||
KEINE separate Normale (Orientierung steckt in den
|
||||
Ecken). Alle übrigen Marker verhalten sich wie im
|
||||
Default-Modus (Center + optionale Normale) — außer
|
||||
dem Root-Link (Board: Boden-/Rail-Marker, Spin
|
||||
unkalibriert), der nur Center bekommt ("ein Punkt
|
||||
pro Marker"). So lassen sich Ecken gezielt für
|
||||
Hand/Finger scharfschalten, ohne Arme/Board zu
|
||||
verändern.
|
||||
"""
|
||||
model = model_markers(fk, state)
|
||||
res: List[float] = []
|
||||
use_mw = bool(cfg.get("use_marker_weight", False))
|
||||
obs_mode = str(cfg.get("marker_observation", "corner_pose")).lower()
|
||||
|
||||
if obs_mode == "corner_points":
|
||||
corner_links = _resolve_corner_links(fk, cfg)
|
||||
roots = {ln for ln, ld in fk.links.items()
|
||||
if not ld.get("parent") or ld.get("parent") not in fk.links}
|
||||
w_n = float(cfg.get("normal_weight", 30.0))
|
||||
use_n = bool(cfg.get("use_normals", True))
|
||||
for mid in marker_ids:
|
||||
if mid not in model or mid not in obs:
|
||||
continue
|
||||
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
|
||||
mm = model[mid]
|
||||
link = mm.get("link")
|
||||
oc = obs[mid].get("corners_mm")
|
||||
mc = mm.get("corners_world")
|
||||
if link in corner_links and oc is not None and mc is not None:
|
||||
dc = (np.asarray(mc, float) - np.asarray(oc, float)) * mw # (4,3)
|
||||
res.extend(dc.reshape(-1).tolist()) # 12 Werte
|
||||
continue
|
||||
# Nicht-Eck-Marker verhalten sich wie im Default-Modus: Center +
|
||||
# optionale Normale — AUSSER auf dem Root-Link (Board: Boden-/Rail-
|
||||
# Marker mit unkalibriertem Spin), der nur Center bekommt ("ein
|
||||
# Punkt pro Marker"). So bleiben Arme/Board unverändert, wenn nur
|
||||
# Hand/Finger über corner_point_links auf Ecken laufen.
|
||||
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
|
||||
res.extend(dp.tolist())
|
||||
if link not in roots and use_n and obs[mid]["normal"] is not None and "normal_world" in mm:
|
||||
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n * mw
|
||||
res.extend(dn.tolist())
|
||||
return np.asarray(res, dtype=float)
|
||||
|
||||
# Default: Center (mm) + optionale Normale (skaliert)
|
||||
w_n = float(cfg.get("normal_weight", 30.0))
|
||||
use_n = bool(cfg.get("use_normals", True))
|
||||
for mid in marker_ids:
|
||||
if mid not in model or mid not in obs:
|
||||
continue
|
||||
mm = model[mid]
|
||||
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
|
||||
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
|
||||
res.extend(dp.tolist())
|
||||
if use_n and obs[mid]["normal"] is not None and "normal_world" in mm:
|
||||
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n * mw
|
||||
res.extend(dn.tolist())
|
||||
return np.asarray(res, dtype=float)
|
||||
|
||||
|
||||
def _state_from_vec(var_names: List[str], vec: np.ndarray, base: Dict[str, float]) -> Dict[str, float]:
|
||||
s = dict(base)
|
||||
for name, val in zip(var_names, vec):
|
||||
s[name] = float(val)
|
||||
return s
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# Method: global bundle adjustment
|
||||
# ==================================================================
|
||||
|
||||
def estimate_global_ba(fk: RobotFK, obs: Dict[int, Dict[str, Any]], var_names: List[str],
|
||||
x0: Dict[str, float], cfg: Dict[str, Any]) -> Dict[str, float]:
|
||||
if not HAVE_SCIPY:
|
||||
print("[WARN] scipy missing — global_ba skipped, returning init")
|
||||
return dict(x0)
|
||||
marker_ids = list(obs.keys())
|
||||
base = {k: 0.0 for k in STATE_KEYS}
|
||||
base.update(x0)
|
||||
|
||||
# One switch (pose_estimation.fit_origin_link): also free that link's
|
||||
# jointToParent.origin Y/Z as 2 extra parameters of THIS SAME solve, no
|
||||
# separate pass. fk.links[...] is mutated in place -- compute() re-reads
|
||||
# it fresh every call (robot_fk.py), so this takes effect immediately and
|
||||
# is left adopted on success (main() writes it back into robot.json).
|
||||
origin_link = cfg.get("fit_origin_link")
|
||||
origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin") if origin_link else None
|
||||
origin = origin if isinstance(origin, list) and len(origin) == 3 else None
|
||||
origin_before = list(origin) if origin else None
|
||||
n_state = len(var_names)
|
||||
|
||||
vec0 = np.array([base.get(v, 0.0) for v in var_names]
|
||||
+ (origin_before[1:3] if origin else []), dtype=float)
|
||||
|
||||
def fun(vec):
|
||||
st = _state_from_vec(var_names, vec[:n_state], base)
|
||||
if origin:
|
||||
origin[1], origin[2] = float(vec[n_state]), float(vec[n_state + 1])
|
||||
return residual_vector(st, fk, obs, marker_ids, cfg)
|
||||
|
||||
loss = cfg.get("robust_loss", "huber")
|
||||
f_scale = float(cfg.get("huber_delta_mm", 8.0))
|
||||
try:
|
||||
sol = least_squares(fun, vec0, loss=loss, f_scale=f_scale,
|
||||
max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(vec0)))
|
||||
state = _state_from_vec(var_names, sol.x[:n_state], base)
|
||||
if origin:
|
||||
origin[1], origin[2] = float(sol.x[n_state]), float(sol.x[n_state + 1])
|
||||
print(f"[INFO] fit_origin_link={origin_link}: Y,Z {origin_before[1:3]} "
|
||||
f"-> [{origin[1]:.3f}, {origin[2]:.3f}]")
|
||||
return state
|
||||
except Exception as exc:
|
||||
print(f"[WARN] global_ba failed: {exc}")
|
||||
if origin:
|
||||
origin[1], origin[2] = origin_before[1], origin_before[2] # restore on failure
|
||||
return dict(base)
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# Method: sequential block-wise FK fit
|
||||
# ==================================================================
|
||||
|
||||
def _multistart_values(vtype: str) -> List[float]:
|
||||
# revolute: scan the circle to escape local minima at large angles
|
||||
if vtype == "revolute":
|
||||
return [0.0, 60.0, 120.0, 180.0, 240.0, 300.0]
|
||||
return [0.0]
|
||||
|
||||
|
||||
def estimate_sequential_fk(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: Dict[str, Any],
|
||||
cfg: Dict[str, Any], seed: Optional[Dict[str, float]] = None
|
||||
) -> Dict[str, float]:
|
||||
"""
|
||||
Estimate block by block along the chain, freezing already-solved variables.
|
||||
|
||||
seed: optional partial/full state (e.g. from 4b_revolute_angle.py) to trust
|
||||
as a starting point. A block is SKIPPED entirely (seed used as-is, no
|
||||
re-fit) only if ALL of its variables are present in seed. Blocks with any
|
||||
missing variable are still fit normally — including their own multi-start
|
||||
— but using the seeded values of EARLIER blocks as fixed context instead
|
||||
of 0. This keeps the local-minimum protection for whatever the seed does
|
||||
NOT cover (see doc/Homing_5_Pose.md "Wichtige Einschraenkung"), while not
|
||||
re-perturbing values the caller already trusts.
|
||||
"""
|
||||
state = {k: 0.0 for k in STATE_KEYS}
|
||||
if seed:
|
||||
state.update({k: v for k, v in seed.items() if k in STATE_KEYS})
|
||||
var_type = chain["var_type"]
|
||||
|
||||
for block in chain["blocks"]:
|
||||
bvars = block["vars"]
|
||||
bmarkers = [m for m in block["markers"] if m in obs]
|
||||
if not bvars:
|
||||
continue
|
||||
if seed and all(v in seed for v in bvars):
|
||||
continue # fully seeded — trust it, don't re-fit
|
||||
if not bmarkers:
|
||||
# unobservable block: leave at seed/0, flag later
|
||||
continue
|
||||
|
||||
if not HAVE_SCIPY:
|
||||
continue
|
||||
|
||||
base = dict(state)
|
||||
|
||||
def fun(vec, _bvars=bvars, _bm=bmarkers, _base=base):
|
||||
st = _state_from_vec(_bvars, vec, _base)
|
||||
return residual_vector(st, fk, obs, _bm, cfg)
|
||||
|
||||
# multi-start over the first revolute variable in the block
|
||||
starts = [[0.0] * len(bvars)]
|
||||
lead_type = var_type.get(bvars[0], "linear")
|
||||
if lead_type == "revolute":
|
||||
starts = []
|
||||
for a0 in _multistart_values("revolute"):
|
||||
s = [0.0] * len(bvars)
|
||||
s[0] = a0
|
||||
starts.append(s)
|
||||
|
||||
best, best_cost = None, float("inf")
|
||||
for s0 in starts:
|
||||
try:
|
||||
sol = least_squares(fun, np.array(s0, dtype=float),
|
||||
loss=cfg.get("robust_loss", "huber"),
|
||||
f_scale=float(cfg.get("huber_delta_mm", 8.0)),
|
||||
max_nfev=200 * max(1, len(bvars)))
|
||||
if sol.cost < best_cost:
|
||||
best_cost, best = sol.cost, sol.x
|
||||
except Exception:
|
||||
continue
|
||||
if best is not None:
|
||||
for name, val in zip(bvars, best):
|
||||
state[name] = float(val)
|
||||
|
||||
# wrap revolute angles to (-180, 180]
|
||||
for v, vt in var_type.items():
|
||||
if vt == "revolute":
|
||||
state[v] = (state[v] + 180.0) % 360.0 - 180.0
|
||||
return state
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# Method: sequential analytic vector (per revolute joint)
|
||||
# ==================================================================
|
||||
|
||||
def estimate_sequential_vector(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: Dict[str, Any],
|
||||
cfg: Dict[str, Any]) -> Dict[str, float]:
|
||||
"""
|
||||
Analytic angle from marker geometry where possible. For revolute joints with
|
||||
>=2 markers on the link, use the perpendicular marker-pair vector. Falls back
|
||||
to the FK block solver for linear / zero-marker / single-marker cases, so it
|
||||
always returns a full state (still cheaper than full sequential_fk because
|
||||
well-populated joints are solved in closed form).
|
||||
"""
|
||||
state = {k: 0.0 for k in STATE_KEYS}
|
||||
var_type = chain["var_type"]
|
||||
link_markers = chain["link_markers"]
|
||||
var_links = chain["var_links"]
|
||||
|
||||
for block in chain["blocks"]:
|
||||
bvars = block["vars"]
|
||||
if len(bvars) == 1 and var_type.get(bvars[0]) == "revolute":
|
||||
v = bvars[0]
|
||||
ln = var_links[v][0]
|
||||
mids = [m for m in link_markers.get(ln, []) if m in obs]
|
||||
if len(mids) >= 2:
|
||||
# model vectors must be expressed in the WORLD frame at angle=0
|
||||
# (the link frame is already rotated by the parents y,z,...), so
|
||||
# use FK marker world positions with this joint set to 0.
|
||||
state_v0 = dict(state)
|
||||
state_v0[v] = 0.0
|
||||
model_v0 = model_markers(fk, state_v0)
|
||||
axis_world = fk.joint_axis_world(ln, state_v0)
|
||||
ang = _angle_from_pairs_world(mids, model_v0, obs, axis_world)
|
||||
if ang is not None:
|
||||
state[v] = ang
|
||||
continue
|
||||
# fallback: block FK fit for this single block
|
||||
_fit_single_block(fk, obs, block, var_type, cfg, state)
|
||||
|
||||
for v, vt in var_type.items():
|
||||
if vt == "revolute":
|
||||
state[v] = (state[v] + 180.0) % 360.0 - 180.0
|
||||
return state
|
||||
|
||||
|
||||
def _angle_from_pairs_world(mids: List[int], model_v0: Dict[int, Dict[str, np.ndarray]],
|
||||
obs: Dict[int, Dict[str, Any]], axis_world: np.ndarray) -> Optional[float]:
|
||||
from itertools import combinations
|
||||
a = np.asarray(axis_world, float)
|
||||
a = a / (np.linalg.norm(a) + 1e-12)
|
||||
angs, ws = [], []
|
||||
for i, j in combinations(mids, 2):
|
||||
if i not in model_v0 or j not in model_v0:
|
||||
continue
|
||||
vm = np.asarray(model_v0[j]["world_mm"], float) - np.asarray(model_v0[i]["world_mm"], float) # world @ angle 0
|
||||
vo = obs[j]["pos_mm"] - obs[i]["pos_mm"] # observed vector (world, mm)
|
||||
vm_p = vm - np.dot(vm, a) * a
|
||||
vo_p = vo - np.dot(vo, a) * a
|
||||
if np.linalg.norm(vm_p) < 5 or np.linalg.norm(vo_p) < 5:
|
||||
continue
|
||||
ang = math.atan2(float(np.dot(a, np.cross(vm_p, vo_p))), float(np.dot(vm_p, vo_p)))
|
||||
angs.append(ang)
|
||||
ws.append(np.linalg.norm(vm_p) * np.linalg.norm(vo_p))
|
||||
if not angs:
|
||||
return None
|
||||
s = sum(w * math.sin(x) for w, x in zip(ws, angs))
|
||||
c = sum(w * math.cos(x) for w, x in zip(ws, angs))
|
||||
return math.degrees(math.atan2(s, c))
|
||||
|
||||
|
||||
def _fit_single_block(fk, obs, block, var_type, cfg, state):
|
||||
if not HAVE_SCIPY:
|
||||
return
|
||||
bvars = block["vars"]
|
||||
bmarkers = [m for m in block["markers"] if m in obs]
|
||||
if not bvars or not bmarkers:
|
||||
return
|
||||
base = dict(state)
|
||||
|
||||
def fun(vec):
|
||||
return residual_vector(_state_from_vec(bvars, vec, base), fk, obs, bmarkers, cfg)
|
||||
|
||||
starts = [[0.0] * len(bvars)]
|
||||
if var_type.get(bvars[0]) == "revolute":
|
||||
starts = [[a0] + [0.0] * (len(bvars) - 1) for a0 in _multistart_values("revolute")]
|
||||
best, best_cost = None, float("inf")
|
||||
for s0 in starts:
|
||||
try:
|
||||
sol = least_squares(fun, np.array(s0, float), loss=cfg.get("robust_loss", "huber"),
|
||||
f_scale=float(cfg.get("huber_delta_mm", 8.0)), max_nfev=200 * max(1, len(bvars)))
|
||||
if sol.cost < best_cost:
|
||||
best_cost, best = sol.cost, sol.x
|
||||
except Exception:
|
||||
continue
|
||||
if best is not None:
|
||||
for name, val in zip(bvars, best):
|
||||
state[name] = float(val)
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# Dispatch
|
||||
# ==================================================================
|
||||
|
||||
def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict[str, Dict[str, Any]]:
|
||||
"""
|
||||
Per-variable confidence from how well its estimation block is determined.
|
||||
A block groups coupled variables (e.g. b,c,e on the fingers); confidence is
|
||||
driven by markers-per-variable in that block:
|
||||
high : >= 2 markers per variable (well over-determined)
|
||||
medium : >= 1 marker per variable
|
||||
low : fewer markers than variables (under-determined — distrust!),
|
||||
OR no own markers seen but a child link's markers were
|
||||
(indirect evidence through the chain, e.g. Ellbow via Arm2)
|
||||
none : no markers at all, not even indirectly (variable left at 0)
|
||||
"""
|
||||
info: Dict[str, Dict[str, Any]] = {}
|
||||
subtree_markers = chain.get("subtree_markers", {})
|
||||
for block in chain["blocks"]:
|
||||
seen = [m for m in block["markers"] if m in obs]
|
||||
indirect = False
|
||||
if not seen and block["anchor"]:
|
||||
seen = [m for m in subtree_markers.get(block["anchor"], []) if m in obs]
|
||||
indirect = bool(seen)
|
||||
nvars = max(1, len(block["vars"]))
|
||||
ratio = len(seen) / nvars
|
||||
if len(seen) == 0:
|
||||
conf = "none"
|
||||
elif indirect:
|
||||
conf = "low" # indirect/coupled through a child link, not direct
|
||||
elif ratio >= 2.0:
|
||||
conf = "high"
|
||||
elif ratio >= 1.0:
|
||||
conf = "medium"
|
||||
else:
|
||||
conf = "low"
|
||||
for v in block["vars"]:
|
||||
info[v] = {"observable": len(seen) > 0, "n_markers": len(seen),
|
||||
"block_vars": len(block["vars"]), "confidence": conf,
|
||||
"block_anchor": block["anchor"], "indirect": indirect}
|
||||
return info
|
||||
|
||||
|
||||
def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any],
|
||||
seed: Optional[Dict[str, float]] = None) -> Dict[str, Any]:
|
||||
"""
|
||||
seed: optional partial/full joint state (e.g. from load_seed_state(), the
|
||||
4b_revolute_angle.py chain) to trust as a starting point for global_ba/
|
||||
hybrid. Passed through to estimate_sequential_fk(), which skips re-fitting
|
||||
any block that is FULLY covered by seed and otherwise still applies its
|
||||
normal per-block multi-start — so variables the seed does NOT cover keep
|
||||
the existing local-minimum protection (see doc/Homing_5_Pose.md "Wichtige
|
||||
Einschraenkung") instead of silently defaulting to an unprotected 0.
|
||||
sequential_vector ignores seed (no x0 input; left untouched on purpose —
|
||||
it is the cheap analytic method, not the one this seeding targets).
|
||||
"""
|
||||
chain = analyze_chain(fk)
|
||||
var_names = chain["ordered_vars"]
|
||||
method = str(cfg.get("method", "hybrid")).lower()
|
||||
obsv = observability(chain, obs)
|
||||
|
||||
if method == "sequential_vector":
|
||||
state = estimate_sequential_vector(fk, obs, chain, cfg)
|
||||
elif method == "sequential_fk":
|
||||
state = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed)
|
||||
else: # global_ba / hybrid (default) — both use the same init->refine path
|
||||
init = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed)
|
||||
state = estimate_global_ba(fk, obs, var_names, init, cfg)
|
||||
|
||||
# final residual stats over all observed markers
|
||||
final_res = residual_vector(state, fk, obs, list(obs.keys()), cfg)
|
||||
rms = float(np.sqrt(np.mean(final_res ** 2))) if final_res.size else 0.0
|
||||
|
||||
return {"state": state, "method": method, "observability": obsv,
|
||||
"residual_rms": rms, "num_markers": len(obs)}
|
||||
|
||||
|
||||
# ==================================================================
|
||||
# CLI
|
||||
# ==================================================================
|
||||
|
||||
def patch_robot_json_origin(robot_path: str, link_name: str, yz: Tuple[float, float]) -> bool:
|
||||
"""
|
||||
Surgically rewrite links.<link_name>.jointToParent.origin[1],[2] (Y,Z) in
|
||||
the robot.json TEXT in place. robot.json has a hand-curated, compact
|
||||
format (markers etc. one per line) -- a full json.load()+json.dump()
|
||||
round-trip would reformat the whole file, so this only touches the one
|
||||
"origin": [...] array belonging to <link_name> (X left untouched).
|
||||
Returns True if a match was found and patched.
|
||||
"""
|
||||
with open(robot_path, "r", encoding="utf-8") as f:
|
||||
text = f.read()
|
||||
link_m = re.search(r'"%s"\s*:\s*\{' % re.escape(link_name), text)
|
||||
if not link_m:
|
||||
return False
|
||||
origin_m = re.search(r'"origin"\s*:\s*\[([^\]]*)\]', text[link_m.end():])
|
||||
if not origin_m:
|
||||
return False
|
||||
parts = [p.strip() for p in origin_m.group(1).split(",")]
|
||||
if len(parts) != 3:
|
||||
return False
|
||||
parts[1] = f"{float(yz[0]):.4f}".rstrip("0").rstrip(".")
|
||||
parts[2] = f"{float(yz[1]):.4f}".rstrip("0").rstrip(".")
|
||||
new_array = f"[{parts[0]}, {parts[1]}, {parts[2]}]"
|
||||
start = link_m.end() + origin_m.start()
|
||||
end = link_m.end() + origin_m.end()
|
||||
with open(robot_path, "w", encoding="utf-8") as f:
|
||||
f.write(text[:start] + '"origin": ' + new_array + text[end:])
|
||||
return True
|
||||
|
||||
|
||||
def main() -> None:
|
||||
ap = argparse.ArgumentParser(description="Estimate robot joint angles from marker poses")
|
||||
ap.add_argument("markers", help="aruco_marker_poses.json (corner_pose) or aruco_positions_*.json (center)")
|
||||
ap.add_argument("-robot", "--robot", required=True)
|
||||
ap.add_argument("-out", "--out", default=None)
|
||||
ap.add_argument("--method", default=None, help="override robot.json method")
|
||||
ap.add_argument("--marker-observation", default=None, dest="marker_observation",
|
||||
help="override robot.json marker_observation "
|
||||
"(corner_pose | corner_points | center_point)")
|
||||
ap.add_argument("--from-state", default=None, metavar="JSON",
|
||||
help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as "
|
||||
"written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid "
|
||||
"instead of the internal cold start. See doc/Homing_5_Pose.md.")
|
||||
args = ap.parse_args()
|
||||
|
||||
robot_data = json.load(open(args.robot, "r", encoding="utf-8"))
|
||||
cfg = load_pose_cfg(robot_data)
|
||||
if args.method:
|
||||
cfg["method"] = args.method
|
||||
if args.marker_observation:
|
||||
cfg["marker_observation"] = args.marker_observation
|
||||
|
||||
fk = RobotFK(robot_data)
|
||||
obs = load_observations(args.markers, cfg.get("use_normals", True),
|
||||
int(cfg.get("min_cameras_per_marker", 2)))
|
||||
seed = load_seed_state(args.from_state) if args.from_state else None
|
||||
print(f"[INFO] method={cfg['method']} | observed markers={len(obs)} | use_normals={cfg.get('use_normals')}"
|
||||
+ (f" | seed={seed}" if seed else ""))
|
||||
if cfg.get("fit_origin_link"):
|
||||
print(f"[INFO] fit_origin_link={cfg['fit_origin_link']} -> robot.json wird bei Erfolg aktualisiert")
|
||||
|
||||
result = estimate_pose(fk, obs, cfg, seed=seed)
|
||||
st = result["state"]
|
||||
|
||||
print("\nEstimated joint values:")
|
||||
for v in ["x", "y", "z", "a", "b", "c", "e"]:
|
||||
ob = result["observability"].get(v, {})
|
||||
unit = "mm" if v in ("x", "e") else "deg"
|
||||
conf = ob.get("confidence", "?")
|
||||
tag = "" if ob.get("observable", False) else " [UNOBSERVABLE -> null]"
|
||||
print(f" {v}: {st.get(v, 0.0):8.2f} {unit} (markers={ob.get('n_markers','?')}, conf={conf}){tag}")
|
||||
print(f"\n[INFO] residual RMS over {result['num_markers']} markers: {result['residual_rms']:.3f}")
|
||||
|
||||
movements = {}
|
||||
for v in ["x", "y", "z", "a", "b", "c", "e"]:
|
||||
ob = result["observability"].get(v, {})
|
||||
observable = ob.get("observable", False)
|
||||
movements[v] = {
|
||||
# Unobservable -> null, never a fabricated 0 (see module docstring).
|
||||
"value": st.get(v, 0.0) if observable else None,
|
||||
"unit": "mm" if v in ("x", "e") else "deg",
|
||||
"observable": observable,
|
||||
"confidence": ob.get("confidence", "none"),
|
||||
"n_markers": ob.get("n_markers", 0),
|
||||
}
|
||||
out = {
|
||||
"schema_version": "1.0",
|
||||
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"method": result["method"],
|
||||
"seeded": seed is not None,
|
||||
"movements": movements,
|
||||
"residual_rms": result["residual_rms"],
|
||||
"num_markers": result["num_markers"],
|
||||
}
|
||||
out_path = args.out or os.path.join(os.path.dirname(args.markers), "robot_state.json")
|
||||
json.dump(out, open(out_path, "w", encoding="utf-8"), indent=2)
|
||||
print(f"[INFO] wrote {out_path}")
|
||||
|
||||
# "Uebernehmen": fit_origin_link's Y/Z is already adopted on `fk` (see
|
||||
# estimate_global_ba) -- write it back into robot.json itself.
|
||||
origin_link = cfg.get("fit_origin_link")
|
||||
if origin_link:
|
||||
origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin")
|
||||
if isinstance(origin, list) and len(origin) == 3:
|
||||
if patch_robot_json_origin(args.robot, origin_link, (origin[1], origin[2])):
|
||||
print(f"[INFO] robot.json aktualisiert: {origin_link}.jointToParent.origin "
|
||||
f"= [{origin[0]}, {origin[1]:.3f}, {origin[2]:.3f}]")
|
||||
else:
|
||||
print(f"[WARN] {origin_link}.jointToParent.origin in {args.robot} nicht gefunden — "
|
||||
f"nicht aktualisiert (Wert nur in {out_path} sichtbar)")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
BIN
scripts/__pycache__/4b_revolute_angle.cpython-311.pyc
Normal file
BIN
scripts/__pycache__/4b_revolute_angle.cpython-311.pyc
Normal file
Binary file not shown.
BIN
scripts/__pycache__/5_pose_estimation.cpython-311.pyc
Normal file
BIN
scripts/__pycache__/5_pose_estimation.cpython-311.pyc
Normal file
Binary file not shown.
BIN
scripts/__pycache__/robot_fk.cpython-311.pyc
Normal file
BIN
scripts/__pycache__/robot_fk.cpython-311.pyc
Normal file
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -192,14 +192,109 @@ class RobotFK:
|
||||
"""Transform a local marker position → world (mm)."""
|
||||
return transform_point(transforms.get(link_name, np.eye(4)), local_pos)
|
||||
|
||||
# ── Marker-Eckpunkte (Mehrpunkt-Residuen, doc/Homing_5_Pose_MultiPoint_Weighted.md Schritt 3) ──
|
||||
|
||||
@staticmethod
|
||||
def _shortest_arc_R(normal: Sequence[float]) -> np.ndarray:
|
||||
"""
|
||||
Rotationsmatrix [0,0,1] → normal (kürzester Bogen).
|
||||
|
||||
Repliziert THREE.Quaternion.setFromUnitVectors(vFrom=[0,0,1], vTo=n)
|
||||
exakt (inkl. antiparallelem Sonderfall), damit die hier erzeugten
|
||||
Ecken zur visuell verifizierten Orientierungs-Konvention in
|
||||
boardViewer.html passen (qNormalLoc dort).
|
||||
"""
|
||||
n = np.asarray(normal, dtype=float)
|
||||
nn = float(np.linalg.norm(n))
|
||||
n = n / nn if nn > 1e-12 else np.array([0.0, 0.0, 1.0])
|
||||
# three.js: quat = (cross([0,0,1], n), dot([0,0,1], n) + 1), dann normalisieren.
|
||||
r = float(n[2]) + 1.0
|
||||
if r < 1e-12:
|
||||
# antiparallel (n == [0,0,-1]): three.js else-Zweig für vFrom=[0,0,1] → (0,-1,0,0)
|
||||
qx, qy, qz, qw = 0.0, -1.0, 0.0, 0.0
|
||||
else:
|
||||
qx, qy, qz, qw = -float(n[1]), float(n[0]), 0.0, r # cross([0,0,1], n) = (-ny, nx, 0)
|
||||
ql = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
|
||||
qx, qy, qz, qw = qx / ql, qy / ql, qz / ql, qw / ql
|
||||
return np.array([
|
||||
[1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)],
|
||||
[2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
|
||||
[2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)],
|
||||
])
|
||||
|
||||
@staticmethod
|
||||
def _marker_plane_corners(half: float) -> np.ndarray:
|
||||
"""
|
||||
Die 4 Eckpunkte in der Marker-Ebene (+Z = Normale), in DERSELBEN
|
||||
Reihenfolge wie die von 3b_corner_marker_poses.py triangulierten
|
||||
`corners_m` (ArUco 0..3, im Uhrzeigersinn von der Vorderseite gesehen).
|
||||
|
||||
Ecke 0 zeigt in Richtung (+h, +h) — dieselbe Konvention wie der visuell
|
||||
kalibrierte Orientierungszeiger (1,1,0) in boardViewer.html. Damit passt
|
||||
sie zu den manuell/visuell gesetzten `spin`-Werten der ARM-Marker, die
|
||||
im Homing die Gelenkwinkel bestimmen (gegen echte corners_m am
|
||||
Seed-Pose verifiziert, test_robot_fk_corners.py, RMS ~1 mm).
|
||||
|
||||
Hinweis: Die Board-Referenzmarker (Set A0) sind ~90° anders kalibriert,
|
||||
ihre Eckreihenfolge passt unter dieser Konvention NICHT — egal, weil
|
||||
Board der Root-Link ist: ihr Eck-Residuum ist konstant bzgl. der
|
||||
Gelenkvariablen und beeinflusst die Schätzung nicht (der robuste
|
||||
Huber-Verlust dämpft es als Ausreißer). Siehe Doc-Notiz.
|
||||
|
||||
Die Drehrichtung 0→1→2→3 ist so, dass 3b's `corner_plane_normal()`
|
||||
(outward = -cross(e01,e02)) wieder +Z liefert — identisch zur
|
||||
Beobachtungs-Konvention.
|
||||
"""
|
||||
h = float(half)
|
||||
return np.array([
|
||||
[ h, h, 0.0], # 0
|
||||
[ h, -h, 0.0], # 1
|
||||
[-h, -h, 0.0], # 2
|
||||
[-h, h, 0.0], # 3
|
||||
])
|
||||
|
||||
@classmethod
|
||||
def marker_corners_local(cls,
|
||||
position: Sequence[float],
|
||||
normal: Sequence[float],
|
||||
size_mm: float,
|
||||
spin_deg: float = 0.0) -> np.ndarray:
|
||||
"""
|
||||
Die 4 Marker-Ecken im LINK-lokalen Frame (mm), Reihenfolge wie `corners_m`.
|
||||
|
||||
Orientierung = Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale,
|
||||
exakt wie boardViewer.html (qSpinLoc.multiply(qNormalLoc)).
|
||||
"""
|
||||
n = np.asarray(normal, dtype=float)
|
||||
R = _rot_axis_angle(n, float(spin_deg)) @ cls._shortest_arc_R(n)
|
||||
plane = cls._marker_plane_corners(float(size_mm) / 2.0) # (4,3)
|
||||
return np.asarray(position, dtype=float) + (R @ plane.T).T # (4,3)
|
||||
|
||||
@classmethod
|
||||
def marker_corners_world(cls,
|
||||
transforms: Dict[str, np.ndarray],
|
||||
link_name: str,
|
||||
position: Sequence[float],
|
||||
normal: Sequence[float],
|
||||
size_mm: float,
|
||||
spin_deg: float = 0.0) -> np.ndarray:
|
||||
"""Die 4 Marker-Ecken im Weltframe (mm), Reihenfolge wie `corners_m`."""
|
||||
T = transforms.get(link_name, np.eye(4))
|
||||
local = cls.marker_corners_local(position, normal, size_mm, spin_deg)
|
||||
return np.array([transform_point(T, c) for c in local])
|
||||
|
||||
def all_markers_world(self,
|
||||
transforms: Dict[str, np.ndarray]
|
||||
) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Returns
|
||||
-------
|
||||
dict marker_id -> {world_mm, local_mm, link, normal_world}
|
||||
dict marker_id -> {world_mm, local_mm, link, normal_world, corners_world}
|
||||
|
||||
corners_world: (4,3) Welt-mm in `corners_m`-Reihenfolge (für den
|
||||
marker_observation="corner_points"-Modus in 5_pose_estimation.py).
|
||||
"""
|
||||
default_size = float((self.robot.get("markerDefaults", {}) or {}).get("size", 25.0))
|
||||
result: Dict[int, Dict[str, Any]] = {}
|
||||
for lname, ldata in self.links.items():
|
||||
T = transforms.get(lname, np.eye(4))
|
||||
@@ -210,11 +305,15 @@ class RobotFK:
|
||||
continue
|
||||
loc = np.array(m["position"], dtype=float)
|
||||
nor = np.array(m.get("normal", [0, 0, 1]), dtype=float)
|
||||
size_mm = float(m.get("size", default_size))
|
||||
spin_deg = float(m.get("spin", 0.0))
|
||||
local_corners = self.marker_corners_local(loc, nor, size_mm, spin_deg)
|
||||
result[mid] = {
|
||||
"world_mm": transform_point(T, loc),
|
||||
"local_mm": loc,
|
||||
"link": lname,
|
||||
"normal_world": (R @ nor) / max(np.linalg.norm(R @ nor), 1e-12),
|
||||
"corners_world": np.array([transform_point(T, c) for c in local_corners]),
|
||||
}
|
||||
return result
|
||||
|
||||
|
||||
502
scripts/robot_rendering.json
Normal file
502
scripts/robot_rendering.json
Normal file
@@ -0,0 +1,502 @@
|
||||
{
|
||||
"_label": "todo3_2026-06-11",
|
||||
"coordinateSystem": {"handedness": "right", "x": "right", "y": "backward", "z": "up"},
|
||||
"units": {"_owner": "appRobotDriver", "length": "mm", "rotation": "degree"},
|
||||
"kinematics": {
|
||||
"_owner": "appRobotDriver",
|
||||
"type": "arm3segmentlinearx"
|
||||
},
|
||||
"motion": {
|
||||
"_owner": "appRobotDriver",
|
||||
"defaultFeedrate": 2300,
|
||||
"speedMode": "legacy",
|
||||
"speedModeOptions": ["legacy", "correct"]
|
||||
},
|
||||
"controllers": {
|
||||
"_owner": "appRobotDriver",
|
||||
"base": { "ip": "fluidNcBase.local", "port": 2300, "protocol": "telnet", "axes": ["x", "y", "z"] },
|
||||
"elbow": { "ip": "fluidNcEllbow.local", "port": 5000, "protocol": "telnet", "axes": ["a", null, null] },
|
||||
"hand": { "ip": "fluidNcHand.local", "port": 5000, "protocol": "telnet", "axes": ["c", "e", "b"] }
|
||||
},
|
||||
"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": {
|
||||
"_owner": "appRobotDriver",
|
||||
"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": 210, "set": "Brett", "position": [20, -20, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 211, "set": "Brett", "position": [250, -10, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 215, "set": "Brett", "position": [250, -90, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 214, "set": "Brett", "position": [350, -10, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 208, "set": "Brett", "position": [350, -90, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 206, "set": "Brett", "position": [650, -10, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 205, "set": "Brett", "position": [750, -90, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 207, "set": "Brett", "position": [750, -10, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 217, "set": "Brett", "position": [650, -90, 0.3], "normal": [0, 0, 1]},
|
||||
{
|
||||
"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",
|
||||
"feedrate": 2000,
|
||||
"controller": "base"
|
||||
},
|
||||
"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",
|
||||
"feedrate": 2300,
|
||||
"controller": "base"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.2, 0.2, 0.9]},
|
||||
"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",
|
||||
"feedrate": 2300,
|
||||
"controller": "base"
|
||||
},
|
||||
"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": [
|
||||
{"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",
|
||||
"feedrate": 2300,
|
||||
"controller": "elbow"
|
||||
},
|
||||
"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": [
|
||||
{"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",
|
||||
"feedrate": 2300,
|
||||
"controller": "hand"
|
||||
},
|
||||
"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",
|
||||
"feedrate": 2300,
|
||||
"controller": "hand"
|
||||
},
|
||||
"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",
|
||||
"feedrate": 2000,
|
||||
"controller": "hand"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"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.4], "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",
|
||||
"feedrate": 2000,
|
||||
"controller": "hand"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"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.4], "spin": -27}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [-24, 0, 9.1],
|
||||
"rotationOfModelDegree": [90, 90, 0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
48
server/buildG92.cjs
Normal file
48
server/buildG92.cjs
Normal file
@@ -0,0 +1,48 @@
|
||||
/**
|
||||
* buildG92.cjs
|
||||
* Baut aus einem Homing-State {x,y,z,a,b,c,e} einen G92-G-Code-String.
|
||||
*
|
||||
* G92 setzt am appRobotDriver die Motorposition OHNE Bewegung (intern als M92
|
||||
* verarbeitet, siehe appRobotDriver/doc/API.md + robot/RobotController.js) —
|
||||
* exakt die Homing-Semantik. Die Achsbuchstaben bilden 1:1 auf die Motorachsen
|
||||
* ab: X→xMotor, Y→alpha, Z→beta, A→a, B→b, C→c, E→e.
|
||||
*
|
||||
* Bekannte Achsen werden immer mit ihrem realen Wert gesendet. Welche Achsen
|
||||
* bekannt sind, hängt vom Pfad ab:
|
||||
* - 5_pose_estimation.py (Fallback) liefert alle 7 (x,y,z,a,b,c,e),
|
||||
* - die 4b-Primärkette (Arm1→y … Hand→b) liefert nur x,y,z,a,b.
|
||||
* Eine Achse, die wirklich fehlt oder als unbeobachtbar `null` markiert ist,
|
||||
* wird per Default WEGGELASSEN — der Driver lässt nicht genannte Achsen
|
||||
* unverändert (M92 setzt nur Achsen mit endlichem Zahlenwert), statt eine
|
||||
* unbekannte Position fälschlich als 0 zu behaupten. `fillMissingWithZero`
|
||||
* erzwingt bei Bedarf das alte 0-Auffüllen.
|
||||
*
|
||||
* CommonJS, damit Jest (CJS) und der ESM-Server dieselbe Funktion nutzen
|
||||
* (gleiches Muster wie spinNormalize.cjs / homingXEstimate.cjs).
|
||||
*/
|
||||
|
||||
// Reihenfolge + Achsbuchstaben wie vom Driver erwartet.
|
||||
const AXES = [
|
||||
['x', 'X'], ['y', 'Y'], ['z', 'Z'],
|
||||
['a', 'A'], ['b', 'B'], ['c', 'C'], ['e', 'E'],
|
||||
];
|
||||
|
||||
/**
|
||||
* @param {Record<string, number|null>} state flacher Joint-State (accumulated_state)
|
||||
* @param {{decimals?: number, fillMissingWithZero?: boolean}} [opts]
|
||||
* @returns {string} z.B. "G92 X164.57 Y-2.09 Z60.58 A86.75 B-46.97 C-64.91 E22.59"
|
||||
*/
|
||||
function buildG92(state = {}, { decimals = 2, fillMissingWithZero = false } = {}) {
|
||||
const parts = [];
|
||||
for (const [key, axis] of AXES) {
|
||||
const num = Number(state?.[key]);
|
||||
if (state?.[key] != null && Number.isFinite(num)) {
|
||||
parts.push(`${axis}${num.toFixed(decimals)}`);
|
||||
} else if (fillMissingWithZero) {
|
||||
parts.push(`${axis}${(0).toFixed(decimals)}`);
|
||||
}
|
||||
}
|
||||
return `G92 ${parts.join(' ')}`;
|
||||
}
|
||||
|
||||
module.exports = { buildG92, AXES };
|
||||
77
server/driverClient.js
Normal file
77
server/driverClient.js
Normal file
@@ -0,0 +1,77 @@
|
||||
/**
|
||||
* driverClient.js – WebSocket-Transport zum appRobotDriver
|
||||
*
|
||||
* Der Driver nimmt Steuerbefehle als Plain-Text-G-Code über einen WebSocket
|
||||
* entgegen (wss://…:2096, self-signed), NICHT über HTTP — siehe
|
||||
* appRobotDriver/doc/API.md. Ein früher angenommenes `POST /api/state` existiert
|
||||
* dort nicht (war Platzhalter, vgl. doc/accessRobotAPI.md). G92 setzt am Driver
|
||||
* die Motorposition ohne Bewegung (intern M92) = exakt die Homing-Semantik.
|
||||
*
|
||||
* DRIVER_WS_URL nicht gesetzt → kein Kontakt, klarer 501-Fehler (analog zum
|
||||
* früheren ROBOT_URL-Verhalten).
|
||||
*/
|
||||
import { WebSocket } from 'ws';
|
||||
|
||||
const DRIVER_WS_URL = process.env.DRIVER_WS_URL || '';
|
||||
|
||||
/** true, wenn ein Driver-WebSocket konfiguriert ist. */
|
||||
export function isDriverConfigured() {
|
||||
return Boolean(DRIVER_WS_URL);
|
||||
}
|
||||
|
||||
/**
|
||||
* Öffnet eine kurzlebige WS-Verbindung zum Driver, sendet eine G-Code-Zeile und
|
||||
* wartet auf die erste Antwort (Positions-JSON bzw. Fehler-Envelope). Der Driver
|
||||
* broadcastet nach jedem G-Code das aktuelle Positions-JSON an alle Clients —
|
||||
* der Sender ist selbst Client und bekommt es zurück.
|
||||
*
|
||||
* @param {string} line z.B. "G92 X1 Y2 …"
|
||||
* @param {{timeoutMs?: number}} [opts]
|
||||
* @returns {Promise<{ok:boolean, sent:string, response?:any, error?:string, note?:string}>}
|
||||
*/
|
||||
export function sendGcode(line, { timeoutMs = 4000 } = {}) {
|
||||
const text = String(line ?? '').trim();
|
||||
if (!text) {
|
||||
return Promise.reject(Object.assign(new Error('Leere G-Code-Zeile'), { statusCode: 400 }));
|
||||
}
|
||||
if (!DRIVER_WS_URL) {
|
||||
return Promise.reject(Object.assign(
|
||||
new Error('DRIVER_WS_URL ist nicht konfiguriert'), { statusCode: 501 }));
|
||||
}
|
||||
|
||||
return new Promise((resolve, reject) => {
|
||||
// Self-signed Cert am Driver → Zertifikatsprüfung deaktivieren (interner Hop).
|
||||
const ws = new WebSocket(DRIVER_WS_URL, { rejectUnauthorized: false });
|
||||
let settled = false;
|
||||
|
||||
const finish = (fn, arg) => {
|
||||
if (settled) return;
|
||||
settled = true;
|
||||
clearTimeout(timer);
|
||||
try { ws.close(); } catch { /* egal */ }
|
||||
fn(arg);
|
||||
};
|
||||
|
||||
// Gesendet, aber keine Antwort rechtzeitig: kein harter Fehler — der Befehl
|
||||
// ist raus, der Driver antwortet nur evtl. nicht broadcastfähig.
|
||||
const timer = setTimeout(() => {
|
||||
finish(resolve, { ok: true, sent: text, response: null, note: 'keine Antwort (Timeout)' });
|
||||
}, timeoutMs);
|
||||
|
||||
ws.on('open', () => ws.send(text));
|
||||
|
||||
ws.on('message', (data) => {
|
||||
const raw = data.toString();
|
||||
let parsed;
|
||||
try { parsed = JSON.parse(raw); } catch { parsed = raw; }
|
||||
if (parsed && typeof parsed === 'object' && parsed.type === 'error') {
|
||||
finish(resolve, { ok: false, sent: text, error: parsed.message || raw, response: parsed });
|
||||
} else {
|
||||
finish(resolve, { ok: true, sent: text, response: parsed });
|
||||
}
|
||||
});
|
||||
|
||||
ws.on('error', (err) => finish(reject, Object.assign(
|
||||
new Error(`Driver-WS-Fehler: ${err.message}`), { statusCode: 502 })));
|
||||
});
|
||||
}
|
||||
@@ -5,18 +5,18 @@
|
||||
* atomisches Write per Temp-Datei ist hier nicht nötig – die Datei wird direkt
|
||||
* überschrieben; bei Bedarf Backup-Strategie ergänzen).
|
||||
*/
|
||||
import fsPromises from 'fs/promises';
|
||||
import { createRequire } from 'module';
|
||||
import { fetchRobot, pushRobot } from './robotConfig.js';
|
||||
const { normalizeSpinDeg } = createRequire(import.meta.url)('./spinNormalize.cjs');
|
||||
|
||||
// ── I/O ───────────────────────────────────────────────────────────────────────
|
||||
|
||||
async function readRobot(robotPath) {
|
||||
return JSON.parse(await fsPromises.readFile(robotPath, 'utf8'));
|
||||
async function readRobot(_robotPath) {
|
||||
return fetchRobot();
|
||||
}
|
||||
|
||||
async function writeRobot(robotPath, data) {
|
||||
await fsPromises.writeFile(robotPath, JSON.stringify(data, null, 2), 'utf8');
|
||||
async function writeRobot(_robotPath, data) {
|
||||
return pushRobot(data);
|
||||
}
|
||||
|
||||
// ── Aktion 1: Marker nach Z-Bereich zuordnen ─────────────────────────────────
|
||||
@@ -374,6 +374,12 @@ export async function assignMarkerId(robotPath, { markerId, set, link, extraMark
|
||||
return { changed: false, error: 'Link muss angegeben werden, um einen neuen Marker hinzuzufügen.' };
|
||||
}
|
||||
|
||||
if (!Array.isArray(em.position_mm)) {
|
||||
return {
|
||||
changed: false,
|
||||
error: `Marker ${id} hat keine triangulierte Position (position_mm fehlt – z.B. Einzelkamera-Marker).`,
|
||||
};
|
||||
}
|
||||
const newMarker = {
|
||||
id,
|
||||
position: em.position_mm.map(v => Math.round(Number(v) * 100) / 100),
|
||||
|
||||
@@ -42,6 +42,7 @@ export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) {
|
||||
* runScript: (args: string[], send: Function) => Promise<number>,
|
||||
* runBoardPipeline: (runDir: string, send: Function) => Promise<void>,
|
||||
* SCRIPT_4B: string,
|
||||
* SCRIPT_5POSE: string,
|
||||
* }} opts
|
||||
*/
|
||||
export async function runHoming({
|
||||
@@ -51,6 +52,7 @@ export async function runHoming({
|
||||
runScript,
|
||||
runBoardPipeline,
|
||||
SCRIPT_4B,
|
||||
SCRIPT_5POSE,
|
||||
}) {
|
||||
// Lauf-Verzeichnis anlegen
|
||||
const ts = makeTimestamp();
|
||||
@@ -85,6 +87,7 @@ export async function runHoming({
|
||||
// ── Schritt 3–6: 4b-Kette (Arm1 → Ellbow → Arm2 → Hand) ─────────────
|
||||
const links = ['Arm1', 'Ellbow', 'Arm2', 'Hand'];
|
||||
let fromState = null;
|
||||
let chainComplete = true;
|
||||
|
||||
for (let i = 0; i < links.length; i++) {
|
||||
const link = links[i];
|
||||
@@ -104,9 +107,9 @@ export async function runHoming({
|
||||
|
||||
const exit = await runScript(args, send);
|
||||
if (exit !== 0) {
|
||||
send({ type: 'error', text: `❌ 4b ${link} Exit ${exit}` });
|
||||
send({ type: 'done', exitCode: exit, runDir: ts });
|
||||
return;
|
||||
send({ type: 'log', text: `⚠ 4b ${link} Exit ${exit} — falle auf 5_pose_estimation.py zurück` });
|
||||
chainComplete = false;
|
||||
break;
|
||||
}
|
||||
fromState = outputPath;
|
||||
|
||||
@@ -120,8 +123,25 @@ export async function runHoming({
|
||||
|
||||
// ── Endergebnis ──────────────────────────────────────────────────────────
|
||||
try {
|
||||
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
|
||||
const finalState = finalData.accumulated_state ?? finalData;
|
||||
let finalState;
|
||||
if (chainComplete) {
|
||||
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
|
||||
finalState = finalData.accumulated_state ?? finalData;
|
||||
} else {
|
||||
// 4b vorzeitig abgebrochen -> 5_pose_estimation.py mit dem letzten
|
||||
// erfolgreichen Zwischenstand (falls vorhanden) als Startwert.
|
||||
send({ type: 'step', step: 6, total: 6, text: '5_pose_estimation.py (Fallback) …' });
|
||||
const poseOut = path.join(runDir, 'robot_state.json');
|
||||
const args = [SCRIPT_5POSE, arucoJson, '-robot', robotJsonPath, '-out', poseOut];
|
||||
if (fromState) args.push('--from-state', fromState);
|
||||
const exit = await runScript(args, send);
|
||||
if (exit !== 0) throw new Error(`5_pose_estimation.py Exit ${exit}`);
|
||||
const poseData = JSON.parse(await fsPromises.readFile(poseOut, 'utf8'));
|
||||
finalState = Object.fromEntries(
|
||||
Object.entries(poseData.movements).map(([k, v]) => [k, v.value])
|
||||
);
|
||||
send({ type: 'analysis', key: 'robot_state', value: poseData });
|
||||
}
|
||||
send({ type: 'log', text: '' });
|
||||
send({ type: 'log', text: `✅ Homing abgeschlossen: ${ts}` });
|
||||
send({ type: 'done', exitCode: 0, state: finalState, runDir: ts });
|
||||
@@ -131,6 +151,108 @@ export async function runHoming({
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Führt den Homing-Ablauf offline aus (Bilder und NPZ bereits im runDir).
|
||||
* Identische 4b-Kette wie runHoming — ohne Webcam-Zugriff und ohne SSE-Stream.
|
||||
* send() akkumuliert Logs; done-Event trägt den finalen State.
|
||||
*
|
||||
* @param {{
|
||||
* robotJsonPath: string,
|
||||
* runDir: string,
|
||||
* send: (obj: object) => void,
|
||||
* runScript: (args: string[], send: Function) => Promise<number>,
|
||||
* runBoardPipelineOffline:(runDir: string, send: Function) => Promise<void>,
|
||||
* SCRIPT_4B: string,
|
||||
* SCRIPT_5POSE: string,
|
||||
* }} opts
|
||||
*/
|
||||
export async function runHomingOffline({
|
||||
robotJsonPath,
|
||||
runDir,
|
||||
send,
|
||||
runScript,
|
||||
runBoardPipelineOffline,
|
||||
SCRIPT_4B,
|
||||
SCRIPT_5POSE,
|
||||
}) {
|
||||
send({ type: 'log', text: `▶ Homing-Offline: ${path.basename(runDir)}` });
|
||||
send({ type: 'log', text: `▶ Robot-JSON: ${robotJsonPath}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
// ── Schritt 1: Marker-Triangulierung (Bilder liegen bereits im runDir) ──────
|
||||
send({ type: 'step', step: 1, total: 5, text: 'Marker-Triangulierung …' });
|
||||
await runBoardPipelineOffline(runDir, send);
|
||||
|
||||
const arucoJson = path.join(runDir, 'aruco_marker_poses.json');
|
||||
try {
|
||||
await fsPromises.access(arucoJson);
|
||||
} catch {
|
||||
send({ type: 'error', text: '❌ aruco_marker_poses.json fehlt – Script 3b hat nicht funktioniert.' });
|
||||
send({ type: 'done', exitCode: -1 });
|
||||
return;
|
||||
}
|
||||
|
||||
// ── Schritt 2: X-Position schätzen ──────────────────────────────────────────
|
||||
const xMm = estimateXFromMarkers(arucoJson, robotJsonPath);
|
||||
send({ type: 'log', text: `▶ Geschätzte X-Position: ${xMm.toFixed(1)} mm` });
|
||||
send({ type: 'analysis', key: 'x_mm', value: xMm });
|
||||
|
||||
// ── Schritte 3–5 (2–4): 4b-Kette Arm1 → Ellbow → Arm2 → Hand ───────────────
|
||||
const links = ['Arm1', 'Ellbow', 'Arm2', 'Hand'];
|
||||
let fromState = null;
|
||||
let chainComplete = true;
|
||||
|
||||
for (let i = 0; i < links.length; i++) {
|
||||
const link = links[i];
|
||||
send({ type: 'step', step: 2 + i, total: 5, text: `Gelenkwinkel ${link} …` });
|
||||
send({ type: 'log', text: `\n─── 4b: ${link} ${'─'.repeat(35 - link.length)}` });
|
||||
|
||||
const outputPath = path.join(runDir, `state_${link}.json`);
|
||||
const args = [SCRIPT_4B, '--robot', robotJsonPath, '--aruco', arucoJson, '--link', link, '--output', outputPath];
|
||||
if (fromState) args.push('--from-state', fromState);
|
||||
else args.push('--x-mm', String(xMm));
|
||||
|
||||
const exit = await runScript(args, send);
|
||||
if (exit !== 0) {
|
||||
send({ type: 'log', text: `⚠ 4b ${link} Exit ${exit} — falle auf 5_pose_estimation.py zurück` });
|
||||
chainComplete = false;
|
||||
break;
|
||||
}
|
||||
fromState = outputPath;
|
||||
|
||||
try {
|
||||
const stateData = JSON.parse(await fsPromises.readFile(outputPath, 'utf8'));
|
||||
send({ type: 'analysis', key: `state_${link}`, value: stateData.accumulated_state ?? stateData });
|
||||
} catch { /* ignorieren */ }
|
||||
}
|
||||
|
||||
// ── Endergebnis ──────────────────────────────────────────────────────────────
|
||||
try {
|
||||
let finalState;
|
||||
if (chainComplete) {
|
||||
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
|
||||
finalState = finalData.accumulated_state ?? finalData;
|
||||
} else {
|
||||
send({ type: 'step', step: 5, total: 5, text: '5_pose_estimation.py (Fallback) …' });
|
||||
const poseOut = path.join(runDir, 'robot_state.json');
|
||||
const args = [SCRIPT_5POSE, arucoJson, '-robot', robotJsonPath, '-out', poseOut];
|
||||
if (fromState) args.push('--from-state', fromState);
|
||||
const exit = await runScript(args, send);
|
||||
if (exit !== 0) throw new Error(`5_pose_estimation.py Exit ${exit}`);
|
||||
const poseData = JSON.parse(await fsPromises.readFile(poseOut, 'utf8'));
|
||||
finalState = Object.fromEntries(
|
||||
Object.entries(poseData.movements).map(([k, v]) => [k, v.value])
|
||||
);
|
||||
}
|
||||
send({ type: 'log', text: '' });
|
||||
send({ type: 'log', text: '✅ Homing-Offline abgeschlossen' });
|
||||
send({ type: 'done', exitCode: 0, state: finalState });
|
||||
} catch (err) {
|
||||
send({ type: 'error', text: `❌ Endzustand konnte nicht gelesen werden: ${err.message}` });
|
||||
send({ type: 'done', exitCode: -1 });
|
||||
}
|
||||
}
|
||||
|
||||
/** Timestamp-String YYYYMMDD_HHmmss */
|
||||
function makeTimestamp() {
|
||||
const now = new Date();
|
||||
|
||||
44
server/robotConfig.js
Normal file
44
server/robotConfig.js
Normal file
@@ -0,0 +1,44 @@
|
||||
import fsPromises from 'fs/promises';
|
||||
import path from 'path';
|
||||
import { fileURLToPath } from 'url';
|
||||
|
||||
const __dirname = path.dirname(fileURLToPath(import.meta.url));
|
||||
|
||||
const ROBOT_URL = process.env.ROBOT_URL || '';
|
||||
const ROBOT_JSON = process.env.ROBOT_JSON
|
||||
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
|
||||
|
||||
/**
|
||||
* Lädt robot.json.
|
||||
* Reihenfolge: (1) ROBOT_URL/api/robot/config, (2) lokale Datei als Fallback.
|
||||
* Schreibt das Ergebnis immer in die lokale Cache-Datei (für Python-Skripte).
|
||||
*/
|
||||
export async function fetchRobot() {
|
||||
if (ROBOT_URL) {
|
||||
const res = await fetch(new URL('/api/robot/config', ROBOT_URL));
|
||||
if (!res.ok) throw new Error(`Driver ${res.status}: ${await res.text()}`);
|
||||
const data = await res.json();
|
||||
await fsPromises.writeFile(ROBOT_JSON, JSON.stringify(data, null, 2), 'utf8');
|
||||
return data;
|
||||
}
|
||||
return JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
|
||||
}
|
||||
|
||||
/**
|
||||
* Speichert robot.json.
|
||||
* Schreibt immer in lokale Cache-Datei; sendet zusätzlich an Driver wenn konfiguriert.
|
||||
*/
|
||||
export async function pushRobot(data) {
|
||||
await fsPromises.writeFile(ROBOT_JSON, JSON.stringify(data, null, 2), 'utf8');
|
||||
if (ROBOT_URL) {
|
||||
const res = await fetch(new URL('/api/robot/config', ROBOT_URL), {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(data),
|
||||
});
|
||||
if (!res.ok) throw new Error(`Driver ${res.status}: ${await res.text()}`);
|
||||
}
|
||||
}
|
||||
|
||||
/** Pfad zur lokalen Cache-Datei – wird an Python-Skripte als -robot-Argument übergeben. */
|
||||
export const robotCachePath = ROBOT_JSON;
|
||||
377
server/server.js
377
server/server.js
@@ -9,7 +9,11 @@ import process from 'process';
|
||||
import { spawn } from 'child_process';
|
||||
import { WebcamClient } from './webcamClient.js';
|
||||
import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId, adoptXAxis, assignFixedMarkersToLink, setJointOriginYZ, setArmMarkerSpin } from './editRobot.js';
|
||||
import { runHoming } from './homingOrchestrator.js';
|
||||
import multer from 'multer';
|
||||
import { runHoming, runHomingOffline } from './homingOrchestrator.js';
|
||||
import { fetchRobot, robotCachePath } from './robotConfig.js';
|
||||
import { sendGcode, isDriverConfigured } from './driverClient.js';
|
||||
import { buildG92 } from './buildG92.cjs';
|
||||
|
||||
const __filename = fileURLToPath(import.meta.url);
|
||||
const __dirname = path.dirname(__filename);
|
||||
@@ -22,11 +26,18 @@ const publicDir = path.join(__dirname, '..', 'public');
|
||||
const snapshotsDir = path.join(publicDir, 'snapshots');
|
||||
const WEBCAM_URL = process.env.WEBCAM_URL || '';
|
||||
const BODYTRACKER_URL = process.env.BODYTRACKER_URL || '';
|
||||
const ROBOT_URL = process.env.ROBOT_URL || '';
|
||||
// Roboter-Transport läuft über den Driver-WebSocket (DRIVER_WS_URL,
|
||||
// server/driverClient.js), nicht mehr über HTTP ROBOT_URL.
|
||||
const HTTPS_KEY_PATH = process.env.HTTPS_KEY_PATH || path.join(__dirname, '..', 'https', 'localhost.key');
|
||||
const HTTPS_CERT_PATH = process.env.HTTPS_CERT_PATH || path.join(__dirname, '..', 'https', 'localhost.pem');
|
||||
const HTTPS_PASSPHRASE = process.env.HTTPS_PASSPHRASE || 'abcd';
|
||||
|
||||
// .html/.js immer revalidieren lassen (kein stilles Stale-Caching durch Browser/Proxy
|
||||
// nach Code-Änderungen, z.B. boardViewer.html) – Bilder/STL etc. bleiben normal cachebar.
|
||||
app.use((req, res, next) => {
|
||||
if (/\.(html|js)$/.test(req.path)) res.setHeader('Cache-Control', 'no-cache');
|
||||
next();
|
||||
});
|
||||
app.use(express.static(publicDir));
|
||||
|
||||
app.get('/api/health', (req, res) => {
|
||||
@@ -431,14 +442,14 @@ app.post('/api/calibration/compute', async (req, res) => {
|
||||
|
||||
// ── Board-Erkennung ───────────────────────────────────────────────────────────
|
||||
|
||||
const boardDataDir = path.join(__dirname, '..', 'data', 'board');
|
||||
const homingDataDir = path.join(__dirname, '..', 'data', 'homing');
|
||||
const ROBOT_JSON = process.env.ROBOT_JSON
|
||||
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
|
||||
const boardDataDir = path.join(__dirname, '..', 'data', 'board');
|
||||
const homingDataDir = path.join(__dirname, '..', 'data', 'homing');
|
||||
const homingOfflineDataDir = path.join(__dirname, '..', 'data', 'homing-offline');
|
||||
const SCRIPT_1 = path.join(__dirname, '..', 'scripts', '1_detect_aruco_observations.py');
|
||||
const SCRIPT_2 = path.join(__dirname, '..', 'scripts', '2_estimate_camera_from_observations.py');
|
||||
const SCRIPT_3B = path.join(__dirname, '..', 'scripts', '3b_corner_marker_poses.py');
|
||||
const SCRIPT_4B = path.join(__dirname, '..', 'scripts', '4b_revolute_angle.py');
|
||||
const SCRIPT_5POSE = path.join(__dirname, '..', 'scripts', '5_pose_estimation.py');
|
||||
|
||||
/**
|
||||
* Führt ein Python-Script aus und leitet stdout/stderr zeilenweise an `send` weiter.
|
||||
@@ -489,6 +500,12 @@ function runScript(args, send) {
|
||||
* @param {{ refSet?: string }} [opts]
|
||||
*/
|
||||
async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
try {
|
||||
await fetchRobot();
|
||||
} catch (err) {
|
||||
send({ type: 'log', text: `⚠ robot.json-Cache: Driver nicht erreichbar – nutze lokale Datei (${err.message})` });
|
||||
}
|
||||
|
||||
// Kameras ermitteln
|
||||
if (!WEBCAM_URL) throw new Error('WEBCAM_URL nicht konfiguriert');
|
||||
const camData = await new WebcamClient(WEBCAM_URL).getCameras();
|
||||
@@ -496,24 +513,31 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
// Pro Kamera: Foto → Script 1 → Script 2
|
||||
for (const camId of cameraIds) {
|
||||
// Phase 1: alle Kameras gleichzeitig fotografieren (Modus-Umschaltung parallel)
|
||||
send({ type: 'log', text: 'Fotos aufnehmen …' });
|
||||
const snapResults = await Promise.all(
|
||||
cameraIds.map(async (camId) => {
|
||||
let snapResp;
|
||||
for (let attempt = 1; attempt <= 2; attempt++) {
|
||||
snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
|
||||
if (snapResp.status !== 503) break;
|
||||
if (attempt < 2) await new Promise(r => setTimeout(r, 2000));
|
||||
}
|
||||
if (!snapResp.ok) return { camId, imgPath: null, error: `HTTP ${snapResp.status}` };
|
||||
const imgPath = path.join(runDir, `${camId}.jpg`);
|
||||
await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer()));
|
||||
return { camId, imgPath, error: null };
|
||||
})
|
||||
);
|
||||
|
||||
// Phase 2: Scripts 1 + 2 pro Kamera (sequenziell, damit Logs lesbar bleiben)
|
||||
for (const { camId, imgPath, error } of snapResults) {
|
||||
send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` });
|
||||
|
||||
// Snapshot
|
||||
send({ type: 'log', text: 'Foto aufnehmen …' });
|
||||
let snapResp;
|
||||
for (let attempt = 1; attempt <= 2; attempt++) {
|
||||
snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
|
||||
if (snapResp.status !== 503) break;
|
||||
if (attempt < 2) await new Promise(r => setTimeout(r, 2000));
|
||||
}
|
||||
if (!snapResp.ok) {
|
||||
send({ type: 'log', text: `⚠ HTTP ${snapResp.status} – Kamera übersprungen` });
|
||||
if (error) {
|
||||
send({ type: 'log', text: `⚠ ${error} – Kamera übersprungen` });
|
||||
continue;
|
||||
}
|
||||
const imgPath = path.join(runDir, `${camId}.jpg`);
|
||||
await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer()));
|
||||
send({ type: 'log', text: `✅ Foto: ${camId}.jpg` });
|
||||
|
||||
// NPZ suchen – neueste Session, die eine NPZ für diese Kamera enthält
|
||||
@@ -531,7 +555,7 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
SCRIPT_1,
|
||||
'-i', imgPath,
|
||||
'-npz', npzPath,
|
||||
'-robot', ROBOT_JSON,
|
||||
'-robot', robotCachePath,
|
||||
'-cameraId', camId,
|
||||
'-outDir', runDir,
|
||||
'--saveDebugImage',
|
||||
@@ -549,7 +573,7 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
continue;
|
||||
}
|
||||
send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' });
|
||||
const script2Args = [SCRIPT_2, '-i', detJson, '-robot', ROBOT_JSON, '-outDir', runDir];
|
||||
const script2Args = [SCRIPT_2, '-i', detJson, '-robot', robotCachePath, '-outDir', runDir];
|
||||
if (refSet) script2Args.push('--refSet', refSet);
|
||||
const exit2 = await runScript(script2Args, send);
|
||||
if (exit2 !== 0) send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` });
|
||||
@@ -567,7 +591,7 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
const exit3b = await runScript([
|
||||
SCRIPT_3B,
|
||||
'--evalDir', runDir,
|
||||
'--robot', ROBOT_JSON,
|
||||
'--robot', robotCachePath,
|
||||
], send);
|
||||
if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` });
|
||||
} else {
|
||||
@@ -576,6 +600,120 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
send({ type: 'log', text: '' });
|
||||
}
|
||||
|
||||
/**
|
||||
* Board-Pipeline für Offline-Homing: Bilder und NPZs liegen bereits im runDir.
|
||||
* Kein Webcam-Zugriff, keine NPZ-Suche — Scripts 1, 2, 3b werden identisch aufgerufen.
|
||||
*
|
||||
* Dateinamen-Konvention im runDir:
|
||||
* {cameraId}.jpg – Kamerabild
|
||||
* {cameraId}_calibration.npz – Kalibrierung
|
||||
* robot_run.json – robot.json für diesen Lauf
|
||||
*
|
||||
* @param {string} runDir
|
||||
* @param {Function} send
|
||||
* @param {{ refSet?: string }} [opts]
|
||||
*/
|
||||
async function runBoardPipelineOffline(runDir, send, { refSet } = {}) {
|
||||
const robotRunPath = path.join(runDir, 'robot_run.json');
|
||||
|
||||
const allFiles = await fsPromises.readdir(runDir);
|
||||
const cameraIds = allFiles
|
||||
.filter(f => /^[a-zA-Z0-9]+\.jpg$/.test(f))
|
||||
.map(f => path.basename(f, '.jpg'))
|
||||
.sort();
|
||||
|
||||
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
for (const camId of cameraIds) {
|
||||
send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` });
|
||||
|
||||
const imgPath = path.join(runDir, `${camId}.jpg`);
|
||||
const npzPath = path.join(runDir, `${camId}_calibration.npz`);
|
||||
|
||||
try { await fsPromises.access(npzPath); } catch {
|
||||
send({ type: 'log', text: `⚠ Keine NPZ für ${camId} – übersprungen` });
|
||||
continue;
|
||||
}
|
||||
|
||||
send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' });
|
||||
const exit1 = await runScript([
|
||||
SCRIPT_1,
|
||||
'-i', imgPath,
|
||||
'-npz', npzPath,
|
||||
'-robot', robotRunPath,
|
||||
'-cameraId', camId,
|
||||
'-outDir', runDir,
|
||||
'--saveDebugImage',
|
||||
], send);
|
||||
if (exit1 !== 0) {
|
||||
send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` });
|
||||
continue;
|
||||
}
|
||||
|
||||
const detJson = path.join(runDir, `${camId}_aruco_detection.json`);
|
||||
try { await fsPromises.access(detJson); } catch {
|
||||
send({ type: 'log', text: '⚠ Detection-JSON fehlt – Script 2 übersprungen' });
|
||||
continue;
|
||||
}
|
||||
send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' });
|
||||
const script2Args = [SCRIPT_2, '-i', detJson, '-robot', robotRunPath, '-outDir', runDir];
|
||||
if (refSet) script2Args.push('--refSet', refSet);
|
||||
const exit2 = await runScript(script2Args, send);
|
||||
if (exit2 !== 0) send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` });
|
||||
|
||||
send({ type: 'log', text: '' });
|
||||
}
|
||||
|
||||
send({ type: 'log', text: '─── 3b: Marker-Triangulierung ────────────────────────────' });
|
||||
const runFiles3b = await fsPromises.readdir(runDir);
|
||||
const numPoses = runFiles3b.filter(f => f.endsWith('_camera_pose.json')).length;
|
||||
if (numPoses >= 2) {
|
||||
send({ type: 'log', text: `▷ 3b_corner_marker_poses (${numPoses} Kamera-Posen)` });
|
||||
const exit3b = await runScript([SCRIPT_3B, '--evalDir', runDir, '--robot', robotRunPath], send);
|
||||
if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` });
|
||||
} else {
|
||||
send({ type: 'log', text: `⚠ Nur ${numPoses} Kamera-Pose(n) – Script 3b braucht ≥2 Kameras` });
|
||||
}
|
||||
send({ type: 'log', text: '' });
|
||||
}
|
||||
|
||||
// ── Multer-Setup für Offline-Homing ──────────────────────────────────────────
|
||||
|
||||
async function prepareOfflineRunDir(req, res, next) {
|
||||
try {
|
||||
const ts = makeTimestamp();
|
||||
const runDir = path.join(homingOfflineDataDir, ts);
|
||||
await fsPromises.mkdir(runDir, { recursive: true });
|
||||
req.offlineRunDir = runDir;
|
||||
req.offlineTs = ts;
|
||||
next();
|
||||
} catch (err) {
|
||||
res.status(500).json({ error: String(err) });
|
||||
}
|
||||
}
|
||||
|
||||
const offlineMulter = multer({
|
||||
storage: multer.diskStorage({
|
||||
destination: (req, file, cb) => cb(null, req.offlineRunDir),
|
||||
filename: (req, file, cb) => {
|
||||
const safe = path.basename(file.originalname).replace(/[^a-zA-Z0-9_.-]/g, '_');
|
||||
cb(null, safe);
|
||||
},
|
||||
}),
|
||||
}).fields([
|
||||
{ name: 'images', maxCount: 10 },
|
||||
{ name: 'calibrations', maxCount: 10 },
|
||||
{ name: 'robot', maxCount: 1 },
|
||||
]);
|
||||
|
||||
function runOfflineUpload(req, res, next) {
|
||||
offlineMulter(req, res, (err) => {
|
||||
if (err) return res.status(400).json({ error: `Upload-Fehler: ${err.message}` });
|
||||
next();
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* POST /api/board/run
|
||||
* 1. Erstellt data/board/{timestamp}/
|
||||
@@ -605,13 +743,13 @@ app.post('/api/board/run', async (req, res) => {
|
||||
|
||||
// Robot-JSON laden und Marker-Anzahl loggen
|
||||
let robotData = null;
|
||||
try { robotData = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8')); } catch {}
|
||||
try { robotData = JSON.parse(await fsPromises.readFile(robotCachePath, 'utf8')); } catch {}
|
||||
const boardMarkers = robotData?.links?.Board?.markers ?? [];
|
||||
const boardMarkerCount = boardMarkers.length;
|
||||
const refMarkerCount = refSet
|
||||
? boardMarkers.filter(m => m.set === refSet).length
|
||||
: boardMarkerCount;
|
||||
send({ type: 'log', text: `▶ Robot-JSON: ${ROBOT_JSON}` });
|
||||
send({ type: 'log', text: `▶ Robot-JSON: ${robotCachePath}` });
|
||||
send({ type: 'log', text: `▶ Board-Marker: ${boardMarkerCount} (links.Board.markers)` });
|
||||
send({ type: 'log', text: `▶ Referenz-Set: ${refSet ? `"${refSet}" (${refMarkerCount} Marker)` : 'alle'}` });
|
||||
send({ type: 'log', text: '' });
|
||||
@@ -696,7 +834,7 @@ app.get('/api/board/latest', async (req, res) => {
|
||||
const runDir = path.join(dataDir, runName);
|
||||
|
||||
let robot = null;
|
||||
try { robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8')); } catch {}
|
||||
try { robot = JSON.parse(await fsPromises.readFile(robotCachePath, 'utf8')); } catch {}
|
||||
|
||||
let files = [];
|
||||
try { files = await fsPromises.readdir(runDir); } catch {}
|
||||
@@ -739,7 +877,7 @@ app.get('/api/board/latest', async (req, res) => {
|
||||
measuredMarkers = JSON.parse(raw);
|
||||
} catch {}
|
||||
|
||||
return res.json({ runDir: runName, robotFile: path.basename(ROBOT_JSON), robot, detections, cameraPoses, measuredMarkers });
|
||||
return res.json({ runDir: runName, robotFile: path.basename(robotCachePath), robot, detections, cameraPoses, measuredMarkers });
|
||||
} catch (err) {
|
||||
return res.status(500).json({ error: String(err) });
|
||||
}
|
||||
@@ -764,12 +902,13 @@ app.post('/api/homing/run', async (req, res) => {
|
||||
try {
|
||||
await fsPromises.mkdir(homingDataDir, { recursive: true });
|
||||
await runHoming({
|
||||
robotJsonPath: ROBOT_JSON,
|
||||
robotJsonPath: robotCachePath,
|
||||
homingDir: homingDataDir,
|
||||
send,
|
||||
runScript,
|
||||
runBoardPipeline,
|
||||
SCRIPT_4B,
|
||||
SCRIPT_5POSE,
|
||||
});
|
||||
} catch (err) {
|
||||
console.error('homing/run error:', err);
|
||||
@@ -781,31 +920,70 @@ app.post('/api/homing/run', async (req, res) => {
|
||||
if (!res.writableEnded) res.end();
|
||||
});
|
||||
|
||||
/**
|
||||
* Konvertiert den FK-State (von 4b_revolute_angle.py / 5_pose_estimation.py)
|
||||
* in die G92-Driver-Konvention (appRobotDriver/doc/Info_G92.md).
|
||||
*
|
||||
* Unterschiede:
|
||||
* b: FK b=0 = gerade Hand; Driver B=180° = gerade Hand → B = 180 − b
|
||||
* c: FK c=0 = neutral Roll; Driver C=90° = neutral → C = c + 90
|
||||
* z: 4b misst Ellbogen RELATIV zu Arm1; Driver braucht absoluten Winkel → Z = y + z
|
||||
*/
|
||||
function fkStateToDriverG92(s) {
|
||||
const d = { ...s };
|
||||
if (d.b != null) d.b = 180 - d.b;
|
||||
if (d.c != null) d.c = d.c + 90;
|
||||
if (d.z != null && d.y != null) d.z = d.y + d.z;
|
||||
return d;
|
||||
}
|
||||
|
||||
/**
|
||||
* POST /api/homing/send-state
|
||||
* Sendet { state: { x, y, z, a, b, c, e } } an ROBOT_URL/api/state.
|
||||
* Baut aus { state: { x, y, z, a, b[, c, e] } } ein G92 und sendet es als
|
||||
* Plain-Text-G-Code über den Driver-WebSocket (DRIVER_WS_URL). G92 setzt am
|
||||
* Driver die Motorposition ohne Bewegung (intern M92) = Homing.
|
||||
* Bekannte Achsen werden real gesendet; wirklich fehlende/unbeobachtbare
|
||||
* Achsen (z.B. c/Palm, e/Greifer in der 4b-Kette) werden weggelassen — der
|
||||
* Driver lässt sie unverändert (siehe server/buildG92.cjs).
|
||||
*/
|
||||
app.post('/api/homing/send-state', async (req, res) => {
|
||||
try {
|
||||
const { state } = req.body ?? {};
|
||||
if (!state) return res.status(400).json({ error: '"state" fehlt' });
|
||||
if (!ROBOT_URL) return res.status(501).json({ error: 'ROBOT_URL ist nicht konfiguriert' });
|
||||
if (!isDriverConfigured())
|
||||
return res.status(501).json({ error: 'DRIVER_WS_URL ist nicht konfiguriert' });
|
||||
|
||||
const url = new URL('/api/state', ROBOT_URL).toString();
|
||||
const upstream = await fetch(url, {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(state),
|
||||
});
|
||||
if (!upstream.ok) {
|
||||
const text = await upstream.text();
|
||||
return res.status(upstream.status).json({ error: `Robot-Fehler: ${text}` });
|
||||
}
|
||||
const result = await upstream.json().catch(() => ({}));
|
||||
return res.json({ ok: true, result });
|
||||
const gcode = buildG92(fkStateToDriverG92(state));
|
||||
const result = await sendGcode(gcode);
|
||||
if (!result.ok)
|
||||
return res.status(502).json({ error: `Robot-Fehler: ${result.error}`, gcode });
|
||||
return res.json({ ok: true, gcode, result: result.response, note: result.note });
|
||||
} catch (err) {
|
||||
console.error('homing/send-state error:', err);
|
||||
return res.status(500).json({ error: String(err) });
|
||||
return res.status(err.statusCode || 500).json({ error: String(err.message || err) });
|
||||
}
|
||||
});
|
||||
|
||||
/**
|
||||
* POST /api/robot/gcode { line: "G92 X… Y…" }
|
||||
* Sendet eine beliebige G-Code-Zeile über den Driver-WebSocket. Transport für
|
||||
* die G-Code-/Befehl-Buttons im Frontend (window.sendCommand) — ersetzt den
|
||||
* toten WSS-Altpfad.
|
||||
*/
|
||||
app.post('/api/robot/gcode', async (req, res) => {
|
||||
try {
|
||||
const line = (req.body?.line ?? '').toString().trim();
|
||||
if (!line) return res.status(400).json({ error: '"line" fehlt' });
|
||||
if (!isDriverConfigured())
|
||||
return res.status(501).json({ error: 'DRIVER_WS_URL ist nicht konfiguriert' });
|
||||
|
||||
const result = await sendGcode(line);
|
||||
if (!result.ok)
|
||||
return res.status(502).json({ error: `Robot-Fehler: ${result.error}`, line });
|
||||
return res.json({ ok: true, line, result: result.response, note: result.note });
|
||||
} catch (err) {
|
||||
console.error('robot/gcode error:', err);
|
||||
return res.status(err.statusCode || 500).json({ error: String(err.message || err) });
|
||||
}
|
||||
});
|
||||
|
||||
@@ -853,6 +1031,92 @@ app.get('/api/homing/run-data', async (req, res) => {
|
||||
}
|
||||
});
|
||||
|
||||
/**
|
||||
* POST /api/homing/run-offline
|
||||
* Vollständiger Homing-Ablauf ohne Live-Kameras.
|
||||
* Bilder, NPZs und robot.json werden per multipart/form-data hochgeladen.
|
||||
* Antwortet synchron mit { ok, runDir, state, files, log }.
|
||||
*
|
||||
* Felder:
|
||||
* images – ein oder mehrere JPEG-Dateien, Name muss {cameraId}.jpg sein
|
||||
* calibrations – je eine NPZ pro Kamera, Name muss {cameraId}_calibration.npz sein
|
||||
* robot – robot.json für diesen Lauf (einmalig, wird nicht dauerhaft gespeichert)
|
||||
* refSet – (Text, optional) Referenz-Set für Script 2, z. B. "A0"
|
||||
*/
|
||||
app.post('/api/homing/run-offline',
|
||||
prepareOfflineRunDir,
|
||||
runOfflineUpload,
|
||||
async (req, res) => {
|
||||
const runDir = req.offlineRunDir;
|
||||
const ts = req.offlineTs;
|
||||
const log = [];
|
||||
|
||||
// robot.json validieren und als robot_run.json speichern
|
||||
const robotFile = req.files?.robot?.[0];
|
||||
if (!robotFile) {
|
||||
return res.status(400).json({ error: '"robot" fehlt – robot.json muss hochgeladen werden', log });
|
||||
}
|
||||
let robotRunPath;
|
||||
try {
|
||||
const content = await fsPromises.readFile(robotFile.path, 'utf8');
|
||||
JSON.parse(content); // Syntaxprüfung
|
||||
robotRunPath = path.join(runDir, 'robot_run.json');
|
||||
await fsPromises.rename(robotFile.path, robotRunPath);
|
||||
} catch (err) {
|
||||
return res.status(400).json({ error: `robot.json ungültig: ${err.message}`, log });
|
||||
}
|
||||
|
||||
// Mindestens ein Bild erforderlich
|
||||
if (!req.files?.images?.length) {
|
||||
return res.status(400).json({ error: 'Mindestens ein Bild ("images") fehlt', log });
|
||||
}
|
||||
|
||||
const refSet = req.body?.refSet || undefined;
|
||||
|
||||
// Logs und done-Event akkumulieren
|
||||
let finalState = null;
|
||||
let exitCode = -1;
|
||||
const send = (obj) => {
|
||||
if (obj.type === 'log') log.push(obj.text);
|
||||
if (obj.type === 'done') { finalState = obj.state ?? null; exitCode = obj.exitCode; }
|
||||
};
|
||||
|
||||
try {
|
||||
await runHomingOffline({
|
||||
robotJsonPath: robotRunPath,
|
||||
runDir,
|
||||
send,
|
||||
runScript,
|
||||
runBoardPipelineOffline: (dir, s) => runBoardPipelineOffline(dir, s, { refSet }),
|
||||
SCRIPT_4B,
|
||||
SCRIPT_5POSE,
|
||||
});
|
||||
} catch (err) {
|
||||
console.error('homing/run-offline error:', err);
|
||||
return res.status(500).json({ error: String(err), log });
|
||||
}
|
||||
|
||||
// Zu wenige Kameras → aruco_marker_poses.json fehlt
|
||||
if (exitCode !== 0) {
|
||||
const arucoExists = await fsPromises.access(path.join(runDir, 'aruco_marker_poses.json'))
|
||||
.then(() => true).catch(() => false);
|
||||
const status = arucoExists ? 500 : 422;
|
||||
return res.status(status).json({ error: 'Homing fehlgeschlagen', log });
|
||||
}
|
||||
|
||||
// Alle JSON-Ausgabedateien einlesen (robot_run.json ausgenommen)
|
||||
const allFiles = await fsPromises.readdir(runDir).catch(() => []);
|
||||
const files = {};
|
||||
for (const f of allFiles.sort()) {
|
||||
if (f.endsWith('.json') && f !== 'robot_run.json') {
|
||||
try { files[f] = JSON.parse(await fsPromises.readFile(path.join(runDir, f), 'utf8')); } catch {}
|
||||
}
|
||||
}
|
||||
|
||||
return res.json({ ok: true, runDir: ts, state: finalState, files, log });
|
||||
}
|
||||
);
|
||||
|
||||
// ── Robot-JSON bearbeiten ─────────────────────────────────────────────────────
|
||||
|
||||
/**
|
||||
@@ -882,7 +1146,7 @@ app.post('/api/robot/assign-by-z', async (req, res) => {
|
||||
}
|
||||
} catch { /* kein 3b-Output vorhanden – nur bestehende robot.json-Marker bearbeiten */ }
|
||||
|
||||
const result = await assignByZRange(ROBOT_JSON, { zMin, zMax, set, link, extraMarkers });
|
||||
const result = await assignByZRange(robotCachePath, { zMin, zMax, set, link, extraMarkers });
|
||||
const added = result.changes.filter(c => c.action === 'added').length;
|
||||
const updated = result.changes.filter(c => c.action === 'updated').length;
|
||||
console.log(`robot/assign-by-z z=[${zMin}..${zMax}] set="${set}" link="${link}" → ${updated} aktualisiert, ${added} neu (von ${extraMarkers.length} 3b-Markern)`);
|
||||
@@ -907,7 +1171,7 @@ app.post('/api/robot/remove-marker', async (req, res) => {
|
||||
if (!['set', 'link'].includes(removeFrom)) {
|
||||
return res.status(400).json({ error: 'removeFrom muss "set" oder "link" sein' });
|
||||
}
|
||||
const result = await removeMarkerAssignment(ROBOT_JSON, { markerId, removeFrom });
|
||||
const result = await removeMarkerAssignment(robotCachePath, { markerId, removeFrom });
|
||||
console.log(`robot/remove-marker id=${markerId} from=${removeFrom} → changed=${result.changed}`);
|
||||
return res.json(result);
|
||||
} catch (err) {
|
||||
@@ -922,7 +1186,7 @@ app.post('/api/robot/remove-marker', async (req, res) => {
|
||||
*/
|
||||
app.get('/api/robot', async (req, res) => {
|
||||
try {
|
||||
const robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
|
||||
const robot = await fetchRobot();
|
||||
return res.json(robot);
|
||||
} catch (err) {
|
||||
return res.status(500).json({ error: String(err) });
|
||||
@@ -936,7 +1200,7 @@ app.get('/api/robot', async (req, res) => {
|
||||
*/
|
||||
app.get('/api/robot/board-sets', async (req, res) => {
|
||||
try {
|
||||
const robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
|
||||
const robot = await fetchRobot();
|
||||
const markers = robot?.links?.Board?.markers ?? [];
|
||||
const sets = [...new Set(markers.map(m => m.set).filter(Boolean))].sort();
|
||||
return res.json({ sets });
|
||||
@@ -967,7 +1231,7 @@ app.post('/api/robot/align-sets', async (req, res) => {
|
||||
}
|
||||
} catch { /* kein 3b-Output vorhanden */ }
|
||||
|
||||
const result = await alignSetToMeasured(ROBOT_JSON, { setToMove, extraMarkers });
|
||||
const result = await alignSetToMeasured(robotCachePath, { setToMove, extraMarkers });
|
||||
if (result.error) return res.status(400).json(result);
|
||||
|
||||
console.log(
|
||||
@@ -1002,7 +1266,7 @@ app.post('/api/robot/assign-id', async (req, res) => {
|
||||
}
|
||||
} catch { /* kein 3b-Output vorhanden */ }
|
||||
|
||||
const result = await assignMarkerId(ROBOT_JSON, { markerId, set, link, extraMarkers });
|
||||
const result = await assignMarkerId(robotCachePath, { markerId, set, link, extraMarkers });
|
||||
if (!result.changed && result.error) return res.status(400).json(result);
|
||||
|
||||
console.log(
|
||||
@@ -1028,7 +1292,7 @@ app.post('/api/robot/adopt-x-axis', async (req, res) => {
|
||||
if (!Array.isArray(direction) || direction.length < 3) {
|
||||
return res.status(400).json({ error: '"direction" muss ein Array [vx,vy,vz] sein.' });
|
||||
}
|
||||
const result = await adoptXAxis(ROBOT_JSON, { direction });
|
||||
const result = await adoptXAxis(robotCachePath, { direction });
|
||||
console.log(
|
||||
`robot/adopt-x-axis dir=[${direction.map(v => Number(v).toFixed(4)).join(', ')}]` +
|
||||
` → ${result.numChanged} Marker, Ursprung=[${result.origin.join(', ')}]` +
|
||||
@@ -1056,7 +1320,7 @@ app.post('/api/robot/assign-fixed-markers', async (req, res) => {
|
||||
if (!targetLink) {
|
||||
return res.status(400).json({ error: '"targetLink" muss angegeben werden.' });
|
||||
}
|
||||
const result = await assignFixedMarkersToLink(ROBOT_JSON, { markerIds, targetLink, measuredPositions });
|
||||
const result = await assignFixedMarkersToLink(robotCachePath, { markerIds, targetLink, measuredPositions });
|
||||
console.log(
|
||||
`robot/assign-fixed-markers [${markerIds.join(',')}] → ${targetLink}` +
|
||||
` added=${result.numAdded} alreadyPresent=${result.numAlreadyPresent}`,
|
||||
@@ -1081,7 +1345,7 @@ app.post('/api/robot/set-joint-origin', async (req, res) => {
|
||||
if (!Number.isFinite(Number(y)) || !Number.isFinite(Number(z))) {
|
||||
return res.status(400).json({ error: '"y" und "z" müssen Zahlen sein.' });
|
||||
}
|
||||
const result = await setJointOriginYZ(ROBOT_JSON, { linkName, y: Number(y), z: Number(z) });
|
||||
const result = await setJointOriginYZ(robotCachePath, { linkName, y: Number(y), z: Number(z) });
|
||||
if (!result.changed) {
|
||||
return res.status(400).json({ error: result.error });
|
||||
}
|
||||
@@ -1107,7 +1371,7 @@ app.post('/api/robot/set-arm-marker-spin', async (req, res) => {
|
||||
if (!linkName) return res.status(400).json({ error: '"linkName" muss angegeben werden.' });
|
||||
if (markerId == null) return res.status(400).json({ error: '"markerId" muss angegeben werden.' });
|
||||
if (!Number.isFinite(Number(spin))) return res.status(400).json({ error: '"spin" muss eine Zahl sein.' });
|
||||
const result = await setArmMarkerSpin(ROBOT_JSON, { linkName, markerId, spin: Number(spin) });
|
||||
const result = await setArmMarkerSpin(robotCachePath, { linkName, markerId, spin: Number(spin) });
|
||||
if (!result.changed) return res.status(400).json({ error: result.error });
|
||||
console.log(`robot/set-arm-marker-spin ${linkName}#${markerId}: ${result.oldSpin}° → ${result.newSpin}°`);
|
||||
return res.json(result);
|
||||
@@ -1260,6 +1524,13 @@ async function startServer() {
|
||||
await checkServiceReachability('BODYTRACKER_URL', new URL('/v1/health', BODYTRACKER_URL).toString());
|
||||
}
|
||||
|
||||
try {
|
||||
await fetchRobot();
|
||||
console.log('✅ robot.json geladen und gecacht.');
|
||||
} catch (err) {
|
||||
console.warn(`⚠ robot.json: Driver nicht erreichbar – nutze lokale Datei: ${err.message}`);
|
||||
}
|
||||
|
||||
const server = await createHttpsServer();
|
||||
const isHttps = Boolean(server);
|
||||
const listenServer = server || app;
|
||||
|
||||
215
test/assignMarkerId.test.js
Normal file
215
test/assignMarkerId.test.js
Normal file
@@ -0,0 +1,215 @@
|
||||
/**
|
||||
* assignMarkerId.test.js
|
||||
* ======================
|
||||
* Integration-Test für server/editRobot.js → assignMarkerId().
|
||||
*
|
||||
* Testet insbesondere den Guard für fehlende position_mm (Marker ohne
|
||||
* triangulierte Position, z.B. Einzelkamera-Marker nach Schritt 5).
|
||||
*
|
||||
* Technisch: editRobot.js ist ein ES-Modul — es wird über den dünnen Runner
|
||||
* test/fixtures/runAssignMarkerId.mjs per spawnSync aufgerufen (gleiche
|
||||
* Strategie wie yAxisRotation.test.js für das Python-Skript).
|
||||
* Datei-I/O läuft gegen echte Temp-Dateien (os.tmpdir()).
|
||||
*/
|
||||
|
||||
const { spawnSync } = require('child_process');
|
||||
const os = require('os');
|
||||
const fs = require('fs');
|
||||
const path = require('path');
|
||||
|
||||
const RUNNER = path.join(__dirname, 'fixtures', 'runAssignMarkerId.mjs');
|
||||
|
||||
// ── Hilfsfunktionen ───────────────────────────────────────────────────────────
|
||||
|
||||
function callAssignMarkerId(robotPath, params) {
|
||||
const proc = spawnSync('node', [RUNNER, robotPath, JSON.stringify(params)], {
|
||||
encoding: 'utf-8',
|
||||
});
|
||||
if (proc.error) throw proc.error;
|
||||
const stdout = (proc.stdout ?? '').trim();
|
||||
if (!stdout) throw new Error(`Kein Output.\nstderr: ${proc.stderr}`);
|
||||
return JSON.parse(stdout);
|
||||
}
|
||||
|
||||
function makeTempRobot(content) {
|
||||
const p = path.join(
|
||||
os.tmpdir(),
|
||||
`robot_test_${Date.now()}_${Math.random().toString(36).slice(2)}.json`
|
||||
);
|
||||
fs.writeFileSync(p, JSON.stringify(content, null, 2), 'utf8');
|
||||
return p;
|
||||
}
|
||||
|
||||
const ROBOT_WITH_42 = () => ({
|
||||
links: {
|
||||
Arm1: { markers: [{ id: 42, set: 'A0', position: [100, 200, 300] }] },
|
||||
},
|
||||
});
|
||||
|
||||
const ROBOT_EMPTY = () => ({
|
||||
links: { Arm1: { markers: [] } },
|
||||
});
|
||||
|
||||
// ── Eingabe-Validierung ───────────────────────────────────────────────────────
|
||||
|
||||
describe('assignMarkerId – Eingabe-Validierung', () => {
|
||||
test('ungültige Marker-ID → changed: false', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, { markerId: -1, link: 'Arm1' });
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/Ungültige Marker-ID/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('kein link für neuen Marker → changed: false', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 99,
|
||||
extraMarkers: [{ marker_id: 99, position_mm: [10, 20, 30] }],
|
||||
});
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/Link/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
});
|
||||
|
||||
// ── Guard: fehlende position_mm ───────────────────────────────────────────────
|
||||
|
||||
describe('assignMarkerId – Guard: fehlende position_mm (z.B. Einzelkamera-Marker)', () => {
|
||||
test('extraMarker ohne position_mm → changed: false, kein Crash', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 55,
|
||||
link: 'Arm1',
|
||||
extraMarkers: [{ marker_id: 55 }], // kein position_mm
|
||||
});
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/position_mm fehlt/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('extraMarker mit position_mm: null → changed: false, kein Crash', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 56,
|
||||
link: 'Arm1',
|
||||
extraMarkers: [{ marker_id: 56, position_mm: null }],
|
||||
});
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/position_mm fehlt/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('extraMarker mit position_mm als String → changed: false, kein Crash', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 57,
|
||||
link: 'Arm1',
|
||||
extraMarkers: [{ marker_id: 57, position_mm: '[1,2,3]' }],
|
||||
});
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/position_mm fehlt/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
});
|
||||
|
||||
// ── Normalfall: Marker hinzufügen ─────────────────────────────────────────────
|
||||
|
||||
describe('assignMarkerId – Normalfall: neuen Marker hinzufügen', () => {
|
||||
test('gültige position_mm → changed: true, action: added, Datei geschrieben', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 77,
|
||||
link: 'Arm1',
|
||||
extraMarkers: [{ marker_id: 77, position_mm: [10.1, 20.22, 30.333] }],
|
||||
});
|
||||
expect(r.changed).toBe(true);
|
||||
expect(r.change.action).toBe('added');
|
||||
expect(r.change.markerId).toBe(77);
|
||||
expect(r.change.newLink).toBe('Arm1');
|
||||
|
||||
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
|
||||
const added = saved.links.Arm1.markers.find(m => m.id === 77);
|
||||
expect(added).toBeDefined();
|
||||
expect(Array.isArray(added.position)).toBe(true);
|
||||
expect(added.position).toHaveLength(3);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('Marker nicht in extraMarkers → changed: false', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, { markerId: 99, link: 'Arm1', extraMarkers: [] });
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/nicht.*vorhanden/i);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('mit set → Marker hat set-Wert in der gespeicherten Datei', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 78,
|
||||
link: 'Arm1',
|
||||
set: 'A0',
|
||||
extraMarkers: [{ marker_id: 78, position_mm: [1, 2, 3] }],
|
||||
});
|
||||
expect(r.changed).toBe(true);
|
||||
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
|
||||
const added = saved.links.Arm1.markers.find(m => m.id === 78);
|
||||
expect(added.set).toBe('A0');
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
});
|
||||
|
||||
// ── Normalfall: bestehenden Marker aktualisieren ──────────────────────────────
|
||||
|
||||
describe('assignMarkerId – Normalfall: bestehenden Marker aktualisieren', () => {
|
||||
test('Set ändern → changed: true, action: updated, Datei geändert', () => {
|
||||
const p = makeTempRobot(ROBOT_WITH_42());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, { markerId: 42, set: 'B0' });
|
||||
expect(r.changed).toBe(true);
|
||||
expect(r.change.action).toBe('updated');
|
||||
expect(r.change.oldSet).toBe('A0');
|
||||
expect(r.change.newSet).toBe('B0');
|
||||
|
||||
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
|
||||
expect(saved.links.Arm1.markers.find(m => m.id === 42).set).toBe('B0');
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('in anderen Link verschieben → oldLink / newLink korrekt, Datei geändert', () => {
|
||||
const p = makeTempRobot(ROBOT_WITH_42());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, { markerId: 42, link: 'Arm2' });
|
||||
expect(r.changed).toBe(true);
|
||||
expect(r.change.oldLink).toBe('Arm1');
|
||||
expect(r.change.newLink).toBe('Arm2');
|
||||
|
||||
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
|
||||
expect(saved.links.Arm1.markers.find(m => m.id === 42)).toBeUndefined();
|
||||
expect(saved.links.Arm2.markers.find(m => m.id === 42)).toBeDefined();
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('bestehender Marker: fehlende position_mm in extraMarkers ist irrelevant', () => {
|
||||
const p = makeTempRobot(ROBOT_WITH_42());
|
||||
try {
|
||||
// Marker 42 ist in robot.json → position_mm-Guard darf nicht zuschlagen
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 42,
|
||||
set: 'C0',
|
||||
extraMarkers: [{ marker_id: 42 }], // kein position_mm – aber irrelevant
|
||||
});
|
||||
expect(r.changed).toBe(true);
|
||||
expect(r.change.action).toBe('updated');
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
});
|
||||
59
test/boardViewerHasXYZ.test.js
Normal file
59
test/boardViewerHasXYZ.test.js
Normal file
@@ -0,0 +1,59 @@
|
||||
/**
|
||||
* boardViewerHasXYZ.test.js
|
||||
* =========================
|
||||
* Unit-Test für die reine Hilfsfunktion `hasXYZ()` aus public/boardViewer.html.
|
||||
*
|
||||
* boardViewer.html ist kein ladbares JS-Modul (Inline-<script type="module">
|
||||
* mit THREE.js/DOM/fetch-Abhängigkeiten) — ein normales require() ist daher
|
||||
* nicht möglich, ohne die Datei in Module aufzuteilen (nicht Teil dieser
|
||||
* Änderung). Stattdessen wird die `hasXYZ`-Funktionsdefinition per Regex aus
|
||||
* der Datei extrahiert und isoliert ausgewertet — testet exakt die Guard-Logik,
|
||||
* die an allen position_mm-Zugriffsstellen in boardViewer.html verwendet wird,
|
||||
* ohne Three.js/DOM laden zu müssen.
|
||||
*/
|
||||
|
||||
const fs = require('fs');
|
||||
const path = require('path');
|
||||
|
||||
const SRC = fs.readFileSync(path.join(__dirname, '../public/boardViewer.html'), 'utf8');
|
||||
|
||||
const match = SRC.match(/function hasXYZ\(arr\)\s*\{[^}]*\}/);
|
||||
if (!match) {
|
||||
throw new Error('hasXYZ() nicht in boardViewer.html gefunden — Guard wurde entfernt/umbenannt?');
|
||||
}
|
||||
// eslint-disable-next-line no-eval
|
||||
const hasXYZ = eval(`(${match[0]})`);
|
||||
|
||||
describe('boardViewer.html: hasXYZ() (Guard für fehlende position_mm)', () => {
|
||||
test('gültiges [x,y,z] → true', () => {
|
||||
expect(hasXYZ([1, 2, 3])).toBe(true);
|
||||
expect(hasXYZ([0, 0, 0])).toBe(true);
|
||||
expect(hasXYZ([-1.5, 200.25, 0])).toBe(true);
|
||||
});
|
||||
|
||||
test('undefined/null (fehlendes position_mm) → false, kein Crash', () => {
|
||||
expect(() => hasXYZ(undefined)).not.toThrow();
|
||||
expect(hasXYZ(undefined)).toBe(false);
|
||||
expect(hasXYZ(null)).toBe(false);
|
||||
});
|
||||
|
||||
test('zu kurzes Array → false', () => {
|
||||
expect(hasXYZ([1, 2])).toBe(false);
|
||||
expect(hasXYZ([])).toBe(false);
|
||||
});
|
||||
|
||||
test('nicht-numerische/NaN-Werte → false', () => {
|
||||
expect(hasXYZ([1, NaN, 3])).toBe(false);
|
||||
expect(hasXYZ(['a', 'b', 'c'])).toBe(false);
|
||||
expect(hasXYZ([1, Infinity, 3])).toBe(false);
|
||||
});
|
||||
|
||||
test('kein Array (z.B. Objekt oder String) → false', () => {
|
||||
expect(hasXYZ({ x: 1, y: 2, z: 3 })).toBe(false);
|
||||
expect(hasXYZ('1,2,3')).toBe(false);
|
||||
});
|
||||
|
||||
test('Array mit mehr als 3 Werten (z.B. mit Zusatzfeld) → true (erste 3 zählen)', () => {
|
||||
expect(hasXYZ([1, 2, 3, 4])).toBe(true);
|
||||
});
|
||||
});
|
||||
49
test/buildG92.test.js
Normal file
49
test/buildG92.test.js
Normal file
@@ -0,0 +1,49 @@
|
||||
/**
|
||||
* buildG92.test.js
|
||||
* Unit-Tests für server/buildG92.cjs
|
||||
*
|
||||
* Sichert ab, dass aus dem Homing-State der korrekte G92-String entsteht:
|
||||
* bekannte Achsen werden real gesendet, wirklich fehlende/null-Achsen per
|
||||
* Default WEGGELASSEN (Driver lässt sie unverändert). Achsbuchstaben +
|
||||
* Reihenfolge müssen zur Driver-Erwartung passen
|
||||
* (X→xMotor, Y→alpha, Z→beta, A→a, B→b, C→c, E→e).
|
||||
*/
|
||||
|
||||
const { buildG92 } = require('../server/buildG92.cjs');
|
||||
|
||||
describe('buildG92', () => {
|
||||
test('Fallback-State (alle 7 DOF) → alle Achsen mit realem Wert', () => {
|
||||
const state = { x: 164.57045, y: -2.08983, z: 60.58375, a: 86.75125, b: -46.96569, c: -64.90875, e: 22.58589 };
|
||||
expect(buildG92(state)).toBe('G92 X164.57 Y-2.09 Z60.58 A86.75 B-46.97 C-64.91 E22.59');
|
||||
});
|
||||
|
||||
test('4b-Primärkette (nur x,y,z,a,b) → c/e werden weggelassen', () => {
|
||||
const state = { x: 192.72935, y: 35.99125, z: -30.87771, a: -1.69522, b: 12.34 };
|
||||
expect(buildG92(state)).toBe('G92 X192.73 Y35.99 Z-30.88 A-1.70 B12.34');
|
||||
});
|
||||
|
||||
test('Reihenfolge ist immer x,y,z,a,b,c,e (unabhängig von Key-Reihenfolge)', () => {
|
||||
const state = { b: 1, a: 2, x: 3, e: 4, z: 5, y: 6, c: 7 };
|
||||
expect(buildG92(state)).toBe('G92 X3.00 Y6.00 Z5.00 A2.00 B1.00 C7.00 E4.00');
|
||||
});
|
||||
|
||||
test('null/undefined/NaN-Achsen werden weggelassen (keine falsche 0)', () => {
|
||||
const state = { x: 10, y: null, z: undefined, a: 0, b: NaN, c: 'abc' };
|
||||
expect(buildG92(state)).toBe('G92 X10.00 A0.00');
|
||||
});
|
||||
|
||||
test('fillMissingWithZero=true füllt fehlende Achsen wieder mit 0', () => {
|
||||
const state = { x: 10, y: 20 };
|
||||
expect(buildG92(state, { fillMissingWithZero: true }))
|
||||
.toBe('G92 X10.00 Y20.00 Z0.00 A0.00 B0.00 C0.00 E0.00');
|
||||
});
|
||||
|
||||
test('decimals steuert die Nachkommastellen', () => {
|
||||
expect(buildG92({ x: 1.23456 }, { decimals: 3 })).toBe('G92 X1.235');
|
||||
});
|
||||
|
||||
test('leerer State → "G92 " ohne Achsen', () => {
|
||||
expect(buildG92({})).toBe('G92 ');
|
||||
expect(buildG92()).toBe('G92 ');
|
||||
});
|
||||
});
|
||||
25
test/fixtures/runAssignMarkerId.mjs
vendored
Normal file
25
test/fixtures/runAssignMarkerId.mjs
vendored
Normal file
@@ -0,0 +1,25 @@
|
||||
/**
|
||||
* Dünner Runner für assignMarkerId – wird von assignMarkerId.test.js per spawnSync aufgerufen.
|
||||
*
|
||||
* Argumente:
|
||||
* node runAssignMarkerId.mjs <robotPath> <paramsJson>
|
||||
*
|
||||
* Gibt das Ergebnis als JSON-Zeile auf stdout aus.
|
||||
* Wirft der Aufruf, erscheint { __error: "<message>" } + Exit 1.
|
||||
*/
|
||||
import { assignMarkerId } from '../../server/editRobot.js';
|
||||
|
||||
const [, , robotPath, paramsJson] = process.argv;
|
||||
if (!robotPath || !paramsJson) {
|
||||
process.stderr.write('Usage: runAssignMarkerId.mjs <robotPath> <paramsJson>\n');
|
||||
process.exit(2);
|
||||
}
|
||||
|
||||
try {
|
||||
const params = JSON.parse(paramsJson);
|
||||
const result = await assignMarkerId(robotPath, params);
|
||||
process.stdout.write(JSON.stringify(result) + '\n');
|
||||
} catch (err) {
|
||||
process.stdout.write(JSON.stringify({ __error: err.message }) + '\n');
|
||||
process.exit(1);
|
||||
}
|
||||
46
test/homing/20260616_120456/aruco_marker_poses.csv
Normal file
46
test/homing/20260616_120456/aruco_marker_poses.csv
Normal file
@@ -0,0 +1,46 @@
|
||||
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
|
||||
47,Board,A0,3,343.56,-286.5,-27.02,0.01282,0.04293,0.999,344.23,-286.54,-27.3,0.727,0.278,23.86
|
||||
51,Board,A0,2,168.06,-172.05,-26.96,-0.06575,-0.00903,0.99779,167.8,-172.08,-27.3,0.432,0.344,23.62
|
||||
54,Board,A0,3,341.85,-330.17,-26.58,0.02766,0.00621,0.9996,342.27,-330.59,-27.3,0.928,0.716,23.56
|
||||
55,Board,A0,3,282.99,-262.34,-26.34,0.0153,0.00966,0.99984,283.72,-262.58,-27.3,1.231,0.961,23.68
|
||||
57,Board,A0,2,602.73,-364.49,-27.92,0.02948,0.01103,0.9995,602.86,-364.05,-27.3,0.774,-0.622,23.95
|
||||
62,Board,A0,3,403.64,-174.91,-26.43,0.02093,-0.01927,0.9996,404.7,-175.1,-27.3,1.381,0.868,23.73
|
||||
65,Board,A0,2,805.42,-298.68,-28.85,0.07317,-0.00718,0.99729,803.39,-297.37,-27.3,2.872,-1.549,24.67
|
||||
66,Board,A0,2,209.91,-362.69,-27.32,0.01431,0.0008,0.9999,209.75,-363.23,-27.3,0.56,-0.021,24.07
|
||||
70,Board,A0,2,598.23,299.66,-24.5,-0.01925,0.12918,0.99143,601.87,300.33,-27.3,4.638,2.798,24.16
|
||||
71,Board,A0,2,750.95,-284.52,-27.87,0.02144,-0.00558,0.99975,749.75,-284.01,-27.3,1.423,-0.571,24.07
|
||||
79,Board,A0,2,312.62,-159.33,-27.38,-0.03005,-0.02443,0.99925,312.3,-159.11,-27.3,0.397,-0.077,23.93
|
||||
80,Board,A0,2,866.21,-337.3,-29.23,0.03205,-0.01144,0.99942,863.59,-335.92,-27.3,3.54,-1.934,23.73
|
||||
82,Board,A0,2,217.92,296.91,-26.35,-0.02521,0.05118,0.99837,219.16,297.24,-27.3,1.599,0.954,23.75
|
||||
84,Board,A0,3,405.0,257.84,-25.4,0.01076,0.0148,0.99983,407.49,258.42,-27.3,3.183,1.897,23.57
|
||||
85,Board,A0,3,504.05,-312.91,-27.97,-0.00509,-0.01089,0.99993,504.58,-312.75,-27.3,0.869,-0.668,23.76
|
||||
86,Board,A0,2,362.68,291.99,-27.28,-0.01296,0.03562,0.99928,362.89,292.01,-27.3,0.209,0.022,23.91
|
||||
87,Board,A0,2,946.89,-246.84,-29.25,0.03555,0.02881,0.99895,943.63,-245.76,-27.3,3.952,-1.95,24.15
|
||||
90,Board,A0,2,639.54,315.8,-24.64,-0.00908,0.0411,0.99911,643.17,316.43,-27.3,4.55,2.665,23.68
|
||||
91,Board,A0,2,719.12,327.68,-24.61,-0.07043,0.08617,0.99379,723.35,328.05,-27.3,5.028,2.687,23.57
|
||||
93,Board,A0,2,932.27,143.24,-24.03,-0.05724,0.02723,0.99799,934.88,143.6,-27.3,4.202,3.272,22.76
|
||||
95,Board,A0,2,186.15,-273.72,-26.9,0.01775,-0.00563,0.99983,186.04,-274.07,-27.3,0.548,0.404,23.8
|
||||
96,Board,A0,3,368.95,-186.48,-26.8,-0.02679,0.03068,0.99917,369.77,-186.49,-27.3,0.961,0.499,23.46
|
||||
97,Board,A0,2,304.05,-359.12,-27.02,0.00663,-0.01188,0.99991,304.35,-359.67,-27.3,0.689,0.284,23.61
|
||||
99,Board,A0,2,963.23,-323.27,-29.97,0.05209,-0.03007,0.99819,959.16,-321.55,-27.3,5.165,-2.671,23.74
|
||||
104,Board,A0,2,822.76,238.49,-24.06,-0.01937,0.04301,0.99889,826.71,239.16,-27.3,5.157,3.245,23.49
|
||||
105,Board,A0,3,524.14,-266.3,-27.48,0.04369,0.05496,0.99753,524.84,-266.25,-27.3,0.728,-0.183,24.08
|
||||
129,Ellbow,,2,392.44,-57.59,273.3,-0.02284,-0.02831,0.99934,,,,,,23.22
|
||||
132,Ellbow,,2,355.65,-57.41,273.66,0.00682,-0.01043,0.99992,,,,,,23.57
|
||||
143,Arm2,,3,368.17,-239.23,261.07,-0.6936,-0.02269,0.72001,,,,,,24.16
|
||||
144,Arm2,,3,356.52,-168.54,237.95,-0.99956,0.02529,0.01551,,,,,,24.28
|
||||
146,Arm2,,2,363.47,-167.73,265.11,-0.69227,-0.01571,0.72147,,,,,,23.95
|
||||
147,unknown,,2,361.52,-238.42,219.07,-0.87822,0.02426,-0.47764,,,,,,25.11
|
||||
148,Arm2,,3,356.84,-275.1,235.17,-0.99971,0.02422,-0.00165,,,,,,24.49
|
||||
197,Arm1,,2,253.16,-55.28,243.87,-0.99795,-0.06394,0.00227,,,,,,24.78
|
||||
200,unknown,,2,231.14,-30.63,109.67,-0.03466,0.04681,0.9983,,,,,,22.96
|
||||
201,unknown,,3,195.05,46.33,96.05,-0.99847,-0.02285,0.05033,,,,,,24.23
|
||||
204,unknown,,3,230.13,118.96,116.92,-0.00845,0.01273,0.99988,,,,,,23.55
|
||||
205,unknown,,2,935.95,-97.07,-6.58,-0.01249,0.09996,0.99491,,,,,,23.18
|
||||
215,unknown,,2,334.82,-96.93,-7.71,-0.02232,0.05948,0.99798,,,,,,23.34
|
||||
243,Arm1,,2,303.21,-79.66,265.92,0.02387,-0.65211,0.75775,,,,,,23.8
|
||||
|
||||
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
|
||||
cam0,-352.63,-78.05,407.29,0.94846,0.07547,-0.30777
|
||||
cam1,-232.55,-598.43,854.78,0.51862,0.43559,-0.73573
|
||||
cam2,-237.23,81.29,837.43,0.70662,-0.07011,-0.70411
|
||||
|
1827
test/homing/20260616_120456/aruco_marker_poses.json
Normal file
1827
test/homing/20260616_120456/aruco_marker_poses.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
test/homing/20260616_120456/cam0.jpg
Normal file
BIN
test/homing/20260616_120456/cam0.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 74 KiB |
2376
test/homing/20260616_120456/cam0_aruco_detection.json
Normal file
2376
test/homing/20260616_120456/cam0_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
411
test/homing/20260616_120456/cam0_camera_pose.json
Normal file
411
test/homing/20260616_120456/cam0_camera_pose.json
Normal file
@@ -0,0 +1,411 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-16T12:05:00Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260616_120456/cam0_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam0",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1424.7584228515625,
|
||||
0.0,
|
||||
635.95947265625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1421.5770263671875,
|
||||
482.1744384765625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.05634751915931702,
|
||||
0.33765655755996704,
|
||||
0.002130246954038739,
|
||||
-0.004022662527859211,
|
||||
-1.182201862335205
|
||||
]
|
||||
},
|
||||
"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": 15,
|
||||
"used_marker_ids": [
|
||||
55,
|
||||
79,
|
||||
47,
|
||||
54,
|
||||
86,
|
||||
96,
|
||||
84,
|
||||
62,
|
||||
85,
|
||||
105,
|
||||
57,
|
||||
59,
|
||||
48,
|
||||
92,
|
||||
71
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.00867338622090616,
|
||||
0.0005736213711697816,
|
||||
0.0003387058057489281,
|
||||
0.00033870238542763406
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 0.6950564647542702,
|
||||
"residual_median_px": 0.40248905261772255,
|
||||
"residual_max_px": 1.4324357568626296,
|
||||
"sigma2_normalized": 1.4339913236727992e-07
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.054954398423433304,
|
||||
-0.995682954788208,
|
||||
-0.07480252534151077
|
||||
],
|
||||
[
|
||||
-0.31208696961402893,
|
||||
0.05403408408164978,
|
||||
-0.9485157132148743
|
||||
],
|
||||
[
|
||||
0.9484628438949585,
|
||||
0.07547000050544739,
|
||||
-0.3077702820301056
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.027871036902070045,
|
||||
0.2804853320121765,
|
||||
0.4656999111175537
|
||||
],
|
||||
"rvec_rad": [
|
||||
1.4158355861217138,
|
||||
-1.414839573060464,
|
||||
0.9451885483658883
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
-0.3526315987110138,
|
||||
-0.07805286347866058,
|
||||
0.40728849172592163
|
||||
],
|
||||
"position_mm": [
|
||||
-352.631591796875,
|
||||
-78.05286407470703,
|
||||
407.2884826660156
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 166.22206115722656,
|
||||
"pitch": -71.525146484375,
|
||||
"yaw": -80.01333618164062
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
5.024150406033972e-07,
|
||||
-2.6272364342807006e-07,
|
||||
-4.335547550274572e-08,
|
||||
3.714638912704353e-08,
|
||||
1.63462035841052e-07,
|
||||
1.0652181985979162e-07
|
||||
],
|
||||
[
|
||||
-2.627236434280717e-07,
|
||||
5.551190890670724e-07,
|
||||
-9.010350610641584e-08,
|
||||
-1.9385507269862047e-07,
|
||||
-1.980878719033606e-07,
|
||||
-9.31153574198941e-08
|
||||
],
|
||||
[
|
||||
-4.335547550274997e-08,
|
||||
-9.01035061064162e-08,
|
||||
6.53136070312492e-07,
|
||||
1.9362202241313713e-07,
|
||||
-1.146589237690379e-07,
|
||||
-7.196585886255944e-08
|
||||
],
|
||||
[
|
||||
3.714638912704331e-08,
|
||||
-1.9385507269862087e-07,
|
||||
1.9362202241313628e-07,
|
||||
1.2484560913735146e-07,
|
||||
3.2658382583721026e-08,
|
||||
3.301097074312566e-08
|
||||
],
|
||||
[
|
||||
1.6346203584105354e-07,
|
||||
-1.980878719033603e-07,
|
||||
-1.1465892376903774e-07,
|
||||
3.265838258372065e-08,
|
||||
1.23652189246973e-07,
|
||||
8.571146080154154e-08
|
||||
],
|
||||
[
|
||||
1.0652181985979293e-07,
|
||||
-9.311535741989419e-08,
|
||||
-7.196585886255985e-08,
|
||||
3.301097074312538e-08,
|
||||
8.571146080154191e-08,
|
||||
1.6707738731885391e-07
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.04061195988417994,
|
||||
0.0426889736419385,
|
||||
0.04630463517881947
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0003533349814798295,
|
||||
0.000351642132354718,
|
||||
0.00040875100895148127
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0004015718950779406,
|
||||
0.0007421256472201426,
|
||||
0.0006731526149433535
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
0.4015718950779406,
|
||||
0.7421256472201425,
|
||||
0.6731526149433535
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.13494216320251604,
|
||||
"pitch": 0.04025296957854174,
|
||||
"yaw": 0.11737953488154021
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 55,
|
||||
"observed_center_px": [
|
||||
1136.5,
|
||||
888.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1136.376220703125,
|
||||
888.4700927734375
|
||||
],
|
||||
"reprojection_error_px": 0.25251166954079585,
|
||||
"confidence": 0.5219323198445638
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
919.0,
|
||||
861.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
919.0020141601562,
|
||||
861.0948486328125
|
||||
],
|
||||
"reprojection_error_px": 0.15516444045362157,
|
||||
"confidence": 0.46679997653940364
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
1151.25,
|
||||
822.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1150.4693603515625,
|
||||
821.73828125
|
||||
],
|
||||
"reprojection_error_px": 0.9334100598419679,
|
||||
"confidence": 0.40553614178530284
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
1235.25,
|
||||
820.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1236.1435546875,
|
||||
821.3695678710938
|
||||
],
|
||||
"reprojection_error_px": 1.4324357568626296,
|
||||
"confidence": 0.11853329759702871
|
||||
},
|
||||
{
|
||||
"marker_id": 86,
|
||||
"observed_center_px": [
|
||||
123.0,
|
||||
842.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
122.65746307373047,
|
||||
842.6725463867188
|
||||
],
|
||||
"reprojection_error_px": 0.35118457834662914,
|
||||
"confidence": 0.39666742757474877
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
954.0,
|
||||
802.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
954.2156982421875,
|
||||
801.8694458007812
|
||||
],
|
||||
"reprojection_error_px": 0.25213117739864394,
|
||||
"confidence": 0.3314216578838723
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
206.75,
|
||||
799.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
207.06143188476562,
|
||||
799.0526123046875
|
||||
],
|
||||
"reprojection_error_px": 0.31584469831421874,
|
||||
"confidence": 0.29360621858836283
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
925.0,
|
||||
770.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
925.062255859375,
|
||||
771.1116943359375
|
||||
],
|
||||
"reprojection_error_px": 0.36701305790910066,
|
||||
"confidence": 0.25370304094044416
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
1120.5,
|
||||
687.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1119.8026123046875,
|
||||
686.31298828125
|
||||
],
|
||||
"reprojection_error_px": 1.3767158087358151,
|
||||
"confidence": 0.20441637110846192
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1038.75,
|
||||
675.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1039.1170654296875,
|
||||
675.0737915039062
|
||||
],
|
||||
"reprojection_error_px": 0.5624861881096861,
|
||||
"confidence": 0.17259443971051047
|
||||
},
|
||||
{
|
||||
"marker_id": 57,
|
||||
"observed_center_px": [
|
||||
1157.25,
|
||||
621.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1157.047607421875,
|
||||
621.402099609375
|
||||
],
|
||||
"reprojection_error_px": 0.40248905261772255,
|
||||
"confidence": 0.15036126370334021
|
||||
},
|
||||
{
|
||||
"marker_id": 59,
|
||||
"observed_center_px": [
|
||||
1027.0,
|
||||
613.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1026.6114501953125,
|
||||
613.2049560546875
|
||||
],
|
||||
"reprojection_error_px": 0.3911520263682778,
|
||||
"confidence": 0.11849981411449093
|
||||
},
|
||||
{
|
||||
"marker_id": 48,
|
||||
"observed_center_px": [
|
||||
1066.0,
|
||||
578.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1065.6326904296875,
|
||||
578.4794921875
|
||||
],
|
||||
"reprojection_error_px": 0.433108513616032,
|
||||
"confidence": 0.08787391972076787
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
892.75,
|
||||
608.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
893.5529174804688,
|
||||
608.5789794921875
|
||||
],
|
||||
"reprojection_error_px": 0.8677004014763569,
|
||||
"confidence": 0.08649209635185473
|
||||
},
|
||||
{
|
||||
"marker_id": 71,
|
||||
"observed_center_px": [
|
||||
1001.5,
|
||||
551.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1001.7471313476562,
|
||||
552.2847290039062
|
||||
],
|
||||
"reprojection_error_px": 0.5890747071577294,
|
||||
"confidence": 0.08751548667261201
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
test/homing/20260616_120456/cam0_debug.jpg
Normal file
BIN
test/homing/20260616_120456/cam0_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 180 KiB |
BIN
test/homing/20260616_120456/cam1.jpg
Normal file
BIN
test/homing/20260616_120456/cam1.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 76 KiB |
3551
test/homing/20260616_120456/cam1_aruco_detection.json
Normal file
3551
test/homing/20260616_120456/cam1_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
691
test/homing/20260616_120456/cam1_camera_pose.json
Normal file
691
test/homing/20260616_120456/cam1_camera_pose.json
Normal file
@@ -0,0 +1,691 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-16T12:05:05Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260616_120456/cam1_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam1",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1367.5723876953125,
|
||||
0.0,
|
||||
672.1165771484375
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1372.3011474609375,
|
||||
445.8396911621094
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.01016925647854805,
|
||||
0.7656787633895874,
|
||||
-0.0031530377455055714,
|
||||
-0.00288817984983325,
|
||||
-2.490830183029175
|
||||
]
|
||||
},
|
||||
"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": 35,
|
||||
"used_marker_ids": [
|
||||
69,
|
||||
64,
|
||||
58,
|
||||
66,
|
||||
103,
|
||||
95,
|
||||
97,
|
||||
51,
|
||||
54,
|
||||
55,
|
||||
75,
|
||||
47,
|
||||
77,
|
||||
74,
|
||||
85,
|
||||
96,
|
||||
52,
|
||||
81,
|
||||
62,
|
||||
57,
|
||||
105,
|
||||
101,
|
||||
71,
|
||||
78,
|
||||
82,
|
||||
65,
|
||||
80,
|
||||
84,
|
||||
99,
|
||||
87,
|
||||
70,
|
||||
91,
|
||||
90,
|
||||
93,
|
||||
104
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.01669716219734741,
|
||||
0.0015530130263999152,
|
||||
0.0008983437053381035,
|
||||
0.0008983157544106726
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 1.7685482048099375,
|
||||
"residual_median_px": 1.5050116514173932,
|
||||
"residual_max_px": 3.68734500446118,
|
||||
"sigma2_normalized": 8.826247430843121e-07
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.8296902179718018,
|
||||
0.04856207221746445,
|
||||
-0.556107759475708
|
||||
],
|
||||
[
|
||||
-0.20650699734687805,
|
||||
0.8988339900970459,
|
||||
0.3865906596183777
|
||||
],
|
||||
[
|
||||
0.5186222195625305,
|
||||
0.4355906546115875,
|
||||
-0.7357253432273865
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
0.31146639585494995,
|
||||
0.15941768884658813,
|
||||
1.010162115097046
|
||||
],
|
||||
"rvec_rad": [
|
||||
0.11326674166552995,
|
||||
-2.4843106472248735,
|
||||
-0.5896093173683601
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
-0.23255103826522827,
|
||||
-0.5984326601028442,
|
||||
0.8547813296318054
|
||||
],
|
||||
"position_mm": [
|
||||
-232.55104064941406,
|
||||
-598.4326782226562,
|
||||
854.7813110351562
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 149.3720703125,
|
||||
"pitch": -31.2398738861084,
|
||||
"yaw": -166.0232696533203
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
5.108318008737832e-07,
|
||||
-2.030876446668622e-07,
|
||||
1.847398283306561e-07,
|
||||
-9.7616015744967e-09,
|
||||
9.054207137354404e-08,
|
||||
1.6652401795418441e-07
|
||||
],
|
||||
[
|
||||
-2.0308764466685943e-07,
|
||||
1.5382020212494465e-06,
|
||||
-6.297556683785447e-07,
|
||||
-3.010072505572984e-07,
|
||||
1.0689229244008563e-07,
|
||||
-8.671432148962064e-07
|
||||
],
|
||||
[
|
||||
1.8473982833065666e-07,
|
||||
-6.297556683785541e-07,
|
||||
2.588424752063583e-06,
|
||||
-9.193339804491363e-08,
|
||||
-1.7555965832587911e-07,
|
||||
6.037665269982221e-08
|
||||
],
|
||||
[
|
||||
-9.76160157449755e-09,
|
||||
-3.0100725055729717e-07,
|
||||
-9.193339804491597e-08,
|
||||
1.1789721480957471e-07,
|
||||
-1.8772945136575335e-08,
|
||||
2.1678392569580849e-07
|
||||
],
|
||||
[
|
||||
9.054207137354437e-08,
|
||||
1.0689229244008568e-07,
|
||||
-1.755596583258787e-07,
|
||||
-1.877294513657538e-08,
|
||||
7.498759148393252e-08,
|
||||
-4.200084672079684e-08
|
||||
],
|
||||
[
|
||||
1.6652401795418214e-07,
|
||||
-8.671432148962051e-07,
|
||||
6.037665269981562e-08,
|
||||
2.1678392569580904e-07,
|
||||
-4.200084672079688e-08,
|
||||
9.640297784807404e-07
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.0409507250203997,
|
||||
0.07106067381441428,
|
||||
0.092180785484839
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0003433616385235466,
|
||||
0.0002738386230682818,
|
||||
0.0009818501812805965
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0010578573566047088,
|
||||
0.0012453368073035017,
|
||||
0.001463792458469902
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
1.0578573566047087,
|
||||
1.2453368073035016,
|
||||
1.463792458469902
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.10570612813240464,
|
||||
"pitch": 0.06594269721083164,
|
||||
"yaw": 0.053541286201001195
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
1132.0,
|
||||
285.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1134.9923095703125,
|
||||
285.38287353515625
|
||||
],
|
||||
"reprojection_error_px": 2.9946010040321855,
|
||||
"confidence": 0.8004430063137599
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
1160.25,
|
||||
421.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1163.9249267578125,
|
||||
421.8023681640625
|
||||
],
|
||||
"reprojection_error_px": 3.68734500446118,
|
||||
"confidence": 0.6780206236463221
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
1061.75,
|
||||
363.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1063.72900390625,
|
||||
363.04876708984375
|
||||
],
|
||||
"reprojection_error_px": 1.9892086730865397,
|
||||
"confidence": 0.6409044457372643
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
861.0,
|
||||
135.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
860.8423461914062,
|
||||
135.11590576171875
|
||||
],
|
||||
"reprojection_error_px": 0.6533989794800673,
|
||||
"confidence": 0.5643748623006126
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
984.5,
|
||||
388.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
985.591064453125,
|
||||
388.5110778808594
|
||||
],
|
||||
"reprojection_error_px": 1.1169178214567097,
|
||||
"confidence": 0.5680230430248981
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
888.0,
|
||||
260.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
888.1265258789062,
|
||||
260.22412109375
|
||||
],
|
||||
"reprojection_error_px": 0.5408857754384888,
|
||||
"confidence": 0.5040987555384636
|
||||
},
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
748.25,
|
||||
129.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
747.3042602539062,
|
||||
128.7545623779297
|
||||
],
|
||||
"reprojection_error_px": 1.204201360130154,
|
||||
"confidence": 0.46133781313759503
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
906.75,
|
||||
392.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
906.9967041015625,
|
||||
392.4247131347656
|
||||
],
|
||||
"reprojection_error_px": 0.25793608860448214,
|
||||
"confidence": 0.5161510750240749
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
707.0,
|
||||
162.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
706.260498046875,
|
||||
162.3862762451172
|
||||
],
|
||||
"reprojection_error_px": 0.824110495347403,
|
||||
"confidence": 0.41077842645846463
|
||||
},
|
||||
{
|
||||
"marker_id": 55,
|
||||
"observed_center_px": [
|
||||
773.75,
|
||||
258.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
773.1185913085938,
|
||||
257.3814392089844
|
||||
],
|
||||
"reprojection_error_px": 1.0738131975688514,
|
||||
"confidence": 0.42449809621175133
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
1118.0,
|
||||
858.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1118.964599609375,
|
||||
858.5298461914062
|
||||
],
|
||||
"reprojection_error_px": 1.0043735845047828,
|
||||
"confidence": 0.32078570148601215
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
707.25,
|
||||
218.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
706.2919921875,
|
||||
217.7268829345703
|
||||
],
|
||||
"reprojection_error_px": 0.9961786487573198,
|
||||
"confidence": 0.3835876006690484
|
||||
},
|
||||
{
|
||||
"marker_id": 77,
|
||||
"observed_center_px": [
|
||||
1064.5,
|
||||
835.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1065.1744384765625,
|
||||
835.8121337890625
|
||||
],
|
||||
"reprojection_error_px": 1.0556649800031064,
|
||||
"confidence": 0.295743044190549
|
||||
},
|
||||
{
|
||||
"marker_id": 74,
|
||||
"observed_center_px": [
|
||||
979.75,
|
||||
758.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
979.26416015625,
|
||||
758.2605590820312
|
||||
],
|
||||
"reprojection_error_px": 0.5512997270124411,
|
||||
"confidence": 0.3359635861403907
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
546.0,
|
||||
163.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
544.5795288085938,
|
||||
163.747314453125
|
||||
],
|
||||
"reprojection_error_px": 1.5050116514173932,
|
||||
"confidence": 0.3151234057766515
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
686.0,
|
||||
331.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
685.0399780273438,
|
||||
331.31890869140625
|
||||
],
|
||||
"reprojection_error_px": 0.9769525321278381,
|
||||
"confidence": 0.3554108905483531
|
||||
},
|
||||
{
|
||||
"marker_id": 52,
|
||||
"observed_center_px": [
|
||||
975.0,
|
||||
818.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
974.9528198242188,
|
||||
818.7924194335938
|
||||
],
|
||||
"reprojection_error_px": 0.5444674562606238,
|
||||
"confidence": 0.28720673485083303
|
||||
},
|
||||
{
|
||||
"marker_id": 81,
|
||||
"observed_center_px": [
|
||||
935.75,
|
||||
766.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
935.2142333984375,
|
||||
766.4912109375
|
||||
],
|
||||
"reprojection_error_px": 0.5358386874512324,
|
||||
"confidence": 0.30145054927990633
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
652.5,
|
||||
337.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
651.360107421875,
|
||||
337.1482238769531
|
||||
],
|
||||
"reprojection_error_px": 1.2889878168290758,
|
||||
"confidence": 0.3191799957107001
|
||||
},
|
||||
{
|
||||
"marker_id": 57,
|
||||
"observed_center_px": [
|
||||
450.25,
|
||||
90.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
449.19036865234375,
|
||||
91.7260513305664
|
||||
],
|
||||
"reprojection_error_px": 1.6205000642145793,
|
||||
"confidence": 0.26860824414385837
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
532.5,
|
||||
214.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
531.2189331054688,
|
||||
214.99610900878906
|
||||
],
|
||||
"reprojection_error_px": 1.3737745582393084,
|
||||
"confidence": 0.28102521367518357
|
||||
},
|
||||
{
|
||||
"marker_id": 101,
|
||||
"observed_center_px": [
|
||||
947.5,
|
||||
889.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
946.0105590820312,
|
||||
889.7289428710938
|
||||
],
|
||||
"reprojection_error_px": 1.4895897592281464,
|
||||
"confidence": 0.25378363660843234
|
||||
},
|
||||
{
|
||||
"marker_id": 71,
|
||||
"observed_center_px": [
|
||||
342.5,
|
||||
164.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
342.4768981933594,
|
||||
166.27276611328125
|
||||
],
|
||||
"reprojection_error_px": 1.522941341361428,
|
||||
"confidence": 0.20828507797372528
|
||||
},
|
||||
{
|
||||
"marker_id": 78,
|
||||
"observed_center_px": [
|
||||
275.25,
|
||||
91.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
276.1173095703125,
|
||||
93.466552734375
|
||||
],
|
||||
"reprojection_error_px": 1.923221043106055,
|
||||
"confidence": 0.1903062602705479
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
845.0,
|
||||
847.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
843.3731079101562,
|
||||
846.445556640625
|
||||
],
|
||||
"reprojection_error_px": 1.8149123919458756,
|
||||
"confidence": 0.2248605201960664
|
||||
},
|
||||
{
|
||||
"marker_id": 65,
|
||||
"observed_center_px": [
|
||||
299.0,
|
||||
144.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
299.8258361816406,
|
||||
145.95309448242188
|
||||
],
|
||||
"reprojection_error_px": 2.120514903549027,
|
||||
"confidence": 0.18160242191392414
|
||||
},
|
||||
{
|
||||
"marker_id": 80,
|
||||
"observed_center_px": [
|
||||
248.0,
|
||||
96.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
248.56906127929688,
|
||||
98.8924331665039
|
||||
],
|
||||
"reprojection_error_px": 2.2167206888850366,
|
||||
"confidence": 0.16263927873225353
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
674.75,
|
||||
748.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
673.0452880859375,
|
||||
746.7835083007812
|
||||
],
|
||||
"reprojection_error_px": 2.4191706147838157,
|
||||
"confidence": 0.21110242529580486
|
||||
},
|
||||
{
|
||||
"marker_id": 99,
|
||||
"observed_center_px": [
|
||||
184.75,
|
||||
102.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
186.11749267578125,
|
||||
105.2986068725586
|
||||
],
|
||||
"reprojection_error_px": 2.892305863695653,
|
||||
"confidence": 0.14526599262838052
|
||||
},
|
||||
{
|
||||
"marker_id": 87,
|
||||
"observed_center_px": [
|
||||
210.75,
|
||||
180.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
212.3033447265625,
|
||||
182.51739501953125
|
||||
],
|
||||
"reprojection_error_px": 2.352990649068414,
|
||||
"confidence": 0.15014511583656154
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
526.25,
|
||||
722.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
524.888671875,
|
||||
720.3948974609375
|
||||
],
|
||||
"reprojection_error_px": 2.301004062219828,
|
||||
"confidence": 0.15652476433958798
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
445.25,
|
||||
709.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
443.6956787109375,
|
||||
707.20751953125
|
||||
],
|
||||
"reprojection_error_px": 2.566640047778054,
|
||||
"confidence": 0.147769899515473
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
498.5,
|
||||
722.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
497.030517578125,
|
||||
720.7365112304688
|
||||
],
|
||||
"reprojection_error_px": 2.2954893222279336,
|
||||
"confidence": 0.1571012027138013
|
||||
},
|
||||
{
|
||||
"marker_id": 93,
|
||||
"observed_center_px": [
|
||||
285.5,
|
||||
521.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
286.38006591796875,
|
||||
519.7478637695312
|
||||
],
|
||||
"reprojection_error_px": 1.7409564253182916,
|
||||
"confidence": 0.1231418173996764
|
||||
},
|
||||
{
|
||||
"marker_id": 104,
|
||||
"observed_center_px": [
|
||||
367.0,
|
||||
617.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
366.587646484375,
|
||||
615.7141723632812
|
||||
],
|
||||
"reprojection_error_px": 1.83281634920599,
|
||||
"confidence": 0.14786630564511977
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
test/homing/20260616_120456/cam1_debug.jpg
Normal file
BIN
test/homing/20260616_120456/cam1_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 214 KiB |
BIN
test/homing/20260616_120456/cam2.jpg
Normal file
BIN
test/homing/20260616_120456/cam2.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 240 KiB |
4221
test/homing/20260616_120456/cam2_aruco_detection.json
Normal file
4221
test/homing/20260616_120456/cam2_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
747
test/homing/20260616_120456/cam2_camera_pose.json
Normal file
747
test/homing/20260616_120456/cam2_camera_pose.json
Normal file
@@ -0,0 +1,747 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-16T12:05:07Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260616_120456/cam2_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam2",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1388.99072265625,
|
||||
0.0,
|
||||
933.082763671875
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1394.8729248046875,
|
||||
562.4996948242188
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.019531700760126114,
|
||||
-0.11213663965463638,
|
||||
0.0026758278254419565,
|
||||
0.0007694826927036047,
|
||||
0.05339815095067024
|
||||
]
|
||||
},
|
||||
"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": 39,
|
||||
"used_marker_ids": [
|
||||
73,
|
||||
51,
|
||||
82,
|
||||
95,
|
||||
66,
|
||||
55,
|
||||
86,
|
||||
79,
|
||||
97,
|
||||
47,
|
||||
54,
|
||||
84,
|
||||
96,
|
||||
60,
|
||||
72,
|
||||
62,
|
||||
53,
|
||||
56,
|
||||
67,
|
||||
46,
|
||||
85,
|
||||
98,
|
||||
68,
|
||||
105,
|
||||
50,
|
||||
70,
|
||||
90,
|
||||
91,
|
||||
76,
|
||||
88,
|
||||
104,
|
||||
100,
|
||||
65,
|
||||
94,
|
||||
80,
|
||||
93,
|
||||
87,
|
||||
49,
|
||||
99
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.010459464703921952,
|
||||
0.000600512883123414,
|
||||
0.0002694619125775263,
|
||||
0.0002694585232559286
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 0.5308468666027378,
|
||||
"residual_median_px": 0.442470638660156,
|
||||
"residual_max_px": 1.0184954029835627,
|
||||
"sigma2_normalized": 7.865855373327773e-08
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.11123913526535034,
|
||||
-0.9937126636505127,
|
||||
-0.012687010690569878
|
||||
],
|
||||
[
|
||||
-0.6987926959991455,
|
||||
0.08728941529989243,
|
||||
-0.7099784016609192
|
||||
],
|
||||
[
|
||||
0.7066220045089722,
|
||||
-0.07011179625988007,
|
||||
-0.7041091322898865
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
0.06501124799251556,
|
||||
0.4216889441013336,
|
||||
0.7629748582839966
|
||||
],
|
||||
"rvec_rad": [
|
||||
1.6611880808196222,
|
||||
-1.8674322262849752,
|
||||
0.7656557967967812
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
-0.2372298538684845,
|
||||
0.08128705620765686,
|
||||
0.8374323844909668
|
||||
],
|
||||
"position_mm": [
|
||||
-237.2298583984375,
|
||||
81.28705596923828,
|
||||
837.432373046875
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -174.31349182128906,
|
||||
"pitch": -44.96072769165039,
|
||||
"yaw": -99.04487609863281
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
7.70808885712673e-08,
|
||||
-5.051963537309151e-08,
|
||||
6.275184896470588e-09,
|
||||
8.219944671662197e-09,
|
||||
3.0401303313197284e-08,
|
||||
4.050671454678838e-08
|
||||
],
|
||||
[
|
||||
-5.051963537309112e-08,
|
||||
1.190708779865382e-07,
|
||||
-7.710624609173114e-09,
|
||||
-3.60891791637598e-08,
|
||||
-3.822077534280391e-08,
|
||||
-5.7320708312388815e-08
|
||||
],
|
||||
[
|
||||
6.275184896470584e-09,
|
||||
-7.710624609173487e-09,
|
||||
3.4448589627302786e-07,
|
||||
6.347555001744274e-08,
|
||||
-3.73214863209708e-08,
|
||||
-3.6939829357622016e-08
|
||||
],
|
||||
[
|
||||
8.219944671662068e-09,
|
||||
-3.6089179163759805e-08,
|
||||
6.34755500174426e-08,
|
||||
2.5430604878286124e-08,
|
||||
2.514907754778194e-09,
|
||||
7.848306963852704e-09
|
||||
],
|
||||
[
|
||||
3.040130331319718e-08,
|
||||
-3.8220775342803935e-08,
|
||||
-3.732148632097092e-08,
|
||||
2.514907754778194e-09,
|
||||
2.366305405059915e-08,
|
||||
3.099727891165752e-08
|
||||
],
|
||||
[
|
||||
4.050671454678818e-08,
|
||||
-5.732070831238883e-08,
|
||||
-3.6939829357622505e-08,
|
||||
7.848306963852644e-09,
|
||||
3.0997278911657537e-08,
|
||||
6.82205068714338e-08
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.015907282309253958,
|
||||
0.019770853151942665,
|
||||
0.033628566543559106
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.00015946976164240708,
|
||||
0.00015382800151662619,
|
||||
0.00026119055662759673
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.00030722285641275344,
|
||||
0.0005444062391567311,
|
||||
0.00035845833746427834
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
0.30722285641275343,
|
||||
0.544406239156731,
|
||||
0.35845833746427835
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.03168101724253478,
|
||||
"pitch": 0.023469916191750515,
|
||||
"yaw": 0.01918342103865859
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
492.5,
|
||||
1044.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
492.4930114746094,
|
||||
1043.94873046875
|
||||
],
|
||||
"reprojection_error_px": 0.051743640398894786,
|
||||
"confidence": 0.2681332709796957
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
1265.25,
|
||||
1035.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1265.3367919921875,
|
||||
1036.1767578125
|
||||
],
|
||||
"reprojection_error_px": 0.6823005105433091,
|
||||
"confidence": 0.4130819814942474
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
547.0,
|
||||
1041.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
547.1077880859375,
|
||||
1041.7156982421875
|
||||
],
|
||||
"reprojection_error_px": 0.11311446441148582,
|
||||
"confidence": 0.28570000902510134
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
1405.75,
|
||||
993.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1406.1728515625,
|
||||
993.415771484375
|
||||
],
|
||||
"reprojection_error_px": 0.5930171760818419,
|
||||
"confidence": 0.7223666558526649
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
1518.5,
|
||||
947.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1519.346923828125,
|
||||
947.2127075195312
|
||||
],
|
||||
"reprojection_error_px": 0.8732264651916143,
|
||||
"confidence": 0.7055288033501755
|
||||
},
|
||||
{
|
||||
"marker_id": 55,
|
||||
"observed_center_px": [
|
||||
1343.5,
|
||||
869.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1343.1153564453125,
|
||||
870.1083374023438
|
||||
],
|
||||
"reprojection_error_px": 0.5256960700643504,
|
||||
"confidence": 0.5911102318971898
|
||||
},
|
||||
{
|
||||
"marker_id": 86,
|
||||
"observed_center_px": [
|
||||
570.5,
|
||||
855.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
570.8598022460938,
|
||||
855.154541015625
|
||||
],
|
||||
"reprojection_error_px": 0.6957219691565115,
|
||||
"confidence": 0.49213911198599
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
1192.75,
|
||||
850.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1192.2608642578125,
|
||||
850.8606567382812
|
||||
],
|
||||
"reprojection_error_px": 0.6077228456730802,
|
||||
"confidence": 0.5414756619284304
|
||||
},
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
1462.0,
|
||||
832.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1462.5380859375,
|
||||
832.4049072265625
|
||||
],
|
||||
"reprojection_error_px": 0.6734139427260099,
|
||||
"confidence": 0.5279372649886516
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
1348.5,
|
||||
797.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1348.350341796875,
|
||||
797.6093139648438
|
||||
],
|
||||
"reprojection_error_px": 0.20540238131674465,
|
||||
"confidence": 0.5088850430842212
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
1406.0,
|
||||
793.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1406.117919921875,
|
||||
793.9895629882812
|
||||
],
|
||||
"reprojection_error_px": 0.2670122344186656,
|
||||
"confidence": 0.4782396466982758
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
620.0,
|
||||
800.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
620.1146240234375,
|
||||
800.3106689453125
|
||||
],
|
||||
"reprojection_error_px": 0.2213253600879973,
|
||||
"confidence": 0.4467793485983816
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
1209.75,
|
||||
783.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1209.1610107421875,
|
||||
782.8501586914062
|
||||
],
|
||||
"reprojection_error_px": 0.6077505767826198,
|
||||
"confidence": 0.43618784694977364
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
588.25,
|
||||
774.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
588.4884033203125,
|
||||
774.8460693359375
|
||||
],
|
||||
"reprojection_error_px": 0.2570320222141527,
|
||||
"confidence": 0.4149513143267376
|
||||
},
|
||||
{
|
||||
"marker_id": 72,
|
||||
"observed_center_px": [
|
||||
705.5,
|
||||
756.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
705.5302124023438,
|
||||
756.4302978515625
|
||||
],
|
||||
"reprojection_error_px": 0.4313571957376901,
|
||||
"confidence": 0.41472917138862403
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
1183.5,
|
||||
747.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1183.393798828125,
|
||||
747.59423828125
|
||||
],
|
||||
"reprojection_error_px": 0.14198430392327666,
|
||||
"confidence": 0.398140004533079
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
683.25,
|
||||
712.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
683.28271484375,
|
||||
713.240478515625
|
||||
],
|
||||
"reprojection_error_px": 0.4915683424421169,
|
||||
"confidence": 0.3239734944910412
|
||||
},
|
||||
{
|
||||
"marker_id": 56,
|
||||
"observed_center_px": [
|
||||
738.25,
|
||||
696.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
737.8375244140625,
|
||||
696.041748046875
|
||||
],
|
||||
"reprojection_error_px": 0.4145829330934417,
|
||||
"confidence": 0.3443049703609756
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
616.0,
|
||||
685.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
616.3634033203125,
|
||||
684.5353393554688
|
||||
],
|
||||
"reprojection_error_px": 0.58989108129412,
|
||||
"confidence": 0.30389163461125196
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
717.25,
|
||||
662.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
716.9480590820312,
|
||||
662.5203247070312
|
||||
],
|
||||
"reprojection_error_px": 0.30262420864781714,
|
||||
"confidence": 0.3149845419556931
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
1316.75,
|
||||
637.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1316.73291015625,
|
||||
636.42822265625
|
||||
],
|
||||
"reprojection_error_px": 0.8219550264218871,
|
||||
"confidence": 0.32688682176810563
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
561.5,
|
||||
643.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
561.7496948242188,
|
||||
642.4434204101562
|
||||
],
|
||||
"reprojection_error_px": 0.6100232332233498,
|
||||
"confidence": 0.29086841462927165
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
734.5,
|
||||
628.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
734.5006103515625,
|
||||
627.9244995117188
|
||||
],
|
||||
"reprojection_error_px": 0.07550295530465688,
|
||||
"confidence": 0.29243937849297746
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1256.0,
|
||||
624.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1255.47509765625,
|
||||
623.591796875
|
||||
],
|
||||
"reprojection_error_px": 0.8418751832866965,
|
||||
"confidence": 0.3029127688517622
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
685.75,
|
||||
633.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
685.4552001953125,
|
||||
632.6983032226562
|
||||
],
|
||||
"reprojection_error_px": 0.4218149716444313,
|
||||
"confidence": 0.28667702620740154
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
581.5,
|
||||
618.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
581.670166015625,
|
||||
617.6889038085938
|
||||
],
|
||||
"reprojection_error_px": 0.3545945757920754,
|
||||
"confidence": 0.2655177345943533
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
566.0,
|
||||
585.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
566.2715454101562,
|
||||
584.881103515625
|
||||
],
|
||||
"reprojection_error_px": 0.29643428238592895,
|
||||
"confidence": 0.23219602732829617
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
559.5,
|
||||
524.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
560.0989379882812,
|
||||
523.4891967773438
|
||||
],
|
||||
"reprojection_error_px": 0.787176375460037,
|
||||
"confidence": 0.21495162665262563
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
737.75,
|
||||
537.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
737.5767211914062,
|
||||
536.794677734375
|
||||
],
|
||||
"reprojection_error_px": 0.26866852861668133,
|
||||
"confidence": 0.2109775341382568
|
||||
},
|
||||
{
|
||||
"marker_id": 88,
|
||||
"observed_center_px": [
|
||||
576.25,
|
||||
491.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
576.6868286132812,
|
||||
491.4295654296875
|
||||
],
|
||||
"reprojection_error_px": 0.442470638660156,
|
||||
"confidence": 0.18751626052772438
|
||||
},
|
||||
{
|
||||
"marker_id": 104,
|
||||
"observed_center_px": [
|
||||
661.25,
|
||||
443.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
661.183837890625,
|
||||
443.0474853515625
|
||||
],
|
||||
"reprojection_error_px": 0.08143883183078994,
|
||||
"confidence": 0.16576757504988693
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
730.75,
|
||||
453.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
730.3933715820312,
|
||||
452.9394836425781
|
||||
],
|
||||
"reprojection_error_px": 0.361726496152142,
|
||||
"confidence": 0.16629537203475994
|
||||
},
|
||||
{
|
||||
"marker_id": 65,
|
||||
"observed_center_px": [
|
||||
1209.0,
|
||||
414.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1208.3629150390625,
|
||||
413.75823974609375
|
||||
],
|
||||
"reprojection_error_px": 0.6814141676114097,
|
||||
"confidence": 0.1641051378333676
|
||||
},
|
||||
{
|
||||
"marker_id": 94,
|
||||
"observed_center_px": [
|
||||
728.5,
|
||||
406.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
728.5389404296875,
|
||||
406.2892150878906
|
||||
],
|
||||
"reprojection_error_px": 0.291824817532418,
|
||||
"confidence": 0.12091049607966889
|
||||
},
|
||||
{
|
||||
"marker_id": 80,
|
||||
"observed_center_px": [
|
||||
1231.0,
|
||||
373.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1230.5965576171875,
|
||||
373.7982482910156
|
||||
],
|
||||
"reprojection_error_px": 0.40631718378054876,
|
||||
"confidence": 0.12350728340782749
|
||||
},
|
||||
{
|
||||
"marker_id": 93,
|
||||
"observed_center_px": [
|
||||
757.5,
|
||||
367.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
757.3904418945312,
|
||||
368.3028869628906
|
||||
],
|
||||
"reprojection_error_px": 0.8103273743701577,
|
||||
"confidence": 0.1305433400143518
|
||||
},
|
||||
{
|
||||
"marker_id": 87,
|
||||
"observed_center_px": [
|
||||
1127.25,
|
||||
335.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1126.9609375,
|
||||
334.5552673339844
|
||||
],
|
||||
"reprojection_error_px": 0.5304189600001262,
|
||||
"confidence": 0.11698041029937327
|
||||
},
|
||||
{
|
||||
"marker_id": 49,
|
||||
"observed_center_px": [
|
||||
742.0,
|
||||
328.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
742.1221923828125,
|
||||
329.2611389160156
|
||||
],
|
||||
"reprojection_error_px": 1.0184954029835627,
|
||||
"confidence": 0.11651950992496915
|
||||
},
|
||||
{
|
||||
"marker_id": 99,
|
||||
"observed_center_px": [
|
||||
1194.25,
|
||||
321.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1193.8394775390625,
|
||||
320.6575012207031
|
||||
],
|
||||
"reprojection_error_px": 0.5346345525253963,
|
||||
"confidence": 0.10558573070242767
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
test/homing/20260616_120456/cam2_debug.jpg
Normal file
BIN
test/homing/20260616_120456/cam2_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 408 KiB |
37
test/homing/20260616_120456/state_Arm1.json
Normal file
37
test/homing/20260616_120456/state_Arm1.json
Normal file
@@ -0,0 +1,37 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm1",
|
||||
"joint": "y",
|
||||
"joint_origin_world_mm": [
|
||||
299.24409288922095,
|
||||
101.1,
|
||||
71.2
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 42.11971851018807,
|
||||
"circular_variance": 0.0,
|
||||
"circular_std_deg": 0.0,
|
||||
"num_pairs_used": 1,
|
||||
"num_markers_matched": 2,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
197,
|
||||
243
|
||||
],
|
||||
"skipped": false,
|
||||
"angle_deg": 42.11971851018807,
|
||||
"baseline_model_mm": 35.0,
|
||||
"baseline_obs_mm": 32.87599167028967,
|
||||
"weight": 1150.6597084601385
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 189.24409288922092,
|
||||
"y": 42.11971851018807
|
||||
}
|
||||
}
|
||||
36
test/homing/20260616_133151/aruco_marker_poses.csv
Normal file
36
test/homing/20260616_133151/aruco_marker_poses.csv
Normal file
@@ -0,0 +1,36 @@
|
||||
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
|
||||
47,Board,A0,3,344.07,-286.3,-27.25,-0.00046,0.04835,0.99883,344.23,-286.54,-27.3,0.29,0.049,23.64
|
||||
48,Board,A0,2,689.52,-321.32,-28.55,0.01818,0.02703,0.99947,688.69,-320.72,-27.3,1.616,-1.254,23.78
|
||||
50,Board,A0,2,571.33,211.68,-25.85,0.00822,0.04745,0.99884,573.41,211.86,-27.3,2.541,1.448,23.82
|
||||
54,Board,A0,3,342.21,-330.38,-27.07,-0.0045,-0.03292,0.99945,342.27,-330.59,-27.3,0.316,0.232,23.88
|
||||
55,Board,A0,2,283.85,-262.4,-26.76,-0.01527,0.02358,0.99961,283.72,-262.58,-27.3,0.584,0.539,23.51
|
||||
56,Board,A0,2,497.11,168.6,-25.78,-0.01905,0.05595,0.99825,498.68,168.67,-27.3,2.185,1.521,23.56
|
||||
57,Board,A0,2,601.71,-363.5,-26.19,0.08577,0.00861,0.99628,602.86,-364.05,-27.3,1.694,1.114,24.87
|
||||
62,Board,A0,2,403.89,-175.17,-26.78,0.05288,0.02731,0.99823,404.7,-175.1,-27.3,0.961,0.516,23.82
|
||||
65,Board,A0,2,804.08,-298.54,-28.26,0.00387,0.04132,0.99914,803.39,-297.37,-27.3,1.663,-0.96,23.39
|
||||
68,Board,A0,2,571.76,170.66,-26.3,-0.02368,0.08555,0.99605,573.73,170.64,-27.3,2.214,1.002,23.99
|
||||
71,Board,A0,2,749.94,-284.72,-27.83,-0.01828,-0.03647,0.99917,749.75,-284.01,-27.3,0.907,-0.531,23.66
|
||||
76,Board,A0,2,682.81,166.72,-25.42,-0.05902,0.09056,0.99414,685.21,166.8,-27.3,3.051,1.881,23.55
|
||||
78,Board,A0,2,823.93,-345.77,-28.33,-0.08861,0.04663,0.99497,823.11,-344.38,-27.3,1.915,-1.026,22.6
|
||||
80,Board,A0,2,865.26,-337.13,-28.53,0.04092,0.04275,0.99825,863.59,-335.92,-27.3,2.401,-1.232,23.61
|
||||
87,Board,A0,2,944.69,-246.96,-28.14,0.02302,0.04013,0.99893,943.63,-245.76,-27.3,1.81,-0.837,23.78
|
||||
93,Board,A0,2,932.11,143.72,-23.95,0.06718,0.06443,0.99566,934.88,143.6,-27.3,4.343,3.346,25.06
|
||||
94,Board,A0,2,873.49,174.05,-24.98,0.05325,0.0697,0.99615,875.7,173.65,-27.3,3.229,2.323,24.42
|
||||
96,Board,A0,2,369.02,-186.37,-26.89,-0.04231,0.05761,0.99744,369.77,-186.49,-27.3,0.864,0.406,23.31
|
||||
97,Board,A0,3,304.13,-359.31,-26.85,0.04194,0.01906,0.99894,304.35,-359.67,-27.3,0.616,0.45,23.99
|
||||
99,Board,A0,2,961.74,-323.22,-29.1,0.04681,0.01859,0.99873,959.16,-321.55,-27.3,3.561,-1.797,24.0
|
||||
129,Ellbow,,3,393.47,-110.57,207.56,0.00825,0.0056,0.99995,,,,,,23.63
|
||||
132,Ellbow,,3,356.94,-110.62,208.02,0.00671,0.01225,0.9999,,,,,,24.03
|
||||
143,Arm2,,3,371.09,-292.28,195.24,-0.70987,-0.0132,0.70421,,,,,,24.23
|
||||
144,Arm2,,3,359.12,-222.05,171.29,-0.99909,0.03674,0.02153,,,,,,24.42
|
||||
146,Arm2,,3,370.78,-222.38,195.96,-0.66898,-0.00514,0.74326,,,,,,24.13
|
||||
148,Arm2,,3,360.2,-328.6,169.11,-0.99937,0.02225,0.02756,,,,,,24.15
|
||||
198,Arm1,,2,300.76,-14.03,160.54,0.02386,0.50667,0.86181,,,,,,24.53
|
||||
205,unknown,,2,934.74,-97.18,-5.56,-0.00513,0.04577,0.99894,,,,,,23.15
|
||||
207,unknown,,2,933.91,-7.47,-5.45,-0.07244,0.04588,0.99632,,,,,,21.75
|
||||
229,Arm1,,2,303.05,-92.5,205.63,0.01472,0.48131,0.87642,,,,,,24.21
|
||||
|
||||
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
|
||||
cam0,-162.87,22.94,405.51,0.8496,-0.39087,-0.35411
|
||||
cam1,-231.98,-599.98,855.31,0.51773,0.43658,-0.73577
|
||||
cam2,1.29,141.6,837.2,0.63268,-0.31236,-0.70863
|
||||
|
1387
test/homing/20260616_133151/aruco_marker_poses.json
Normal file
1387
test/homing/20260616_133151/aruco_marker_poses.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
test/homing/20260616_133151/cam0.jpg
Normal file
BIN
test/homing/20260616_133151/cam0.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 79 KiB |
2431
test/homing/20260616_133151/cam0_aruco_detection.json
Normal file
2431
test/homing/20260616_133151/cam0_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
313
test/homing/20260616_133151/cam0_camera_pose.json
Normal file
313
test/homing/20260616_133151/cam0_camera_pose.json
Normal file
@@ -0,0 +1,313 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-16T13:31:56Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260616_133151/cam0_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam0",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1424.7584228515625,
|
||||
0.0,
|
||||
635.95947265625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1421.5770263671875,
|
||||
482.1744384765625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.05634751915931702,
|
||||
0.33765655755996704,
|
||||
0.002130246954038739,
|
||||
-0.004022662527859211,
|
||||
-1.182201862335205
|
||||
]
|
||||
},
|
||||
"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": 8,
|
||||
"used_marker_ids": [
|
||||
96,
|
||||
97,
|
||||
47,
|
||||
54,
|
||||
62,
|
||||
85,
|
||||
105,
|
||||
57
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.003887444653024265,
|
||||
0.0002937877072128,
|
||||
0.0002404866802724321,
|
||||
0.00024048508666086002
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 0.49031638365688124,
|
||||
"residual_median_px": 0.5013350027616654,
|
||||
"residual_max_px": 0.6062763519311439,
|
||||
"sigma2_normalized": 9.253292293692929e-08
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.4343292713165283,
|
||||
-0.8994036912918091,
|
||||
-0.04930584505200386
|
||||
],
|
||||
[
|
||||
-0.29921603202819824,
|
||||
0.19569094479084015,
|
||||
-0.9339029788970947
|
||||
],
|
||||
[
|
||||
0.8496045470237732,
|
||||
-0.39086830615997314,
|
||||
-0.35411033034324646
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.03010912984609604,
|
||||
0.32548150420188904,
|
||||
0.2909383177757263
|
||||
],
|
||||
"rvec_rad": [
|
||||
1.1187772559005047,
|
||||
-1.8519635596120632,
|
||||
1.2365255960114159
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
-0.16287051141262054,
|
||||
0.022944524884223938,
|
||||
0.4055078625679016
|
||||
],
|
||||
"position_mm": [
|
||||
-162.87051391601562,
|
||||
22.94452476501465,
|
||||
405.50787353515625
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -132.17526245117188,
|
||||
"pitch": -58.16868209838867,
|
||||
"yaw": -145.43643188476562
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
1.446615004541035e-06,
|
||||
-2.3750243241143832e-07,
|
||||
5.813263188255589e-07,
|
||||
-8.863593861370775e-09,
|
||||
1.854753583118696e-07,
|
||||
2.483118756120673e-07
|
||||
],
|
||||
[
|
||||
-2.375024324114069e-07,
|
||||
1.6848056153960325e-06,
|
||||
4.5522840438908317e-07,
|
||||
-4.2882786235818766e-07,
|
||||
-7.198460805062003e-07,
|
||||
-6.031524743678258e-07
|
||||
],
|
||||
[
|
||||
5.813263188256213e-07,
|
||||
4.5522840438904717e-07,
|
||||
3.6098691381176423e-06,
|
||||
5.380515895797557e-07,
|
||||
-8.518199008797127e-07,
|
||||
-6.107991066544038e-07
|
||||
],
|
||||
[
|
||||
-8.86359386136767e-09,
|
||||
-4.2882786235819084e-07,
|
||||
5.380515895797531e-07,
|
||||
2.6838864216504313e-07,
|
||||
3.692099582285058e-08,
|
||||
6.359201722744009e-08
|
||||
],
|
||||
[
|
||||
1.8547535831184565e-07,
|
||||
-7.198460805061971e-07,
|
||||
-8.518199008797362e-07,
|
||||
3.692099582284795e-08,
|
||||
4.978035271371442e-07,
|
||||
4.941045438616945e-07
|
||||
],
|
||||
[
|
||||
2.483118756120473e-07,
|
||||
-6.031524743678251e-07,
|
||||
-6.107991066544345e-07,
|
||||
6.359201722743617e-08,
|
||||
4.941045438616975e-07,
|
||||
7.922266489950106e-07
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.06891267606855489,
|
||||
0.0743699574610443,
|
||||
0.10886000794288055
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0005180623921546932,
|
||||
0.0007055519308577818,
|
||||
0.0008900711482769288
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.000713244150286227,
|
||||
0.0010694082952597134,
|
||||
0.0011346670581527643
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
0.713244150286227,
|
||||
1.0694082952597133,
|
||||
1.1346670581527643
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.146033675708339,
|
||||
"pitch": 0.07444664358168214,
|
||||
"yaw": 0.10571491927224817
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
590.25,
|
||||
907.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
590.2493896484375,
|
||||
907.3632202148438
|
||||
],
|
||||
"reprojection_error_px": 0.1367811469334124,
|
||||
"confidence": 0.4047568547615518
|
||||
},
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
969.75,
|
||||
871.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
969.3050537109375,
|
||||
871.2380981445312
|
||||
],
|
||||
"reprojection_error_px": 0.4451054417933899,
|
||||
"confidence": 0.8722907081171155
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
797.0,
|
||||
872.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
797.1061401367188,
|
||||
872.037353515625
|
||||
],
|
||||
"reprojection_error_px": 0.47466566984270314,
|
||||
"confidence": 0.7759117848206195
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
873.75,
|
||||
847.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
874.1572875976562,
|
||||
847.6990966796875
|
||||
],
|
||||
"reprojection_error_px": 0.6062763519311439,
|
||||
"confidence": 0.7360188268067743
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
540.75,
|
||||
875.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
540.740966796875,
|
||||
875.5213623046875
|
||||
],
|
||||
"reprojection_error_px": 0.521440554145685,
|
||||
"confidence": 0.5424197481555236
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
691.5,
|
||||
715.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
691.7570190429688,
|
||||
714.4783325195312
|
||||
],
|
||||
"reprojection_error_px": 0.5815459987199515,
|
||||
"confidence": 0.3363291739196003
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
607.25,
|
||||
720.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
606.82470703125,
|
||||
719.9483642578125
|
||||
],
|
||||
"reprojection_error_px": 0.521400259141854,
|
||||
"confidence": 0.2839120657198857
|
||||
},
|
||||
{
|
||||
"marker_id": 57,
|
||||
"observed_center_px": [
|
||||
690.75,
|
||||
629.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
690.8707275390625,
|
||||
630.2158813476562
|
||||
],
|
||||
"reprojection_error_px": 0.48126974638147696,
|
||||
"confidence": 0.2029035186492315
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
test/homing/20260616_133151/cam0_debug.jpg
Normal file
BIN
test/homing/20260616_133151/cam0_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 194 KiB |
BIN
test/homing/20260616_133151/cam1.jpg
Normal file
BIN
test/homing/20260616_133151/cam1.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 80 KiB |
3123
test/homing/20260616_133151/cam1_aruco_detection.json
Normal file
3123
test/homing/20260616_133151/cam1_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user