diff --git a/data/evaluations/Scene8/scene_reconstruction.json b/data/evaluations/Scene8/scene_reconstruction.json new file mode 100644 index 0000000..b05bbb0 --- /dev/null +++ b/data/evaluations/Scene8/scene_reconstruction.json @@ -0,0 +1,3437 @@ +{ + "schema_version": "1.0", + "stage": "scene_reconstruct_A", + "created_utc": "2026-06-04T13:54:16Z", + "frame": "arbitrary S (reference camera pose = identity)", + "reference_camera": "g", + "summary": { + "num_cameras": 7, + "num_markers": 48, + "num_observations": 152, + "reproj_rms_px_init": 2.4156316717515254, + "reproj_median_px_init": 0.9773907044501946, + "reproj_rms_px_final": 2.466516859509576, + "reproj_median_px_final": 0.8550176666237796, + "dropped_cameras": [], + "insufficient_view_markers": [ + 50, + 52, + 57, + 61, + 65, + 68, + 73, + 80, + 81, + 83, + 87, + 90, + 101, + 113, + 122, + 124, + 205, + 207, + 211, + 215, + 219 + ] + }, + "cameras": [ + { + "camera_id": "a", + "world_to_camera": { + "rotation_matrix": [ + [ + -0.15590641579618744, + -0.8998171777547127, + 0.4074582630541813 + ], + [ + 0.5779840178412675, + 0.2514079760640594, + 0.7763559136706817 + ], + [ + -0.8010166444173629, + 0.35654323186525017, + 0.4808838312705377 + ] + ], + "translation_m": [ + -0.5360293065669756, + -1.0698115923760827, + 0.36500127818565087 + ], + "rvec_rad": [ + -0.3832136929286453, + 1.1031208731288538, + 1.3489674755494148 + ] + }, + "center_m": [ + 0.8271356936064381, + -0.34350794597903134, + 0.8734409133819786 + ], + "num_markers": 25, + "is_reference": false + }, + { + "camera_id": "b", + "world_to_camera": { + "rotation_matrix": [ + [ + -0.697767113992809, + -0.6525841409628496, + 0.29538956243226716 + ], + [ + 0.5604099975368583, + -0.24047287057414612, + 0.7925360768940231 + ], + [ + -0.4461632989062246, + 0.7185448750646057, + 0.5335087377236715 + ] + ], + "translation_m": [ + -0.5453599315857014, + -1.04628606140325, + 0.5416134518870686 + ], + "rvec_rad": [ + -0.12211239338052182, + 1.2238319215976818, + 2.001881490588979 + ] + }, + "center_m": [ + 0.44746298796989614, + -0.9966702252162738, + 0.7013575729221918 + ], + "num_markers": 21, + "is_reference": false + }, + { + "camera_id": "c", + "world_to_camera": { + "rotation_matrix": [ + [ + -0.7827251152280548, + -0.5649757536954338, + 0.2610436586617356 + ], + [ + 0.18579201904184733, + 0.18819557444613633, + 0.9643981291039726 + ], + [ + -0.5939888211503498, + 0.8033584651294622, + -0.0423374167049978 + ] + ], + "translation_m": [ + -0.4035083201704091, + -1.3993541122018798, + 0.8530716525725328 + ], + "rvec_rad": [ + -0.354459634329032, + 1.8819866649846453, + 1.6524927065955595 + ] + }, + "center_m": [ + 0.4508677547279763, + -0.6499424997673833, + 1.4909846259933626 + ], + "num_markers": 23, + "is_reference": false + }, + { + "camera_id": "d", + "world_to_camera": { + "rotation_matrix": [ + [ + -0.9854764710803342, + -0.15213069267086635, + 0.07544784486341391 + ], + [ + 0.05278469256762608, + 0.14786047052117324, + 0.987598631776998 + ], + [ + -0.16139981777434745, + 0.9772377057833677, + -0.13768284285875187 + ] + ], + "translation_m": [ + -0.18347288900731182, + -1.4415774617556525, + 1.0661650742352033 + ], + "rvec_rad": [ + -0.0986716381518911, + 2.255604072385204, + 1.9514990027376153 + ] + }, + "center_m": [ + 0.06736385663228682, + -0.8566562470350618, + 1.5843352012743213 + ], + "num_markers": 22, + "is_reference": false + }, + { + "camera_id": "e", + "world_to_camera": { + "rotation_matrix": [ + [ + -0.994442064470621, + 0.10504260560344103, + -0.007143627891286558 + ], + [ + 0.0051727372952398085, + 0.11651334050453277, + 0.9931756563032291 + ], + [ + 0.10515808670893392, + 0.9876186979257305, + -0.11640912467328945 + ] + ], + "translation_m": [ + -0.08928964835586116, + -1.5570558053568935, + 1.491289008573736 + ], + "rvec_rad": [ + -0.11330330126653441, + -2.28976971532128, + -2.0362912600160303 + ] + }, + "center_m": [ + -0.23756024048339613, + -1.2820279183277248, + 1.7193917174866538 + ], + "num_markers": 18, + "is_reference": false + }, + { + "camera_id": "f", + "world_to_camera": { + "rotation_matrix": [ + [ + -0.8951223482690926, + 0.41181001241005144, + -0.17078786639588456 + ], + [ + -0.4337763209444099, + -0.7160539053937425, + 0.5469048436047184 + ], + [ + 0.10292757170533845, + 0.5636304802343486, + 0.8195892853946056 + ] + ], + "translation_m": [ + 0.24103686585828027, + -0.7825466874618142, + 0.16265458968455132 + ], + "rvec_rad": [ + 0.05044383241385238, + -0.8255145077229474, + -2.5502536157942837 + ] + }, + "center_m": [ + -0.14043437961089367, + -0.7512840909265713, + 0.33583478683666645 + ], + "num_markers": 31, + "is_reference": false + }, + { + "camera_id": "g", + "world_to_camera": { + "rotation_matrix": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "translation_m": [ + 0.0, + 0.0, + 0.0 + ], + "rvec_rad": [ + 0.0, + 0.0, + 0.0 + ] + }, + "center_m": [ + 0.0, + 0.0, + 0.0 + ], + "num_markers": 33, + "is_reference": true + } + ], + "markers": [ + { + "marker_id": 40, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.8117966512960357, + -0.31591731842540705, + 0.4911032934764768 + ], + [ + 0.35417413231691997, + 0.935041881723338, + 0.01604255032206632 + ], + [ + -0.4642702671312254, + 0.1609127942153113, + 0.8709536105417564 + ] + ], + "translation_m": [ + -0.09221411326715057, + -0.10952679499816183, + 1.3343387582752304 + ], + "rvec_rad": [ + 0.07743362724777467, + 0.5106503460364887, + 0.35816610936982984 + ] + }, + "center_m": [ + -0.09221411326715057, + -0.10952679499816183, + 1.3343387582752304 + ], + "normal": [ + 0.4911032934764768, + 0.01604255032206632, + 0.8709536105417564 + ], + "corners_m": [ + [ + -0.1063105378886686, + -0.1022659481305816, + 1.3421535465420622 + ], + [ + -0.0860156216062677, + -0.0934115948226586, + 1.3305467898637815 + ], + [ + -0.07811768864563254, + -0.11678764186574206, + 1.3265239700083986 + ], + [ + -0.09841260492803343, + -0.12564199517366506, + 1.3381307266866793 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 41, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.9333083091309853, + -0.23429998649090997, + 0.27210129811785244 + ], + [ + -0.3510106685243259, + 0.43560472458545574, + -0.8288787815482636 + ], + [ + 0.07567767629338792, + -0.8691099126400456, + -0.4887953038457358 + ] + ], + "translation_m": [ + -0.08753804414307852, + -0.07462492200376963, + 1.2883080647045984 + ], + "rvec_rad": [ + -0.5246320599805188, + 2.5614524511256502, + -1.5219598324798085 + ] + }, + "center_m": [ + -0.08753804414307852, + -0.07462492200376963, + 1.2883080647045984 + ], + "normal": [ + 0.27210129811785244, + -0.8288787815482636, + -0.4887953038457358 + ], + "corners_m": [ + [ + -0.07880044011007757, + -0.06479222958989736, + 1.2764982198429304 + ], + [ + -0.1021331478383522, + -0.0735674963030055, + 1.278390161750265 + ], + [ + -0.09627564817607946, + -0.08445761441764191, + 1.3001179095662663 + ], + [ + -0.07294294044780483, + -0.07568234770453376, + 1.2982259676589316 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 43, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.34217877223929716, + 0.7776353832842666, + -0.5274437396473097 + ], + [ + 0.888105128994594, + 0.45098935325677814, + 0.08875743970242045 + ], + [ + 0.3068924366651985, + -0.4380545786923873, + -0.844940955572833 + ] + ], + "translation_m": [ + -0.05320966876876075, + -0.08821695812225866, + 1.2589205408419593 + ], + "rvec_rad": [ + -1.3912245118518478, + -2.203345594027068, + 0.2917325586430903 + ] + }, + "center_m": [ + -0.05320966876876075, + -0.08821695812225866, + 1.2589205408419593 + ], + "normal": [ + -0.5274437396473097, + 0.08875743970242045, + -0.844940955572833 + ], + "corners_m": [ + [ + -0.0392119918247162, + -0.09368090531898136, + 1.2496087031499894 + ], + [ + -0.04776646113069863, + -0.0714782770941165, + 1.2572810140666193 + ], + [ + -0.0672073457128053, + -0.08275301092553596, + 1.2682323785339291 + ], + [ + -0.058652876406822874, + -0.10495563915040082, + 1.2605600676172992 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 44, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.045292003848452445, + 0.9141616982676131, + 0.40281139979879566 + ], + [ + -0.1406437287079098, + 0.40504417764517475, + -0.9034149410602417 + ], + [ + -0.9890237489375711, + -0.01573542424650526, + 0.14691637233901075 + ] + ], + "translation_m": [ + -0.07848131702012896, + -0.06831954394672347, + 1.3454082820288753 + ], + "rvec_rad": [ + 0.8036378238200074, + 1.2600621607915212, + -0.9549409689330637 + ] + }, + "center_m": [ + -0.07848131702012896, + -0.06831954394672347, + 1.3454082820288753 + ], + "normal": [ + 0.40281139979879566, + -0.9034149410602417, + 0.14691637233901075 + ], + "corners_m": [ + [ + -0.06762044583988945, + -0.06149844511730992, + 1.3575743860875136 + ], + [ + -0.06648814574367815, + -0.06501453833500766, + 1.3328487923640744 + ], + [ + -0.08934218820036847, + -0.07514064277613704, + 1.333242177970237 + ], + [ + -0.09047448829657978, + -0.07162454955843929, + 1.3579677716936762 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 45, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.18041664216553366, + 0.18358276832438652, + 0.9663059569329313 + ], + [ + -0.6414584500981908, + 0.7667204642554457, + -0.025899546126142092 + ], + [ + -0.7456412622886024, + -0.6151724123091573, + 0.25608984967466275 + ] + ], + "translation_m": [ + -0.04768533711259222, + -0.10252334626978926, + 1.3121336774987173 + ], + "rvec_rad": [ + -0.4350749995214797, + 1.2639737519363072, + -0.6091487124386283 + ] + }, + "center_m": [ + -0.04768533711259222, + -0.10252334626978926, + 1.3121336774987173 + ], + "normal": [ + 0.9663059569329313, + -0.025899546126142092, + 0.25608984967466275 + ], + "corners_m": [ + [ + -0.04764576053560656, + -0.0849211098403688, + 1.3137645381234604 + ], + [ + -0.04313534448146822, + -0.10095757109282358, + 1.2951235065662454 + ], + [ + -0.04772491368957788, + -0.12012558269920973, + 1.3105028168739743 + ], + [ + -0.052235329743716225, + -0.10408912144675495, + 1.3291438484311893 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 46, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7010490938729763, + 0.7128272542121773, + 0.02018597612716666 + ], + [ + 0.6381838409822694, + 0.6397670009000507, + -0.4282751097933095 + ], + [ + -0.3182004919685011, + -0.2873595138701375, + -0.9034229113207846 + ] + ], + "translation_m": [ + -0.5451764487749671, + 0.12482147590884117, + 1.6395143037405011 + ], + "rvec_rad": [ + 1.1125666495394495, + 2.671652463961071, + -0.5893298866128667 + ] + }, + "center_m": [ + -0.5451764487749671, + 0.12482147590884117, + 1.6395143037405011 + ], + "normal": [ + 0.02018597612716666, + -0.4282751097933095, + -0.9034229113207846 + ], + "corners_m": [ + [ + -0.5275029944239027, + 0.12484126540781344, + 1.6398998159667306 + ], + [ + -0.5450292217707271, + 0.14079586143237016, + 1.6319448036675182 + ], + [ + -0.5628499031260314, + 0.1248016864098689, + 1.6391287915142716 + ], + [ + -0.545323675779207, + 0.10884709038531216, + 1.647083803813484 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 47, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7076114822495236, + 0.7024490950028647, + -0.07649352337478292 + ], + [ + 0.6720576909904644, + 0.6356286827124814, + -0.3798876645717262 + ], + [ + -0.2182302686824112, + -0.3202209341108907, + -0.9218644711604581 + ] + ], + "translation_m": [ + -0.04217509247454117, + -0.068736158330749, + 1.7240062369659797 + ], + "rvec_rad": [ + 1.1659193011924396, + 2.7696105646484415, + -0.593864021955309 + ] + }, + "center_m": [ + -0.04217509247454117, + -0.068736158330749, + 1.7240062369659797 + ], + "normal": [ + -0.07649352337478292, + -0.3798876645717262, + -0.9218644711604581 + ], + "corners_m": [ + [ + -0.024549335258886314, + -0.06919152093422379, + 1.7227313536481237 + ], + [ + -0.0422396223151244, + -0.05239007865946218, + 1.7172755969310634 + ], + [ + -0.05980084969019602, + -0.06828079572727422, + 1.7252811202838356 + ], + [ + -0.04211056263395793, + -0.08508223800203582, + 1.730736877000896 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 48, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7202619367809038, + 0.6925935974443418, + -0.039202693832517066 + ], + [ + 0.6459926697791223, + 0.6490582306357257, + -0.40176720104516084 + ], + [ + -0.25281656001192454, + -0.3147022752112315, + -0.9149023253662716 + ] + ], + "translation_m": [ + -0.2786590475927004, + -0.3236214798144746, + 1.8373341028838286 + ], + "rvec_rad": [ + 1.1186324373600305, + 2.7445655927389407, + -0.5987406386449637 + ] + }, + "center_m": [ + -0.2786590475927004, + -0.3236214798144746, + 1.8373341028838286 + ], + "normal": [ + -0.039202693832517066, + -0.40176720104516084, + -0.9149023253662716 + ], + "corners_m": [ + [ + -0.2609983534148848, + -0.32358316030376705, + 1.8365605314438374 + ], + [ + -0.2790049018344074, + -0.307433343559289, + 1.8302401174435392 + ], + [ + -0.29631974177051595, + -0.32365979932518213, + 1.8381076743238198 + ], + [ + -0.27831319335099336, + -0.3398096160696602, + 1.844428088324118 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 51, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.724874363267463, + 0.6887617777555675, + -0.012820724651133653 + ], + [ + 0.6335046206335146, + 0.6591782645793521, + -0.4051615864592044 + ], + [ + -0.2706086715417237, + -0.30181323531141413, + -0.9141551935406156 + ] + ], + "translation_m": [ + 0.0027595318690011965, + 0.13251569413706155, + 1.6344227097177537 + ], + "rvec_rad": [ + 1.094710582655172, + 2.7306017987507314, + -0.5853077866955831 + ] + }, + "center_m": [ + 0.0027595318690011965, + 0.13251569413706155, + 1.6344227097177537 + ], + "normal": [ + -0.012820724651133653, + -0.4051615864592044, + -0.9141551935406156 + ], + "corners_m": [ + [ + 0.020429983631789078, + 0.13283661468638452, + 1.6340326526706326 + ], + [ + 0.002308124550102503, + 0.14867423020222237, + 1.6272674358820896 + ], + [ + -0.014910919893786685, + 0.13219477358773857, + 1.6348127667648749 + ], + [ + 0.00321093918789989, + 0.11635715807190071, + 1.6415779835534179 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 53, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7138778300055241, + 0.6950162787056101, + 0.08562018547521291 + ], + [ + 0.5975224138510827, + 0.6683210647752829, + -0.44307326631518557 + ], + [ + -0.36516490627137155, + -0.2651402019910177, + -0.8923873959867333 + ] + ], + "translation_m": [ + -0.5342391244784079, + 0.1789649552102978, + 1.6152290759927392 + ], + "rvec_rad": [ + 1.0408877604411158, + 2.6370404307403006, + -0.570327786072947 + ] + }, + "center_m": [ + -0.5342391244784079, + 0.1789649552102978, + 1.6152290759927392 + ], + "normal": [ + 0.08562018547521291, + -0.44307326631518557, + -0.8923873959867333 + ], + "corners_m": [ + [ + -0.5166279481195187, + 0.17984993834685029, + 1.6164793847962438 + ], + [ + -0.5344748938696569, + 0.19478799869312735, + 1.6073502621394593 + ], + [ + -0.5518503008372971, + 0.1780799720737453, + 1.6139787671892347 + ], + [ + -0.534003355087159, + 0.16314191172746823, + 1.6231078898460192 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 54, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6967609509697401, + 0.7162859419120655, + -0.03819197065992386 + ], + [ + 0.6595992198169542, + 0.6188776186556734, + -0.42652006089274197 + ], + [ + -0.2818741677071682, + -0.3223739172858562, + -0.9036714065594657 + ] + ], + "translation_m": [ + -0.009323192990838015, + -0.09321876680502098, + 1.7347044196469243 + ], + "rvec_rad": [ + 1.1550915935498423, + 2.7026949588214775, + -0.6287160896241756 + ] + }, + "center_m": [ + -0.009323192990838015, + -0.09321876680502098, + 1.7347044196469243 + ], + "normal": [ + -0.03819197065992386, + -0.42652006089274197, + -0.9036714065594657 + ], + "corners_m": [ + [ + 0.008339893170184557, + -0.09372778681953699, + 1.7341981727771907 + ], + [ + -0.009079130604058948, + -0.07723780632411313, + 1.7271513185845115 + ], + [ + -0.02698627915186059, + -0.09270974679050496, + 1.735210666516658 + ], + [ + -0.009567255377617082, + -0.10919972728592882, + 1.7422575207093371 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 55, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7364910233837682, + 0.6751803017601002, + -0.04138275716128323 + ], + [ + 0.6301928593980793, + 0.6626122944991448, + -0.40472448300325403 + ], + [ + -0.24584127488849886, + -0.32415506674077843, + -0.9135018118577976 + ] + ], + "translation_m": [ + -0.018183107826582758, + -0.008831147529532395, + 1.6971306819575418 + ], + "rvec_rad": [ + 1.0880116336344536, + 2.7610134986974226, + -0.6075116704070587 + ] + }, + "center_m": [ + -0.018183107826582758, + -0.008831147529532395, + 1.6971306819575418 + ], + "normal": [ + -0.04138275716128323, + -0.40472448300325403, + -0.9135018118577976 + ], + "corners_m": [ + [ + -0.0005372162622844026, + -0.008425904590769076, + 1.6961517595593882 + ], + [ + -0.018949491846878608, + 0.0073289168941829096, + 1.6900057276871758 + ], + [ + -0.03582899939088111, + -0.009236390468295714, + 1.6981096043556954 + ], + [ + -0.017416723806286907, + -0.0249912119532477, + 1.7042556362279078 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 56, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7148370369458081, + 0.6989166786747144, + 0.02287983568217697 + ], + [ + 0.6244908595113151, + 0.6527551591098101, + -0.4288611297871905 + ], + [ + -0.31467312722467444, + -0.2922775710276665, + -0.9030806411815122 + ] + ], + "translation_m": [ + -0.5059710081309676, + 0.1364290554714553, + 1.6341767570271635 + ], + "rvec_rad": [ + 1.0858102241157388, + 2.6834742163229994, + -0.5916694228775384 + ] + }, + "center_m": [ + -0.5059710081309676, + 0.1364290554714553, + 1.6341767570271635 + ], + "normal": [ + 0.02287983568217697, + -0.4288611297871905, + -0.9030806411815122 + ], + "corners_m": [ + [ + -0.4882990866857111, + 0.1367823592164365, + 1.634456701479626 + ], + [ + -0.5061700126093563, + 0.15239463070421938, + 1.6265898732990092 + ], + [ + -0.5236429295762242, + 0.13607575172647413, + 1.6338968125747009 + ], + [ + -0.5057720036525789, + 0.12046348023869125, + 1.6417636407553178 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 58, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6871527672626851, + 0.726379020344412, + -0.013949668337498417 + ], + [ + 0.6643033249129555, + 0.6204236932434062, + -0.4168639266856794 + ], + [ + -0.2941465059333973, + -0.29571601185208685, + -0.9088618560493505 + ] + ], + "translation_m": [ + 0.12747127599027963, + 0.1848440195377404, + 1.6094126472415267 + ], + "rvec_rad": [ + 1.160948120249955, + 2.6850977365459507, + -0.5948650624597887 + ] + }, + "center_m": [ + 0.12747127599027963, + 0.1848440195377404, + 1.6094126472415267 + ], + "normal": [ + -0.013949668337498417, + -0.4168639266856794, + -0.9088618560493505 + ], + "corners_m": [ + [ + 0.14514042333536836, + 0.18429552414187103, + 1.609393028417543 + ], + [ + 0.12796160415380123, + 0.20090310726469493, + 1.602039365769208 + ], + [ + 0.10980212864519091, + 0.18539251493360978, + 1.6094322660655103 + ], + [ + 0.12698094782675803, + 0.16878493181078588, + 1.6167859287138453 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 59, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6919512072645708, + 0.7216007083561577, + -0.02226981061873108 + ], + [ + 0.6720311766385212, + 0.6325347053909883, + -0.38506355852220064 + ], + [ + -0.26377570849293064, + -0.2814112012266514, + -0.9226213261318131 + ] + ], + "translation_m": [ + -0.26538639422385363, + -0.25570197061664496, + 1.807590293653906 + ], + "rvec_rad": [ + 1.1655965242347532, + 2.7157938563973922, + -0.5574217064188496 + ] + }, + "center_m": [ + -0.26538639422385363, + -0.25570197061664496, + 1.807590293653906 + ], + "normal": [ + -0.02226981061873108, + -0.38506355852220064, + -0.9226213261318131 + ], + "corners_m": [ + [ + -0.24771699527859453, + -0.2561956765072391, + 1.8073698499947346 + ], + [ + -0.26501577546020877, + -0.2393948970912761, + 1.8007754572824113 + ], + [ + -0.2830557931691127, + -0.2552082647260508, + 1.8078107373130776 + ], + [ + -0.2657570129874985, + -0.2720090441420138, + 1.814405130025401 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 62, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6832377272936132, + 0.7297738811312378, + -0.02482519730653837 + ], + [ + 0.6776055443490732, + 0.6209939070789963, + -0.39397626024693355 + ], + [ + -0.27209728824458096, + -0.2860011359932221, + -0.9187852937115025 + ] + ], + "translation_m": [ + -0.1739827430796758, + -0.02834107030026324, + 1.7080406730977846 + ], + "rvec_rad": [ + 1.1801734077080246, + 2.7026960905155626, + -0.5702024815456567 + ] + }, + "center_m": [ + -0.1739827430796758, + -0.02834107030026324, + 1.7080406730977846 + ], + "normal": [ + -0.02482519730653837, + -0.39397626024693355, + -0.9187852937115025 + ], + "corners_m": [ + [ + -0.15632009797436514, + -0.029048715766139203, + 1.7078668750009267 + ], + [ + -0.1734010411567055, + -0.012108577157412374, + 1.701064442794812 + ], + [ + -0.19164538818498644, + -0.027633424834387278, + 1.7082144711946425 + ], + [ + -0.1745644450026461, + -0.04457356344311411, + 1.7150169034007572 + ] + ], + "num_cameras": 5 + }, + { + "marker_id": 63, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.695400969545243, + 0.7180057899863815, + -0.02975192601443366 + ], + [ + 0.6728132168870146, + 0.6359690405553814, + -0.3779758651517527 + ], + [ + -0.2524675558119961, + -0.2828622721415865, + -0.925337272707209 + ] + ], + "translation_m": [ + -0.4087260190369339, + -0.32898064329775195, + 1.8432145157433344 + ], + "rvec_rad": [ + 1.1652348066760825, + 2.728484915959026, + -0.5536533476661596 + ] + }, + "center_m": [ + -0.4087260190369339, + -0.32898064329775195, + 1.8432145157433344 + ], + "normal": [ + -0.02975192601443366, + -0.3779758651517527, + -0.925337272707209 + ], + "corners_m": [ + [ + -0.3910584345427886, + -0.3294411955018974, + 1.8428345817892144 + ], + [ + -0.4084434587814197, + -0.312620865079722, + 1.8365228928939146 + ], + [ + -0.4263936035310792, + -0.3285200910936065, + 1.8435944496974543 + ], + [ + -0.4090085792924481, + -0.3453404215157819, + 1.8499061385927542 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 64, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7140552901156776, + 0.7000861734702845, + 0.0020957990243052893 + ], + [ + 0.6364829220798455, + 0.6504238968006518, + -0.41453376746817144 + ], + [ + -0.2915725168093125, + -0.2946660893051481, + -0.9100315177262234 + ] + ], + "translation_m": [ + 0.15717995738503657, + 0.24889823176565049, + 1.5797132810791037 + ], + "rvec_rad": [ + 1.1038541900250087, + 2.70437373842889, + -0.5857184907766741 + ] + }, + "center_m": [ + 0.15717995738503657, + 0.24889823176565049, + 1.5797132810791037 + ], + "normal": [ + 0.0020957990243052893, + -0.41453376746817144, + -0.9100315177262234 + ], + "corners_m": [ + [ + 0.1748567256798611, + 0.24907249394966055, + 1.5796746114229057 + ], + [ + 0.15700534342696915, + 0.2649845670016567, + 1.572385298502673 + ], + [ + 0.13950318909021203, + 0.24872396958164042, + 1.5797519507353017 + ], + [ + 0.157354571343104, + 0.23281189652964426, + 1.5870412636555344 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 66, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7235774117761768, + 0.6772170869467811, + -0.1334644009262574 + ], + [ + 0.6544335061819633, + 0.611620462200025, + -0.44456405185823283 + ], + [ + -0.2194368135789232, + -0.4090200818609561, + -0.8857483036849608 + ] + ], + "translation_m": [ + 0.11681587547251061, + -0.028050421476265075, + 1.7067726568222987 + ], + "rvec_rad": [ + 1.1480721898145643, + 2.7769136668005827, + -0.7359109143544106 + ] + }, + "center_m": [ + 0.11681587547251061, + -0.028050421476265075, + 1.7067726568222987 + ], + "normal": [ + -0.1334644009262574, + -0.44456405185823283, + -0.8857483036849608 + ], + "corners_m": [ + [ + 0.1343258067065476, + -0.028585584526039303, + 1.7044028659687733 + ], + [ + 0.11623637141214317, + -0.012224746871490222, + 1.6989169456293003 + ], + [ + 0.09930594423847364, + -0.027515258426490847, + 1.7091424476758241 + ], + [ + 0.11739537953287806, + -0.04387609608103993, + 1.7146283680152972 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 69, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7188980196669104, + 0.6949376059368287, + -0.01572771991687688 + ], + [ + 0.637507449503972, + 0.6501313721811166, + -0.4134168002547049 + ], + [ + -0.2770737972922293, + -0.30723105761085895, + -0.91040605670952 + ] + ], + "translation_m": [ + 0.20515279224579902, + 0.17129726399972234, + 1.61529946506907 + ], + "rvec_rad": [ + 1.1055080736631973, + 2.7208944568737112, + -0.5979098514309188 + ] + }, + "center_m": [ + 0.20515279224579902, + 0.17129726399972234, + 1.61529946506907 + ], + "normal": [ + -0.01572771991687688, + -0.4134168002547049, + -0.91040605670952 + ], + "corners_m": [ + [ + 0.22282573756584576, + 0.17145506303318664, + 1.614922499315087 + ], + [ + 0.204853287074173, + 0.18739274927078595, + 1.6079956543827814 + ], + [ + 0.18747984692575229, + 0.17113946496625804, + 1.615676430823053 + ], + [ + 0.20545229741742504, + 0.15520177872865873, + 1.6226032757553586 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 71, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7541336647103926, + 0.6563775465959361, + 0.021235161293782084 + ], + [ + 0.5966836429423072, + 0.6983393254597462, + -0.39533633372297505 + ], + [ + -0.27431924102324734, + -0.2854657647444365, + -0.9182909403687393 + ] + ], + "translation_m": [ + -0.35368978053522115, + -0.34636811595009087, + 1.8518391574488648 + ], + "rvec_rad": [ + 1.0203979681312147, + 2.7448944189543765, + -0.554393579320423 + ] + }, + "center_m": [ + -0.35368978053522115, + -0.34636811595009087, + 1.8518391574488648 + ], + "normal": [ + 0.021235161293782084, + -0.39533633372297505, + -0.9182909403687393 + ], + "corners_m": [ + [ + -0.33605839039389207, + -0.34509741991862286, + 1.85169982590235 + ], + [ + -0.3549117320116519, + -0.3301803288450652, + 1.8448418448767687 + ], + [ + -0.37132117067655024, + -0.3476388119815589, + 1.8519784889953796 + ], + [ + -0.3524678290587904, + -0.36255590305511654, + 1.858836470020961 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 72, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6709713007501998, + 0.7412538139493136, + -0.018447137316700973 + ], + [ + 0.6626047744265674, + 0.5882420593222732, + -0.4636013293244631 + ], + [ + -0.3327948714699531, + -0.3232863482269054, + -0.8858518558841594 + ] + ], + "translation_m": [ + -0.4827925049493476, + 0.19658294078812888, + 1.6079563205668883 + ], + "rvec_rad": [ + 1.1778354913111126, + 2.6387055395140218, + -0.66019771647201 + ] + }, + "center_m": [ + -0.4827925049493476, + 0.19658294078812888, + 1.6079563205668883 + ], + "normal": [ + -0.018447137316700973, + -0.4636013293244631, + -0.8858518558841594 + ], + "corners_m": [ + [ + -0.4651396910156037, + 0.1956534068493252, + 1.6080751771074264 + ], + [ + -0.48191397353435866, + 0.21221852620998938, + 1.5997553053206777 + ], + [ + -0.5004453188830915, + 0.19751247472693256, + 1.6078374640263502 + ], + [ + -0.48367103636433656, + 0.18094735536626838, + 1.616157335813099 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 79, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7328153664878876, + 0.6804230376326247, + -0.002475580338963196 + ], + [ + 0.6191393191678832, + 0.6652949560080529, + -0.4171919521881653 + ], + [ + -0.2822200242710513, + -0.30725740246417876, + -0.9088150232755909 + ] + ], + "translation_m": [ + -0.11634153766219738, + 0.04314359507152759, + 1.6752999422331982 + ], + "rvec_rad": [ + 1.0706979808482873, + 2.7245466691192664, + -0.5968674432543459 + ] + }, + "center_m": [ + -0.11634153766219738, + 0.04314359507152759, + 1.6752999422331982 + ], + "normal": [ + -0.002475580338963196, + -0.4171919521881653, + -0.9088150232755909 + ], + "corners_m": [ + [ + -0.09867605761069097, + 0.04372054053202971, + 1.6749869750057842 + ], + [ + -0.11699644177288816, + 0.059199023511226787, + 1.667931474399008 + ], + [ + -0.13400701771370377, + 0.04256664961102547, + 1.6756129094606123 + ], + [ + -0.11568663355150659, + 0.02708816663182839, + 1.6826684100673885 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 82, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6933577892018168, + 0.7205136294879587, + -0.010727808502085906 + ], + [ + 0.6631258054395003, + 0.6321655108854224, + -0.40078789029508743 + ], + [ + -0.2819913869489332, + -0.2850032922074085, + -0.9161080619214939 + ] + ], + "translation_m": [ + -0.3876324340879457, + 0.4125149574768814, + 1.503442177949539 + ], + "rvec_rad": [ + 1.1524768860560468, + 2.7000569104373002, + -0.5712170862899757 + ] + }, + "center_m": [ + -0.3876324340879457, + 0.4125149574768814, + 1.503442177949539 + ], + "normal": [ + -0.010727808502085906, + -0.40078789029508743, + -0.9161080619214939 + ], + "corners_m": [ + [ + -0.3699590413543235, + 0.4121279537949554, + 1.503404529133808 + ], + [ + -0.3872929860843689, + 0.4287060989309429, + 1.4963547444600849 + ], + [ + -0.40530582682156785, + 0.41290196115880734, + 1.50347982676527 + ], + [ + -0.38797188209152245, + 0.39632381602281985, + 1.5105296114389932 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 85, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7342255975250167, + 0.6779495007816598, + -0.036017305964358676 + ], + [ + 0.6204601940818848, + 0.6485384669759424, + -0.4409387762630594 + ], + [ + -0.27557561484808846, + -0.3460958411226471, + -0.8968142222660753 + ] + ], + "translation_m": [ + -0.1447902283923301, + -0.19838929160926008, + 1.7803781877942575 + ], + "rvec_rad": [ + 1.0811296869565972, + 2.73076322562019, + -0.6553297413630573 + ] + }, + "center_m": [ + -0.1447902283923301, + -0.19838929160926008, + 1.7803781877942575 + ], + "normal": [ + -0.036017305964358676, + -0.4409387762630594, + -0.8968142222660753 + ], + "corners_m": [ + [ + -0.12713803966349665, + -0.19803831319808438, + 1.7794966849658256 + ], + [ + -0.14549367960162207, + -0.18252680834603724, + 1.7726072945946234 + ], + [ + -0.16244241712116356, + -0.1987402700204358, + 1.7812596906226894 + ], + [ + -0.14408677718303814, + -0.21425177487248293, + 1.7881490809938916 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 86, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6949427868329084, + 0.7190459663780061, + -0.005236531717662363 + ], + [ + 0.6578866664347215, + 0.6328604432190641, + -0.40825579424666975 + ], + [ + -0.2902406883197466, + -0.2871594637898757, + -0.9128525539207253 + ] + ], + "translation_m": [ + -0.4989466150022749, + 0.32168644945011726, + 1.5486420977626627 + ], + "rvec_rad": [ + 1.144458618151732, + 2.6935206211611606, + -0.5780050281968881 + ] + }, + "center_m": [ + -0.4989466150022749, + 0.32168644945011726, + 1.5486420977626627 + ], + "normal": [ + -0.005236531717662363, + -0.40825579424666975, + -0.9128525539207253 + ], + "corners_m": [ + [ + -0.4812717555871385, + 0.32137362165992156, + 1.548680613069286 + ], + [ + -0.4986453252579612, + 0.3378207883207896, + 1.5414245958612924 + ], + [ + -0.5166214744174114, + 0.32199927724031296, + 1.5486035824560394 + ], + [ + -0.4992479047465886, + 0.3055521105794449, + 1.555859599664033 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 92, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7335173347785275, + 0.6785095892615056, + -0.039712175205955144 + ], + [ + 0.640131289447982, + 0.6700271864740147, + -0.3758929390868893 + ], + [ + -0.22843872668413612, + -0.3011449927624547, + -0.9258117743283629 + ] + ], + "translation_m": [ + -0.34886056963627543, + -0.2068474746514125, + 1.7891329330666437 + ], + "rvec_rad": [ + 1.0992643689840556, + 2.775465865252569, + -0.5644020953309183 + ] + }, + "center_m": [ + -0.34886056963627543, + -0.2068474746514125, + 1.7891329330666437 + ], + "normal": [ + -0.039712175205955144, + -0.3758929390868893, + -0.9258117743283629 + ], + "corners_m": [ + [ + -0.331210233085775, + -0.2064737759385871, + 1.7882241047406646 + ], + [ + -0.34954816645523823, + -0.19047049370238756, + 1.7825131365735614 + ], + [ + -0.36651090618677584, + -0.20722117336423793, + 1.7900417613926227 + ], + [ + -0.34817297281731263, + -0.22322445560043747, + 1.795752729559726 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 95, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.666197649124675, + 0.7455674386995158, + -0.017603597694794276 + ], + [ + 0.685372868694678, + 0.6027640858685741, + -0.408582289929765 + ], + [ + -0.29401483492841696, + -0.28426158927661405, + -0.9125517112492582 + ] + ], + "translation_m": [ + 0.06737977807341616, + 0.04779000085075359, + 1.6722738042609337 + ], + "rvec_rad": [ + 1.201546146295701, + 2.671484757936556, + -0.5817740186248412 + ] + }, + "center_m": [ + 0.06737977807341616, + 0.04779000085075359, + 1.6722738042609337 + ], + "normal": [ + -0.017603597694794276, + -0.408582289929765, + -0.9125517112492582 + ], + "corners_m": [ + [ + 0.08502684167121854, + 0.0467573910654273, + 1.6723957198315813 + ], + [ + 0.06837190044310168, + 0.06389171278279424, + 1.6650453489583708 + ], + [ + 0.04973271447561378, + 0.048822610636079886, + 1.6721518886902862 + ], + [ + 0.06638765570373065, + 0.03168828891871294, + 1.6795022595634967 + ] + ], + "num_cameras": 5 + }, + { + "marker_id": 96, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7200068283829704, + 0.6939626657076123, + 0.0024465661406447015 + ], + [ + 0.6270070357206603, + 0.6520411868689745, + -0.42626807033050157 + ], + [ + -0.29740938828269425, + -0.30538190717605324, + -0.9045935808587358 + ] + ], + "translation_m": [ + -0.14274924787597068, + -0.015335225311137173, + 1.7016118041285908 + ], + "rvec_rad": [ + 1.08953114841537, + 2.7025624261450107, + -0.6034623196647813 + ] + }, + "center_m": [ + -0.14274924787597068, + -0.015335225311137173, + 1.7016118041285908 + ], + "normal": [ + 0.0024465661406447015, + -0.42626807033050157, + -0.9045935808587358 + ], + "corners_m": [ + [ + -0.1250746291998384, + -0.015022298421783245, + 1.7015121476424238 + ], + [ + -0.14307479990941266, + 0.0006528774712332638, + 1.6940769129353566 + ], + [ + -0.16042386655210297, + -0.0156481522004911, + 1.7017114606147579 + ], + [ + -0.1424236958425287, + -0.03132332809350761, + 1.709146695321825 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 97, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7243540172648056, + 0.6892456851365386, + -0.01586326552135678 + ], + [ + 0.6321590080130549, + 0.6548260693839462, + -0.4142195160094725 + ], + [ + -0.2751113342999304, + -0.31006967664677343, + -0.9100387625611764 + ] + ], + "translation_m": [ + 0.03941872078195951, + -0.09154998948208347, + 1.7336897134987739 + ], + "rvec_rad": [ + 1.0951644255163204, + 2.726065292538384, + -0.6002822312462143 + ] + }, + "center_m": [ + 0.03941872078195951, + -0.09154998948208347, + 1.7336897134987739 + ], + "normal": [ + -0.01586326552135678, + -0.4142195160094725, + -0.9100387625611764 + ], + "corners_m": [ + [ + 0.05708871706197631, + -0.09126665121494733, + 1.7332527342194384 + ], + [ + 0.03897986663035617, + -0.07546267601462095, + 1.7263749508619401 + ], + [ + 0.021748724501942703, + -0.09183332774921961, + 1.7341266927781094 + ], + [ + 0.039857574933562845, + -0.10763730294954599, + 1.7410044761356076 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 102, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7603016482478879, + 0.6489479192766273, + -0.028425371379822775 + ], + [ + 0.6065137954438019, + 0.693558594607862, + -0.3887383333065064 + ], + [ + -0.23255627191692496, + -0.31279877543254964, + -0.9209096082026719 + ] + ], + "translation_m": [ + -0.32401627437280844, + -0.23539325926723667, + 1.8008546708916142 + ], + "rvec_rad": [ + 1.0371106665912322, + 2.7878267961381833, + -0.5795251340220072 + ] + }, + "center_m": [ + -0.32401627437280844, + -0.23539325926723667, + 1.8008546708916142 + ], + "normal": [ + -0.028425371379822775, + -0.3887383333065064, + -0.9209096082026719 + ], + "corners_m": [ + [ + -0.306400654778752, + -0.23430519927768592, + 1.799851639597669 + ], + [ + -0.3254081959849492, + -0.21914235439159088, + 1.7940377327997459 + ], + [ + -0.3416318939668649, + -0.23648131925678742, + 1.8018577021855595 + ], + [ + -0.3226243527606677, + -0.2516441641428825, + 1.8076716089834826 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 103, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.685911988127566, + 0.7274818625136262, + -0.01717219428596503 + ], + [ + 0.6627188286902944, + 0.6147551109798787, + -0.4276446043427663 + ], + [ + -0.300546999057142, + -0.3047068972600101, + -0.9037838282022554 + ] + ], + "translation_m": [ + 0.06440046000681708, + 0.1633602383278899, + 1.6196494545611182 + ], + "rvec_rad": [ + 1.1620020709927052, + 2.678446815260702, + -0.6121374898900958 + ] + }, + "center_m": [ + 0.06440046000681708, + 0.1633602383278899, + 1.6196494545611182 + ], + "normal": [ + -0.01717219428596503, + -0.4276446043427663, + -0.9037838282022554 + ], + "corners_m": [ + [ + 0.08206788313983199, + 0.1627606918565097, + 1.6195974558335824 + ], + [ + 0.06492008343664284, + 0.17932866257376706, + 1.6120837808571538 + ], + [ + 0.046733036873802175, + 0.1639597847992701, + 1.619701453288654 + ], + [ + 0.06388083657699133, + 0.14739181408201274, + 1.6272151282650826 + ] + ], + "num_cameras": 6 + }, + { + "marker_id": 105, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6896790257500767, + 0.7223831366032202, + -0.050054424297144365 + ], + [ + 0.6877193798775335, + 0.6318078716249738, + -0.3575763805029377 + ], + [ + -0.22668236804234648, + -0.2810363273742639, + -0.9325415201022953 + ] + ], + "translation_m": [ + -0.19192166508915712, + -0.17865059218820728, + 1.7722486059441027 + ], + "rvec_rad": [ + 1.1910340995389863, + 2.748494354692045, + -0.5394001518265172 + ] + }, + "center_m": [ + -0.19192166508915712, + -0.17865059218820728, + 1.7722486059441027 + ], + "normal": [ + -0.050054424297144365, + -0.3575763805029377, + -0.9325415201022953 + ], + "corners_m": [ + [ + -0.1742708880597409, + -0.17934948604136428, + 1.7715691814524537 + ], + [ + -0.19151286370349282, + -0.16215650154442593, + 1.765902122251395 + ], + [ + -0.20957244211857334, + -0.17795169833505028, + 1.7729280304357518 + ], + [ + -0.1923304664748214, + -0.19514468283198863, + 1.7785950896368103 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 114, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.943124031898827, + 0.03484642667247523, + -0.3306097200674235 + ], + [ + -0.32048993236311224, + 0.35955996246809213, + -0.8763577104378278 + ], + [ + 0.08833608384339921, + 0.9324711040767449, + 0.3502775704396557 + ] + ], + "translation_m": [ + -0.11001399481032256, + -0.038155480516225686, + 1.404241560116225 + ], + "rvec_rad": [ + 1.1847841786694777, + -0.27440980385239727, + -0.23274557156165282 + ] + }, + "center_m": [ + -0.11001399481032256, + -0.038155480516225686, + 1.404241560116225 + ], + "normal": [ + -0.3306097200674235, + -0.8763577104378278, + 0.3502775704396557 + ], + "corners_m": [ + [ + -0.12136746487565196, + -0.02965485683083563, + 1.4147932478691418 + ], + [ + -0.09778936407818128, + -0.037667105139913434, + 1.417001649965227 + ], + [ + -0.09866052474499316, + -0.04665610420161574, + 1.3936898723633082 + ], + [ + -0.12223862554246384, + -0.03864385589253794, + 1.3914814702672231 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 115, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.3403735371155999, + 0.05487384769441184, + 0.9386877628213899 + ], + [ + 0.8461012024712138, + 0.4176238129044416, + -0.3312145921784277 + ], + [ + -0.4101933817215748, + 0.9069615271522637, + 0.09571926586352417 + ] + ], + "translation_m": [ + -0.05603427141279365, + -0.02017481933303706, + 1.3939980027875902 + ], + "rvec_rad": [ + 1.020516261454259, + 1.1117603717952815, + 0.6521369371052362 + ] + }, + "center_m": [ + -0.05603427141279365, + -0.02017481933303706, + 1.3939980027875902 + ], + "normal": [ + 0.9386877628213899, + -0.3312145921784277, + 0.09571926586352417 + ], + "corners_m": [ + [ + -0.0596030175305585, + -0.025530786702621714, + 1.4104624391485132 + ], + [ + -0.0510936791026685, + -0.004378256640841367, + 1.4002076046054739 + ], + [ + -0.0524655252950288, + -0.014818851963452406, + 1.3775335664266672 + ], + [ + -0.0609748637229188, + -0.035971382025232757, + 1.3877884009697066 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 120, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.9372630568175224, + -0.0023119793812551026, + -0.3486152852019177 + ], + [ + -0.32377222797067445, + 0.36500736870607237, + -0.8728924132933995 + ], + [ + 0.12926525720385734, + 0.9310017591506528, + 0.34135936743908263 + ] + ], + "translation_m": [ + -0.1032248362230994, + -0.012115231113795023, + 1.4751705742764027 + ], + "rvec_rad": [ + 1.1842553557179327, + -0.31372826653720015, + -0.21103844496966742 + ] + }, + "center_m": [ + -0.1032248362230994, + -0.012115231113795023, + 1.4751705742764027 + ], + "normal": [ + -0.3486152852019177, + -0.8728924132933995, + 0.34135936743908263 + ], + "corners_m": [ + [ + -0.11496952417558412, + -0.0035054861553356874, + 1.4851922805507376 + ], + [ + -0.09153794775514607, + -0.011599791854602548, + 1.488423911980834 + ], + [ + -0.09148014827061468, + -0.020724976072254358, + 1.465148868002068 + ], + [ + -0.11491172469105274, + -0.012630670372987497, + 1.4619172365719715 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 198, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6804098291817715, + -0.7327451888745092, + 0.011267321513669215 + ], + [ + -0.6615269329585601, + 0.6075175897002139, + -0.4396640708259922 + ], + [ + 0.3153166366103656, + -0.3066053919716712, + -0.8980916168691391 + ] + ], + "translation_m": [ + -0.08294047231137834, + 0.16810992221276685, + 1.4789772969715955 + ], + "rvec_rad": [ + 1.1646179977255695, + -2.661241698825467, + 0.6233495125646704 + ] + }, + "center_m": [ + -0.08294047231137834, + 0.16810992221276685, + 1.4789772969715955 + ], + "normal": [ + 0.011267321513669215, + -0.4396640708259922, + -0.8980916168691391 + ], + "corners_m": [ + [ + -0.08359466430753756, + 0.18397297874600152, + 1.47120327161432 + ], + [ + -0.10060491003708186, + 0.16743480542203754, + 1.479086187529579 + ], + [ + -0.08228628031521912, + 0.1522468656795322, + 1.486751322328871 + ], + [ + -0.06527603458567482, + 0.16878503900349617, + 1.478868406413612 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 206, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7104130743640638, + -0.7033259418896286, + -0.025414232974179413 + ], + [ + -0.635490478789318, + 0.6565746460940126, + -0.4062777196384828 + ], + [ + 0.302432000854279, + -0.27247450077317115, + -0.9133961524375327 + ] + ], + "translation_m": [ + -0.4887770999565317, + -0.09681965726358548, + 1.713179002988308 + ], + "rvec_rad": [ + 1.09863956596507, + -2.69189969487317, + 0.55698752518491 + ] + }, + "center_m": [ + -0.4887770999565317, + -0.09681965726358548, + 1.713179002988308 + ], + "normal": [ + -0.025414232974179413, + -0.4062777196384828, + -0.9133961524375327 + ], + "corners_m": [ + [ + -0.4886885108006012, + -0.08066884320254385, + 1.705992671717965 + ], + [ + -0.5064488376597028, + -0.0965561051722768, + 1.713553471739322 + ], + [ + -0.48886568911246214, + -0.1129704713246271, + 1.7203653342586511 + ], + [ + -0.4711053622533605, + -0.09708320935489416, + 1.7128045342372942 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 208, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6696613299563814, + -0.7426547344251824, + 0.004201023317266239 + ], + [ + -0.678367489499494, + 0.6093707672100687, + -0.4104690210721953 + ], + [ + 0.30227678103222594, + -0.277725068198163, + -0.9118648661633595 + ] + ], + "translation_m": [ + -0.20076991121449295, + 0.05130887862032314, + 1.6392068088196068 + ], + "rvec_rad": [ + 1.1872712140149513, + -2.6660104589961846, + 0.5749896223222203 + ] + }, + "center_m": [ + -0.20076991121449295, + 0.05130887862032314, + 1.6392068088196068 + ], + "normal": [ + 0.004201023317266239, + -0.4104690210721953, + -0.9118648661633595 + ], + "corners_m": [ + [ + -0.20168232877035297, + 0.06740560682919268, + 1.631956785704227 + ], + [ + -0.2184238620192625, + 0.05044641959170532, + 1.6395137052300326 + ], + [ + -0.19985749365863292, + 0.035212150411453605, + 1.6464568319349866 + ], + [ + -0.1831159604097234, + 0.05217133764894096, + 1.638899912409181 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 210, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7271552706016013, + -0.6864636809995008, + -0.003581494789556744 + ], + [ + -0.6240375056143963, + 0.6631846281466438, + -0.4132352121692442 + ], + [ + 0.2860461571545303, + -0.29825117545219104, + -0.9106172809246001 + ] + ], + "translation_m": [ + -0.0018527883704032389, + 0.32501012797078416, + 1.5131149430235773 + ], + "rvec_rad": [ + 1.0788989364143466, + -2.717585628060634, + 0.5857468232145981 + ] + }, + "center_m": [ + -0.0018527883704032389, + 0.32501012797078416, + 1.5131149430235773 + ], + "normal": [ + -0.003581494789556744, + -0.4132352121692442, + -0.9106172809246001 + ], + "corners_m": [ + [ + -0.001344143500376982, + 0.3411004046427972, + 1.5058112263659933 + ], + [ + -0.019523025265417018, + 0.32549946700243726, + 1.5129623802948566 + ], + [ + -0.0023614332404294958, + 0.30891985129877114, + 1.5204186596811613 + ], + [ + 0.015817448524610537, + 0.32452078893913106, + 1.513267505752298 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 214, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6889187221873684, + -0.7245993282387658, + 0.018622774649678486 + ], + [ + -0.6561771508954263, + 0.6125391238947321, + -0.44071234194317055 + ], + [ + 0.3079326888501517, + -0.3158348226750851, + -0.8974551932677827 + ] + ], + "translation_m": [ + -0.25610999847075094, + 0.10788069714737583, + 1.6133698676927664 + ], + "rvec_rad": [ + 1.1539416431160756, + -2.6733935757129155, + 0.6322611164276344 + ] + }, + "center_m": [ + -0.25610999847075094, + 0.10788069714737583, + 1.6133698676927664 + ], + "normal": [ + 0.018622774649678486, + -0.44071234194317055, + -0.8974551932677827 + ], + "corners_m": [ + [ + -0.2565560060463934, + 0.12373965058225281, + 1.605572773798701 + ], + [ + -0.2737789741010776, + 0.10733522180986715, + 1.6132710910199546 + ], + [ + -0.2556639908951085, + 0.09202174371249885, + 1.6211669615868318 + ], + [ + -0.23844102284042426, + 0.10842617248488451, + 1.6134686443655781 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 217, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6815829121233712, + -0.7315357188221343, + -0.0173270305825369 + ], + [ + -0.65784341686564, + 0.6229487229071295, + -0.42329295708163606 + ], + [ + 0.3204477692042257, + -0.27711077336646317, + -0.9058271614917914 + ] + ], + "translation_m": [ + -0.4228551787247361, + -0.1516932110273688, + 1.7310429222755055 + ], + "rvec_rad": [ + 1.1499666584396027, + -2.657162096940084, + 0.5797128492672539 + ] + }, + "center_m": [ + -0.4228551787247361, + -0.1516932110273688, + 1.7310429222755055 + ], + "normal": [ + -0.0173270305825369, + -0.42329295708163606, + -0.9058271614917914 + ], + "corners_m": [ + [ + -0.42347958880847064, + -0.13568330928020916, + 1.7235734404933718 + ], + [ + -0.4405191616115549, + -0.15212939470185016, + 1.7315846347234776 + ], + [ + -0.42223076864100156, + -0.16770311277452843, + 1.7385124040576392 + ], + [ + -0.4051911958379173, + -0.15125702735288743, + 1.7305012098275334 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 229, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7043154872687161, + -0.7092296221905335, + 0.030545660917780687 + ], + [ + -0.6480361041523287, + 0.6247880871986401, + -0.4355330685605125 + ], + [ + 0.28980838860961755, + -0.32654737650475313, + -0.8996543273887809 + ] + ], + "translation_m": [ + -0.012268541823910102, + 0.10959996833275021, + 1.5092148355789812 + ], + "rvec_rad": [ + 1.1349150200725515, + -2.6998146110033026, + 0.6372344978743428 + ] + }, + "center_m": [ + -0.012268541823910102, + 0.10959996833275021, + 1.5092148355789812 + ], + "normal": [ + 0.030545660917780687, + -0.4355330685605125, + -0.8996543273887809 + ], + "corners_m": [ + [ + -0.01232996851043282, + 0.12551027072463733, + 1.5015103885150516 + ], + [ + -0.029937855692150725, + 0.1093093681208291, + 1.508755598230292 + ], + [ + -0.012207115137387385, + 0.0936896659408631, + 1.5169192826429108 + ], + [ + 0.005400772044330518, + 0.10989056854467132, + 1.5096740729276703 + ] + ], + "num_cameras": 3 + }, + { + "marker_id": 232, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.7183343907352538, + 0.4464826553927455, + 0.5335250148966348 + ], + [ + -0.6241044929365521, + -0.7524158363011944, + -0.2106276125762795 + ], + [ + 0.30739109450896696, + -0.48427641664303356, + 0.8191380025978299 + ] + ], + "translation_m": [ + -0.0629998493756285, + 0.05639140287796347, + 1.6039755052971074 + ], + "rvec_rad": [ + -0.6168338076301503, + 0.5097301543953826, + -2.41321846574843 + ] + }, + "center_m": [ + -0.0629998493756285, + 0.05639140287796347, + 1.6039755052971074 + ], + "normal": [ + 0.5335250148966348, + -0.2106276125762795, + 0.8191380025978299 + ], + "corners_m": [ + [ + -0.048439636299028505, + 0.054787511085905446, + 1.5940796614077075 + ], + [ + -0.06639799606740986, + 0.03918489876249164, + 1.6017644387704315 + ], + [ + -0.07756006245222849, + 0.0579952946700215, + 1.6138713491865073 + ], + [ + -0.059601702683847145, + 0.0735979069934353, + 1.6061865718237833 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 243, + "pose_in_S": { + "rotation_matrix": [ + [ + -0.6999584612139846, + 3.470811539552088e-05, + 0.7141835557966157 + ], + [ + -0.6465268589649761, + -0.42487373400487244, + -0.6336287010463137 + ], + [ + 0.30341584205811156, + -0.9052526216190173, + 0.2974164047931177 + ] + ], + "translation_m": [ + 0.009769815823822704, + 0.10322731090567824, + 1.5613501004876782 + ], + "rvec_rad": [ + -0.9100816434293578, + 1.3762858410947536, + -2.166318092700037 + ] + }, + "center_m": [ + 0.009769815823822704, + 0.10322731090567824, + 1.5613501004876782 + ], + "normal": [ + 0.7141835557966157, + -0.6336287010463137, + 0.2974164047931177 + ], + "corners_m": [ + [ + 0.018519730440439953, + 0.10599797496767954, + 1.546241744691714 + ], + [ + 0.00102076891009034, + 0.08983480349355513, + 1.553827140743167 + ], + [ + 0.0010199012072054526, + 0.10045664684367694, + 1.5764584562836423 + ], + [ + 0.01851886273755507, + 0.11661981831780135, + 1.5688730602321894 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 244, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.6740459496274129, + 0.04213655303293157, + -0.7374866566178551 + ], + [ + -0.7061789303401182, + 0.3296818587849857, + -0.6265949172565664 + ], + [ + 0.21673342182175231, + 0.9431513043444195, + 0.25197666752867154 + ] + ], + "translation_m": [ + -0.11357621185627663, + 0.05081710902588825, + 1.5883211921347533 + ], + "rvec_rad": [ + 1.1416216303974343, + -0.6939709532125229, + -0.5442237289214118 + ] + }, + "center_m": [ + -0.11357621185627663, + 0.05081710902588825, + 1.5883211921347533 + ], + "normal": [ + -0.7374866566178551, + -0.6265949172565664, + 0.25197666752867154 + ], + "corners_m": [ + [ + -0.12147507931370764, + 0.06376536888995205, + 1.5974014156662866 + ], + [ + -0.10462393057302233, + 0.04611089563144909, + 1.6028197512118305 + ], + [ + -0.10567734439884562, + 0.03786884916182445, + 1.57924096860322 + ], + [ + -0.12252849313953093, + 0.055523322420327405, + 1.5738226330576761 + ] + ], + "num_cameras": 2 + }, + { + "marker_id": 245, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.06879428579222246, + -0.7021767839865607, + 0.7086713697283309 + ], + [ + 0.35806135607786455, + -0.6456458055050683, + -0.6744876271047469 + ], + [ + 0.9311602501857661, + 0.3001487262408459, + 0.2070056294161563 + ] + ], + "translation_m": [ + -0.05778893340892064, + 0.03888680727859496, + 1.5840275169503621 + ], + "rvec_rad": [ + 1.5552210299609288, + -0.3550240914286651, + 1.6918152565669546 + ] + }, + "center_m": [ + -0.05778893340892064, + 0.03888680727859496, + 1.5840275169503621 + ], + "normal": [ + 0.7086713697283309, + -0.6744876271047469, + 0.2070056294161563 + ], + "corners_m": [ + [ + -0.06742607178115544, + 0.0263404677588083, + 1.5761398729010507 + ], + [ + -0.06570621463634987, + 0.03529200166075491, + 1.5994188791556947 + ], + [ + -0.04815179503668585, + 0.051433146798381624, + 1.5919151609996736 + ], + [ + -0.049871652181491415, + 0.04248161289643501, + 1.5686361547450296 + ] + ], + "num_cameras": 4 + }, + { + "marker_id": 248, + "pose_in_S": { + "rotation_matrix": [ + [ + 0.03429945011852731, + -0.6580757240973516, + 0.7521701197703299 + ], + [ + 0.2929608910140103, + -0.7129314482533455, + -0.6371049100640054 + ], + [ + 0.9555090078371022, + 0.24220877656504003, + 0.1683372938389548 + ] + ], + "translation_m": [ + -0.03231508869038346, + 0.06322741078791845, + 1.5724968280004574 + ], + "rvec_rad": [ + 1.6274823736717328, + -0.37635085321979095, + 1.7602311340607333 + ] + }, + "center_m": [ + -0.03231508869038346, + 0.06322741078791845, + 1.5724968280004574 + ], + "normal": [ + 0.7521701197703299, + -0.6371049100640054, + 0.1683372938389548 + ], + "corners_m": [ + [ + -0.040969778368081945, + 0.0506537565470765, + 1.5635805751095566 + ], + [ + -0.04011229211511876, + 0.05797777882242676, + 1.5874683003054841 + ], + [ + -0.02366039901268497, + 0.0758010650287604, + 1.5814130808913582 + ], + [ + -0.024517885265648152, + 0.06847704275341014, + 1.5575253556954307 + ] + ], + "num_cameras": 3 + } + ] +} \ No newline at end of file diff --git a/data/robot/robot.json b/data/robot/robot.json index ac293ab..4ed30f3 100644 --- a/data/robot/robot.json +++ b/data/robot/robot.json @@ -170,81 +170,82 @@ "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, "position": [20, -20, 0.3], "normal": [0, 0, 1]}, - {"id": 211, "position": [250, -10, 0.3], "normal": [0, 0, 1]}, - {"id": 215, "position": [250, -90, 0.3], "normal": [0, 0, 1]}, - {"id": 214, "position": [350, -10, 0.3], "normal": [0, 0, 1]}, - {"id": 208, "position": [350, -90, 0.3], "normal": [0, 0, 1]}, - {"id": 206, "position": [650, -10, 0.3], "normal": [0, 0, 1]}, - {"id": 205, "position": [750, -90, 0.3], "normal": [0, 0, 1]}, - {"id": 207, "position": [750, -10, 0.3], "normal": [0, 0, 1]}, - {"id": 217, "position": [650, -90, 0.3], "normal": [0, 0, 1]}, + {"id": 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, "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 48, "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 49, "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 50, "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 51, "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 52, "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 53, "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 54, "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 55, "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 56, "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 57, "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 58, "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 59, "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 60, "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 61, "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 62, "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 63, "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 64, "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 65, "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 66, "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 67, "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 68, "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 69, "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 70, "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 71, "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 72, "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 73, "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 74, "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 75, "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 76, "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 77, "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 78, "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 79, "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 80, "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 81, "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 82, "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 83, "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 84, "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 85, "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 86, "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 87, "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 88, "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 89, "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 90, "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 91, "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 92, "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 93, "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 94, "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 95, "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 96, "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 97, "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 98, "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 99, "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 100, "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 101, "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 102, "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 103, "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 104, "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 105, "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90} + {"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": [ { diff --git a/doc/callibrate_scene_roadmap.md b/doc/callibrate_scene_roadmap.md index df70ef9..29496c0 100644 --- a/doc/callibrate_scene_roadmap.md +++ b/doc/callibrate_scene_roadmap.md @@ -12,6 +12,12 @@ Datum: 2026-06-04 > 3. **Welt = Roboter (Konvention 2)**, Roboter steht *nicht* bei 0/0/0. > 4. **Primär eine Aufnahme (7 Bilder)**, *ohne* zusätzliche Base-Marker; mehrere Posen als Fallback. > 5. **Ausgabe vorerst `robot.calibrated.json`** (Debugging); später in-place nach `robot.json`. +> 6. **Code generisch & `robot.json`-unabhängig.** `robot.json` ist nur ein Beispiel und wird später +> gegen ein anderes geprüft. KEINE festen Marker-IDs, Link-/Set-Namen, Achsen oder Gelenk-Variablen +> im Code (Auto-Discovery aus den Daten). Daten-spezifisches (z.B. die `set`-Zuordnung) gehört in +> `robot.json`, nicht in den Code. +> 7. **Aktuelle Marker-Positionen = brauchbare Startwerte.** Die relativen Bezüge innerhalb jedes Sets +> gelten als korrekt → direkte Grundlage für den Kabsch-Fit und die BA-Initialisierung. --- @@ -218,3 +224,134 @@ QA: Reprojektions-RMS je Kamera; Set-Fit-Residuum (mm); Co-Visibility-Graph zusa „A0", „Brett" — ein Set = ein physisches Objekt.) 3. **Lose-Marker-Orientierung:** reicht Position + Normale, oder wird der Spin (Drehung um die Normale) gebraucht? (Bestimmt die nötige Genauigkeit von Phase C / 3b.) + +--- + +## 11. Umsetzungs-Log: Mathematik & Anwendung + +Pro abgeschlossener Phase: *was* gemacht wurde, *welche Mathematik* dahinter steckt, *wie* man es +anwendet. (Wird mit jeder Phase fortgeschrieben.) + +### P0 — Marker-Klassifizierung & `set`-Tags — ✅ erledigt (2026-06-04) + +**Was gemacht** +- Neu: `pipeline/marker_sets.py` — liest ein beliebiges `robot.json`, klassifiziert jeden Marker in + `arm` / `set` / `loose` und gibt einen Report aus. Vollständig generisch (keine festen Namen/IDs). +- Daten: `data/robot/robot.json` mit `set`-Tags versehen — `Brett` (9 Board-Oberflächen-Marker, + z≈0.3) und `A0` (60 Papier-Marker, z≈−27.3). Chirurgisch eingefügt (kompaktes Custom-Format der + Datei bleibt erhalten, nur +1 Zeile), FK numerisch exakt invariant. + +**Mathematik / Logik** + +1) *Statisch (Welt) vs. beweglich (Roboter):* Ein Link `L` ist beweglich, wenn auf dem Pfad + `L → Wurzel` mindestens ein Gelenk vom Typ revolute/linear liegt: + ``` + movable(L) = ∃ A ∈ chain(L→root): type(jointToParent(A)) ∈ {revolute, linear} + ``` + Die Wurzel und nur über `fixed` angebundene Links sind statisch (= Welt). Generisch, da nur + Gelenk-Typen geprüft werden — keine Link-Namen. + +2) *Rollen* eines Markers `m` auf Link `L`: + ``` + role(m) = arm falls movable(L) → Welt-Referenz, NICHT kalibriert + = set(s) falls ¬movable(L) ∧ m.set = s → starres Objekt, 6-DoF kalibrieren + = loose falls ¬movable(L) ∧ m hat kein set → einzeln vermessen + ``` + +3) *Modell eines starren Sets `S`* (die eigentliche Kalibriergröße der Phase C): + Marker `i` hat eine **bekannte, fixe** lokale Lage `p_iˡᵒᵏ` (die vertrauten relativen Bezüge). + Das Set hat eine **unbekannte** starre Platzierung `(R_S, t_S) ∈ SE(3)`: + ``` + p_iʷᵉˡᵗ = R_S · p_iˡᵒᵏ + t_S R_S ∈ SO(3) (Verdrehung), t_S ∈ ℝ³ (Verschiebung) + ``` + `(R_S, t_S)` = die gesuchten 6 DoF je Set. Schätzung aus gemessenen Welt-Positionen `q_i` + per **Kabsch / orthogonalem Procrustes** (ohne Skalierung): + ``` + min_{R∈SO(3), t} Σ_i ‖ R·p_iˡᵒᵏ + t − q_i ‖² + p̄=mean(pˡᵒᵏ), q̄=mean(q), H = Σ (p_iˡᵒᵏ−p̄)(q_i−q̄)ᵀ, U Σ Vᵀ = svd(H) + R = V·diag(1,1,det(V Uᵀ))·Uᵀ, t = q̄ − R·p̄ + ``` + (vorhanden als `rigid_transform_no_scale`). Weil die aktuellen `robot.json`-Positionen brauchbare + Startwerte sind und die relativen Bezüge stimmen, gilt `p_iˡᵒᵏ` = aktuelle Set-Positionen + (ggf. zentriert) und `(R_S,t_S) ≈ Identität` als Initialwert. + +4) *Loser Marker:* eigene unbekannte Pose `(R_m, t_m)`, einzeln aus den triangulierten Ecken + (Position = Eckmittel, Orientierung aus Eckebene + Eckreihenfolge). + +5) *FK-Invarianz:* `set` ist reine Metadaten; `p^welt = T_Link(θ)·p^lok` bleibt unberührt + (verifiziert: max |Δ| = 0). + +**Anwendung** +``` +# Report (Mensch): +python pipeline/marker_sets.py -robot data/robot/robot.json +# Report (Maschine): +python pipeline/marker_sets.py -robot data/robot/robot.json --json +``` +```python +from marker_sets import load_robot, classify_markers, get_sets, get_loose_markers, get_arm_markers +data = load_robot("data/robot/robot.json") +sets = get_sets(data) # {"A0":[MarkerInfo,...], "Brett":[...]} +loose = get_loose_markers(data) # [MarkerInfo,...] +arm = get_arm_markers(data) # {id: MarkerInfo} +``` +*Neues `robot.json` vorbereiten:* an jeden Marker eines starren Objekts `"set": ""` ergänzen +(Name frei wählbar, ein Set = ein physisches Objekt); lose Marker ohne `set`; Arm-Marker (an +beweglichen Links) brauchen keinen Eintrag. Der Code liest die Sets dann automatisch. + +### P1 — Ankerlose, metrische Rekonstruktion (Phase A) — ✅ erledigt (2026-06-04) + +**Was gemacht** +- Neu: `pipeline/scene_reconstruct.py` — rekonstruiert aus den ArUco-Eckbeobachtungen ALLER Kameras + die Kamera- und Marker-Posen in einem gemeinsamen Frame `S`, **ohne** Welt-Anker, **ohne** + `robot.json` (nur Marker-Kantenlänge nötig). Vollständig generisch. + +**Mathematik** + +Konventionen: `E_c` = world(S)→Kamera c, `G_m` = Marker-lokal→world(S), also `M_{c←m} = E_c · G_m`. + +1) *Per-Marker-PnP (tentativ):* für jedes (Kamera c, Marker m) löst IPPE_SQUARE die Pose eines + Quadrats bekannter Größe. **Problem:** ein planares Quadrat hat eine **2-fache Flip-Ambiguität** + → naive Verkettung liefert ~⅓ gespiegelte Knoten (empirisch verifiziert). + +2) *Flip-robuste Initialisierung* (Referenzkamera `c0`, `E_{c0}=I`), iterativ: + - **Kamera-Pose per RANSAC-PnP** gegen die bereits platzierten 3D-Ecken + `X_S = G_m · corner_local`. RANSAC verwirft gespiegelte Marker als Ausreißer. + - **Marker-Ecken triangulieren** (lineares DLT über alle platzierten Kameras) — Triangulation + ist **flip-frei**; daraus Marker-Pose via Kabsch `local→tri`. Korrigiert Flips aus Schritt 1. + - Marker mit nur 1 Kamera sind nicht triangulierbar → als `insufficient_views` markiert + (unbeobachtbar, **nicht** rekonstruiert; Konvention „unbekannt = null"). + +3) *Globale Bündelausgleichung* (`scipy.least_squares`, robuste Huber-Loss, **dünnbesetzte** + Jacobi): minimiert die Reprojektion aller Ecken + ``` + min_{E_c, G_m} Σ_{(c,m)} Σ_{k=1..4} ρ( ‖ π(K_c, D_c; E_c·G_m·corner_k) − u_{c,m,k} ‖ ) + ``` + Gauge: Referenzkamera `E_{c0}=I` fix (entfernt die 6-DoF-Starrkörperfreiheit). + +4) *Similarity-Gauge / Skala:* Ankerlose SfM bestimmt die Struktur nur bis auf eine **7-DoF- + Ähnlichkeit** (Rotation, Translation, **Skala**). Der absolute Maßstab kommt hier provisorisch aus + der angenommenen Markergröße — empirisch ~0.93× gegenüber der echten Welt (konsistent über alle + Szenen, also ein systematischer Markergrößen-/Rand-Versatz). **Die echte Skala + Lage fixiert + erst Phase B über die bekannte mm-Geometrie des Roboters** (robuster als die Markergröße). Die + *Form* ist bereits korrekt. + +**Validierung (Sim, gegen FK-Ground-Truth)** +- Reprojektion median **0.7–1.6 px** über Scene5/6/8/10/11/12 — besser als die bestehende, + board-verankerte Pipeline (3.2–4.9 px), weil keine falschen Marker-Positionen angenommen werden. +- Form (nach Similarity-Ausrichtung): Residuum **median ~3–7 mm** = Sensor-Rauschboden der Renders + (`markerOffsetMaxMm: 4` + Rauschen); Übereinstimmung mit der bestehenden Triangulation **1.9 mm**. +- Skalenfaktor konsistent **0.92–0.94** (→ Phase B). + +**Anwendung** +``` +python pipeline/scene_reconstruct.py --evalDir data/evaluations/Scene8 +# -> /scene_reconstruction.json (Kamera- & Marker-Posen in S, Reproj-Statistik, +# Liste insufficient_view_markers) +``` +```python +import scene_reconstruct as sr +res = sr.reconstruct("data/evaluations/Scene8") # dict; res["cameras"], res["markers"] +``` +Voraussetzung: `*_aruco_detection.json` je Kamera (Pipeline-Schritt 1). Marker-Kantenlänge wird +aus den Detektionen gelesen (Fallback `--markerSize`). diff --git a/pipeline/marker_sets.py b/pipeline/marker_sets.py new file mode 100644 index 0000000..2144bbc --- /dev/null +++ b/pipeline/marker_sets.py @@ -0,0 +1,261 @@ +#!/usr/bin/env python3 +""" +marker_sets.py +============== +Klassifiziert die Marker aus robot.json in drei Rollen für die Szenen-Kalibrierung +(siehe doc/callibrate_scene_roadmap.md, Phase P0): + + arm : Marker an BEWEGLICHEN Roboter-Links (Arm1, Ellbow, Arm2, Finger, ...). + Ihre Lage je Link ist bekannt -> Welt-Referenz, wird NICHT kalibriert. + set : Marker an einem STATISCHEN Link (Welt-Wurzel) MIT "set"-Feld. Gleiches + "set" = ein starres Objekt mit fixen relativen Bezügen -> es wird nur die + 6-DoF-Platzierung des ganzen Sets kalibriert. + loose : Marker an einem statischen Link OHNE "set"-Feld. Lose, einzeln zu vermessen. + +Die Set-Zugehörigkeit steht ausschließlich in robot.json (optionales Feld "set" am +Marker). Hier wird nichts hartkodiert — die Sets ergeben sich per Auto-Discovery aus +den vorhandenen "set"-Werten. + +"Beweglich" = es liegt irgendwo auf der Kette bis zur Wurzel ein revolute/linear-Joint. +Damit zählt die Wurzel (Board) und jeder rein über "fixed"-Joints angebundene Link als +statisch (Welt), alles ab dem ersten Gelenk als Arm. + +Public API +---------- +data = load_robot("data/robot/robot.json") +cls = classify_markers(data) # id -> MarkerInfo +sets = get_sets(data) # set_name -> [MarkerInfo, ...] +loose = get_loose_markers(data) # [MarkerInfo, ...] +arm = get_arm_markers(data) # id -> MarkerInfo +rep = set_summary(data) # serialisierbarer Report (für QA / --json) + +CLI +--- +python pipeline/marker_sets.py -robot data/robot/robot.json +python pipeline/marker_sets.py -robot data/robot/robot.json --json +""" +from __future__ import annotations + +import argparse +import json +import sys +from dataclasses import dataclass, asdict +from typing import Any, Dict, List, Optional + + +# ────────────────────────────────────────────────────────────── +# Datentyp +# ────────────────────────────────────────────────────────────── + +@dataclass +class MarkerInfo: + id: int + link: str + role: str # "arm" | "set" | "loose" + set_name: Optional[str] # nur bei role == "set" + position: List[float] # im Link-Frame (mm), wie in robot.json + normal: Optional[List[float]] + spin: Optional[float] + + +# ────────────────────────────────────────────────────────────── +# Laden +# ────────────────────────────────────────────────────────────── + +def load_robot(path: str) -> Dict[str, Any]: + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + +# ────────────────────────────────────────────────────────────── +# Link-Topologie: statisch (Welt) vs. beweglich (Arm) +# ────────────────────────────────────────────────────────────── + +_MOVABLE_JOINTS = ("revolute", "linear") + + +def link_is_movable(links: Dict[str, Any], name: str) -> bool: + """ + True, wenn auf der Kette von `name` bis zur Wurzel mindestens ein + revolute/linear-Joint liegt (Marker dort gehören zum beweglichen Roboter). + """ + seen = set() + cur: Optional[str] = name + while cur and cur in links and cur not in seen: + seen.add(cur) + joint = links[cur].get("jointToParent") or {} + if str(joint.get("type", "")).lower() in _MOVABLE_JOINTS: + return True + cur = links[cur].get("parent") + return False + + +def root_links(links: Dict[str, Any]) -> List[str]: + return [n for n, l in links.items() + if not l.get("parent") or l.get("parent") not in links] + + +# ────────────────────────────────────────────────────────────── +# Klassifizierung +# ────────────────────────────────────────────────────────────── + +def _set_name_of(marker: Dict[str, Any]) -> Optional[str]: + val = marker.get("set") + if val is None: + return None + s = str(val).strip() + return s or None + + +def classify_markers(robot_data: Dict[str, Any]) -> Dict[int, MarkerInfo]: + """ + Liefert id -> MarkerInfo über alle Links. Bei doppelten IDs gewinnt das erste + Vorkommen (zusätzlich als Warnung in set_summary sichtbar). + """ + links = robot_data.get("links", {}) or {} + out: Dict[int, MarkerInfo] = {} + + for link_name, link in links.items(): + movable = link_is_movable(links, link_name) + for mk in link.get("markers", []) or []: + if "id" not in mk or "position" not in mk: + continue + mid = int(mk["id"]) + if mid in out: + continue # erstes Vorkommen behalten + set_name = _set_name_of(mk) + if movable: + role = "arm" + set_name = None + elif set_name is not None: + role = "set" + else: + role = "loose" + + normal = mk.get("normal") + spin = mk.get("spin") + out[mid] = MarkerInfo( + id=mid, + link=link_name, + role=role, + set_name=set_name, + position=[float(v) for v in mk["position"]], + normal=[float(v) for v in normal] if normal is not None else None, + spin=float(spin) if spin is not None else None, + ) + return out + + +def get_arm_markers(robot_data: Dict[str, Any]) -> Dict[int, MarkerInfo]: + return {m.id: m for m in classify_markers(robot_data).values() if m.role == "arm"} + + +def get_sets(robot_data: Dict[str, Any]) -> Dict[str, List[MarkerInfo]]: + """set_name -> Liste der Marker (role == 'set'), gruppiert nach 'set'-Wert.""" + sets: Dict[str, List[MarkerInfo]] = {} + for m in classify_markers(robot_data).values(): + if m.role == "set" and m.set_name is not None: + sets.setdefault(m.set_name, []).append(m) + return sets + + +def get_loose_markers(robot_data: Dict[str, Any]) -> List[MarkerInfo]: + return [m for m in classify_markers(robot_data).values() if m.role == "loose"] + + +# ────────────────────────────────────────────────────────────── +# QA / Report +# ────────────────────────────────────────────────────────────── + +def _duplicate_ids(robot_data: Dict[str, Any]) -> List[int]: + links = robot_data.get("links", {}) or {} + seen, dups = set(), set() + for link in links.values(): + for mk in link.get("markers", []) or []: + if "id" not in mk: + continue + mid = int(mk["id"]) + (dups if mid in seen else seen).add(mid) + return sorted(dups) + + +def set_summary(robot_data: Dict[str, Any]) -> Dict[str, Any]: + cls = classify_markers(robot_data) + sets = get_sets(robot_data) + loose = get_loose_markers(robot_data) + arm = [m for m in cls.values() if m.role == "arm"] + + warnings: List[str] = [] + for mid in _duplicate_ids(robot_data): + warnings.append(f"Marker-ID {mid} kommt mehrfach vor (erstes Vorkommen gewertet)") + for name, members in sets.items(): + links_used = sorted({m.link for m in members}) + if len(members) < 3: + warnings.append( + f"Set '{name}' hat nur {len(members)} Marker — 6-DoF-Platzierung " + f"nicht voll bestimmbar (>=3 nicht-kollineare nötig)") + if len(links_used) > 1: + warnings.append(f"Set '{name}' verteilt sich über mehrere Links {links_used} " + f"— ein Set sollte ein physisches Objekt sein") + + return { + "counts": { + "total": len(cls), + "arm": len(arm), + "set": sum(len(v) for v in sets.values()), + "loose": len(loose), + "num_sets": len(sets), + }, + "root_links": root_links(robot_data.get("links", {}) or {}), + "sets": {name: sorted(m.id for m in members) for name, members in sorted(sets.items())}, + "loose_ids": sorted(m.id for m in loose), + "arm_ids": sorted(m.id for m in arm), + "warnings": warnings, + } + + +# ────────────────────────────────────────────────────────────── +# CLI +# ────────────────────────────────────────────────────────────── + +def main() -> None: + ap = argparse.ArgumentParser(description="Marker aus robot.json in arm/set/loose klassifizieren") + ap.add_argument("-robot", "--robot", required=True, help="Pfad zu robot.json") + ap.add_argument("--json", action="store_true", help="Report als JSON ausgeben") + args = ap.parse_args() + + data = load_robot(args.robot) + rep = set_summary(data) + + if args.json: + print(json.dumps(rep, indent=2, ensure_ascii=False)) + return + + c = rep["counts"] + print(f"robot.json: {args.robot}") + print(f"Wurzel-Link(s): {rep['root_links']}") + print(f"\nMarker gesamt: {c['total']} | arm: {c['arm']} set: {c['set']} " + f"loose: {c['loose']} (Sets: {c['num_sets']})") + + print("\nSets (starr, fixe interne Lage -> 6-DoF kalibrieren):") + if rep["sets"]: + for name, ids in rep["sets"].items(): + print(f" {name:10s} ({len(ids):3d}): {ids}") + else: + print(" (keine)") + + print(f"\nLose Marker (einzeln zu vermessen): {rep['loose_ids'] or '(keine)'}") + print(f"Arm-Marker (Welt-Referenz, nicht kalibriert): {rep['arm_ids']}") + + if rep["warnings"]: + print("\n[WARN]") + for w in rep["warnings"]: + print(f" - {w}") + + +if __name__ == "__main__": + try: + main() + except Exception as exc: # pragma: no cover + print(f"[ERROR] {exc}", file=sys.stderr) + sys.exit(1) diff --git a/pipeline/scene_reconstruct.py b/pipeline/scene_reconstruct.py new file mode 100644 index 0000000..dc72032 --- /dev/null +++ b/pipeline/scene_reconstruct.py @@ -0,0 +1,424 @@ +#!/usr/bin/env python3 +""" +scene_reconstruct.py — Phase A der Szenen-Kalibrierung +======================================================== +Ankerlose, metrische Rekonstruktion ALLER Kamera- und Marker-Posen aus den +ArUco-Eckbeobachtungen mehrerer Kameras — OHNE bekannten Welt-Anker, allein aus +der bekannten Marker-Kantenlänge. + +Vollständig generisch: kein robot.json nötig, keine festen Marker-IDs/Namen. + +Warum nicht einfach Einzel-Marker-PnP? +-------------------------------------- +Ein planares Quadrat hat bei PnP eine 2-fache Flip-Ambiguität. Verkettet man solche +Einzelposen naiv, sind ~1/3 der Knoten gespiegelt -> unbrauchbar. Stattdessen: + +1) je (Kamera, Marker): PnP (IPPE_SQUARE) als TENTATIVE Startpose. +2) Referenzkamera c0 (meiste Beobachtungen) definiert den Szenen-Frame S (E_{c0}=I). +3) Iteration (flip-robust): + a) Kameras per cv2.solvePnPRansac gegen die bereits platzierten 3D-Ecken + neu bestimmen -> geflippte Marker fallen als RANSAC-Ausreißer raus. + b) Marker-Ecken über alle platzierten Kameras TRIANGULIEREN (DLT, flip-frei) + und die Marker-Pose per Kabsch aus den triangulierten Ecken neu setzen. +4) globale Bündelausgleichung (least_squares, Huber, dünnbesetzte Jacobi) über + die Reprojektion ALLER Ecken. Gauge: c0 = Identität; Maßstab via Markergröße. + +Geometrie-Konventionen +---------------------- +- E_c = world(S) -> camera c (X_cam = E_c X_S) +- G_m = marker m local -> world(S) (X_S = G_m X_local) +- M_{c<-m} = E_c @ G_m (local -> camera) +- Ecken-Reihenfolge wie ArUco/Pipeline: TL, TR, BR, BL. + +Ein-/Ausgabe +------------ +--evalDir : Ordner mit render_*_aruco_detection.json (Eckpunkte + Intrinsik) +--out : scene_reconstruction.json (Default: /scene_reconstruction.json) + +Das Ergebnis (Posen in S) ist Eingang für Phase B (robot_register.py). +""" +from __future__ import annotations + +import argparse +import glob +import json +import os +import re +import sys +import time +from typing import Dict, List, Optional, Tuple + +import numpy as np +import cv2 + +try: + from scipy.optimize import least_squares + from scipy.sparse import lil_matrix + HAVE_SCIPY = True +except ImportError: # pragma: no cover + HAVE_SCIPY = False + + +# ────────────────────────────────────────────────────────────── +# SE(3) / Geometrie-Helfer +# ────────────────────────────────────────────────────────────── + +def rt_to_T(rvec, tvec) -> np.ndarray: + R, _ = cv2.Rodrigues(np.asarray(rvec, dtype=float).reshape(3, 1)) + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = np.asarray(tvec, dtype=float).reshape(3) + return T + + +def T_to_rt(T: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + rvec, _ = cv2.Rodrigues(np.ascontiguousarray(T[:3, :3])) + return rvec.reshape(3), T[:3, 3].copy() + + +def inv_T(T: np.ndarray) -> np.ndarray: + R, t = T[:3, :3], T[:3, 3] + Ti = np.eye(4) + Ti[:3, :3] = R.T + Ti[:3, 3] = -R.T @ t + return Ti + + +def local_corners(size_m: float) -> np.ndarray: + """Marker-Ecken im lokalen Frame (TL, TR, BR, BL), z=0.""" + h = size_m / 2.0 + return np.array([[-h, h, 0.0], [h, h, 0.0], [h, -h, 0.0], [-h, -h, 0.0]], dtype=float) + + +def kabsch(P: np.ndarray, Q: np.ndarray) -> np.ndarray: + """R,t mit Q ≈ R P + t (ohne Skalierung); P,Q: Nx3. Liefert 4x4.""" + pc, qc = P.mean(0), Q.mean(0) + H = (P - pc).T @ (Q - qc) + U, _, Vt = np.linalg.svd(H) + d = np.sign(np.linalg.det(Vt.T @ U.T)) + R = Vt.T @ np.diag([1.0, 1.0, d]) @ U.T + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = qc - R @ pc + return T + + +def triangulate_point(obs: List[Tuple]) -> Optional[np.ndarray]: + """Mehrbild-DLT eines 3D-Punkts. obs: [(K,D,R,t,uv), ...] mit X_cam=R X+t.""" + A = [] + for K, D, R, t, uv in obs: + und = cv2.undistortPoints(np.array([[uv]], dtype=np.float64), K, D).reshape(2) + x, y = float(und[0]), float(und[1]) + P = np.hstack([R, t.reshape(3, 1)]) + A.append(x * P[2] - P[0]) + A.append(y * P[2] - P[1]) + _, _, Vt = np.linalg.svd(np.asarray(A, dtype=float)) + X = Vt[-1] + return X[:3] / X[3] if abs(X[3]) > 1e-12 else None + + +# ────────────────────────────────────────────────────────────── +# Laden der Detektionen +# ────────────────────────────────────────────────────────────── + +class Cam: + __slots__ = ("id", "K", "D", "obs") + + def __init__(self, cid, K, D): + self.id = cid + self.K = K + self.D = D + self.obs: Dict[int, np.ndarray] = {} # marker_id -> (4,2) px + + +def load_detections(eval_dir: str, cameras: Optional[List[str]] = None, + default_size: Optional[float] = None + ) -> Tuple[Dict[str, Cam], Dict[int, float]]: + cams: Dict[str, Cam] = {} + sizes: Dict[int, float] = {} + for det_path in sorted(glob.glob(os.path.join(eval_dir, "*_aruco_detection.json"))): + m = re.match(r"render_([A-Za-z0-9]+)_aruco_detection\.json", os.path.basename(det_path)) + if not m: + continue + cid = m.group(1) + if cameras and cid not in cameras: + continue + det = json.load(open(det_path, "r", encoding="utf-8")) + K = np.array(det["camera"]["camera_matrix"], dtype=float).reshape(3, 3) + D = np.array(det["camera"]["distortion_coefficients"], dtype=float).reshape(-1, 1) + cam = Cam(cid, K, D) + det_size = (det.get("vision_config", {}) or {}).get("MarkerSize", None) + for d in det.get("detections", []): + if d.get("type", "aruco") != "aruco": + continue + pts = d.get("image_points_px") + if pts is None: + continue + mid = int(d["marker_id"]) + cam.obs[mid] = np.array(pts, dtype=float).reshape(4, 2) + s = d.get("marker_size_m", det_size if det_size is not None else default_size) + if s is not None: + sizes[mid] = float(s) + if cam.obs: + cams[cid] = cam + return cams, sizes + + +# ────────────────────────────────────────────────────────────── +# Schritt 1: Per-Marker-PnP (tentative Startposen) +# ────────────────────────────────────────────────────────────── + +def pnp_all(cams: Dict[str, Cam], sizes: Dict[int, float], default_size: float + ) -> Dict[Tuple[str, int], Dict]: + out: Dict[Tuple[str, int], Dict] = {} + for cid, cam in cams.items(): + for mid, corners_px in cam.obs.items(): + size = sizes.get(mid, default_size) + ok, rvec, tvec = cv2.solvePnP(local_corners(size), corners_px, cam.K, cam.D, + flags=cv2.SOLVEPNP_IPPE_SQUARE) + if not ok: + ok, rvec, tvec = cv2.solvePnP(local_corners(size), corners_px, cam.K, cam.D, + flags=cv2.SOLVEPNP_ITERATIVE) + if ok: + out[(cid, mid)] = {"M": rt_to_T(rvec, tvec)} + return out + + +# ────────────────────────────────────────────────────────────── +# Schritt 2+3: flip-robuste Initialisierung +# ────────────────────────────────────────────────────────────── + +def initialize_poses(cams, sizes, default_size, pnp, n_iter=6, min_pts_pnp=8, + ransac_px=3.0) -> Tuple[Dict[str, np.ndarray], Dict[int, np.ndarray], str]: + cam_ids = sorted(cams) + all_mk = sorted({m for _, m in pnp}) + + # Referenzkamera = meiste Beobachtungen + ref = max(cam_ids, key=lambda c: len(cams[c].obs)) + E: Dict[str, np.ndarray] = {ref: np.eye(4)} + G: Dict[int, np.ndarray] = {} + for m in cams[ref].obs: # tentative Startmarker aus c0 + if (ref, m) in pnp: + G[m] = pnp[(ref, m)]["M"].copy() + + def corners3D(m): + return (G[m][:3, :3] @ local_corners(sizes.get(m, default_size)).T).T + G[m][:3, 3] + + for _ in range(n_iter): + # (a) Kameras gegen platzierte 3D-Ecken (RANSAC -> Flip-Ausreißer raus) + for c in cam_ids: + if c == ref: + continue + obj, img = [], [] + for m in cams[c].obs: + if m in G: + obj.append(corners3D(m)) + img.append(cams[c].obs[m]) + if sum(len(o) for o in obj) < min_pts_pnp: + continue + O = np.vstack(obj).astype(np.float64) + I = np.vstack(img).astype(np.float64) + ok, rvec, tvec, inl = cv2.solvePnPRansac( + O, I, cams[c].K, cams[c].D, reprojectionError=ransac_px, + iterationsCount=200, flags=cv2.SOLVEPNP_ITERATIVE) + if ok and inl is not None and len(inl) >= 6: + E[c] = rt_to_T(rvec, tvec) + + # (b) Marker triangulieren (flip-frei) bzw. einfügen + for m in all_mk: + placed = [c for c in cam_ids if c in E and (c, m) in pnp] + if len(placed) >= 2: + tri, ok = [], True + for ci in range(4): + o = [(cams[c].K, cams[c].D, E[c][:3, :3], E[c][:3, 3], cams[c].obs[m][ci]) + for c in placed] + X = triangulate_point(o) + if X is None: + ok = False + break + tri.append(X) + if ok: + G[m] = kabsch(local_corners(sizes.get(m, default_size)), np.array(tri)) + elif placed and m not in G: + c = placed[0] + G[m] = inv_T(E[c]) @ pnp[(c, m)]["M"] + + return E, G, ref + + +# ────────────────────────────────────────────────────────────── +# Schritt 4: Globale Bündelausgleichung (Referenzkamera fix) +# ────────────────────────────────────────────────────────────── + +def reproj_stats(obs_pairs, cams, sizes, default_size, E, G) -> Tuple[float, float]: + """(RMS, Median) der Per-Beobachtungs-Reprojektionsfehler (px).""" + per = [] + for (c, m) in obs_pairs: + rvec, tvec = T_to_rt(E[c] @ G[m]) + proj, _ = cv2.projectPoints(local_corners(sizes.get(m, default_size)), + rvec, tvec, cams[c].K, cams[c].D) + d = proj.reshape(4, 2) - cams[c].obs[m] + per.append(float(np.sqrt(np.mean(d * d)))) + if not per: + return 0.0, 0.0 + a = np.asarray(per) + return float(np.sqrt(np.mean(a * a))), float(np.median(a)) + + +def bundle_adjust(pnp, cams, sizes, default_size, E, G, ref, + huber_px=2.0, max_nfev=120, verbose=0): + if not HAVE_SCIPY: + print("[WARN] scipy fehlt -> BA uebersprungen") + return E, G + cam_opt = [c for c in sorted(E) if c != ref] + mk_ids = sorted(G) + ci = {c: i for i, c in enumerate(cam_opt)} + mi = {m: j for j, m in enumerate(mk_ids)} + ncam, nmk = len(cam_opt), len(mk_ids) + obs = [(c, m) for (c, m) in pnp if c in E and m in G] + + def pack(): + x = np.zeros(6 * ncam + 6 * nmk) + for c in cam_opt: + r, t = T_to_rt(E[c]); x[6*ci[c]:6*ci[c]+3] = r; x[6*ci[c]+3:6*ci[c]+6] = t + for m in mk_ids: + r, t = T_to_rt(G[m]); o = 6*ncam + 6*mi[m]; x[o:o+3] = r; x[o+3:o+6] = t + return x + + def unpack(x): + Ee = {ref: np.eye(4)} + for c in cam_opt: + Ee[c] = rt_to_T(x[6*ci[c]:6*ci[c]+3], x[6*ci[c]+3:6*ci[c]+6]) + Gg = {} + for m in mk_ids: + o = 6*ncam + 6*mi[m] + Gg[m] = rt_to_T(x[o:o+3], x[o+3:o+6]) + return Ee, Gg + + def residuals(x): + Ee, Gg = unpack(x) + res = np.empty(8 * len(obs)) + for k, (c, m) in enumerate(obs): + rvec, tvec = T_to_rt(Ee[c] @ Gg[m]) + proj, _ = cv2.projectPoints(local_corners(sizes.get(m, default_size)), + rvec, tvec, cams[c].K, cams[c].D) + res[8*k:8*k+8] = (proj.reshape(4, 2) - cams[c].obs[m]).ravel() + return res + + Sp = lil_matrix((8 * len(obs), 6 * ncam + 6 * nmk), dtype=int) + for k, (c, m) in enumerate(obs): + rows = slice(8*k, 8*k+8) + if c in ci: + Sp[rows, 6*ci[c]:6*ci[c]+6] = 1 + o = 6*ncam + 6*mi[m] + Sp[rows, o:o+6] = 1 + + sol = least_squares(residuals, pack(), jac_sparsity=Sp, method="trf", + loss="huber", f_scale=huber_px, max_nfev=max_nfev, verbose=verbose) + return unpack(sol.x) + + +# ────────────────────────────────────────────────────────────── +# Hauptlauf +# ────────────────────────────────────────────────────────────── + +def reconstruct(eval_dir, cameras=None, default_size=0.025, + huber_px=2.0, max_nfev=120, verbose=0) -> Dict: + cams, sizes = load_detections(eval_dir, cameras, default_size) + if len(cams) < 2: + raise RuntimeError(f"brauche >=2 Kameras, gefunden: {sorted(cams)}") + pnp = pnp_all(cams, sizes, default_size) + E0, G0, ref = initialize_poses(cams, sizes, default_size, pnp) + + # Marker je #platzierter Kameras; nur >=2 sind triangulierbar/zuverlässig. + ncams_of = {} + for (c, m) in pnp: + if c in E0 and m in G0: + ncams_of[m] = ncams_of.get(m, 0) + 1 + weak = sorted(m for m in G0 if ncams_of.get(m, 0) < 2) # Einzelbild -> unbeobachtbar + G0 = {m: T for m, T in G0.items() if ncams_of.get(m, 0) >= 2} + + pairs0 = [(c, m) for (c, m) in pnp if c in E0 and m in G0] + rms0, med0 = reproj_stats(pairs0, cams, sizes, default_size, E0, G0) + E, G = bundle_adjust(pnp, cams, sizes, default_size, E0, G0, ref, huber_px, max_nfev, verbose) + pairs = [(c, m) for (c, m) in pnp if c in E and m in G] + rms1, med1 = reproj_stats(pairs, cams, sizes, default_size, E, G) + dropped = [c for c in cams if c not in E] + + cameras_out = [] + for c in sorted(E): + Ec = E[c] + rvec, _ = T_to_rt(Ec) + cameras_out.append({ + "camera_id": c, + "world_to_camera": {"rotation_matrix": Ec[:3, :3].tolist(), + "translation_m": Ec[:3, 3].tolist(), "rvec_rad": rvec.tolist()}, + "center_m": (-Ec[:3, :3].T @ Ec[:3, 3]).tolist(), + "num_markers": len(cams[c].obs), + "is_reference": bool(c == ref), + }) + markers_out = [] + for m in sorted(G): + Gm = G[m] + size = sizes.get(m, default_size) + corners_S = (Gm[:3, :3] @ local_corners(size).T).T + Gm[:3, 3] + rvec, _ = T_to_rt(Gm) + markers_out.append({ + "marker_id": int(m), + "pose_in_S": {"rotation_matrix": Gm[:3, :3].tolist(), + "translation_m": Gm[:3, 3].tolist(), "rvec_rad": rvec.tolist()}, + "center_m": Gm[:3, 3].tolist(), + "normal": (Gm[:3, :3] @ np.array([0.0, 0.0, 1.0])).tolist(), + "corners_m": corners_S.tolist(), + "num_cameras": int(ncams_of.get(m, 0)), + }) + + return { + "schema_version": "1.0", + "stage": "scene_reconstruct_A", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "frame": "arbitrary S (reference camera pose = identity)", + "reference_camera": ref, + "summary": {"num_cameras": len(E), "num_markers": len(G), "num_observations": len(pairs), + "reproj_rms_px_init": rms0, "reproj_median_px_init": med0, + "reproj_rms_px_final": rms1, "reproj_median_px_final": med1, + "dropped_cameras": dropped, + "insufficient_view_markers": weak}, + "cameras": cameras_out, + "markers": markers_out, + } + + +def main() -> None: + ap = argparse.ArgumentParser(description="Phase A: ankerlose metrische Szenen-Rekonstruktion") + ap.add_argument("--evalDir", required=True, help="Ordner mit render_*_aruco_detection.json") + ap.add_argument("--cameras", default=None, help="Kommagetrennte Kamera-IDs (Default: alle)") + ap.add_argument("--markerSize", type=float, default=0.025, help="Fallback-Kantenlänge (m)") + ap.add_argument("--huberPx", type=float, default=2.0) + ap.add_argument("--maxNfev", type=int, default=120) + ap.add_argument("--out", default=None) + ap.add_argument("-v", "--verbose", action="count", default=0) + args = ap.parse_args() + + cams = args.cameras.split(",") if args.cameras else None + res = reconstruct(args.evalDir, cams, args.markerSize, args.huberPx, args.maxNfev, args.verbose) + out = args.out or os.path.join(args.evalDir, "scene_reconstruction.json") + json.dump(res, open(out, "w", encoding="utf-8"), indent=2) + s = res["summary"] + print(f"[INFO] Kameras={s['num_cameras']} Marker(>=2 Views)={s['num_markers']} " + f"Obs={s['num_observations']} | RMS {s['reproj_rms_px_init']:.3f}->{s['reproj_rms_px_final']:.3f}px " + f"(median {s['reproj_median_px_final']:.3f}px) | ref-Kamera={res['reference_camera']}") + if s["dropped_cameras"]: + print(f"[WARN] nicht verbundene Kameras verworfen: {s['dropped_cameras']}") + if s["insufficient_view_markers"]: + print(f"[INFO] {len(s['insufficient_view_markers'])} Marker mit <2 Views (unbeobachtbar, " + f"nicht rekonstruiert): {s['insufficient_view_markers']}") + print(f"[INFO] -> {out}") + + +if __name__ == "__main__": + try: + main() + except Exception as exc: + print(f"[ERROR] {exc}", file=sys.stderr) + sys.exit(1) diff --git a/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.json b/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.json new file mode 100644 index 0000000..d5c8e48 --- /dev/null +++ b/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.json @@ -0,0 +1,33 @@ +{ + "page_format": "A3", + "orientation": "portrait", + "page_size_mm": { + "width": 297.0, + "height": 420.0 + }, + "seed": 223, + "num_arucos": 10, + "aruco_size_mm": 50.0, + "aruco_dictionary": "DICT_4X4_250", + "aruco_start_id": 46, + "page_border_margin_mm": 20.0, + "forbidden_rectangle_mm": { + "x": 143.5, + "y": 205.0, + "w": 10.0, + "h": 10.0 + }, + "forbidden_rectangle_margin_mm": 30.0, + "placements": [ + {"id": 46, "position": [96.26, 119.02, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 47, "position": [158.68, 110.29, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 48, "position": [-43.31, 5.63, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 49, "position": [63.52, -4.25, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 50, "position": [27.77, 149.72, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 51, "position": [-98.03, 38.72, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 52, "position": [-120.71, 148.79, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 53, "position": [72.76, 59.77, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 54, "position": [-156.67, 34.05, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 55, "position": [-110.82, 93.01, -27.3], "normal": [0, 0, 1], "spin": 90} + ] +} diff --git a/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.pdf b/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.pdf new file mode 100644 index 0000000..1893286 --- /dev/null +++ b/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet223.pdf @@ -0,0 +1,74 @@ +%PDF-1.3 +% ReportLab Generated PDF document http://www.reportlab.com +1 0 obj +<< +/F1 2 0 R /F2 3 0 R +>> +endobj +2 0 obj +<< +/BaseFont /Helvetica /Encoding /WinAnsiEncoding /Name /F1 /Subtype /Type1 /Type /Font +>> +endobj +3 0 obj +<< +/BaseFont /Times-Roman /Encoding /WinAnsiEncoding /Name /F2 /Subtype /Type1 /Type /Font +>> +endobj +4 0 obj +<< +/Contents 8 0 R /MediaBox [ 0 0 841.8898 1190.551 ] /Parent 7 0 R /Resources << +/Font 1 0 R /ProcSet [ /PDF /Text /ImageB /ImageC /ImageI ] +>> /Rotate 0 /Trans << + +>> + /Type /Page +>> +endobj +5 0 obj +<< +/PageMode /UseNone /Pages 7 0 R /Type /Catalog +>> +endobj +6 0 obj +<< +/Author (OpenAI) /CreationDate (D:20260609100839+02'00') /Creator (ReportLab PDF Library - www.reportlab.com) /Keywords () /ModDate (D:20260609100839+02'00') /Producer (ReportLab PDF Library - www.reportlab.com) + /Subject (A0 poster with random ArUco placement) /Title (A3_10Arucos_50mm_Seet223) /Trapped /False +>> +endobj +7 0 obj +<< +/Count 1 /Kids [ 4 0 R ] /Type /Pages +>> +endobj +8 0 obj +<< +/Filter [ /ASCII85Decode /FlateDecode ] /Length 2256 +>> +stream +Gasb^c)rG9Ol;/QtIaGHgCFun\\R[^AH)LDaL5K8hc6K6Q-e17IqXj](D4N^H9n1io5//-oU((jsCT;e\51G!$RnTZ;p57s"r=ne@2]#of7ERf;T(gFg`c@q3Mg:nHfM!N+P>$DsH3q.!3+"eSYI0L5WMthC6fq'?W/5'@0@#\6[V+To(cr+Jm>A-jSP#LkC\&faD@kR&*@VQ3F(gseG:WuLoc.FaHNRCE#&%so`Eirncr^:LCP^ZGJV@\d\#ea#Yb#dWDS>dH*Ak:,?d"k'VQ@&n=r-B\F^/o(IQee!"-6Su;1[96Cg4Mjqtcn,8XX\5l:QUPiGQ#;QK_uSBM!YSb@Q+39DR2O0O:nJ.N]<)+8>6?]5CL^I>_h^+!]OhUgUfIV;Kbl1Kp=d8KARCY8Sok8%=?Q*/1mAplQ`VSF$Ti5[jA:t\helFKEmgVa;g4kXm!f[kiPWB7PZsXGpt]DRlh:XoVst#(YMJRVcJuDqQXH"MN5Pco&iDG,G>ESjLAM-2-!JT+YocXgZQ*QIt_*@V;T$m(9?GDcVtSIHn7&c_Ek/I:juH@p:mc1*G.ABs/'uXMiSR@"@&nBX&kl@Q=FiH]-RZ2eKO*Yta#<]2a4D"]uj*J9>c1lf/be4,93Z3Fpm_oo.u4Yats4H4D[A]u@(".mqD#(h*q>Qa7NOYq`htWi[Dl+K[B`>%;X`!e@Ga(o'0D!0+%!/!mtb)ZqP^XN'k.)J^dta$%mhB)6B&Wh&p`mKboFfrbEnbOsZE+KUNCLPN+VkN3N,]4+SUrZa3EL7$fJ!7p65)OB/f0d!_:OUJBWC,*?&!N]K-1b_$t-J;-^7*JLTR\\#.q,QQEW,4)lf&9_2DoIaoo>O^g>,JY55Y#,;R*L&R'l_AM_mj`"".C-hN]Lb,Aou5=U7*b\e#i"'Yru5Ce4R[b%Cfr]`&[q?'r\pRfF5Uk-;m7iV3<6.-MqP3K3:G9W7].I%[`r!%&U*CB=P&J$fCdkGhfHj1`iRJkKQXtY#1m(c204)!Y,Mo(So0@%@km8_@KCA))qB+])e=ppN-YO[l3W.-:*<"hJH--pV@5W\okEl!O["aNs/.Z\lSI^dk-R*dPoUm/MEkhs[]cVC5[1tjVZWlfpX9/KT(g\7V=))K8^4^T]01\3h6m<,Wae"L3^]fL$g=`U&luY\PQck]-m+F_B8:Q"ZYqS,o]u%5GIn!8(ilOKbgGUG%S7?C*O&?.K#,XWadI7qXP[f<_d`NM#pjIL1Dil1XNj[t\9s(h![GJ(7=bqiTLc*3t=,]#[*iZWf)u\m@Y>euOMsrkY^t"i0t:`9]R(0tY-<)R_RtHJ\XQk1s.Xm*6bdG7Gen/r,hM3Q)hZ%_5HImH*6e&@4HRR,'cUloT.UH'c!#K\HB1:%sCHb8\;NDZK*5N3FgfOH$a*>qUuYeu!HS]'Duq0?3W]Mf&9qoe,:u1:6/>jp&(j1?AU(fP3o5=,:_LL/TdL)D\?[hE6R.?]a_pN-Gge]Cc6,lNK,T?@,>+(W]\YXH*P#\i5.^D_^S7`j@5I_VY/M('*ZH&U*D-1q6km$)M6-Dp3).^j04endstream +endobj +xref +0 9 +0000000000 65535 f +0000000073 00000 n +0000000114 00000 n +0000000221 00000 n +0000000330 00000 n +0000000533 00000 n +0000000601 00000 n +0000000936 00000 n +0000000995 00000 n +trailer +<< +/ID +[<292de92922b99d411f4915f95f3215c2><292de92922b99d411f4915f95f3215c2>] +% ReportLab generated PDF document -- digest (http://www.reportlab.com) + +/Info 6 0 R +/Root 5 0 R +/Size 9 +>> +startxref +3342 +%%EOF diff --git a/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.json b/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.json new file mode 100644 index 0000000..9075ade --- /dev/null +++ b/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.json @@ -0,0 +1,33 @@ +{ + "page_format": "A3", + "orientation": "portrait", + "page_size_mm": { + "width": 297.0, + "height": 420.0 + }, + "seed": 224, + "num_arucos": 10, + "aruco_size_mm": 50.0, + "aruco_dictionary": "DICT_4X4_250", + "aruco_start_id": 46, + "page_border_margin_mm": 10.0, + "forbidden_rectangle_mm": { + "x": 143.5, + "y": 205.0, + "w": 10.0, + "h": 10.0 + }, + "forbidden_rectangle_margin_mm": 30.0, + "placements": [ + {"id": 46, "position": [-103.49, -14.02, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 47, "position": [17.33, -11.06, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 48, "position": [14.91, 182.28, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 49, "position": [-133.58, 94.98, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 50, "position": [82.51, 132.7, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 51, "position": [147.43, 136.07, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 52, "position": [159.13, -7.87, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 53, "position": [-151.21, 155.25, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 54, "position": [80.2, 56.76, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 55, "position": [94.41, -7.18, -27.3], "normal": [0, 0, 1], "spin": 90} + ] +} diff --git a/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.pdf b/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.pdf new file mode 100644 index 0000000..fe1265e --- /dev/null +++ b/setup/generateTabletopPDF/A3_10Arucos_50mm_Seet224.pdf @@ -0,0 +1,74 @@ +%PDF-1.3 +% ReportLab Generated PDF document http://www.reportlab.com +1 0 obj +<< +/F1 2 0 R /F2 3 0 R +>> +endobj +2 0 obj +<< +/BaseFont /Helvetica /Encoding /WinAnsiEncoding /Name /F1 /Subtype /Type1 /Type /Font +>> +endobj +3 0 obj +<< +/BaseFont /Times-Roman /Encoding /WinAnsiEncoding /Name /F2 /Subtype /Type1 /Type /Font +>> +endobj +4 0 obj +<< +/Contents 8 0 R /MediaBox [ 0 0 841.8898 1190.551 ] /Parent 7 0 R /Resources << +/Font 1 0 R /ProcSet [ /PDF /Text /ImageB /ImageC /ImageI ] +>> /Rotate 0 /Trans << + +>> + /Type /Page +>> +endobj +5 0 obj +<< +/PageMode /UseNone /Pages 7 0 R /Type /Catalog +>> +endobj +6 0 obj +<< +/Author (OpenAI) /CreationDate (D:20260609100923+02'00') /Creator (ReportLab PDF Library - www.reportlab.com) /Keywords () /ModDate (D:20260609100923+02'00') /Producer (ReportLab PDF Library - www.reportlab.com) + /Subject (A0 poster with random ArUco placement) /Title (A3_10Arucos_50mm_Seet224) /Trapped /False +>> +endobj +7 0 obj +<< +/Count 1 /Kids [ 4 0 R ] /Type /Pages +>> +endobj +8 0 obj +<< +/Filter [ /ASCII85Decode /FlateDecode ] /Length 2250 +>> +stream +Gasb_c&UjC%#"*@'L!jj:dg/icS[tN92[l"9gd-nL7tLL%>OEW=N_Lh[pFT-]cp&?4m9C1lc6,l-KdJAF,3uF!FYX=,OD"c#\uFglsimhX_UXuWEi/K^.R#B6+'u28J9RVaHtpKDVlQ)Iin-O(B&5p/\IjcTp8Eg\.-!tQ76h9%^AH#7ZYa(19@4.Ye?j\o7YaFC!6X`Q@fC=\MkoAK\bJrG'cU/m1!IZ>LOn#n85krj+ae8+_G#4cH?.M2Lp/UehR>0di5WrQaD@GP8DUSYVmR'f";$bH&Ghs`^@?Y\R-Sj2h^".#@Vfd#eP.CfUYCN`q>PkP,R9q7/C%Nqb-m(l?UlO7pCCecpR;c,T,9Y(jdnI4*BAJ'4Q;kVlHnj#g99UA>Q_d1L6sh-XFjem&<\]`mX,UicSL*Z*"[;V&@,`HD6mI[os-nTjK,cLV:O3];I+,ZjXR8#?-gF"DU"Rg`9J0s.N#3r<%;CZXrBaS6&=%s\IXLEb(1u6Ed)ssjUecd/S8:,aI:sG7\ZCXU).84\0:54L9Q$[&9d4:Yb"5Bo0Z%Y!,FRpXQ9Gdjt13$RpHpr_F(s:XEanMdi5WrQhhR*U).330>Q:D!3FF:J@-@@2QY*GE_4J8Yq?Gt?(N]lR>4+MXViG?_id-FU("<7,cQC#dLo+fLt+A[pBe)V=2"U,L9Qt2S`/FmjaaSi)o.nKJ9AOggiIPM!5$8\@Ei$#]!n&LD\DKK\R'eD]eSi!A(L=URkOMaQ]DoZ:%KL9)`2r,5Z!jaL.At*>H5a,]4g=#!>!"_Rk4LSVog-&NBI%I)A%ABt7Y02=WDggh0iP38p+U$"h+U`a@QfLS\LeKTVUW6/`8h!`C?&H/C(u6tk7U9ec\lNDp11_YN*+:F%J2LsoldfOr^$.t2>8s6V5QInI!3hak^fh4&lT%@L>W3U-'"&L92X4F(l(r9XQ:;HLXn1i(j_jo.Bre<52T/Wk\]kJddWL6dN#e^pW\/o(>BJtpCDd9-*Q'r;S@;M6@R/R_%:Dm_^m\NU-=]3>Jk1]#QOpJM5tANPPnfN#^m\MYh3.kIh-/$k253L8A(CX'T+_KG0uU.tMqMt7EAr;_W<1W^G$@>dD%0qJ#Qq0I&@1gH/ubbeT%EqfGu`/\;E8=U;Ok@og;hh"Mkb_b,)2b+O5eq6NR7@r!3eOlJ@0b:G+V&+CH4e!I/euU,uM#fCPb2);15E37E\Y:\"U?GUFE[!>?]6K>E:99CgGek]jU3\>+R8VgGm<$WYJsom=WGhYCJRT5o[OEDstAa%Pd9q!S6lE)ReK`b5'O=0GXr+`r]\Y2h^#D!7nht/'6rY8iRRdlT"sHjT)NN"2J(MJ2bY;>GTYapWPaTR.VF!eoCamo*4D9Dq]H%F7V!W(:3E<@*KY+g)'pnjUe\*%Bm11'$JdiRH`G?jO`_eCM0i2W$l1^o&6%r/6k.KDr!#=%u\pXQ!23l_Ya(WT<3[(F*cpS]>qo=hC!&84d6e%reW(C9E=&S2:G)hU`^tK>"X;Qq!G8@MsNJ;U/rDnLZ"2LA\R!#qo]B-1O98;4Zs!AOi9`i,!">,A7endstream +endobj +xref +0 9 +0000000000 65535 f +0000000073 00000 n +0000000114 00000 n +0000000221 00000 n +0000000330 00000 n +0000000533 00000 n +0000000601 00000 n +0000000936 00000 n +0000000995 00000 n +trailer +<< +/ID +[<544804e2078c10a71a717f7cd03ded29><544804e2078c10a71a717f7cd03ded29>] +% ReportLab generated PDF document -- digest (http://www.reportlab.com) + +/Info 6 0 R +/Root 5 0 R +/Size 9 +>> +startxref +3336 +%%EOF diff --git a/setup/generateTabletopPDF/a3_aruco.py b/setup/generateTabletopPDF/a3_aruco.py new file mode 100644 index 0000000..e917ba1 --- /dev/null +++ b/setup/generateTabletopPDF/a3_aruco.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import json +import random +from dataclasses import dataclass +from pathlib import Path +from typing import List, Tuple + +from reportlab.pdfgen import canvas +from reportlab.lib.units import mm +from reportlab.pdfbase import pdfmetrics + +try: + import cv2 +except ImportError as exc: + raise SystemExit( + "OpenCV ist erforderlich. Installiere es mit: pip install opencv-contrib-python" + ) from exc + + +# ========================= +# Header / Parameter +# ========================= + +PAGE_FORMAT = "A3" # z.B. A0, A1, A2, A3, A4 +ORIENTATION = "portrait" # "portrait" oder "landscape" + +NUM_ARUCOS = 10 +ARUCO_SIZE_MM = 50.0 +ARUCO_START_ID = 46 # erster Marker aus DICT_4X4_250 +SEED = 224 # Zufalls-Seed für reproduzierbare Verteilung + +PAGE_BORDER_MARGIN_MM = 10.0 # Abstand aller Marker vom Seitenrand + +FORBIDDEN_RECT_W_MM = 10.0 +FORBIDDEN_RECT_H_MM = 10.0 +FORBIDDEN_RECT_MARGIN_MM = 30.0 # keine ArUcos innerhalb dieses Abstands + +LINE_WIDTH_MM = 1.0 # Linienstärke des Rechtecks +TEXT_FONT = "Times-Roman" +TEXT_SIZE_PT = 8 +TEXT_GAP_MM = 4.0 + +OUTPUT_BASENAME = f"{PAGE_FORMAT}_{NUM_ARUCOS}Arucos_{int(ARUCO_SIZE_MM)}mm_Seet{SEED}" + + +# ========================= +# DIN-Formate +# ========================= + +DIN_SIZES_MM = { + "A0": (841.0, 1189.0), + "A1": (594.0, 841.0), + "A2": (420.0, 594.0), + "A3": (297.0, 420.0), + "A4": (210.0, 297.0), +} + + +@dataclass(frozen=True) +class RectMM: + x: float + y: float + w: float + h: float + + def intersects(self, other: "RectMM") -> bool: + return not ( + self.x + self.w <= other.x + or other.x + other.w <= self.x + or self.y + self.h <= other.y + or other.y + other.h <= self.y + ) + + +def mm_to_pt(value_mm: float) -> float: + return value_mm * mm + + +def get_page_size_mm(page_format: str, orientation: str) -> Tuple[float, float]: + if page_format not in DIN_SIZES_MM: + raise ValueError(f"Unbekanntes Format: {page_format}. Unterstützt: {sorted(DIN_SIZES_MM)}") + + w_mm, h_mm = DIN_SIZES_MM[page_format] + if orientation.lower() == "portrait": + return w_mm, h_mm + if orientation.lower() == "landscape": + return h_mm, w_mm + raise ValueError("ORIENTATION muss 'portrait' oder 'landscape' sein.") + + +def centered_rect(page_w_mm: float, page_h_mm: float, rect_w_mm: float, rect_h_mm: float) -> RectMM: + return RectMM( + x=(page_w_mm - rect_w_mm) / 2.0, + y=(page_h_mm - rect_h_mm) / 2.0, + w=rect_w_mm, + h=rect_h_mm, + ) + + +def expand_rect(rect: RectMM, margin_mm: float) -> RectMM: + return RectMM( + x=rect.x - margin_mm, + y=rect.y - margin_mm, + w=rect.w + 2.0 * margin_mm, + h=rect.h + 2.0 * margin_mm, + ) + + +def get_aruco_dictionary(): + # DICT_4X4_250 hat IDs 0..249 + return cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250) + + +def marker_module_pattern(marker_id: int) -> List[List[int]]: + """ + Liefert ein 6x6-Raster (inkl. schwarzem Rand). + 1 = schwarz, 0 = weiss + """ + aruco_dict = get_aruco_dictionary() + img = cv2.aruco.generateImageMarker(aruco_dict, marker_id, 600) # nur zum Abtasten + modules = 6 + cell = img.shape[0] // modules + + pattern: List[List[int]] = [] + for r in range(modules): + row: List[int] = [] + for c in range(modules): + cy = int((r + 0.5) * cell) + cx = int((c + 0.5) * cell) + pixel = int(img[cy, cx]) # 0 = schwarz, 255 = weiss + row.append(1 if pixel < 128 else 0) + pattern.append(row) + return pattern + + +def draw_aruco_vector( + c: canvas.Canvas, + x_mm: float, + y_mm: float, + size_mm: float, + marker_id: int, + page_h_mm: float, +) -> None: + """ + Zeichnet den Marker als Vektor-Rechtecke. + x_mm, y_mm = linke obere Ecke in mm. + """ + pattern = marker_module_pattern(marker_id) + modules = len(pattern) + cell_mm = size_mm / modules + + for r in range(modules): + for col in range(modules): + if pattern[r][col] == 1: + cell_x_mm = x_mm + col * cell_mm + #cell_y_mm = y_mm + (modules - 1 - r) * cell_mm + cell_y_mm = y_mm + r * cell_mm + c.rect( + mm_to_pt(cell_x_mm), + mm_to_pt(page_h_mm - cell_y_mm - cell_mm), + mm_to_pt(cell_mm), + mm_to_pt(cell_mm), + stroke=0, + fill=1, + ) + + +def draw_aruco_label( + c: canvas.Canvas, + x_mm: float, + y_mm: float, + size_mm: float, + page_h_mm: float, + marker_id: int, +) -> None: + c.setFont(TEXT_FONT, TEXT_SIZE_PT) + text = str(marker_id) + text_width_pt = pdfmetrics.stringWidth(text, TEXT_FONT, TEXT_SIZE_PT) + text_x_pt = mm_to_pt(x_mm + size_mm / 2.0) - text_width_pt / 2.0 + font = pdfmetrics.getFont(TEXT_FONT) + ascent_mm = (font.face.ascent / 1000.0) * TEXT_SIZE_PT * 0.352777777777778 + text_baseline_y_mm = y_mm + size_mm + TEXT_GAP_MM + ascent_mm + c.drawString(text_x_pt, mm_to_pt(page_h_mm - text_baseline_y_mm), text) + + +def place_markers( + page_w_mm: float, + page_h_mm: float, + num_markers: int, + marker_size_mm: float, + border_margin_mm: float, + forbidden_area: RectMM, + forbidden_margin_mm: float, + seed: int, +) -> List[dict]: + rng = random.Random(seed) + placed: List[RectMM] = [] + result: List[dict] = [] + + allowed_x_min = border_margin_mm + allowed_y_min = border_margin_mm + allowed_x_max = page_w_mm - border_margin_mm - marker_size_mm + allowed_y_max = page_h_mm - border_margin_mm - marker_size_mm + + if allowed_x_max < allowed_x_min or allowed_y_max < allowed_y_min: + raise RuntimeError("Das Poster ist zu klein für Randabstand und Markergrösse.") + + excluded = expand_rect(forbidden_area, forbidden_margin_mm) + + attempts = 0 + max_attempts = 200000 + + while len(result) < num_markers and attempts < max_attempts: + attempts += 1 + x = rng.uniform(allowed_x_min, allowed_x_max) + y = rng.uniform(allowed_y_min, allowed_y_max) + candidate = RectMM(x=x, y=y, w=marker_size_mm, h=marker_size_mm) + + if any(candidate.intersects(other) for other in placed): + continue + if candidate.intersects(excluded): + continue + + placed.append(candidate) + result.append( + { + "id": ARUCO_START_ID + len(result), + "x_mm": round(x, 2), + "y_mm": round(y, 2), + "size_mm": marker_size_mm, + } + ) + + if len(result) < num_markers: + raise RuntimeError( + f"Nicht alle Marker konnten platziert werden: {len(result)} von {num_markers} " + f"(nach {attempts} Versuchen)." + ) + + return result + + +def main() -> None: + page_w_mm, page_h_mm = get_page_size_mm(PAGE_FORMAT, ORIENTATION) + + if ARUCO_START_ID < 0 or ARUCO_START_ID + NUM_ARUCOS > 250: + raise ValueError("ARUCO_START_ID + NUM_ARUCOS muss in den Bereich 0..249 von DICT_4X4_250 fallen.") + + forbidden_rect = centered_rect(page_w_mm, page_h_mm, FORBIDDEN_RECT_W_MM, FORBIDDEN_RECT_H_MM) + + # Neues lokales Koordinatensystem + origin_x_mm = forbidden_rect.x + forbidden_rect.w - 90.0 + origin_y_mm = forbidden_rect.y + + placements = place_markers( + page_w_mm=page_w_mm, + page_h_mm=page_h_mm, + num_markers=NUM_ARUCOS, + marker_size_mm=ARUCO_SIZE_MM, + border_margin_mm=PAGE_BORDER_MARGIN_MM, + forbidden_area=forbidden_rect, + forbidden_margin_mm=FORBIDDEN_RECT_MARGIN_MM, + seed=SEED, + ) + + pdf_path = Path(f"{OUTPUT_BASENAME}.pdf") + json_path = Path(f"{OUTPUT_BASENAME}.json") + + c = canvas.Canvas(str(pdf_path), pagesize=(mm_to_pt(page_w_mm), mm_to_pt(page_h_mm))) + c.setTitle(pdf_path.stem) + c.setAuthor("OpenAI") + c.setSubject("A0 poster with random ArUco placement") + + # Weisser Hintergrund + c.setFillColorRGB(1, 1, 1) + c.rect(0, 0, mm_to_pt(page_w_mm), mm_to_pt(page_h_mm), stroke=0, fill=1) + + # Rechteck mit 1 mm schwarzer Linie + c.setStrokeColorRGB(0, 0, 0) + c.setLineWidth(mm_to_pt(LINE_WIDTH_MM)) + c.rect( + mm_to_pt(forbidden_rect.x), + mm_to_pt(page_h_mm - forbidden_rect.y - forbidden_rect.h), + mm_to_pt(forbidden_rect.w), + mm_to_pt(forbidden_rect.h), + stroke=1, + fill=0, + ) + + # Koordinatensystem + + ARROW_LEN_MM = 50.0 + + # X-Achse (rot, nach unten) + c.setStrokeColorRGB(1, 0, 0) + c.setLineWidth(mm_to_pt(1.0)) + + c.line( + mm_to_pt(origin_x_mm), + mm_to_pt(page_h_mm - origin_y_mm), + mm_to_pt(origin_x_mm), + mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)), + ) + + # Pfeilspitze + c.line( + mm_to_pt(origin_x_mm), + mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)), + mm_to_pt(origin_x_mm - 4), + mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM - 4)), + ) + c.line( + mm_to_pt(origin_x_mm), + mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM)), + mm_to_pt(origin_x_mm + 4), + mm_to_pt(page_h_mm - (origin_y_mm + ARROW_LEN_MM - 4)), + ) + + # Y-Achse (grün, nach rechts) + c.setStrokeColorRGB(0, 0.7, 0) + + c.line( + mm_to_pt(origin_x_mm), + mm_to_pt(page_h_mm - origin_y_mm), + mm_to_pt(origin_x_mm + ARROW_LEN_MM), + mm_to_pt(page_h_mm - origin_y_mm), + ) + + # Pfeilspitze + c.line( + mm_to_pt(origin_x_mm + ARROW_LEN_MM), + mm_to_pt(page_h_mm - origin_y_mm), + mm_to_pt(origin_x_mm + ARROW_LEN_MM - 4), + mm_to_pt(page_h_mm - origin_y_mm - 4), + ) + c.line( + mm_to_pt(origin_x_mm + ARROW_LEN_MM), + mm_to_pt(page_h_mm - origin_y_mm), + mm_to_pt(origin_x_mm + ARROW_LEN_MM - 4), + mm_to_pt(page_h_mm - origin_y_mm + 4), + ) + + c.setStrokeColorRGB(0, 0, 0) + + # ArUcos zeichnen + c.setFillColorRGB(0, 0, 0) + for item in placements: + draw_aruco_vector( + c=c, + x_mm=item["x_mm"], + y_mm=item["y_mm"], + size_mm=item["size_mm"], + marker_id=item["id"], + page_h_mm=page_h_mm, + ) + draw_aruco_label( + c=c, + x_mm=item["x_mm"], + y_mm=item["y_mm"], + size_mm=item["size_mm"], + page_h_mm=page_h_mm, + marker_id=item["id"], + ) + + c.showPage() + c.save() + + # JSON mit Positionen + with json_path.open("w", encoding="utf-8") as f: + meta = { + "page_format": PAGE_FORMAT, + "orientation": ORIENTATION, + "page_size_mm": {"width": page_w_mm, "height": page_h_mm}, + "seed": SEED, + "num_arucos": NUM_ARUCOS, + "aruco_size_mm": ARUCO_SIZE_MM, + "aruco_dictionary": "DICT_4X4_250", + "aruco_start_id": ARUCO_START_ID, + "page_border_margin_mm": PAGE_BORDER_MARGIN_MM, + "forbidden_rectangle_mm": { + "x": round(forbidden_rect.x, 2), + "y": round(forbidden_rect.y, 2), + "w": forbidden_rect.w, + "h": forbidden_rect.h, + }, + "forbidden_rectangle_margin_mm": FORBIDDEN_RECT_MARGIN_MM, + } + + f.write(json.dumps(meta, indent=2, ensure_ascii=False)[:-2]) + f.write(',\n "placements": [\n') + + for index, p in enumerate(placements): + item = { + "id": p["id"], + "position": [ + round((p["y_mm"] + ARUCO_SIZE_MM / 2) - origin_y_mm, 2), + -1*round(origin_x_mm - (p["x_mm"] + ARUCO_SIZE_MM / 2), 2), + -27.3, + ], + "normal": [0, 0, 1], + "spin": 90, + } + line = json.dumps(item, ensure_ascii=False) + if index < len(placements) - 1: + line += "," + f.write(f" {line}\n") + + f.write(" ]\n}\n") + + print(f"PDF geschrieben: {pdf_path.resolve()}") + print(f"JSON geschrieben: {json_path.resolve()}") + + +if __name__ == "__main__": + main() \ No newline at end of file