From 18ae7316761ec47c6968da120dc22ac1536c7a6a Mon Sep 17 00:00:00 2001 From: rongjchen Date: Thu, 11 Jun 2026 15:42:36 -0700 Subject: [PATCH] Add files via upload --- 4 beacons ver2/4 beacons data.csv | 5 + 4 beacons ver2/__init__.py | 1 + .../__pycache__/io_utils.cpython-38.pyc | Bin 0 -> 15565 bytes .../__pycache__/math_utils.cpython-38.pyc | Bin 0 -> 1786 bytes .../__pycache__/model.cpython-38.pyc | Bin 0 -> 2711 bytes .../__pycache__/position_data.cpython-38.pyc | Bin 0 -> 1902 bytes .../__pycache__/solver.cpython-38.pyc | Bin 0 -> 5479 bytes 4 beacons ver2/io_utils.py | 649 ++++++++++++++++++ 4 beacons ver2/main.py | 122 ++++ 4 beacons ver2/math_utils.py | 51 ++ 4 beacons ver2/model.py | 82 +++ 4 beacons ver2/position_data.py | 71 ++ 4 beacons ver2/solver.py | 202 ++++++ 13 files changed, 1183 insertions(+) create mode 100644 4 beacons ver2/4 beacons data.csv create mode 100644 4 beacons ver2/__init__.py create mode 100644 4 beacons ver2/__pycache__/io_utils.cpython-38.pyc create mode 100644 4 beacons ver2/__pycache__/math_utils.cpython-38.pyc create mode 100644 4 beacons ver2/__pycache__/model.cpython-38.pyc create mode 100644 4 beacons ver2/__pycache__/position_data.cpython-38.pyc create mode 100644 4 beacons ver2/__pycache__/solver.cpython-38.pyc create mode 100644 4 beacons ver2/io_utils.py create mode 100644 4 beacons ver2/main.py create mode 100644 4 beacons ver2/math_utils.py create mode 100644 4 beacons ver2/model.py create mode 100644 4 beacons ver2/position_data.py create mode 100644 4 beacons ver2/solver.py diff --git a/4 beacons ver2/4 beacons data.csv b/4 beacons ver2/4 beacons data.csv new file mode 100644 index 0000000..a66d008 --- /dev/null +++ b/4 beacons ver2/4 beacons data.csv @@ -0,0 +1,5 @@ +Point_Name,X_m,Y_m,Z_m,X_Ri,Y_Ri,Z_Ri +beacon1,0,0,0,-0.0857,0.19,0.4667 +beacon2,0,0,0,0,0.2159,0.0381 +beacon3,0,0,0,0,-0.2159,0.0381 +beacon4,0,0,0,-0.0857,-0.19,0.4667 diff --git a/4 beacons ver2/__init__.py b/4 beacons ver2/__init__.py new file mode 100644 index 0000000..d705e69 --- /dev/null +++ b/4 beacons ver2/__init__.py @@ -0,0 +1 @@ +"""Four-beacon beacon solver copy.""" diff --git a/4 beacons ver2/__pycache__/io_utils.cpython-38.pyc b/4 beacons ver2/__pycache__/io_utils.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..46cd461002af2556ffa650a8963ece36bd61c17e GIT binary patch literal 15565 zcmd6OTW}oLnO@(f=Yqjta0l-#h!Qz8I3g)XmMD^wo_9-}~#s!#NF~Gyl1?`fG1#+JB?M;Aa|zi&|0h zPU)H^w2CHlVKnu1qhjckH=E|VRk1j4HB*%o>a3<+N%L=}k`0Q;rJA|*p~{f1y`!~e zEX~cY<>Y(1Mt{Pp41Ggu8P~NhX~GuiZ)qa!4&OB^1(Y))i*oj^Q5g{zMNSNf{KtkE z7KLx6Dn&6OiXZD@RE&Mgs*H+pF(D?!)W@cn7Bi?BL(M+1AMFo_gD8)qJR%OEd{`Vo zc|yD-9v4Ts-xK1=k5l59IF4~AF>gtnz`Q3g@07SCp2NsFe4iI*(VG4yxZhEvFL0i6 zSAgw}r@rQ_xXq3$J!h#c9e>5e?}j|N=+^4(mgBXX zw_I7y}H`*<9Mni8yy6e>GR;{_=Vga(f?$n&6M$;|F-J3Pf$D*X`_C}&n zq>B;aC3f0ggU0lnD-Gwo^R93^{>q&5jX9@s!)dfIk&oTcfN!*fd&li9zu9PEQX1}S z9k=z>w+ENxosEaDwl>xmT{*Wo7tj!2Z#8R+Zu74^2zA4}*h0f|*4x5m2-lIxIkZt) zn&bJk#ir|Y+6};>-rVrGmH^#o0jyrKQ`x>91Br+0G;XzhoW;!zw^et~V!+dLXXc)s zo1Z(4@6&T<%DH=VUrKs3RBN@`evQF|qV;CYUs*sVTN9$n^VSD%7Uh8)>UI^8akYw$ z8)gW-VhRJ;V@Bgw83`8Juei4wb@x>%+cG$DrPlBOoZBmI%W1WHh{8=AfS`7()&K-? zzzZcS%2cbZ+PYhkz{&mcD`($ZX|KDt z+j8x_ORv7_%%6Goz4;hUo&%IU^IoG}-S8VtuiV*`DfHZruV*01=v>~jV-wGqo_raZ zojVq$uy8w5!p7~iL|SBUyR#yPQVP$gg!_+@4GKV|f*3(5EsB8EXf$-i?KEqfZ?rla zesD$s?qsvE#)vFj$yZNu8{8eI*{%bLlL~01_s`jbAP%F3VU#&idl4l{&^qd|{4D7L-Gzo@}4OAcdRF@-YUMJj2K9r~Fd6i?x zgxadMr3-zVR%(P;W64;E47rGPswK8@UF9g6mUKxycCRp7t%9n!vRZutPva#P^k8ye zEm&gQrp{YHL;6s@2eTOzXf;Ax%gPD0u5IEASd*k{Q8IWLQL5f-dv3|%)kGQhPQB`H zcHGEBF3S3{R(BU`^|jsW1SH)%1j!P{dL$%OlPI;g>AT(z(9uGBpnJBz*o-U){b~%z zZLO<^I%FIRnHT;SI%|@S0zf9nMj4P;K@^fO!Ck{n3DS{8RlD{fkKoBk zd_6iILI%sg7ioSMS|npGlrqvrQ<v|V5M;9^&B#1dn^G!$)uj^XZ)U}}CTf)MfGv7Df(Vjui zuK7*#x~8NG+(xZyh1OPzuqrfGZK5zDy=;ig2SzuACux3?^1kUq-ngs3uZO9w=4V6g zu8!Z_)=;Qz>s>pvSMwsbPZL9Hn*2WI%yV0+HBdXeX3GDF+5)#j%}3fNM%-c;GLZgl z15(8Bw~Zw{TP^q_V&nt8n?~#8DI}#`Pcwc|jN(p=srpp zU*6G{7P1b$@31sg^PMK7Bj5RABJ|uNx)eP3+9ToE@9f^G)&1tCGw+bNFy|~oM)*{Z zQi9fg@23wQJP3|o-Kf`H&s*APqG1zf1hDQ}JVddxPVg)pARZ>H6*5X9Y_YI5Af62f zd*#wtl#WkGK8}^kqe!Cct9R;<%^<2pxwlE;W0|3nx!v$rgsk1JjMTg}qU{7Z;{M>D zE-rI>Gs<7tX!(tG)q}ZJWOPJ@iBuH_7p1(FTF0$e4e-uN#&46@*4d0wOW2(sr8*Mm z5?M58DJy4bNxgMR5)2|sw%dM`NzSS=q~K8{nK#NT#?mU-P-B21F+a>LyM9%*ejX!v z&mqx_jBe-?dQm@U*g7Z_O7`crl`$+GUp&nj1>_2-FFdfU!}?*Ph_a*+f^4(l`R`)q zHzcv`It`e7i01dm(VHl+5@RqK0uhNxkO2~O4kQTI5hMs?3M5F6Ns!gmzo{oAh(#KZ zv@LU6dj&)V*EStIeuagJzOa_aah6kka?=U+!$in!(_GCAA`{ zCIN`#VMx5VW)0t=vp?%}8Xbtbsxld4;i0VVD#l`h)-d^uqSi?ZfcWp5J;0fM}TB_Pn`5cR4-UVcmJ|(M?S8d9E zyWRBUSqu^ly|GR(h%uT5Cy0P+(o@E;-OCu=GutAqhW4fz1~8`x+%O&(hVl7?C0{~q zWCA536PO$vQ49^)Ifqd^{<#rzJjmx6+$e>;K<(R}Vr+yx$%hy)v0i3u{~j4&fd**- zzaGdv$qx|2AS-kY-w44jgbC*IoDWer)IKtZfskr_RSQn}mY?$N5CZZ(;1KU3y8OLRU(>xGw!mW1HXE8B zX(IOta9YhDn!=vZLMu!ynZVzn;Pjg{=|Rttz{pFMBagSh1jE7a?J>6RE)=AJJJgvuFA2LNe zbga-SjnfH6%Fbo1H(5cslmVIZWut?iPP5@hCdj0G8q>>{kwhuk-e?JVhAPTPO16B3 z@~`s4P7MfI8CG+|T5XittYTMDDxjY|7>X{QTlHK5U6nxB!bl#`a?m$WGKhH)@e24Q zT9*gL7VwubrjWA?$<%4T!qkT%siD@lMcC838!*#+C}tk?tq>+2 z#yo@b3M0)J3sbYYOV;7A@kUv? z;>*T1mDA<&e0dMQAv9!u;~>NQW`_6;5lIQOqTeljdk6P182(XeqXUZ?sQ-ENj4Zl( zyMQcT#FM{^uSaCq#s$iJ9voMIPgbj-_AnlFUw?to3=;E|$hbt`%gA4r$1zy&wVeYf z!4d+BUxPL5+LgIW_`f`N?Xr`Iifx#Wz(7DBQL_$B$(bE!Hd{(XX|^@g-9Bc48M(e9 zGp4+PX(L+!H%c*`+_m3m;$eH+CIBdoXB)-OZ98wwFxRsz`PcC5C-~y?XD{9#8d%Uh zLYZJ6@y~Z3LS1e!1-yTF;Q3xYc>|lW+3@8>BlWg~Qd?buo=(!K+Rij2du1B)syIwC zNii-fEGFDYK2yk$mS~b}vI2G>fF;bjGzlFMtm`ndE{(*n_jO-ig;j&tD0Dn%f2)>A z0gqon4j2w?4JCQmhpm}0nQAkr1>2j@z<`(VMB%svo6`_9H)vb?G}0F1vdv}N*SG92 zCGui;+1NJTGrH++hU@Hsx@JDR<7&pu<8)~O70FNHrC)^U?QNR< z9ZIfK@(m=x0G zaJg@m=a(d@C=Ad0dTD`)YQk1he3qZ;m2|wW0T!M;x9qmuI~{r9FVLS=90+L-f)fgi zJz`c>T+ka}GD*i^vbXO(?9cApcEZ0s+BONp*!E%EQm72DFvG4i4NH@N@&Fr=6>pM6 zbnAZllJYObTRR_b%~TvyZ%7g~ckvL}LEY#5{n!t7oB=I{nCUnz<^(d#vS-zDLLE@Y zRTa3baSrFPWGdAWASKfbFGj&QUPu<=0j%B7A=HA})QZI=l9ZT8ZJl z#%;MCnd`MXN)52;Zlj5EqlHoBGGuo@ve(I`)?%j0GGSz`!rK&OUDi!-r%O|^PFoU` z5I>hLO>bh#O6Cr!2k_8UGORC9F4Y8N?o{lR#_|e&xwsh@H!G?2hM-#dffZ&Qh8^f0 z4FR_uOeMr^JqBoG(WA;pavx)c8)XH6RD)RJRdQ>t+d(H7fPm>_XZQR{fdxueRF%x3 zj)V_~CD`dloWdVsOCDj7mYyQ(3#5&VF@`U+ZDT^8fHspiNr%DTXLg2kaQuB{r(t2{ zR)&7?ft8wpxMt{6pIcTDy3Q1IAu1Q3soM{X2Qc!Wlrv!O;S%NKcQ7ZxcgKf>26j08 zEoAmOoFMR$%Lx=?IRmGY0~0N|nduKqT@*e}lbab1-4xfs*$f9W9Kh^`qE?e!GbN_k z6*U8Q^gcMT_KO23XT%Kta^fKR91@37lNCpBH6M$nzMgOibw{s3ELwAAaOvfC3ywA4 zfjyUulYM4F^c;$*3yUZut+rxC38xv5vz5_E-7->;P5qGS=K~j!;8k*L0X0A3Z_+!E5}qN z33myOMFmyYa)HZ>?O0DCa-MuBPvnL56gn)TZIIH6kU5kc9VRx&W)8}*_QOgKOa59~ zoOaI@ zyF9lTCLdI3XhBK*&kq`g`{zQAC)mlC6Wh7hllT*RuBQ~$$94j;&$1Ik^qC7J@J4BM znDa^I%>5B{N++Sw0ySJAe;Mt9uj~Y9Z@gL6dUnoaN5#~T!p>_pY<({U;svv8pCM$$Q3Cn z)#o1M?HVEOK?0DL&49wFFWLPp%huyRSdeZ07Eu!%dHD70xm@KU27?!`uilTKqE6_W zd`N$y^JH`;J&Wusp&mj_=$b$|4eqU(Wgr;bJG#7v6zI2VLd*r~uA0QwVM{^nU44pB z*Mvxpuz-CUQs%15%6gaz&23V|%>{MmiN}`gXm|%e<=;jv8QxVGgZyn2*>({Xt8`RQ z%i8NzMyOIsLAkfJC?6v(i<;7>t_K%1WM>Rh?anMfjhry%Iioi_9I_Mixf07s<&pK zW#BgrBj@Y*&1_}K-$!>FQWF8TSI0Tf19II$UqL;2@>la~mi(LiY*_4lrpe!+v3zZ- zKy6^kwJ`e#G)1L-rf4mzHknSL4T05Qy|!3?|WCZM{{7kRLB$SLxxYbmG(rf>WIWjOZ&|FzCJ^X$G#Do(V z>}@r>W_kaGSlnt3%<)HR6>wRDUVzf1!?dv14EdSJ0g>`88|pNEMIPxe@3ugf0W%L% zOI9TcvDF_|*_hs7r%NVG7#5c;+WC^XOz!8A1(q+EeFTqR9gMMpKN`VRw^ztROEiWJ z{l{e!C(&29zT(!`z)DG)I3EnpscM!Y&NCamN7~1=;MAKcE(_z5 zB-Da$cu^@kr=1RhyW%0BZpPJe1^r4R^0#REZAuvNe~)r(zvVmBzb`*W4|>clngIht zlvph1Kql_VF-%H{yvi*kh`C@MMVMF_QTssaMHh?=x&?2t0tQI?Lw#)tihk0rCo5HA zSt*g7DuraWn2qkbV&B7LHM(Nt2P5)Z;4jP37Os|2gzOAy`*SO0=qDanHeE9!(iwwz zUe1^>A%TA$9DC%I+r=VG`6(u%@D;TWb>{}TALZ%GcaE$Eglo8C2BKzwH(WANI!IxX zTuPyI0p2a(mbzPODS4Kja{Hb2SE!ByEI|~19iuDO_iL2<5+%Pu2_QnjOC+a&Rr9gX zvgP+F*#*MCkGk&?2n)#cqH92lW{lveM}g%LD6s$*%B%QAnur8$`Ad|L)KBt9pJW=c9$N$VO|E3cZgFZl~da_(z>L?%3!Q6^Cd@6BPTpI{_>{EKm>b@AH3K6+b7;^a8!CkI_52>{Bo|0ym- z)*>9Y^6z25(j=_&q#$P(Ya0SmA8?+)tn`aKX$eW(Ni;$M2<;7sgD#Be!h`U^K0hEn zyxLGcR0+2U_PD6n+fNk%s4Hy&GqZ~9Ow^UWsvQl%gtzQgZ#M+u`|_%=LfQy4_oZf| zQ{8;DP{}-n!UU4f31kSP7{9* zL$o<9UqM-!p>2^t`5MY93@IaWC<~N_xP2CLXwpZSLyvqtL_GyH@mzlzkj82@jqb!d z+RY!wK~jH$2!%6XsQm4XIP%)K+R+^M35x zVkDgEg)#9gFakRW&cfa(=cf@|H62cWsCzjCPZfEU(_$D>ECQ%tB%V$Jm(&{3=Mw72 z-~-geI9u)}6!&b6p?*@WAnAjesn9~pX~mJi`)1Y*jBy7~_R-E;*O1#!Is6_7GfB@d zBMx%Q@1y=un8g@})flZ=)EtR>IBd#(EG**rY)C;?P$YS*$HR0omY-V%Hx);h3_mZP zfbce>YW!g??ehy;;Gy09SlyGD(~u`H`!SXCN5F?i!XnJ!W8t_s{(;>B6uSo)8d%UbuaKNcRwX=Y)TH)Rge zfw@NG{gGNtxF5dy)SAULC&fuo7EgU(Y)xSwNBrp! z@af_n^C=thM;{tDJD~#`-(%fbe zF9WD|Az<*mh44aU3pze%3sO7@RylB}z7>dk-sk6)QXU)|WO6-A&7$kyb`isWnteRf z;R5L`@(+-Zta!(51}ArLyk1uqAzg40&4P>1vL%OG&#L~vPyN$zR8BA-kD%7aF^q6B zB?H8*)F3@}8V~djX*DD>1<&*z#hiA}N)k~bSq`N6*MzLM;N=MncALm5&*3&E%SDvH2MCv}L?2;phB*#<}Mbq)!oKsTIsIhc7V zc^M)ZfrG{nb~yu6D-Q+R9qg$ppbha%PGFIbD#$n_u9VB=;GkL;8TsN>@Jl*mGr>6d zca*J)U1@;>bQ;8<0ZcpYl`xw8H?-AruiokK+Nl>rnX`#4Zs1iI*yw0Kakv0loSu97 z*}13B%m&Zw?G5wrp7y88r|_9O`E2E0~W$ArW-bmK264XaQF!`PY;zQ}Q~JYw{-2 zAPsQWZo~1oMx|nDZtz{G^Tz|Tk}* z%>5O%as{SYqFqifmXN1JE%wT#!^*--7D||H8PM_~3Mv5D#)}c2??t1pL5+u_7j&;B zH~^EQCHWnnzW$6DH&?bjWrg|yjc5_fSq?f;09o}wZDDHZ=2CI6g~ zA5!wil>7@y`e*kqsrV<9{3}Wxg;4U}(bN4%D#=Si^lk&kNbqE!_ zWrRqjD#4|aX6FGc0C^_PRS%)(byQP)c6I$$B}d_<%EQkwWtAca_=swc8uu;Gl}rus z<{fem_%vghIRgBHab~lU-30CC_W|)%W|BpK4h0~J0VR|4%ZLN66*_#6)Xbwuv}t8J zSB|>ADLpo+!=PYrfFhaJDWrDFqCo5noOTmzTvvJ0sbK|#Zk^+2IoJd_kQ;`H0vv?} zBQs?GrIoS&(#Ckbzx#I9_zPpiegN~vPYpYRU`Pvwj)Q110e2xe2|N)h(W&O-G4 zscjd?+K9hAOdSP0p^!?rAh9w-f9^rXhGP;INE&B~$D#h}?V5tQV@x)x{H(F2FqZsJ zfKy4Q_q|y2|LZ7nEJbNzLH+|2ls9Q%L6I(E^-P21KcZZ}%Ri>#KS466gY)B_B3_!| z7kcQmzNHPkn1$sSX6JE{RgT_ai~5UiYqvAyrJXCQ}kdsdD(!&nAL5& zhzIg1O2~=97nft^)s=sf>iB9B&EQ+9E;#Lc0F8RKiYRkVy>@tkaL6NTkTsOY87RD= qFl?NWiPwt`jFk%cv4h3w;sl#AKAU-`@N?Q!&#;jgI3E7{KmIQz|Icm! literal 0 HcmV?d00001 diff --git a/4 beacons ver2/__pycache__/math_utils.cpython-38.pyc b/4 beacons ver2/__pycache__/math_utils.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..626889b15618f2553c4259e887a269c953ef334f GIT binary patch literal 1786 zcmZWq&rcgi6rP#gwHF(xLudk`t>zLXmcXh2RV}L8B!JXIh^jW}!9ubY&)8nGzcf1z z*jg^7SN?*w5;^8y+G|hTdX7}J-;7NHbXJ~yKW1m%eBXP|uNM|-1mm1P9*!!6{AHAz z!-4XYG)R63L69~PRIrG~tj(yAoycjs!WEvV{J}(3_`kVr&+2o~YoZRl@$3(%`p zUxfb?(SYs?=V#KqH`zT1^nj0a7==2NIq#;)8>zGuyr&@0XT6#i{GkNtd#+Bh`@_7F=L)-xB<~Gv})_FhGvkjWnk|kML4RQ-b>DiOY zs*?7A(#^w(ENj06(Mawq1spuGGOf`ERNaRR@M)%8NVh;Y1RyV` zpud2iNkPu(8PRkI;0q$yDyhRJoK@0yrn@HBbOT zOuGeeX&4#FS>S1qW#A;-!dZb-uw}AB&QQID^N!qXnAcNUdx1GC)IsqJk%l1X0=TFPXZMRKxseF+|q z(9w=0HrU+b)1-TVWv_5kH#iOR(m$3m6Jea&Or1R1we687AHoQ147S-!q)M%)>Dx3` z<{!cmRJsZFPzKhr5&`_E4-@u4XGq5A5?{d6_KYRJONwh6^b`1JtSlNi+>x_pxgL literal 0 HcmV?d00001 diff --git a/4 beacons ver2/__pycache__/model.cpython-38.pyc b/4 beacons ver2/__pycache__/model.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..809c4d3c23283ecd887122aa09c785ad0cfc7e65 GIT binary patch literal 2711 zcma)8O>Z1U8t&?^>G|{|nK&^9R4Z6TqfO=jQM7{9Zb-;#F9ZZy4vpDXd#c9Mc7IV- z z;eWf9^`n_=wg8j+*3gpQV8E=tB0ek2#-M z;~UHHVIer(=kc`2sVvfCE@&33ImH}f#TlK6BBKwI{euOUa+2MqWg(+77AlETGc82I zbG7OD;FAv~+2q{ZeoU%voacp#RZ`^g@z|+av5clg`gY~8X;yjih(9TpJ9#2ZLKLF1^RjZ~7eWbh z@o`J|Fv0{vm^7?hmds|Ahu55?As5|9^V_oxORH@h62i+I#Tm5xx7? zy}i5ZG?Dmy@z!2eFrH4zlPcI>=Z49DX}-7YfCPAY#Ml9>7{R;jT;ewN#1O8EOfQcm z+VD)xi7%Qtws~sk_Amfs1|DN;=wKbp0o|c}3h=aj(>l9^@6XC@;8Wz(V-DVU8n9vJ zYwUD})kCXmX?NEGfbR1CM}D^ zA^|DmGl0y_4V(bW7|}yl`2aso4=Oh=L?)mrR^{*$UU>zw%p`z`p}20k=irJhvn*&5 zN}bezHr+MzqPub69XO=fq6Q354Qs$UM7#>$*b!G?n%t?OvCYzTGkNqJnqy2hplR4e zPHjdIa$7kyXkbvyJ6E{?fz&+nn9UsR-LRN@&DsT+BQVN%ZUW#Dq))pe>n5xL(0p|U z?+t6!(!OpD4QT%I?;q9zR13`S$L(d+nuu*&;-I1pNc}t$}b!1l@Tjc7^HlrBtQz=!VudGUxsS%;u&vYyA67E z3|6FPkI0)uP`o=g%&D9tS2*Q!!h@a}kL5z}hWhP1YSdmfbTj9_XokBjJT={3hfMt% z(?i=RAtT?~4e2z{HM3I?&0wvaLlA8TlsdBH<9g=q0KX$g{7d=F*Y0-~Ykd#Wen}d1 zK}q|trOk?Oup-bNRaPuRbTaV4}00Gfo`4lky8WR*Mk{OenyC;gVVn5nb8+N z;N6yP;YM2+4RwIg5OrdRI&npB=_|G3cb5+yERvMbb%Zl$IF&p%96wIf$%SP6qpk*m z1LQ@VuH_rjlB#%rf(q#|byD*4pV8{#aBeC@osY|HkRab> z)xu%taS5fVO(Q+7OqRT^D3vGT{D9Z0UTa02fR!JYCC^#q&!vi|N0oov*e*^mjTO)F zjIUnirg+2L(s3$UN+?ziBFGWQD6wMa+mxM==FiKK+`64S%8hP)S|Foio zN>bS%VHn&fim8{Ek*#;%IISV7sO3EP3+nJ9<$pCPBM0bkK0B~S&S-0UaCPe!H|MW0 literal 0 HcmV?d00001 diff --git a/4 beacons ver2/__pycache__/position_data.cpython-38.pyc b/4 beacons ver2/__pycache__/position_data.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..de9ffc28264ea566d437179dd5a14dbacd7388d7 GIT binary patch literal 1902 zcmZWq&5s*36t_Jy$>eLB&1T!(Zh`Ps6cs9H3x`s%Ewlxs)e6x?f>BW99XrWnCo`$X zyPH(mQ{fniE1a@NB>n;Z6XX+8Pq}e!59K{4t(NkvXaD^C#(sY9`OSBgN|6A4`d736 zM~;vuI5|rWOl|{bedq)cG$sQYP>PtvEO6}T4!j^|Wlo$Q_<;}D4GNOVg7Bs^C%(FvK*4m-7`!vS=+$xLpTMv2Z$jt_@25g?%> zL&v`pg3!8;yO*!!*enM^asD4bv<_d)k6)eswrD z1ujI}?pwvr=;SRQ@}kt%&nWi_VD)OH>sb7WuPPT@#xa55A`By7rjfB|lz1Ark`f{07+~ zCsgDobc2Z61W?D-YZG#d{6Zfxux7nWxx*bdP+uh%OMf!7l4>NQSC8>ge_fz$he$c&c>B)A}HQsSA3VT5zjuM&aaV(96 z$C1!IX$*(3`1sMKo#;q59!M4Oxbe+0 z{Th1;zL3h$?+yK7j@duQ6a)KcwlsIX9k}6`Crw9f!IHX&-6m?H3HO_h@!>T{b;RSq z-H#(3R6`#2WV@sIK-vUvZW(77YvX5!`@=Yer{jRz4CAA~&!ldT;%KlyV?;%i@oXqV zy}%2L^-SXzm3vtD`( zE}@a3I&gRr*_yEEN~m7UG*{YcqyMs-&=IG~%Ct<2tWN86gKn@It+6VtvLc1XR$#3R z(keaA7=78E<$U!qd>vx+Ul3LfOg&p6u-a*&>0_u72d-*qHghyRr6*LoGj9S2-k>|n z>AZ$I_n4L-%YSB@Wck_(@P@2JKn>gkjpdBt%vKmz`yDL2&aO>CwFU<^i^hT4s?ZAc z9(0hhO7XF}3OFcXP|{8ZhAglVvfftjqxOfWZBqj4A{N%RaS^`aMfDLVZUbjnA>e z5ghjpvn$OtQ-WP%NFb=OnL?i!FF70x#|wj2;ctKujH7)hzBa76#i-r}-JpJE9b&ln zci3>NE3m$}+PjD@V}}>Bwl8jhHrc>fW6qe4zXxTrz`r#}#bGQz!_Z~;q{`ICC^Hx0 M?b2m)eAmD7FGDEVp#T5? literal 0 HcmV?d00001 diff --git a/4 beacons ver2/__pycache__/solver.cpython-38.pyc b/4 beacons ver2/__pycache__/solver.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6142cf520d847854f9901d3fdbce55debb7e91c1 GIT binary patch literal 5479 zcmb_gO>Er873SaWa=AZR%d#E)#9`gE&AQUYv6ZA%-B@;Pr$3IISW4`$N?2-#k|=S> z^^hxD3#ttw8|a}1e2-H|JtaV|1p*Z4p{JaRqDP*JrmZ2gm)-*R(0*^YyONUt?Vi8cs#aC>3;s~6X4PcL)n1X9 z&Wd*=R@_#$W$O@|W+hg}zw$t3RW@`dXAPrQV}~9nY?vLsqgjVpk&Un;=pSXr&>vyX zV?>FKiEo*WV}6lMJSeg!*>TK2!d_rgSfPyXIIdHmQ>?+BWv9fK#-9>*Yho^qJB24a z_h5{jW@oU%QKrpH^%uJn*E&JqEcvdv9CrBBlIt|Xz>GqF!{v=Vl3-mI>sTJ~15b|ePXl~iTTR;qOZ zFUCr0D{3_ls)^bv{z6p^G+fD&p)TbOD9KfsayGlFxN7LN% z-pIt0PA~R^+}`A5xfJQ8>$%zyzOunb=sJqgZlt^nDQ`Xh!Bw9J9 zjzYTYDnA|PS#Dc?PmZOXLIO#%{9T0=mKEGDtSh@RUreylKL4x~>xr~2Zz+kgQ)K$C zOgia)_k6qUZ^FW`Q#0GC=>*JN_MJF(1G9IQmK(%TPe$g_X0O%wNQO7Yn{9WGPPGUB zmG z5sib26iS4HtSjLKq0UT+=s|foC@IHsP##?7t|AneUsr{C3f*(pgwdD_yyKd;R-uso zmd#ZsGJ`Of>bg8Mn{Wy4JnOZb6?dxbJArEw5;XXe*b6^SuimO}hqn`Y&EZ7o4y zcwb|ATni&WAGC`fz*`Uw3|M`sd*w9_tTEeRkxWL&9N;Oop0{8N&|yuF>dL!=*LYkJ*HixO}FoUW;bBN_(bjFfWRopy|$-U9~a2 z+||sM`OL~+FaE};pEG8`J59XhxIS0cGbNBt(!B2lj=uu44|$81!QurX6}p*g)81j` zgu&L}5q8TO%l2Vs`>->cu$MX6#^=;HEEcgz05)@D$MYjH@`#>A#y_kWav5KK8vTd= zT>bQCzy9nmFYSGTUiZ*Ga+`@sYUboK^`bBsfVr3IE07}WBuWcetDWX=x!%fZoEGEI zhp9p+X|d(pwh8Vd;jNj8kTdr`R6E&!8;k zX}x(`T!gs+gpWzv%0=nE{Pw$B21e7rlwd~y)L%e%J(>!lX3Xim(A;r#Y0$2Ns54dof-FXi^^7Mt9)GwPHIwITQ}~@ z{2dw3fa^T=+`RObls)@~l$6#EC0Y$vI9p}VE2CHKlAJ3$!$~En?kZbDSosuI25;@K zSpB!X)rm7gqmZAK?x{kWLvKleas#7#5*Z>1;B2?91_HpPSF53%9R!zln?rzU`X59Am>p$aA82DnjxAoXP+} z1pmgIz{(6i3EhE*6jB6?Uao7ISCT_=N)PxUkQMx;ckeXLQz91Fy^Nxc7|D^susuS% zh58YJyH<{HJIb71U~nCG1^kL|W2x?@n`)8Qb<-T&WyR&sV1T82(Xxk-=87DIKTDHN z5h;SCm8(c!kciyMQayRW%8Aohr7K==-d_=HE9XaXbhBQ|0RDNJ{Q{8}iF}L5w~5RU z5oi7mT2>{CrM*yT6$g1*Dz933=K8U-WK{^UC^(2%Z>fAW>fO8jc?dGZ&IAO}(<6GY zR5~f2l&i9#l;so3h-}Iu%G3BOG9s}&hTu{~dsrTkCje3-N)7S3CLdM!1scUh`(=T@pixeOwV_a>ppUfTADbB{_3*CsFEcV%+nN{<`N3avl<`1sww zzy9$r&h^-TI`=5=bmDO8cGrkv-b1~vK`|slz;0Qjf4yF+VpM%B)tk|V-FA@t865y^ zmS|@p9Y{FULc&6=9ddxO?%n%(P?=DdrGU>I(F=}+^`d;#vwB74^PY@ZKk2MqC3-I-WT@<}1l`*#4OMp8=31Z2rjQcDgc!^z=fBsr2CO^zi` zATW$0DDX_vW zTH;>*=InLpdF1q4<3Aa{fziPt*3$)`!d%6QXE9e##sRkpsLjA!-mD?}F8d+ID zDP?#1@)ycqle;%N9wD}S8%B!)X4-st{!NpIx1vdNC5+9v`Ut;<*?gYJ0*Iy2U8_v? z)|<|(p`H`LHwmI6ew~KjAo3Owi^z|NEE0K}$U8)AB0nbL5LqJ9B*H+t$FDnH>;)_0 zT#lK!B$7<3!W)f-2qN95=0)L+vpA@?kb)uq6vdHCbvjbUuN+)u$wfWJ2TM_M-)MAS zoeTRc;r^yaRT@1RfWsz5DMta1gu_C}3oi?h$_B{z9;@xLYN7k$!YUTPFAN}4heqNx zfk_xNi8l`-c4oCPcIx|LHQost_1uA2GJrOLbln%`#TyHgd943*QO##(rOh=voFjGa zC)A?WY;^T2h|DIX(go3K&(w!F70X$W{06lsqH;14{v1fEkv35#`mWnfi?)%gW%Z4_f=Y$6Eg|j@Ix7Lz4|q14ZsI!fn3Sn?Tl2 z?>96A*}?Z;c^xD1B-8_nCi68s9m&uWbji=n&G9p6@o6H0H#)Z<8vFdqbNpr8rm+`k z*d-#DiM&GOdqloZgy!=ffGlLm*AJ;LZeO9+t047bX~DKx*vwu4EqCxv1uuX&iUY|i z4c>ma5m#x-GO=?(r-fHJC#bD^q1}m5?qp^o?1`)h?++2Dd$$TSl?o9nhc`^FFOsV) z;i4)jvnQ&IGNU4i98ejBr>t1k&t@;EUZMmtA}rCAGEy5|(UIKL None: + self.replay_file = Path(replay_file) + self.file = None + + def __enter__(self) -> "ReplayInput": + self.file = self.replay_file.open("rb") + return self + + def __exit__(self, exc_type, exc, traceback) -> None: + if self.file is not None: + self.file.close() + + def readline(self) -> bytes: + if self.file is None: + return b"" + + line = self.file.readline() + if line: + return line + + self.file.seek(0) + return self.file.readline() + + +def get_config(csv_path: str | Path | None = None) -> tuple[np.ndarray, np.ndarray]: + """Load a four-beacon configuration CSV.""" + if csv_path is None: + try: + from tkinter import Tk, filedialog + except Exception as exc: + raise ValueError("csv_path is required when tkinter is unavailable") from exc + + root = Tk() + root.withdraw() + selected = filedialog.askopenfilename( + title="Select the 4-beacon configuration CSV file", + filetypes=[("CSV files", "*.csv"), ("All files", "*.*")], + ) + root.destroy() + + if not selected: + raise RuntimeError("Program Terminated: No configuration file was selected.") + + csv_path = selected + + csv_path = Path(csv_path) + config_data = pd.read_csv(csv_path) + + if config_data.shape[1] < 7: + raise ValueError("Configuration CSV must have at least 7 columns") + + if config_data.shape[0] != 4: + raise ValueError( + f"Four-beacon configuration must have exactly 4 rows, got {config_data.shape[0]}" + ) + + bmeasure = config_data.iloc[:, 1:4].to_numpy(dtype=float) + ri = config_data.iloc[:, -3:].to_numpy(dtype=float) + + print(f"Successfully loaded configuration from: {csv_path.name}") + print("Loaded 4 beacon positions from last three CSV columns.") + + return bmeasure, ri + + +def list_serial_ports() -> list[str]: + """Return available serial port names.""" + try: + from serial.tools import list_ports + except ImportError as exc: + raise ImportError( + "pyserial is required for serial input. Install it with: pip install pyserial" + ) from exc + + return [p.device for p in list_ports.comports()] + + +def parse_sensor_line(raw_data: str, rows: int) -> tuple[int, float, float] | None: + """Parse one indexed sensor line: beacon_number,y,z.""" + raw_data = raw_data.strip() + + if not raw_data: + return None + + parts = raw_data.split(",") + + if len(parts) < 3: + print(f"Serial warning: ignored invalid line: {raw_data!r}") + return None + + try: + beacon_number = int(round(float(parts[0]))) + y_val = float(parts[1]) + z_val = float(parts[2]) + except ValueError: + print(f"Serial warning: ignored non-numeric line: {raw_data!r}") + return None + + if not 1 <= beacon_number <= rows: + print(f"Serial warning: invalid beacon number {beacon_number}. Line ignored.") + return None + + return beacon_number - 1, y_val, z_val + + +def parse_unlabeled_sensor_line(raw_data: str) -> tuple[float, float] | None: + """Parse one unlabeled sensor line: y,z.""" + raw_data = raw_data.strip() + + if not raw_data: + return None + + parts = raw_data.split(",") + + if len(parts) != 2: + print(f"Serial warning: ignored invalid unlabeled line: {raw_data!r}") + return None + + try: + y_val = float(parts[0]) + z_val = float(parts[1]) + except ValueError: + print(f"Serial warning: ignored non-numeric unlabeled line: {raw_data!r}") + return None + + return y_val, z_val + + +def parse_quadrant_order(quadrant_order: str) -> list[str]: + """Return quadrant labels for beacon rows 1..4.""" + labels = [ + part.strip().upper() + for part in quadrant_order.replace(";", ",").split(",") + if part.strip() + ] + + valid = {"TR", "BR", "BL", "TL"} + + if len(labels) != 4 or set(labels) != valid: + raise ValueError( + 'quadrant_order must contain TR,BR,BL,TL exactly once, for example "TR,BR,BL,TL"' + ) + + return labels + + +def assign_quadrant_beacons( + points: list[tuple[float, float]], + corner_fraction: float = 0.25, + quadrant_order: str = "TR,BR,BL,TL", + min_points_per_quadrant: int = 5, +) -> list[tuple[int, float, float]]: + """Estimate four beacon corners from a batch of unlabeled PSD points.""" + if not 0.0 < corner_fraction <= 1.0: + raise ValueError("corner_fraction must be greater than 0 and at most 1") + + arr = np.asarray(points, dtype=float) + + if arr.ndim != 2 or arr.shape[1] != 2: + raise ValueError("points must be a list of y,z pairs") + + if arr.shape[0] < 4 * min_points_per_quadrant: + raise ValueError("Not enough points for quadrant assignment") + + x_values = arr[:, 0] + y_values = arr[:, 1] + + low_x, high_x = np.percentile(x_values, [5, 95]) + low_y, high_y = np.percentile(y_values, [5, 95]) + + mid_x = (low_x + high_x) / 2.0 + mid_y = (low_y + high_y) / 2.0 + + masks = { + "TR": (x_values >= mid_x) & (y_values >= mid_y), + "BR": (x_values >= mid_x) & (y_values < mid_y), + "BL": (x_values < mid_x) & (y_values < mid_y), + "TL": (x_values < mid_x) & (y_values >= mid_y), + } + + ideal_corners = { + "TR": np.array([high_x, high_y], dtype=float), + "BR": np.array([high_x, low_y], dtype=float), + "BL": np.array([low_x, low_y], dtype=float), + "TL": np.array([low_x, high_y], dtype=float), + } + + centers: dict[str, np.ndarray] = {} + + for label, mask in masks.items(): + quadrant_points = arr[mask] + + if quadrant_points.shape[0] < min_points_per_quadrant: + raise ValueError( + f"Not enough points in {label} quadrant. " + f"Got {quadrant_points.shape[0]}, need at least {min_points_per_quadrant}." + ) + + distances = np.linalg.norm(quadrant_points - ideal_corners[label], axis=1) + + keep_count = max( + min_points_per_quadrant, + int(np.ceil(quadrant_points.shape[0] * corner_fraction)), + ) + keep_count = min(keep_count, quadrant_points.shape[0]) + + closest = quadrant_points[np.argsort(distances)[:keep_count]] + centers[label] = np.median(closest, axis=0) + + ordered_labels = parse_quadrant_order(quadrant_order) + + print( + "Quadrant centers: " + + ", ".join( + f"{label}=({centers[label][0]:.4f}, {centers[label][1]:.4f})" + for label in ["TR", "BR", "BL", "TL"] + ) + ) + + print( + "Quadrant assignment: " + + ", ".join( + f"Beacon {i + 1}={label}" + for i, label in enumerate(ordered_labels) + ) + ) + + return [ + (row_index, centers[label][0], centers[label][1]) + for row_index, label in enumerate(ordered_labels) + ] + + +class UnlabeledBeaconTracker: + """Convert a fast unlabeled y,z stream into four labeled beacon rows.""" + + sequence = [0, 1, 2, 3, 0, 3, 2, 1] + + def __init__( + self, + rows: int, + stable_samples: int = 3, + stable_radius: float = 0.03, + pivot_repeat_radius: float = 0.25, + pivot_min_distance: float = 0.5, + pivot_neighbor: str = "auto-x", + ) -> None: + if rows != 4: + raise ValueError("Four-beacon unlabeled tracking requires exactly 4 beacons") + + if stable_samples < 2: + raise ValueError("stable_samples must be at least 2") + + if pivot_neighbor not in {"auto-x", "auto-x-inverted", "beacon2", "beacon4"}: + raise ValueError( + 'pivot_neighbor must be "auto-x", "auto-x-inverted", "beacon2", or "beacon4"' + ) + + self.stable_samples = stable_samples + self.stable_radius = stable_radius + self.pivot_repeat_radius = pivot_repeat_radius + self.pivot_min_distance = pivot_min_distance + self.pivot_neighbor = pivot_neighbor + + self.window: list[np.ndarray] = [] + self.recent_stable: list[np.ndarray] = [] + + self.synced = False + self.sequence_index = 0 + + self.release_radius = max(stable_radius * 3.0, 0.1) + self.last_stable_point: np.ndarray | None = None + self.waiting_for_movement = False + + def add_point(self, y_val: float, z_val: float) -> list[tuple[int, float, float]]: + stable_point = self._stable_point(y_val, z_val) + + if stable_point is None: + return [] + + if self.synced: + row_index = self.sequence[self.sequence_index] + self.sequence_index = (self.sequence_index + 1) % len(self.sequence) + + return [(row_index, stable_point[0], stable_point[1])] + + return self._sync_from_pivot(stable_point) + + def _stable_point(self, y_val: float, z_val: float) -> np.ndarray | None: + point = np.array([y_val, z_val], dtype=float) + + if self.waiting_for_movement and self.last_stable_point is not None: + if np.linalg.norm(point - self.last_stable_point) <= self.release_radius: + self.window.clear() + return None + + self.waiting_for_movement = False + self.window.clear() + + self.window.append(point) + + if len(self.window) > self.stable_samples: + self.window.pop(0) + + if len(self.window) < self.stable_samples: + return None + + points = np.vstack(self.window) + center = np.mean(points, axis=0) + distances = np.linalg.norm(points - center, axis=1) + + if float(np.max(distances)) > self.stable_radius: + return None + + self.window.clear() + self.last_stable_point = center + self.waiting_for_movement = True + + return center + + def _sync_from_pivot(self, stable_point: np.ndarray) -> list[tuple[int, float, float]]: + self.recent_stable.append(stable_point) + + if len(self.recent_stable) > 3: + self.recent_stable.pop(0) + + if len(self.recent_stable) < 3: + print( + f"Stable point found while syncing: " + f"({stable_point[0]:.4f}, {stable_point[1]:.4f})" + ) + return [] + + repeated_before, pivot, repeated_after = self.recent_stable + + repeat_distance = np.linalg.norm(repeated_before - repeated_after) + + pivot_distance = min( + np.linalg.norm(repeated_before - pivot), + np.linalg.norm(repeated_after - pivot), + ) + + print( + "Stable point found while syncing: " + f"({stable_point[0]:.4f}, {stable_point[1]:.4f}); " + f"pivot check repeat={repeat_distance:.4f}, pivot_dist={pivot_distance:.4f}" + ) + + if repeat_distance > self.pivot_repeat_radius or pivot_distance < self.pivot_min_distance: + return [] + + neighbor_index = self._infer_repeated_neighbor(pivot, repeated_after) + + if neighbor_index == 1: + self.sequence_index = 2 + else: + self.sequence_index = 6 + + self.synced = True + self.recent_stable.clear() + + print(f"Pivot sync found: beacon 1 plus repeated beacon {neighbor_index + 1}.") + + return [ + (0, pivot[0], pivot[1]), + (neighbor_index, repeated_after[0], repeated_after[1]), + ] + + def _infer_repeated_neighbor(self, pivot: np.ndarray, repeated: np.ndarray) -> int: + if self.pivot_neighbor == "beacon2": + return 1 + + if self.pivot_neighbor == "beacon4": + return 3 + + if self.pivot_neighbor == "auto-x-inverted": + return 3 if repeated[0] >= pivot[0] else 1 + + return 1 if repeated[0] >= pivot[0] else 3 + + +def get_serial( + port: str | None = None, + baud_rate: int = 115200, + replay_file: str | Path | None = None, + rows: int = 4, + min_required: int | None = None, + timeout_seconds: float | None = None, + serial_input: str = "unlabeled", + serial_format: str = "raw", + unlabeled_method: str = "quadrant", + cluster_samples: int = 300, + corner_fraction: float = 0.25, + quadrant_order: str = "TR,BR,BL,TL", + image_width: float = 320.0, + image_height: float = 240.0, + flip_y: bool = True, + stable_samples: int = 3, + stable_radius: float = 0.03, + pivot_repeat_radius: float = 0.25, + pivot_min_distance: float = 0.5, + pivot_neighbor: str = "auto-x", +) -> np.ndarray: + """Read serial measurements until enough unique beacon rows are received.""" + if rows != 4: + raise ValueError("Four-beacon serial input requires rows=4") + + if min_required is None: + min_required = rows + + if min_required < 1 or min_required > rows: + raise ValueError("min_required must be between 1 and rows") + + if serial_format not in {"raw", "pixel"}: + raise ValueError('serial_format must be either "raw" or "pixel"') + + if serial_input not in {"indexed", "unlabeled"}: + raise ValueError('serial_input must be either "indexed" or "unlabeled"') + + if unlabeled_method not in {"quadrant", "pivot"}: + raise ValueError('unlabeled_method must be either "quadrant" or "pivot"') + + if cluster_samples < 4: + raise ValueError("cluster_samples must be at least 4") + + parse_quadrant_order(quadrant_order) + + if replay_file is None: + try: + import serial + except ImportError as exc: + raise ImportError( + "pyserial is required for serial input. Install it with: pip install pyserial" + ) from exc + else: + serial = None + replay_path = Path(replay_file) + + if not replay_path.exists(): + raise FileNotFoundError(f"Replay file not found: {replay_path}") + + if replay_file is None and port is None: + available_ports = list_serial_ports() + + if not available_ports: + raise SerialNoDeviceError("No serial connections detected. Looking for file instead.") + + port = available_ports[0] + print(f"Available serial ports: {available_ports}") + + if replay_file is None: + print(f"Connecting to device on {port} at {baud_rate} baud...") + else: + print(f"Replaying serial data from: {Path(replay_file)}") + + print(f"Waiting for {min_required} unique beacon measurement(s)...") + + if serial_input == "indexed": + print('Expected format: "beacon_number,y,z", for example: "1,36,32"') + else: + print('Expected format: "y,z", for example: "0.0000,-6.5000"') + print(f"Unlabeled method: {unlabeled_method}") + + if unlabeled_method == "quadrant": + print(f"Quadrant mode: collecting {cluster_samples} samples before assignment.") + else: + print("Pivot sequence: 1,2,3,4,1,4,3,2") + print( + "Unlabeled filter: " + f"{stable_samples} samples within {stable_radius} radius; " + f"pivot repeat radius {pivot_repeat_radius}" + ) + + print(f"Serial format: {serial_format}") + + bmeasure = np.zeros((rows, 3), dtype=float) + received = np.zeros(rows, dtype=bool) + + start_time = time.time() + + tracker = ( + UnlabeledBeaconTracker( + rows=rows, + stable_samples=stable_samples, + stable_radius=stable_radius, + pivot_repeat_radius=pivot_repeat_radius, + pivot_min_distance=pivot_min_distance, + pivot_neighbor=pivot_neighbor, + ) + if serial_input == "unlabeled" and unlabeled_method == "pivot" + else None + ) + + input_source = ( + ReplayInput(replay_file) + if replay_file is not None + else serial.Serial(port, baud_rate, timeout=1) + ) + + with input_source as open_mv: + if serial_input == "unlabeled" and unlabeled_method == "quadrant": + raw_points: list[tuple[float, float]] = [] + + while len(raw_points) < cluster_samples: + if timeout_seconds is not None and time.time() - start_time > timeout_seconds: + raise TimeoutError( + f"Timed out waiting for serial data. " + f"Collected {len(raw_points)} of {cluster_samples} samples." + ) + + raw_data = open_mv.readline().decode("utf-8", errors="replace").strip() + + parsed_unlabeled = parse_unlabeled_sensor_line(raw_data) + + if parsed_unlabeled is None: + continue + + y_val, z_val = parsed_unlabeled + + if add_point is not None: + add_point(y_val, z_val) + + raw_points.append(parsed_unlabeled) + + accepted = assign_quadrant_beacons( + raw_points, + corner_fraction=corner_fraction, + quadrant_order=quadrant_order, + ) + + for row_index, y_val, z_val in accepted: + if serial_format == "pixel": + pixel_x = y_val + pixel_y = z_val + y_measure = pixel_x - image_width / 2.0 + + if flip_y: + z_measure = image_height / 2.0 - pixel_y + else: + z_measure = pixel_y - image_height / 2.0 + else: + y_measure = y_val + z_measure = z_val + + bmeasure[row_index, 1] = y_measure + bmeasure[row_index, 2] = z_measure + received[row_index] = True + + print( + f"Beacon {row_index + 1}: raw=({y_val}, {z_val}), " + f"measurement=({y_measure}, {z_measure}) " + f"[{np.count_nonzero(received)}/{min_required}]" + ) + + print("Matrix bmeasure successfully populated.") + + return bmeasure + + while np.count_nonzero(received) < min_required: + if timeout_seconds is not None and time.time() - start_time > timeout_seconds: + raise TimeoutError( + f"Timed out waiting for serial data. " + f"Received {np.count_nonzero(received)} of {min_required} required beacons." + ) + + raw_data = open_mv.readline().decode("utf-8", errors="replace").strip() + + if serial_input == "indexed": + parsed = parse_sensor_line(raw_data, rows) + + if parsed is None: + continue + + row_index, y_val, z_val = parsed + accepted = [(row_index, y_val, z_val)] + + else: + parsed_unlabeled = parse_unlabeled_sensor_line(raw_data) + + if parsed_unlabeled is None: + continue + + y_val, z_val = parsed_unlabeled + + if add_point is not None: + add_point(y_val, z_val) + + assert tracker is not None + accepted = tracker.add_point(y_val, z_val) + + for row_index, y_val, z_val in accepted: + if serial_format == "pixel": + pixel_x = y_val + pixel_y = z_val + y_measure = pixel_x - image_width / 2.0 + + if flip_y: + z_measure = image_height / 2.0 - pixel_y + else: + z_measure = pixel_y - image_height / 2.0 + else: + y_measure = y_val + z_measure = z_val + + bmeasure[row_index, 1] = y_measure + bmeasure[row_index, 2] = z_measure + received[row_index] = True + + print( + f"Beacon {row_index + 1}: raw=({y_val}, {z_val}), " + f"measurement=({y_measure}, {z_measure}) " + f"[{np.count_nonzero(received)}/{min_required}]" + ) + + print("Matrix bmeasure successfully populated.") + + return bmeasure \ No newline at end of file diff --git a/4 beacons ver2/main.py b/4 beacons ver2/main.py new file mode 100644 index 0000000..8526a81 --- /dev/null +++ b/4 beacons ver2/main.py @@ -0,0 +1,122 @@ +"""Command-line entry point for the four-beacon solver.""" + +from __future__ import annotations + +import argparse +import threading + +from solver import run_loop + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Four-beacon solver") + + parser.add_argument("--config", help="Path to 4-beacon configuration CSV.") + parser.add_argument("--no-serial", action="store_true") + parser.add_argument("--port", help="Serial port, for example COM4.") + parser.add_argument("--baud-rate", type=int, default=115200) + parser.add_argument("--replay-file") + parser.add_argument("--min-beacons", type=int) + parser.add_argument("--serial-timeout", type=float) + + parser.add_argument( + "--serial-input", + choices=["indexed", "unlabeled"], + default="unlabeled", + ) + + parser.add_argument( + "--serial-format", + choices=["raw", "pixel"], + default="raw", + ) + + parser.add_argument( + "--unlabeled-method", + choices=["quadrant", "pivot"], + default="quadrant", + ) + + parser.add_argument("--cluster-samples", type=int, default=60) + parser.add_argument("--corner-fraction", type=float, default=0.25) + parser.add_argument("--quadrant-order", default="TR,BR,BL,TL") + + parser.add_argument("--image-width", type=float, default=320.0) + parser.add_argument("--image-height", type=float, default=240.0) + parser.add_argument("--no-flip-y", action="store_true") + + parser.add_argument("--stable-samples", type=int, default=5) + parser.add_argument("--stable-radius", type=float, default=0.03) + parser.add_argument("--pivot-repeat-radius", type=float, default=0.25) + parser.add_argument("--pivot-min-distance", type=float, default=0.5) + + parser.add_argument( + "--pivot-neighbor", + choices=["auto-x", "auto-x-inverted", "beacon2", "beacon4"], + default="auto-x", + ) + + parser.add_argument("--focal-length", type=float, default=26.0) + parser.add_argument("--tolerance", type=float, default=1e-3) + parser.add_argument("--max-iters", type=int, default=60) + parser.add_argument("--once", action="store_true") + + parser.add_argument( + "--live-plot", + action="store_true", + help="Show live Y,Z plot while solver is running.", + ) + + return parser.parse_args() + + +def run_solver(args): + run_loop( + csv_path=args.config, + use_serial=not args.no_serial, + focal_length=args.focal_length, + tolerance=args.tolerance, + max_iters=args.max_iters, + once=args.once, + port=args.port, + baud_rate=args.baud_rate, + replay_file=args.replay_file, + min_beacons=args.min_beacons, + serial_timeout=args.serial_timeout, + serial_input=args.serial_input, + serial_format=args.serial_format, + unlabeled_method=args.unlabeled_method, + cluster_samples=args.cluster_samples, + corner_fraction=args.corner_fraction, + quadrant_order=args.quadrant_order, + image_width=args.image_width, + image_height=args.image_height, + flip_y=not args.no_flip_y, + stable_samples=args.stable_samples, + stable_radius=args.stable_radius, + pivot_repeat_radius=args.pivot_repeat_radius, + pivot_min_distance=args.pivot_min_distance, + pivot_neighbor=args.pivot_neighbor, + ) + + +def main() -> None: + args = parse_args() + + if args.live_plot: + from position_data import start_plot + + solver_thread = threading.Thread( + target=run_solver, + args=(args,), + daemon=True + ) + solver_thread.start() + + start_plot() + else: + run_solver(args) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/4 beacons ver2/math_utils.py b/4 beacons ver2/math_utils.py new file mode 100644 index 0000000..2b46b74 --- /dev/null +++ b/4 beacons ver2/math_utils.py @@ -0,0 +1,51 @@ +"""Math utilities converted from MATLAB files: skewp.m, DCM.m, unit.m.""" + +from __future__ import annotations + +import numpy as np + + +def as_col3(vector: np.ndarray | list | tuple) -> np.ndarray: + """Return input as a 3-element float vector with shape (3,).""" + arr = np.asarray(vector, dtype=float).reshape(-1) #convert to decimal umber + if arr.size != 3: + raise ValueError(f"Expected a 3-element vector, got shape {np.asarray(vector).shape}") + return arr + + +def skewp(p: np.ndarray | list | tuple) -> np.ndarray: + """Return the 3x3 skew-symmetric matrix for vector p.""" + p = as_col3(p) + return np.array( + [ + [0.0, -p[2], p[1]], + [p[2], 0.0, -p[0]], + [-p[1], p[0], 0.0], + ], + dtype=float, + ) + + +def dcm(p: np.ndarray | list | tuple) -> np.ndarray: + """Direction cosine matrix converted from DCM.m.""" + p = as_col3(p) + identity = np.eye(3) + pcross = skewp(p) + p_dot = float(p @ p) # dot product, matrix multiply + numerator = 8.0 * (pcross @ pcross) - 4.0 * (1.0 - p_dot) * pcross + denominator = (1.0 + p_dot) ** 2 + return identity + numerator / denominator + + +def unit(bmeasure: np.ndarray | list | tuple) -> np.ndarray: + """Normalize each row of a measurement matrix to unit length.""" + bmeasure = np.asarray(bmeasure, dtype=float) + if bmeasure.ndim != 2: + raise ValueError("bmeasure must be a 2D array") + + # Computes the length of each row. + norms = np.linalg.norm(bmeasure, axis=1, keepdims=True) + + if np.any(norms == 0): + raise ValueError("Cannot normalize a row with zero norm") + return bmeasure / norms diff --git a/4 beacons ver2/model.py b/4 beacons ver2/model.py new file mode 100644 index 0000000..ef4a375 --- /dev/null +++ b/4 beacons ver2/model.py @@ -0,0 +1,82 @@ +"""Core beacon solver math converted from Bibguess.m, pos_partial.m, orient_partial.m, MSM.m.""" + +from __future__ import annotations + +import numpy as np + +from math_utils import as_col3, dcm, skewp + + +def bibguess(p: np.ndarray | list | tuple, rs: np.ndarray | list | tuple, ri: np.ndarray | list | tuple) -> tuple[np.ndarray, np.ndarray]: + """Compute Bi and bguess for one beacon.""" + rs = as_col3(rs) # target/camera position guess + ri = as_col3(ri) # known beacon position + + diff = ri - rs + denom = float(diff @ diff) + if denom == 0: + raise ValueError("Ri and Rs are identical; cannot compute line-of-sight unit vector") + + bi = diff / np.sqrt(denom) + bguess = dcm(p) @ bi + return bi, bguess + + +def pos_partial(bi: np.ndarray | list | tuple, c: np.ndarray, rs: np.ndarray | list | tuple, ri: np.ndarray | list | tuple) -> np.ndarray: + """Position partial derivative matrix converted from pos_partial.m.""" + bi = as_col3(bi) + rs = as_col3(rs) + ri = as_col3(ri) + + denom = np.linalg.norm(ri - rs) + if denom == 0: + raise ValueError("Ri and Rs are identical; position partial is undefined") + + identity = np.eye(3) + return (-(c @ (identity - np.outer(bi, bi)))) / denom + + +def orient_partial(bguess: np.ndarray | list | tuple, p: np.ndarray | list | tuple) -> np.ndarray: + """Orientation partial derivative matrix converted from orient_partial.m.""" + bguess = as_col3(bguess) + p = as_col3(p) + + identity = np.eye(3) + bcross = skewp(bguess) + pcross = skewp(p) + p_dot = float(p @ p) + + leftside = (4.0 / (1.0 + p_dot) ** 2) * bcross + rightside = (1.0 - p_dot) * identity - 2.0 * pcross + 2.0 * np.outer(p, p) + return leftside @ rightside + + +def msm(p: np.ndarray | list | tuple, rs: np.ndarray | list | tuple, bmeasure: np.ndarray | list | tuple, ri: np.ndarray | list | tuple, n_beacons: int = 4) -> tuple[np.ndarray, np.ndarray]: + """Build measurement sensitivity matrix H and residual vector deltab.""" + p = as_col3(p) + rs = as_col3(rs) + ri = np.asarray(ri, dtype=float) + bmeasure = np.asarray(bmeasure, dtype=float).reshape(-1) + + if ri.shape[0] < n_beacons or ri.shape[1] != 3: + raise ValueError(f"ri must have at least {n_beacons} rows and exactly 3 columns") + expected_measurements = 3 * n_beacons + if bmeasure.size != expected_measurements: + raise ValueError(f"bmeasure must contain {expected_measurements} values, got {bmeasure.size}") + + c = dcm(p) + h_blocks: list[np.ndarray] = [] + bguess_all: list[np.ndarray] = [] + + for i in range(n_beacons): + beacon_ri = ri[i, :] + bi, bguess = bibguess(p, rs, beacon_ri) + hi_rs = pos_partial(bi, c, rs, beacon_ri) + hi_p = orient_partial(bguess, p) + h_blocks.append(np.hstack((hi_rs, hi_p))) + bguess_all.append(bguess) + + h = np.vstack(h_blocks) + bguess_vector = np.concatenate(bguess_all) # need to check + deltab = bmeasure - bguess_vector + return h, deltab diff --git a/4 beacons ver2/position_data.py b/4 beacons ver2/position_data.py new file mode 100644 index 0000000..90b84c1 --- /dev/null +++ b/4 beacons ver2/position_data.py @@ -0,0 +1,71 @@ +from collections import deque +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation + +TRAIL_LENGTH = 100 +points = deque(maxlen=TRAIL_LENGTH) + +fig = None +ax = None +scatter = None +line_plot = None +colors = None + + +def add_point(x, y): + points.append((x, y)) + + +def start_plot(): + global fig, ax, scatter, line_plot, colors + + fig, ax = plt.subplots(figsize=(8, 8)) + + ax.set_xlim(-5, 5) + ax.set_ylim(-5, 5) + ax.set_aspect("equal") + + ax.set_xticks(np.arange(-5, 6, 1)) + ax.set_yticks(np.arange(-5, 6, 1)) + ax.set_xticks(np.arange(-5, 5.1, 0.1), minor=True) + ax.set_yticks(np.arange(-5, 5.1, 0.1), minor=True) + + ax.grid(which="major", linestyle="-", linewidth=0.6, alpha=0.6) + ax.grid(which="minor", linestyle="-", linewidth=0.5, alpha=0.6) + + ax.axhline(0, color="black", lw=1.0) + ax.axvline(0, color="black", lw=1.0) + + ax.set_title("Live Serial Position Tracker") + ax.set_xlabel("X") + ax.set_ylabel("Y") + + colors = np.zeros((TRAIL_LENGTH, 4)) + for i in range(TRAIL_LENGTH): + t = i / (TRAIL_LENGTH - 1) + colors[i] = (1 - t, 0, t, 1) + + scatter = ax.scatter([], [], s=3) + line_plot, = ax.plot([], [], lw=0.8, color="cyan", alpha=0.6) + + ani = FuncAnimation( + fig, + update_plot, + interval=20, + blit=True, + cache_frame_data=False + ) + + plt.show() + + +def update_plot(_): + if len(points) > 0: + data = np.array(points) + + scatter.set_offsets(data) + scatter.set_color(colors[-len(data):]) + line_plot.set_data(data[:, 0], data[:, 1]) + + return scatter, line_plot \ No newline at end of file diff --git a/4 beacons ver2/solver.py b/4 beacons ver2/solver.py new file mode 100644 index 0000000..60be93e --- /dev/null +++ b/4 beacons ver2/solver.py @@ -0,0 +1,202 @@ +"""Runnable four-beacon solver.""" + +from __future__ import annotations + +from dataclasses import dataclass +import time + +import numpy as np + +from io_utils import SerialNoDeviceError, get_config, get_serial +from math_utils import dcm, unit +from model import msm + + +@dataclass +class SolveResult: + rs: np.ndarray + p: np.ndarray + iterations: int + converged: bool + + +def prepare_bmeasure_vector(bmeasure: np.ndarray, focal_length: float = 320.0) -> np.ndarray: + """Apply focal length and flatten measurements beacon by beacon.""" + bmeasure = np.asarray(bmeasure, dtype=float).copy() + if bmeasure.ndim != 2 or bmeasure.shape[1] != 3: + raise ValueError("bmeasure must be a matrix with 3 columns") + + if np.all(bmeasure[:, 0] == 0): + bmeasure[:, 0] = focal_length + + b_unit = unit(bmeasure) + return b_unit.reshape(-1) + + +def has_real_measurements(bmeasure: np.ndarray) -> bool: + """Return True when bmeasure has non-zero camera/image-plane data.""" + bmeasure = np.asarray(bmeasure, dtype=float) + return bmeasure.ndim == 2 and bmeasure.shape[1] == 3 and np.any(bmeasure[:, 1:] != 0) + + +def print_pose_details(result: SolveResult) -> None: + """Print pose plus derived values that are easier to compare to a tape measure.""" + distance_m = float(np.linalg.norm(result.rs)) + object_center_camera_frame = dcm(result.p) @ (-result.rs) + + print("object center:", object_center_camera_frame) + print("Rs:", result.rs) + print("p:", result.p) + print(f"distance: {distance_m:.4f} m ({distance_m * 3.28084:.2f} ft)") + # print("object center in camera frame:", object_center_camera_frame) + + +def solve_pose( + bmeasure: np.ndarray, + ri: np.ndarray, + guess_rs: np.ndarray | None = None, + guess_p: np.ndarray | None = None, + weight: np.ndarray | None = None, + tolerance: float = 1e-5, + max_iters: int = 200, + focal_length: float = 320.0, +) -> SolveResult: + """Run the iterative least-squares pose solve.""" + guess_rs = np.zeros(3, dtype=float) if guess_rs is None else np.asarray(guess_rs, dtype=float).reshape(3) + guess_p = np.zeros(3, dtype=float) if guess_p is None else np.asarray(guess_p, dtype=float).reshape(3) + ri = np.asarray(ri, dtype=float) + n_beacons = ri.shape[0] + + b_unit_vector = prepare_bmeasure_vector(bmeasure, focal_length=focal_length) + weight = np.eye(b_unit_vector.size) if weight is None else np.asarray(weight, dtype=float) + + for iteration in range(1, max_iters + 1): + if not np.all(np.isfinite(guess_rs)) or not np.all(np.isfinite(guess_p)): + return SolveResult(rs=guess_rs, p=guess_p, iterations=iteration, converged=False) + + if np.linalg.norm(guess_rs) > 1e6 or np.linalg.norm(guess_p) > 1e6: + return SolveResult(rs=guess_rs, p=guess_p, iterations=iteration, converged=False) + + try: + h, deltab = msm(guess_p, guess_rs, b_unit_vector, ri, n_beacons=n_beacons) + except OverflowError: + return SolveResult(rs=guess_rs, p=guess_p, iterations=iteration, converged=False) + + normal_matrix = h.T @ weight @ h + rhs = h.T @ weight @ deltab + + try: + deltax = np.linalg.solve(normal_matrix, rhs) + except np.linalg.LinAlgError: + deltax = np.linalg.lstsq(normal_matrix, rhs, rcond=None)[0] + + if not np.all(np.isfinite(deltax)): + return SolveResult(rs=guess_rs, p=guess_p, iterations=iteration, converged=False) + + guess_rs = guess_rs + deltax[:3] + guess_p = guess_p + deltax[3:6] + + if np.linalg.norm(deltax) < tolerance: + return SolveResult(rs=guess_rs, p=guess_p, iterations=iteration, converged=True) + + return SolveResult(rs=guess_rs, p=guess_p, iterations=max_iters, converged=False) + + +def run_loop( + csv_path: str | None = None, + use_serial: bool = True, + focal_length: float = 320.0, + tolerance: float = 1e-5, + max_iters: int = 200, + once: bool = False, + port: str | None = None, + baud_rate: int = 115200, + replay_file: str | None = None, + min_beacons: int | None = None, + serial_timeout: float | None = None, + serial_input: str = "unlabeled", + serial_format: str = "raw", + unlabeled_method: str = "quadrant", + cluster_samples: int = 300, + corner_fraction: float = 0.25, + quadrant_order: str = "TR,BR,BL,TL", + image_width: float = 320.0, + image_height: float = 240.0, + flip_y: bool = True, + stable_samples: int = 3, + stable_radius: float = 0.03, + pivot_repeat_radius: float = 0.25, + pivot_min_distance: float = 0.5, + pivot_neighbor: str = "auto-x", +) -> None: + """Run the MASTERLOOP-style workflow for four beacons.""" + bmeasure, ri = get_config(csv_path) + + if ri.shape[0] != 4: + raise ValueError(f"Four-beacon solver requires exactly 4 CSV rows, got {ri.shape[0]}") + + if min_beacons is None: + min_beacons = ri.shape[0] + + while True: + if use_serial: + try: + bmeasure = get_serial( + port=port, + baud_rate=baud_rate, + replay_file=replay_file, + rows=ri.shape[0], + min_required=min_beacons, + timeout_seconds=serial_timeout, + serial_input=serial_input, + serial_format=serial_format, + unlabeled_method=unlabeled_method, + cluster_samples=cluster_samples, + corner_fraction=corner_fraction, + quadrant_order=quadrant_order, + image_width=image_width, + image_height=image_height, + flip_y=flip_y, + stable_samples=stable_samples, + stable_radius=stable_radius, + pivot_repeat_radius=pivot_repeat_radius, + pivot_min_distance=pivot_min_distance, + pivot_neighbor=pivot_neighbor, + ) + except SerialNoDeviceError as exc: + print(exc) + if once: + break + print("Waiting for a serial device...") + time.sleep(1) + continue + except Exception as exc: + print(f"Serial read failed with error: {exc}") + if once: + break + print("Waiting for better serial data...") + time.sleep(1) + continue + else: + if not has_real_measurements(bmeasure): + print("No serial input is being used, and the CSV does not contain measurement data.") + print("This is only a config file, so the solver will not run.") + break + + result = solve_pose( + bmeasure=bmeasure, + ri=ri, + tolerance=tolerance, + max_iters=max_iters, + focal_length=focal_length, + ) + + if result.converged: + print_pose_details(result) + else: + print("Solver did not converge with the current measurements.") + print("Last Rs:", result.rs) + print("Last p:", result.p) + + if once: + break