From b38de6b50636c3cff5498bf237d564066ff983ca Mon Sep 17 00:00:00 2001 From: "Cameron (3539)" Date: Sat, 19 Oct 2024 01:23:23 -0400 Subject: [PATCH] Calibration Rotation! (#1464) Rotate camera calibration coefficients based on camera rotation. Probably. Seems to work. Maybe. --------- Co-authored-by: Matt --- docs/source/conf.py | 1 + .../design-descriptions/image-rotation.md | 61 +++++ .../images/image_corner_frames.png | Bin 0 -> 14733 bytes .../contributing/design-descriptions/index.md | 6 + docs/source/docs/contributing/index.md | 1 + .../components/dashboard/tabs/InputTab.vue | 20 -- .../configuration/CameraConfiguration.java | 17 +- .../CameraCalibrationCoefficients.java | 109 ++++++-- .../vision/calibration/CameraLensModel.java | 2 +- .../vision/calibration/JsonMatOfDouble.java | 48 ++-- .../vision/camera/LibcameraGpuSettables.java | 6 +- .../vision/frame/FrameStaticProperties.java | 31 +++ .../frame/provider/CpuImageProcessor.java | 2 +- .../provider/LibcameraGpuFrameProvider.java | 2 +- .../vision/opencv/ImageRotationMode.java | 57 +++- .../vision/pipe/impl/Draw2dCrosshairPipe.java | 7 - .../vision/pipe/impl/Draw3dTargetsPipe.java | 53 +--- .../vision/processes/VisionModule.java | 4 +- .../processes/VisionSourceSettables.java | 4 +- .../pipeline/CalibrationRotationPipeTest.java | 252 ++++++++++++++++++ .../photonvision/server/RequestHandler.java | 8 +- .../photonvision/estimation/OpenCVHelp.java | 59 ++++ 22 files changed, 614 insertions(+), 136 deletions(-) create mode 100644 docs/source/docs/contributing/design-descriptions/image-rotation.md create mode 100644 docs/source/docs/contributing/design-descriptions/images/image_corner_frames.png create mode 100644 docs/source/docs/contributing/design-descriptions/index.md create mode 100644 photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java diff --git a/docs/source/conf.py b/docs/source/conf.py index 09c6604cc..dca2301a4 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -36,6 +36,7 @@ extensions = [ "sphinxcontrib.ghcontributors", "sphinx_design", "myst_parser", + "sphinx.ext.mathjax", ] # Configure OpenGraph support diff --git a/docs/source/docs/contributing/design-descriptions/image-rotation.md b/docs/source/docs/contributing/design-descriptions/image-rotation.md new file mode 100644 index 000000000..e307c26dd --- /dev/null +++ b/docs/source/docs/contributing/design-descriptions/image-rotation.md @@ -0,0 +1,61 @@ +# Calibration and Image Rotation + +## Rotating Points + +To stay consistent with the OpenCV camera coordinate frame, we put the origin in the top left, with X right, Y down, and Z out (as required by the right-hand rule). Intuitively though, if I ask you to rotate an image 90 degrees clockwise though, you'd probably rotate it about -Z in this coordinate system. Just be aware of this inconsistency. + +![](images/image_corner_frames.png) + +If we have any one point in any of those coordinate systems, we can transform it into any of the other ones using standard geometry libraries by performing relative transformations (like in this pseudocode): + +``` +Translation2d tag_corner1 = new Translation2d(); +Translation2d rotated = tag_corner1.relativeTo(ORIGIN_ROTATED_90_CCW); +``` + +## Image Distortion + +The distortion coefficients for OPENCV8 is given in order `[k1 k2 p1 p2 k3 k4 k5 k6]`. Mrcal names these coefficients `[k_0 k_1, k_2, k_3, k_4, k_5, k_6, k_7]`. + +```{math} + \begin{align*} + \vec P &\equiv \frac{\vec p_{xy}}{p_z} \\ + r &\equiv \left|\vec P\right| \\ + \vec P_\mathrm{radial} &\equiv \frac{ 1 + k_0 r^2 + k_1 r^4 + k_4 r^6}{ 1 + k_5 r^2 + k_6 r^4 + k_7 r^6} \vec P \\ + \vec P_\mathrm{tangential} &\equiv + \left[ \begin{aligned} + 2 k_2 P_0 P_1 &+ k_3 \left(r^2 + 2 P_0^2 \right) \\ + 2 k_3 P_0 P_1 &+ k_2 \left(r^2 + 2 P_1^2 \right) + \end{aligned}\right] \\ + \vec q &= \vec f_{xy} \left( \vec P_\mathrm{radial} + \vec P_\mathrm{tangential} \right) + \vec c_{xy} + \end{align*} +``` + +From this, we observe at `k_0, k_1, k_4, k_5, k_6, k_7` depend only on the norm of {math}`\vec P`, and will be constant given a rotated image. However, `k_2` and `k_3` go with {math}`P_0 \cdot P_1`, `k_3` with {math}`P_0^2`, and `k_2` with {math}`P_1^2`. + +Let's try a concrete example. With a 90 degree CCW rotation, we have {math}`P0=-P_{1\mathrm{rotated}}` and {math}`P1=P_{0\mathrm{rotated}}`. Let's substitute in + +```{math} + \begin{align*} + \left[ \begin{aligned} + 2 k_2 P_0 P_1 &+ k_3 \left(r^2 + 2 P_0^2 \right) \\ + 2 k_3 P_0 P_1 &+ k_2 \left(r^2 + 2 P_1^2 \right) + \end{aligned}\right] &= + \left[ \begin{aligned} + 2 k_{2\mathrm{rotated}} (-P_{1\mathrm{rotated}}) P_{0\mathrm{rotated}} &+ k_{3\mathrm{rotated}} \left(r^2 + 2 (-P_{1\mathrm{rotated}})^2 \right) \\ + 2 k_{3\mathrm{rotated}} (-P_{1\mathrm{rotated}}) P_{0\mathrm{rotated}} &+ k_{2\mathrm{rotated}} \left(r^2 + 2 P_{0\mathrm{rotated}}^2 \right) + \end{aligned}\right] \\ + &= + \left[ \begin{aligned} + -2 k_{2\mathrm{rotated}} P_{1\mathrm{rotated}} P_{0\mathrm{rotated}} &+ k_{3\mathrm{rotated}} \left(r^2 + 2 P_{1\mathrm{rotated}}^2 \right) \\ + -2 k_{3\mathrm{rotated}} P_{1\mathrm{rotated}} P_{0\mathrm{rotated}} &+ k_{2\mathrm{rotated}} \left(r^2 + 2 P_{0\mathrm{rotated}}^2 \right) + \end{aligned}\right] + \end{align*} +``` + +By inspection, this results in just applying another 90 degree rotation to the k2/k3 parameters. Proof is left as an exercise for the reader. Note that we can repeat this rotation to yield equations for tangential distortion for 180 and 270 degrees. + +```{math} + k_2'=-k_3 + k_3'=k_2 +``` diff --git a/docs/source/docs/contributing/design-descriptions/images/image_corner_frames.png b/docs/source/docs/contributing/design-descriptions/images/image_corner_frames.png new file mode 100644 index 0000000000000000000000000000000000000000..e00a8987649623b37b3225569a98b9023d0f10da GIT binary patch literal 14733 zcmeIZhgVZi7$zJmR!}TdK?FpkiIhkQh^TZzN2IAVDTW$)01G10q$4Gf-h&Wnp#()r zK+K%#ip;4ddbwtfeD#E}z&+ddl>I zk?Us57X|D4&mpgO+0aeI4*|Etf_B!u2(zRen9aXC=XiG7T+uU9T>Ja`C&2)peLPJ$ zI0F7z=0<@@)yZbS!9_w4`7{JF?q!f)T3c)8?jUL02MK9Nqdy_l#Rn!HB*h*+-5Q^r zmDS>>t{xiG+uO^+!osN1d+ZGh=@NKLG%Fihbab@PRW)ntEW3enM{{TA;@{Ro)m}Ke z)7AwQ$HB{0a&vPTVK^MFwY9arzFt4Q{_lsdFb#F}g8Y14A%&2`5r{^~@S?1(t(S(% z;ZW1N{#4(j-+hCFdGpU2Vm-h>BeZoxmMSLc_3`)E>4VYuQ;5UyN2sW(MlJrW57N&S zmz4ZBHKk1X)z;RhA?LWi+n6`mwy2i9VHntZZ!|R`{Dk2sn7^zoF^V^c{@TZg2t$=` zVA2}M__guxZ$(8#k43B@9S6e*W_O+nvxd=+bahP*i>Qm&vSu4&BRDhQJK94&VGm)} zOkm>K0}f@q`G(*}hF!X%(4^93pu}otXD5$rK6cMqFietzpdH`e#L$(!myGQYLPUsL z2eEAZx^q>dR|rgJrR+@ps!sH3{!r*ZG_rMLHKG=7NRjpaKD#7FUnb>ID_#G`DO&V% zWIkwo3@i%@g?jol5eu8yxSF2Rx!vhI8PbhxMXvWdV;sxb;1bN{nCG&0jmtsvK1S!(o!}+uea2q{bH%`7mp@%LxDx{eVBsJ+6veA{ z@)10$< zH8f)T#|8#+=ZjZ8$(Q{;%hjwt2IDC(tHBo(2#Sfh{!Wy@CRv^t;6&WXYeitqtD3B&X3F2$q)198NY&Y6BaRFB6S}3 zle8G#o8{+xp`)v-_~5~m2fx3WqI<(JNI(ErrBm~)k&cdw)ww^#7C{cZfos2hsSn_{ zUfgCB1-4^hVIg)>Ir-C7+w!M{^L>TU%cYKEwP2nO!xh?X*ZI5mt6B##*iqyd*A`t_ zi9PY>C99g*u4Pp=JGzwNURnAY^cqQl+QWG1l9iI&ShIPA!-gmXa^cCu(R%!?Y zmOU>wH=zWvMy76v?U`WMb-fo96FJ?;#xOi? z@{+{X6q?3xQu+v-ttbU43=@wtMo=^;4sOgtaJGmx(cL#yzFCoD6W`y`$;ik6e=yBp zU;q9*g&6`lC3w5s%F@!I`r(5apjYqriu{qtQio;yys-Y6Erc4EkrqdERFsaPA)nC$ zm+WM%yE*s;YCK`@QJH{20kh{q$H|lqQ=k?Oy(%V~OCtitu?VFV?~v8|BRPy24~zEw z?R|OMDyfvAnXso->n_#n)MY;MF{}~+^T#>wAYW;tcB|REu_abu8k&-lGBP-LTWBrl z#&VQzK_r3AR+|&i^Y#ej{&?iMI~O8_)1|MUJ$rWJ?MdbEm`2p@CJ9fFcMcGy?Dk$+ z2TM0gamj1NrKT$MD{6mF&&^foNAwVR2j}nv^V-O7%2n8))qJu2a>BH_!DNFYMNS^p zdvC-_(9hUk?A~?WXL`Q`fA0`nFwL^tXbirFP?7ibH-a171c6_t0DOt4ZWa@xkql8x~Cu zY#6>e5SZdOjgO2>eUES$D9OZPHK~{F?ChqrJUuJ+24aCZzOGwK zZ7QV&&r`~)tE+3AC?=H?hT+GN&V+l|qnRORvTnaMQ-L{a6|iS~`Ep%INaL=`g9mRC z4}=wTg+y+TsfD+c=j7ykV&?AD1p+%a;E{hB^=v`fLe_uXQ-nQZ_x{@_Ha6Lm$xp+% zN!VE_Itb*ux=@<H*&tK^6B`@CO&}*k(le+%zcXS~>K<>@TijzvQhK5FzE-&@A&*p;m ze0$T+pUMHb1qCrp=b#=`xAMr{aMz<-;em3_(aQ3*?N!;9=Y|>Y%T-hyRQ|%qo0p{% z@~;5NJ+m|N3`VhyATn}qyqkK^D_V&d!AC_!2^p7FY}u%E#LU`O`d-vJ9Rt*L%|z<0 zY`=sKuCmg|w8}8T5ci>RR-t7>smZv_etM`99E(IE4b6Dt(|xv<^hc6Ge6;T?P~l7{7B(u& z$$2@>_4dl;%QObgat9CY7`XJV%*nbB^cU~8Q;vucsg;7 z$!Waq$cT6VE!>;vQ3ORqJj^MNHN@fzKF7rH5WD_5eIQkAI?rN#%eT8EV|Eh z-G5PU!i~x>+L)|UYC@@!f8~3O4x9pTW;i!sR<6cWxkFG;@Y~bE($Z2F>MvGRXI(Nh{Z@}XBXT;07W4&=*n@p4eV zXY4RXS;vWQf~UW{?aEF>NKTp49H3-yaBxFIgB=k*=0<+AF8QgboOwuYdSOF6^#CF0 zD}Y+EleBx)SY});H_&XVqZ5S;z={aDepxC2uvJS->w?&XkZ4m#eygi{Lb1R=c9pED z?=2L^@|`k_BuC1tP3auqD4p)HCqW(iN}n%`R2i0e3_AziJ`$RaE*JcblOM5h?3RFx zGZHl!dRTz}9xX==WrtR1jVw+2TJ=RH+Fq9GtUN;?0yyI0QqpTDBrI$&k9_s#E0?Tn zb!zJCkmb7VIT_b7s{vHByoVBbQv-h*a_7sBIg@KIoio;Tst8k>4TfDaYT02Qt!J(e zI2#3yy(qS{w5)jS-}v*Vo`C_qN7ZdemQ?w~cR!_mvN?f~@oH8QMktKDsl zM#D+Bbk5aavR8He!0OJ=u(s0erjTB{LV#qBh(n?M``9E%W5dqxA!+x8(ER<4pOvm| zmmtS$On2;f zSHDf}(~q(85$D+tjFhez*z`MdnTA1xNiwc|n=AO-oyv!Bh?b^@jL-Zi!eg^<2T8s> zIlPsNt#nM7k59h}@l=^djVLO#9{ z+}1KO;xEl!gA(h!1bHXmz4V0nRHtsJ4W-~KtA=Tt)ltZO63i=aG1w-{@1R(vT(Cm} za!1l%&aiZ`W|~i!y{pvRc3Gl!3mcA>^P@}eWGg3ok>9umXEr&W`E;4#IGT#Z{u{t7 z-74xSHRk=*Qehn&vtAlWo6775Z2t@y0R&Q&%-IQBd-el z$CM-fCd!OgJ^1k>3x=zGwJ#Pvwyyv+)mSF#7H*-2s42*j^dxMcZ#q_<>eI8{KF10X z_QGXOJe3jQ^{(}lt)I&|P*{r$`WG9dd8(goPpOlxv2(pK;vbzJkk==cS14`Cu<{9oflc=PqRnhfcVYU?u?Mx0zEvHMh(NW3 zZQZMJm43weZ0H=tPLCY3cty2mqnxb{{9biOLYDPAJmbK|!U zb@7)0@r%$%7#_*Y>Hg$Qgp_=fDmgT1A>Lj(Z+2(zsD9M_dJ5pg#sMnczo9WCxN%8 zgrgBg7x4F{e?<7F%?p^^%Krqnk8Fo!+Sh;Z9Ou|+%69GN;~af_y|T=8%MmEJsOhn~ zl?Qz9ifX;uN1z2Go1<$A_4j=!?3v3pLZRw}_Th#@_xm7)&vpN?vpY~vqAXTo% znEh>U$E25#GC=twwEMVwe&>16^b=zZ)n6^!D?B4pCm27l37=>Dy7i zm!s4WPL`^~lr0X1?JKXV(Q<6p8jm5VtgtIHC$=`zPhJN61!h4=RzeN04-M3OG%oo(X!~04z{57Z-o#4?-W3Wz_{{A#GPj}Te z7@qjs>3dH~vg?Efv}^VBI9}z@1Bl&eZufSo0`zg@O&crU#38GD6nb;_L96 zrYh1pfDTUWrbNGLJ$>MX;|^vU7yXeF!%~|{XW6U#6(Ns0N>KAvHJpA^exmy=S3OWR zZD%FmqRP4T#0ojH`YWx0OKfioof>D21R^1BK(HICsKSEu&H zd=A#%XHtNUI3osXo_mb3l%k^TAvwOFlaTA&7H8F*d@$sKr#Boeos=lIu3YFV*RS#Y z=R8UGZgK|Pbo=DaviWQmfemq;6%sKo^|T*9@m(b(Djn@_aDLSR_`u2X1L?JQJdjg* z8Sv#SIOM`w(bFN1zRhZ@;O5rI#fC|~ggUzGFHd+;$bA*<(6+dm0iD#d5Cu5(JX`re z?=0q3<@Qo=M*}}tLRIqdfbprq+V&LAsSUCe2iiQ>9_$IueOc}nc%b;hRDgO6Tz)L? zvYE;-G90y_r4kj|j15N*{+O&a;f(#sq?4u$+ zE)yaQF6+X;m-Sv&JBXCbGJ+QkdkUaF0=G>*O!lRe_w8PW?<1*(jA!B-r{}zH)fa9_ zt=%>PEo;XqQRn7S-fHBG?KtdXQ*aAnrh&2R>=kKP(aczz)Kkq)-C)GB^Z$(yx+|yn!)O^K6(LL233BO*lN%hgH`G<(!ogpN$A8@q^OcbG zPc#uc;}FSekEmk%UkyVpyiZ){$&1NQ%ga))3Te8 zVJ}fsnVWg6At{6o(3@ob>)pQjWSQZFxu4Fjsd+jk&vx zoN04is*aUQzxkW8%aZR#o8h??1In*Zk?jSt!{N7%f#=|UfI#Gb!#&KY=7O-qHS}qP zn0HRnLIVTPgkYH+t@*eeKv@%;1nIx;KjZ1|T4-WY;^Ns{2E7Y_{Xyz_-wA7W`h2%3 zAg$wy2Xt>X?g7t_uYCZ=So9NnOL`^1E8hHh(rq*QZe`eO8URFni;;{ZVH;!*!MUse z*Plhx*W;oLzohX(((Ay>!?GpMvD}X+?|-Zw`Zr4nJiWl7jCo;evUJQY0V-kzN zPmNcm@D~dpY4aHW_nFMUl=#_ytDv+>6HEPCpO$p49=Pwdb+P_Kk$wMdLDC)en2ZaI zBxUlIm>eNLYi%1!6Lb7{YSf4}Nv9binI9NWrRw&DHnu~klQ}*42>F7tFru)K~yTsQAeEvFHmT&C+QLsb@$#*ai3?R_jmU$o6;Tu z))w$(CYGEl?p|GLNl~OJYaPvh(%>o+0>mUDm+A3az=FoauZ&!d{Nc+@TbGTYp#_E_ zdm3D#Bd16WMNvybl;+*8eW{L;lVAAy#G%(*( zr&xE8Pe|3k>dTrhMGxvu5Rw+-egBB2+wZS}U|8&9PV#GY0lp2jSvDG-EgWN@Zo00l z{3OBeVF2i|r=i;!loIFICUm%V?-}v8q!0)BsLjnd8Nwjtx6(}^)$r!l&ZeXBLESTK zHm!CANgRoW98f83DEP)R=1Vhe!PK3;S{&zAZU2RR0+i0TDSO3|B5PeVfOJ9jB~P?# zw)bg((KDO1cS;XI*KTGCC%U<&?f)%BZAzl9J9!DBp8kjPQX-RB%)#ylSe)!3f;(rL zd>6IV_GipMB_yVxLi{!i6`m62wfa|uVfd#L*eVC)&%?Z0HsuNP)oWCo5ODjK($ZgS z)xLNah&&*DrOi>DQc#R2k)Go;(;}vXfp842F$(_ zd1qCflF<*G#xwE*rfgaq#h<#1R-yTxx|jdcNgUexFU9}UEiurF7m$Dd8UFuS3Hjfi z<^Og1ze;*d$4EG^d0opq3)cOaCvbE7;fS3sTIKGJEIb45lQc^6?~kWmlLz`A(^RD{ z$J4s6^WCg+4Tva^bM0azF`kdK2p5oVF-(gdzb1p7F`Mp(k3*Xk4M)?{$%he37XDlM z1}&tF&oOV&!o<3#I%5v2V*mRsVtr(y6)l{Gjso8;A42Q+tVdRSN$jHl zEv{BbHy(y3+0fm?xY{VYgj*~j4|x1r2rT*7w_A$>n-u_Sr3F0ZX;TPjYDCc&Z1Eed zJE%4W7K&&j%h;@S5$4Ow2RKnWQ7`=s-RM0t z8j=Ut2jICyxO_Gzn0&yvWFL88$k(8ZQFI9HX+5~b8QqaP7z=Qb_7D^qW4M{(EO+Rd z5x-Cv(~%Ne;{)|SBOdf0(I3CHmhB3K+y?II!Y^6Z>HUe-OWGorNfRzKcQG^CoJIyi zxd06IqL|xSluKL4?D9C3@2qPT^N5S<1^<^^ZS65!CG}TKFK$sryWIbDNyDCUs7>}lnWXA(gnYGr zpsaPg9GVyqzjrbqp8*lBq>=|*zhOZh#ZB}JAwPek)^W=NsZ^ttK?~~; zsKwmwH-DQEkQ?o(eFz62X2|2;BuF!ScB)tJ1u1n%MHQ9I0zioO?!R?;YVdKbD=?qY z5@mAoJ&$ldn&~-Jif*7ZiIqwxI>bVj7NnjssgY@}@g3P`xc;2+c_^z34iG!>q?-+A=vQ!wDVQ;jilBDFqgxV6$8rjJ=uzOc<+ zjK}^GLtC(tpaj?r1w0SsM@i?o!|_%MAYXQUmzGrq=^UPjGOXt(4lU54_@49M4uB)w zE`A*uAcsJb{`!u*wKYGodVw&@ zQR5*s-#Ow9K2~Sw#ZRpElugfWU)}z3EcMb4q%MSIz5w7vh-Zh0{L?Lv=J|Uf_6Q5? z#3A*;kd_u|`WF?%*gAQWZ!Rm~5MivG2H6*YjoT7+lNoV3&=WUks#dp+lb%z9TaHMH z$&1zAhn~3g-`$~s1dr~Rn1X=@JiQ6c2?aP?7wh}~vGsa}u>H~|?xI8sMa(unny)Hc zt*IIp`SJ8G58JJ}{W7ya_KZnQM$2p;mppu(r?e+xrZAFpt)>hu7lb(TUrT51)b1Dg zwXjVp*i2B1g?s06`st3w8+O@%1OZhTu|W?O_6j4+s!CjVi;k!S@(PmuB0B)v)1SBv z6cHp|=e-2##_rL?Ez?u7j_bxL^1V}15B2i&n|{^#S%jng)B6&p}54(0&euvWOYtbogk=?u9C_3pG7fUUIm zE3kRE+B8};uXf{Rqipd?zswNqEuGUr>t z&S7n3Pc@|Qsm4bJ9))YQw+k9O&o=hk(DvMQ;^mKQsNG|dq!+h99(6=Q%%6b7VT&3_ zh{;9w^h~9AIbP51ndNqtAvg=$49i(=HxTXu;6_&urpDT<(K1d;-&jFbx0Sjcz0;=_ z!93>E&Z5#}`YEd&y4cvr5UMnDk1hac^LADRF<5QV1?em#`AcFHKk_Gbj~Fu_wjPR= zo6L-qHr8MaC9Qh~(W-wO7E|=F4$$fP1;Nm5%a&3eOcsJ$SdWucNg-aYYB#iL)K$2g zhX$A-Z+?Q0Y=$&mg3h_HpD?B9U26+mPgn%{@#Yao+bdML?zmFqn!Ya^r@-3R`aHIz z=tf)O7eLSBw}a88g>9)AwRB|b_8C0c%O_)X&ee9*p4Z49o6;0i5cBIdyXpylL{si+ltL(Gxi1y9~pGpl?9BZ3-7}+Z#0m z&JoR*E`0}W8e5GE57s`$kW1Gntluv`bA=nf<^J~?Vz6qdk@du9g+HkK;iA}?6` zPA1?S+p-arEcZyRyZn<45uraG_I(0~f|^m-#L5(xbGAJ=d6s>((gD9Z5ri3b(bcFl zJD__0>x8Ia9&s{PwSn5*qatAYD`H^hL`$Hr+&cXwV(cZma;Iwnr^+1!=HDKLtT&Uo zIE+1K4rj;_9QBC)} zzq8DIE(dF~rwu1?ON)OvNl$U6_;w?F*9oCtFU*vr)Rx5nVb$F#b@Ot_Z0~PlZIU6^ zwy(942S(@FXLoA0LP5c{CE9l)JQK9*f90J+hwxdSP-;BO$zr;XCmT`Y<;K3r^wWPT zoN7~=nFSP`fM=Uswv&c7VKYq3Az5=*G1G8TckeWfDA4=LZ{;5tc*6L`)yd487WF0v z39lEShmRL^+k8qQChr0~2dPy|v(Zj>CA#_>d2}&!*?3^Ieb=Ie;c@z)vsSprJqsN~ zwY-qm(mZK>_Qd3C>GZzG@j#5H)T3{{D>rxOYbbrWEYXK?3V%c{`h2H02`F(gwEeKi zn%)Ywb;k=pH4E(*B#eEusxq^=lSFss;BPuf7wJ{&-Y50pwJ>nA7yc~33~Hcu1o@dg zZ$#}u0}nmewIF5O_%zn%eVy0@*q{=R8; z4ft|}8!lafHBf2r<>XJM2`|+*M|NbmSAz|&F|tadHYOv@YiEXAhvfbKa(XXHqJY34 zh^O8sol5rZI%RyEF3v zejw3C(oLH=&xVdo`DcTk^*gArqlM~NGe^-;JE&yadcXZri)Za-90ypC<-oI?j^0^Y zlVW(lJh-0lFfcriZ{4lD_FwIt2j$zjpQu#7nt7?d3gfH;N-PmmrP!}XS`u>^6Qw>K zyWWW_ntX!**n7plFTHAGlvXe)o}Zdq%uT^_&{%ledgu(b0|^8Q1BhxfK4EdMl7azs z<$A&+_{ZhSL2nGL*c_65M?2o{m2Z~~@HI)V8rRRB6RrTM((6pp-z!ILT30SbuwH9B zAqPx{vB>MWmbo}RZH83Wwpr3LsJ8;YAS}Fa+cfb}SYx5?o)5Z82GW{^oA(ZhAuz;^9ZPRIA|QC^RecJ6BwZ_Eg) zakINk(;|%)`9GbB;1vn4ImpLBTWT${vssh#kG)E^@pZ;ab7+Bd>dvk!CMBTD4n)pYQs1q*S7|%jKYI-SyF(tBz2lCW z_c@VzR!p#Ms`h0fjZjEtM!B&IB_wWIc71PuSoGDs&k<|BHnAEikIC<9qRFW}CRjA3 zM)C?Q0u6N+JU11{5A&biFU2)F9&BTZiBORgnEnGBlN`hrKS)n*LLKACETW?SK;ZA5 z;D~a_ivWh4U_V3WwFqMhXCpYc9j7MFnz3w2d4qa1{@$I&GEi6!){>y_8s5+}ZXY*# zCS0ywrMCDpkT>gRL5z5AOXnxY+egS_Z4RZA?0oxHIKd9xQOkUA#?7l0~sfmA~}%Oim4t7j&^SzWUJ9lH0I($VmC z=9UL2n-1h935JjR62n2MoA>S9AT;Jme4y26&2y~qTLmXty;_EC9c!x-shZzv4a-IN z0ljVv%y_(;euBn+*k6lV5kV=G(?4wQ^C4W@_<_@PS?bjA%dF`?byoBypddxp@;>RZ z`L6`VnWR=5rR7;DrBf)`0s;GgM$;jRjdY7Zdby*4(fULcUz<=BbV1Gh!)UDv3{vaJ z9_0=Qw18=8W@e_W+zbdmk`8~qFnDT!gnurf?O?q!Zy9AbhID(by2Rss6f%A`4ejp^ z5*;{siY0)_wPH5L%e(Hg6ER??LVmvec79F#E6F~jEKr=b0l{Z zYi4Fz+JiJph3^uA%9h1X;slLuWCIR$d~{D=Usgs&^XZM}U~bQIX8C1`341&5*sP_Q zve#6?`-Cg~cRR|s`DS7qE6@7hum6L!$K8&-8IIcyj z3p`^6>8k$yxCR!ONZ3Y1Y*&GRry+uPYmI*tk!>y(}fMn{QElMd@He|q}+nXX(>Ij1|lzcmIJ zm_?hYVJniZUudCmd5b~+`^dPst5>harKA*MaeyiRR4}fst!>3=#`Fy!$M+_4^{eQ~ z0*Z0GVqOEHJU%9-xqE(YP7gr;`*&Oc8w+rAJFM8A9DNDK0~lbGKRK@e@wY2m_@pHu zDdxKUZRNQaj`p^9>l|DD_y@nVY=}xbfBrn4^!th(QnswzbMe4|B*&( z9PO(b-YiCdZLOgZW|@b_M7^%1QGdS3$C`4pswHz&x5)l44BKV)^Sd!yYA*P_oWQ`q zHAy1d47E1rNV;L0nWKY4O!+TZSvXqO@CIR5tEV#>&bSu*9V17}IUHmNA7G7ek7MhJ zt*taRW-j<|T@#EB@St;HcjBO`v9%*;$mKtwk` z*}F30cr3Lq;xbbFsF#eTeff}nZcZSiC$%%$gz&unWbn@IWr4Y^JWkR39Ne5o0Uy=Q-hOL& ztjL&7&(N^7d%8S-{x#MJ`IgrM*u+eOpT-`R;K-##!#{^g))) z!77X{dLdfH5AR0!EJW_VdGqGSj|ag}<8nlAA(eQH)Vg1(=Jl>>{B-zl^WN2VeR#t|WgWm!)!ni^oy6w5%{Qg2?K7xfQgXc_&howwT zG{BJ0$nTSpu%m|VI%nPUQZd4}lg|yfW^+g-R%)2>7I_I4Jue{X*479ZwS@n?7woab z&8Rn6wb5s+FQsaS=x_e4f8qeps!hE_cvb4wD!Hrz>>@7|m=V9Yw)pV*A4xxO1rUV! z;dETSyLIl)V>_icEo;-Q&{u;zTvJ3ojNal$RSoWpFI?Gm5oSQ{!0k5%98&miqsAGf z4jRMQdON`>357Aur-0KANc+>Wcw5i4V=Z1Mi=r}$>dfP_vTjPf-TiZ-7sl^}t*EGQ z4QTOSN@sbj_-GfH#T?~K{qG{fp0N8PN?*XK2IY{wH%Qoh31BYspmaPw2)xXF#s>ZD zWHoQ?Yi8J5cyMaeWAb0Mp6TJPE*;Y<77I5im#OAYsqP(HI*Qt+!~ViM7pEFR-n=|9h4hg(bNFaZF!o8Tj7dQNGq$%>a6o@lIkqiBb-NW+md^-gD$4+H*?;l=adsP z?&mv~`d;hY+>KZ!{-G-Z200)hF)p3s{2pihTW8vdCx~6vQ|m#bz+Yu`R`Hu!fGXMA zH_2o@?E^UXz`@yolQ#o6r4tHXFkowpHh|eKeBBV77|6&_uFz95EPR?7asJn@NG1`l zZxt1u&BaFmUD%HO%?$?NG^21TRr7q;bSZj@vhwSn+jDdx3yc6yUqo$GLL~^CDyRwH zIiL}w7YTc&7wTc%p zmjefZyj-V6?wjS&sup!fEs|@qxa0>O79t;`ML4_EjqjRIdE#=GWaBSdJLF$?Lwy$) z5rLa!`*s8Msm!v<&ddakt+OUx%M>2}lV9ZiHDs8@<+&5*;&Ib-L|{)uV&?{A=XtLz zdw+yf7CCoxUSRC#j%3t5Hda=iMFcq2BTfiaaB+2={uXtUms?d6uB;4>FI^Ky0E;Av zNl8gr1}ro$!$*yf${U0;Uj01IEsjq|UwZA*Uz5&wkSA;SX6?(yI#5?^`^%#dE$&f` zJkiPz`Gtk!WT7u1nW~;qS$ZVRje=aWGJ72zog07QC7*nCN5s#SdUiS)7WU?|eyfWN znS<)>bfkF4<)hR|-9D8XZ3S;CEg$y6lh9~kg}xe$&QWahuU|2W59nj#FMzhZWGf1qHy_ETDnp9`k39r+;1jIcLArMLe5UE%HHV<-uNGzV|A*k)jQwJ z#;a!Y0gnOiT+-oFx2jW@Mm?KXp?WXFoE%{|#d|4M#8Ha_EVbJRw|Oj@S7b!uRw_8( zaAR_?)Shl%j>lP58HsIl8xN>XWfV2#yj+6AO&dii@ZwZwW3FwOc$G5hj|9D=PM z(*io__ep>dqMC`_6wQPa30R{=Ic2}nC?#GTWkCQSBZWt%;vNx6QF6SjdA^c&gFWnc z{yK5FSTbJVy_;vh^`JKQo5P>43SllWs9tPTu`2Ox4(TD#`oDkwdU<)-&890WD}NH3 znwrXqj*pEc+M4d@qcaVNdpzR#ZI3{BG2ukwoCoH-J+WMGuz_)UH(kv=_&uJD5ETSO ziI;epR2QPJjfmR;R{os#hSQldXUK{o`+{qpUU3N4Cofe}6begtiWJN;s$Jq2793PE zyM#?(Rs6?#s6d3hxeq_=!ApUB#-*PW_JI>-;B-@XmZ8fC9?8qg8^x_JVhy7APl?j^ z_#4?#kEN0lmtNooz|kZ<$F%%aX?AJ1oUE*WD}U>;e!nv^GGvx26wTfXh}>3-6O5!& zOK%BZ^XxSeIN}Dt#xVw#;Oe%tw6wv&rxHX{O_ziHt*Wck%ZJ4Ju{ z0|We)hAP6i)^{hizqW}HRuueG$;*q;%ha&FNd1$A!eNq{?rO-R3Dd5+uoe&x!2!EL zJTl+@&daAy2lMpvv5ueNoDXU30=yccJ6*+wsHaQsWiC4T#Dk&4<_r H&))nGvjOI| literal 0 HcmV?d00001 diff --git a/docs/source/docs/contributing/design-descriptions/index.md b/docs/source/docs/contributing/design-descriptions/index.md new file mode 100644 index 000000000..f1a598f89 --- /dev/null +++ b/docs/source/docs/contributing/design-descriptions/index.md @@ -0,0 +1,6 @@ +# Software Architecture Design Descriptions + +```{toctree} +:maxdepth: 1 +image-rotation +``` diff --git a/docs/source/docs/contributing/index.md b/docs/source/docs/contributing/index.md index d9f1137b3..4de6c437a 100644 --- a/docs/source/docs/contributing/index.md +++ b/docs/source/docs/contributing/index.md @@ -4,4 +4,5 @@ building-photon building-docs developer-docs/index +design-descriptions/index ``` diff --git a/photon-client/src/components/dashboard/tabs/InputTab.vue b/photon-client/src/components/dashboard/tabs/InputTab.vue index f69812ad6..d81230093 100644 --- a/photon-client/src/components/dashboard/tabs/InputTab.vue +++ b/photon-client/src/components/dashboard/tabs/InputTab.vue @@ -130,32 +130,12 @@ const interactiveCols = computed(() => tooltip="Controls blue automatic white balance gain, which affects how the camera captures colors in different conditions" @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraBlueGain: args }, false)" /> - - - Warning! A known bug affects rotation of calibrated camera. Turn off rotation here and rotate using - cameraToRobotTransform in your robot code. - it.resolution.equals(calibration.resolution)) + .filter(it -> it.unrotatedImageSize.equals(calibration.unrotatedImageSize)) .findAny() - .ifPresent(calibrations::remove); + .ifPresent( + (it) -> { + it.release(); + calibrations.remove(it); + }); calibrations.add(calibration); } diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java index 99c5fc012..2bba3c2bc 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java @@ -27,12 +27,13 @@ import java.util.List; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; import org.opencv.core.Size; +import org.photonvision.vision.opencv.ImageRotationMode; import org.photonvision.vision.opencv.Releasable; @JsonIgnoreProperties(ignoreUnknown = true) public class CameraCalibrationCoefficients implements Releasable { @JsonProperty("resolution") - public final Size resolution; + public final Size unrotatedImageSize; @JsonProperty("cameraIntrinsics") public final JsonMatOfDouble cameraIntrinsics; @@ -56,9 +57,6 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonProperty("lensmodel") public final CameraLensModel lensmodel; - @JsonIgnore private final double[] intrinsicsArr = new double[9]; - @JsonIgnore private final double[] distCoeffsArr = new double[5]; - /** * Contains all camera calibration data for a particular resolution of a camera. Designed for use * with standard opencv camera calibration matrices. For details on the layout of camera @@ -87,7 +85,7 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonProperty("calobjectSize") Size calobjectSize, @JsonProperty("calobjectSpacing") double calobjectSpacing, @JsonProperty("lensmodel") CameraLensModel lensmodel) { - this.resolution = resolution; + this.unrotatedImageSize = resolution; this.cameraIntrinsics = cameraIntrinsics; this.distCoeffs = distCoeffs; this.calobjectWarp = calobjectWarp; @@ -100,10 +98,93 @@ public class CameraCalibrationCoefficients implements Releasable { observations = List.of(); } this.observations = observations; + } - // do this once so gets are quick - getCameraIntrinsicsMat().get(0, 0, intrinsicsArr); - getDistCoeffsMat().get(0, 0, distCoeffsArr); + public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotation) { + if (rotation == ImageRotationMode.DEG_0) { + return this; + } + Mat rotatedIntrinsics = getCameraIntrinsicsMat().clone(); + Mat rotatedDistCoeffs = getDistCoeffsMat().clone(); + double cx = getCameraIntrinsicsMat().get(0, 2)[0]; + double cy = getCameraIntrinsicsMat().get(1, 2)[0]; + double fx = getCameraIntrinsicsMat().get(0, 0)[0]; + double fy = getCameraIntrinsicsMat().get(1, 1)[0]; + + // only adjust p1 and p2 the rest are radial distortion coefficients + + double p1 = getDistCoeffsMat().get(0, 2)[0]; + double p2 = getDistCoeffsMat().get(0, 3)[0]; + + // A bunch of horrifying opaque rotation black magic. See image-rotation.md for more details. + switch (rotation) { + case DEG_0: + break; + case DEG_270_CCW: + // FX + rotatedIntrinsics.put(0, 0, fy); + // FY + rotatedIntrinsics.put(1, 1, fx); + + // CX + rotatedIntrinsics.put(0, 2, unrotatedImageSize.height - cy); + // CY + rotatedIntrinsics.put(1, 2, cx); + + // P1 + rotatedDistCoeffs.put(0, 2, p2); + // P2 + rotatedDistCoeffs.put(0, 3, -p1); + + break; + case DEG_180_CCW: + // CX + rotatedIntrinsics.put(0, 2, unrotatedImageSize.width - cx); + // CY + rotatedIntrinsics.put(1, 2, unrotatedImageSize.height - cy); + + // P1 + rotatedDistCoeffs.put(0, 2, -p1); + // P2 + rotatedDistCoeffs.put(0, 3, -p2); + break; + case DEG_90_CCW: + // FX + rotatedIntrinsics.put(0, 0, fy); + // FY + rotatedIntrinsics.put(1, 1, fx); + + // CX + rotatedIntrinsics.put(0, 2, cy); + // CY + rotatedIntrinsics.put(1, 2, unrotatedImageSize.width - cx); + + // P1 + rotatedDistCoeffs.put(0, 2, -p2); + // P2 + rotatedDistCoeffs.put(0, 3, p1); + + break; + } + + JsonMatOfDouble newIntrinsics = JsonMatOfDouble.fromMat(rotatedIntrinsics); + + JsonMatOfDouble newDistCoeffs = JsonMatOfDouble.fromMat(rotatedDistCoeffs); + + rotatedIntrinsics.release(); + rotatedDistCoeffs.release(); + + var rotatedImageSize = new Size(unrotatedImageSize.height, unrotatedImageSize.width); + + return new CameraCalibrationCoefficients( + rotatedImageSize, + newIntrinsics, + newDistCoeffs, + calobjectWarp, + observations, + calobjectSize, + calobjectSpacing, + lensmodel); } @JsonIgnore @@ -118,12 +199,12 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonIgnore public double[] getIntrinsicsArr() { - return intrinsicsArr; + return cameraIntrinsics.data; } @JsonIgnore public double[] getDistCoeffsArr() { - return distCoeffsArr; + return distCoeffs.data; } @JsonIgnore @@ -140,7 +221,7 @@ public class CameraCalibrationCoefficients implements Releasable { @Override public String toString() { return "CameraCalibrationCoefficients [resolution=" - + resolution + + unrotatedImageSize + ", cameraIntrinsics=" + cameraIntrinsics + ", distCoeffs=" @@ -149,16 +230,12 @@ public class CameraCalibrationCoefficients implements Releasable { + observations.size() + ", calobjectWarp=" + Arrays.toString(calobjectWarp) - + ", intrinsicsArr=" - + Arrays.toString(intrinsicsArr) - + ", distCoeffsArr=" - + Arrays.toString(distCoeffsArr) + "]"; } public UICameraCalibrationCoefficients cloneWithoutObservations() { return new UICameraCalibrationCoefficients( - resolution, + unrotatedImageSize, cameraIntrinsics, distCoeffs, calobjectWarp, diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java index 3294e8996..99568c54a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java @@ -28,7 +28,7 @@ public enum CameraLensModel { /** Mrcal steriographic lens model. See LENSMODEL_STEREOGRAPHIC in the mrcal docs */ LENSMODEL_STERIOGRAPHIC, /** - * Mrcal splined-steriographic lens model. See LENSMODEL_SPLINED_STEREOGRAPHIC_ in the mrcal docs + * Mrcal splined-steriographic lens model. See LENSMODEL_SPLINED_STEREOGRAPHIC in the mrcal docs */ LENSMODEL_SPLINED_STERIOGRAPHIC } diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java index 7dbb820ec..aa80685f9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java @@ -26,7 +26,6 @@ import org.ejml.simple.SimpleMatrix; import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.vision.opencv.Releasable; /** JSON-serializable image. Data is stored as a raw JSON array. */ @@ -41,6 +40,7 @@ public class JsonMatOfDouble implements Releasable { @JsonIgnore private Matrix wpilibMat = null; @JsonIgnore private MatOfDouble wrappedMatOfDouble; + private boolean released = false; public JsonMatOfDouble(int rows, int cols, double[] data) { this(rows, cols, CvType.CV_64FC1, data); @@ -57,36 +57,14 @@ public class JsonMatOfDouble implements Releasable { this.data = data; } - private static boolean isCameraMatrixMat(Mat mat) { - return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3; - } - - private static boolean isDistortionCoeffsMat(Mat mat) { - return mat.type() == CvType.CV_64FC1 && mat.cols() == 5 && mat.rows() == 1; - } - - private static boolean isCalibrationMat(Mat mat) { - return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat); - } - @JsonIgnore public static double[] getDataFromMat(Mat mat) { - if (!isCalibrationMat(mat)) return null; - double[] data = new double[(int) (mat.total() * mat.elemSize())]; mat.get(0, 0, data); - - int dataLen = -1; - - if (isCameraMatrixMat(mat)) dataLen = 9; - if (isDistortionCoeffsMat(mat)) dataLen = 5; - - // truncate Mat data to correct number data points. - return Arrays.copyOfRange(data, 0, dataLen); + return data; } public static JsonMatOfDouble fromMat(Mat mat) { - if (!isCalibrationMat(mat)) return null; return new JsonMatOfDouble(mat.rows(), mat.cols(), getDataFromMat(mat)); } @@ -98,11 +76,20 @@ public class JsonMatOfDouble implements Releasable { this.wrappedMat = new Mat(this.rows, this.cols, this.type); this.wrappedMat.put(0, 0, this.data); } + + if (this.released) { + throw new RuntimeException("This calibration object was already released"); + } + return this.wrappedMat; } @JsonIgnore public MatOfDouble getAsMatOfDouble() { + if (this.released) { + throw new RuntimeException("This calibration object was already released"); + } + if (this.wrappedMatOfDouble == null) { this.wrappedMatOfDouble = new MatOfDouble(); getAsMat().convertTo(wrappedMatOfDouble, CvType.CV_64F); @@ -110,6 +97,7 @@ public class JsonMatOfDouble implements Releasable { return this.wrappedMatOfDouble; } + @SuppressWarnings("unchecked") @JsonIgnore public Matrix getAsWpilibMat() { if (wpilibMat == null) { @@ -120,12 +108,14 @@ public class JsonMatOfDouble implements Releasable { @Override public void release() { - getAsMat().release(); - } + if (wrappedMat != null) { + wrappedMat.release(); + } + if (wrappedMatOfDouble != null) { + wrappedMatOfDouble.release(); + } - public Packet populatePacket(Packet packet) { - packet.encode(this.data); - return packet; + this.released = true; } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java index 7d87974c1..249f3fe05 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java @@ -55,6 +55,10 @@ public class LibcameraGpuSettables extends VisionSourceSettables { } } + public ImageRotationMode getRotation() { + return m_rotationMode; + } + public LibcameraGpuSettables(CameraConfiguration configuration) { super(configuration); @@ -212,7 +216,7 @@ public class LibcameraGpuSettables extends VisionSourceSettables { getConfiguration().path, mode.width, mode.height, - (m_rotationMode == ImageRotationMode.DEG_180 ? 180 : 0)); + (m_rotationMode == ImageRotationMode.DEG_180_CCW ? 180 : 0)); if (r_ptr == 0) { logger.error("Couldn't create a zero copy Pi Camera while switching video modes"); if (!LibCameraJNI.destroyCamera(r_ptr)) { diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java index cedda04d2..45af6af14 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java @@ -21,6 +21,7 @@ import edu.wpi.first.cscore.VideoMode; import org.opencv.core.Point; import org.photonvision.common.util.numbers.DoubleCouple; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.opencv.ImageRotationMode; /** Represents the properties of a frame. */ public class FrameStaticProperties { @@ -35,6 +36,10 @@ public class FrameStaticProperties { public final double verticalFocalLength; public CameraCalibrationCoefficients cameraCalibration; + // CameraCalibrationCoefficients hold native memory, so cache them here to avoid extra allocations + private final FrameStaticProperties[] cachedRotationStaticProperties = + new FrameStaticProperties[4]; + /** * Instantiates a new Frame static properties. * @@ -85,6 +90,32 @@ public class FrameStaticProperties { } } + public FrameStaticProperties rotate(ImageRotationMode rotation) { + if (rotation == ImageRotationMode.DEG_0) { + return this; + } + + int newWidth = imageWidth; + int newHeight = imageHeight; + + if (rotation == ImageRotationMode.DEG_90_CCW || rotation == ImageRotationMode.DEG_270_CCW) { + newWidth = imageHeight; + newHeight = imageWidth; + } + + if (cameraCalibration == null) { + return new FrameStaticProperties(newWidth, newHeight, fov, null); + } + + if (cachedRotationStaticProperties[rotation.ordinal()] == null) { + cachedRotationStaticProperties[rotation.ordinal()] = + new FrameStaticProperties( + newWidth, newHeight, fov, cameraCalibration.rotateCoefficients(rotation)); + } + + return cachedRotationStaticProperties[rotation.ordinal()]; + } + /** * Calculates the horizontal and vertical FOV components from a given diagonal FOV and image size. * diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java index 396c72e3a..2238db496 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java @@ -99,7 +99,7 @@ public abstract class CpuImageProcessor extends FrameProvider { outputMat, m_processType, input.captureTimestamp, - input.staticProps); + input.staticProps.rotate(m_rImagePipe.getParams().rotation)); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java index 0f9f301b7..513deaada 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java @@ -101,7 +101,7 @@ public class LibcameraGpuFrameProvider extends FrameProvider { processedMat, type, MathUtils.wpiNanoTime() - latency, - settables.getFrameStaticProperties()); + settables.getFrameStaticProperties().rotate(settables.getRotation())); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java b/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java index e72dd50e2..4fa3dea7e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java +++ b/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java @@ -17,19 +17,62 @@ package org.photonvision.vision.opencv; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import org.opencv.core.Core; +import org.opencv.core.Point; + +/** + * An image rotation about the camera's +Z axis, which points out of the camera towards the world. + * This is mirrored relative to what you might traditionally think of as image rotation, which is + * about an axis coming out of the image towards the viewer or camera. TODO: pull this from + * image-rotation.md + */ public enum ImageRotationMode { - DEG_0(-1), - DEG_90(0), - DEG_180(1), - DEG_270(2); + DEG_0(-1, new Rotation2d()), + // rotating an image matrix clockwise is a ccw rotation about camera +Z, lmao + DEG_90_CCW(Core.ROTATE_90_COUNTERCLOCKWISE, new Rotation2d(Units.degreesToRadians(90))), + DEG_180_CCW(Core.ROTATE_180, new Rotation2d(Units.degreesToRadians(180))), + DEG_270_CCW(Core.ROTATE_90_CLOCKWISE, new Rotation2d(Units.degreesToRadians(-90))); public final int value; + public final Rotation2d rotation2d; - ImageRotationMode(int value) { + private ImageRotationMode(int value, Rotation2d tr) { this.value = value; + this.rotation2d = tr; } - public boolean isRotated() { - return this.value == DEG_90.value || this.value == DEG_270.value; + /** + * Rotate a point in an image + * + * @param point The point in the unrotated image + * @param width Image width, in pixels + * @param height Image height, in pixels + * @return The point in the rotated frame + */ + public Point rotatePoint(Point point, double width, double height) { + Pose2d offset; + switch (this) { + case DEG_0: + return point; + case DEG_90_CCW: + offset = new Pose2d(width, 0, rotation2d); + break; + case DEG_180_CCW: + offset = new Pose2d(width, height, rotation2d); + break; + case DEG_270_CCW: + offset = new Pose2d(0, height, rotation2d); + break; + default: + throw new RuntimeException("Totally bjork"); + } + + var pointAsPose = new Pose2d(point.x, point.y, new Rotation2d()); + var ret = pointAsPose.relativeTo(offset); + + return new Point(ret.getX(), ret.getY()); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java index 53f5852e7..cdb160982 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java @@ -47,13 +47,6 @@ public class Draw2dCrosshairPipe double y = params.frameStaticProperties.centerY; double scale = params.frameStaticProperties.imageWidth / (double) params.divisor.value / 32.0; - if (this.params.rotMode == ImageRotationMode.DEG_270 - || this.params.rotMode == ImageRotationMode.DEG_90) { - var tmp = x; - x = y; - y = tmp; - } - switch (params.robotOffsetPointMode) { case Single: if (params.singleOffsetPoint.x != 0 && params.singleOffsetPoint.y != 0) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java index 99927ea0f..e3e24d188 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java @@ -18,7 +18,6 @@ package org.photonvision.vision.pipe.impl; import java.awt.*; -import java.util.ArrayList; import java.util.List; import org.apache.commons.lang3.tuple.Pair; import org.opencv.calib3d.Calib3d; @@ -28,6 +27,7 @@ import org.opencv.imgproc.Imgproc; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ColorHelper; +import org.photonvision.estimation.OpenCVHelp; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.FrameDivisor; import org.photonvision.vision.pipe.MutatingPipe; @@ -92,7 +92,11 @@ public class Draw3dTargetsPipe if (params.redistortPoints) { // Distort the points, so they match the image they're being overlaid on - distortPoints(tempMat, tempMat); + tempMat.fromList( + OpenCVHelp.distortPoints( + tempMat.toList(), + params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(), + params.cameraCalibrationCoefficients.getDistCoeffsMat())); } var bottomPoints = tempMat.toList(); @@ -108,7 +112,11 @@ public class Draw3dTargetsPipe if (params.redistortPoints) { // Distort the points, so they match the image they're being overlaid on - distortPoints(tempMat, tempMat); + tempMat.fromList( + OpenCVHelp.distortPoints( + tempMat.toList(), + params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(), + params.cameraCalibrationCoefficients.getDistCoeffsMat())); } var topPoints = tempMat.toList(); @@ -223,45 +231,6 @@ public class Draw3dTargetsPipe return null; } - private void distortPoints(MatOfPoint2f src, MatOfPoint2f dst) { - var pointsList = src.toList(); - var dstList = new ArrayList(); - final Mat cameraMatrix = params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(); - // k1, k2, p1, p2, k3 - final Mat distCoeffs = params.cameraCalibrationCoefficients.getDistCoeffsMat(); - var cx = cameraMatrix.get(0, 2)[0]; - var cy = cameraMatrix.get(1, 2)[0]; - var fx = cameraMatrix.get(0, 0)[0]; - var fy = cameraMatrix.get(1, 1)[0]; - var k1 = distCoeffs.get(0, 0)[0]; - var k2 = distCoeffs.get(0, 1)[0]; - var k3 = distCoeffs.get(0, 4)[0]; - var p1 = distCoeffs.get(0, 2)[0]; - var p2 = distCoeffs.get(0, 3)[0]; - - for (Point point : pointsList) { - // To relative coordinates <- this is the step you are missing. - double x = (point.x - cx) / fx; // cx, cy is the center of distortion - double y = (point.y - cy) / fy; - - double r2 = x * x + y * y; // square of the radius from center - - // Radial distortion - double xDistort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2); - double yDistort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2); - - // Tangential distortion - xDistort = xDistort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x)); - yDistort = yDistort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y); - - // Back to absolute coordinates. - xDistort = xDistort * fx + cx; - yDistort = yDistort * fy + cy; - dstList.add(new Point(xDistort, yDistort)); - } - dst.fromList(dstList); - } - private void divideMat2f(MatOfPoint2f src, MatOfPoint dst) { var hull = src.toArray(); var pointArray = new Point[hull.length]; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index f378f511c..68ed0e4a4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -619,8 +619,8 @@ public class VisionModule { public void addCalibrationToConfig(CameraCalibrationCoefficients newCalibration) { if (newCalibration != null) { - logger.info("Got new calibration for " + newCalibration.resolution); - visionSource.getSettables().getConfiguration().addCalibration(newCalibration); + logger.info("Got new calibration for " + newCalibration.unrotatedImageSize); + visionSource.getSettables().addCalibration(newCalibration); } else { logger.error("Got null calibration?"); } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index 56db6214f..edbf3c07a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -114,8 +114,8 @@ public abstract class VisionSourceSettables { configuration.calibrations.stream() .filter( it -> - it.resolution.width == videoMode.width - && it.resolution.height == videoMode.height) + it.unrotatedImageSize.width == videoMode.width + && it.unrotatedImageSize.height == videoMode.height) .findFirst() .orElse(null)); } diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java new file mode 100644 index 000000000..90fae648e --- /dev/null +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java @@ -0,0 +1,252 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.pipeline; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.io.IOException; +import java.util.Arrays; +import java.util.List; +import java.util.stream.Collectors; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; +import org.junitpioneer.jupiter.cartesian.CartesianTest; +import org.junitpioneer.jupiter.cartesian.CartesianTest.Enum; +import org.opencv.core.Point; +import org.opencv.core.Size; +import org.photonvision.common.configuration.ConfigManager; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.LogLevel; +import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TestUtils; +import org.photonvision.estimation.OpenCVHelp; +import org.photonvision.mrcal.MrCalJNILoader; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.calibration.CameraLensModel; +import org.photonvision.vision.calibration.JsonMatOfDouble; +import org.photonvision.vision.frame.FrameStaticProperties; +import org.photonvision.vision.opencv.ImageRotationMode; +import org.photonvision.vision.pipe.impl.SolvePNPPipe; +import org.photonvision.vision.pipe.impl.SolvePNPPipe.SolvePNPPipeParams; +import org.photonvision.vision.target.TargetModel; +import org.photonvision.vision.target.TrackedTarget; + +public class CalibrationRotationPipeTest { + @BeforeAll + public static void init() throws IOException { + TestUtils.loadLibraries(); + MrCalJNILoader.forceLoad(); + + var logLevel = LogLevel.DEBUG; + Logger.setLevel(LogGroup.Camera, logLevel); + Logger.setLevel(LogGroup.WebServer, logLevel); + Logger.setLevel(LogGroup.VisionModule, logLevel); + Logger.setLevel(LogGroup.Data, logLevel); + Logger.setLevel(LogGroup.Config, logLevel); + Logger.setLevel(LogGroup.General, logLevel); + ConfigManager.getInstance().load(); + } + + @Test + public void meme() { + var s = new Size(200, 100); + + var p = new Point(2, 1); + + { + var angle = ImageRotationMode.DEG_90_CCW; + var expected = new Point(p.y, s.width - p.x); + var rotatedP = angle.rotatePoint(p, s.width, s.height); + assertEquals(expected.x, rotatedP.x, 1e-6); + assertEquals(expected.y, rotatedP.y, 1e-6); + } + { + var angle = ImageRotationMode.DEG_180_CCW; + var expected = new Point(s.width - p.x, s.height - p.y); + var rotatedP = angle.rotatePoint(p, s.width, s.height); + assertEquals(expected.x, rotatedP.x, 1e-6); + assertEquals(expected.y, rotatedP.y, 1e-6); + } + { + var angle = ImageRotationMode.DEG_270_CCW; + var expected = new Point(s.height - p.y, p.x); + var rotatedP = angle.rotatePoint(p, s.width, s.height); + assertEquals(expected.x, rotatedP.x, 1e-6); + assertEquals(expected.y, rotatedP.y, 1e-6); + } + } + + @CartesianTest + public void testUndistortImagePointsWithRotation(@Enum ImageRotationMode rot) { + if (rot == ImageRotationMode.DEG_0) { + return; + } + + CameraCalibrationCoefficients coeffs = + new CameraCalibrationCoefficients( + new Size(1270, 720), + new JsonMatOfDouble( + 3, + 3, + new double[] { + 900, 0, 500, + 0, 951, 321, + 0, 0, 1 + }), + new JsonMatOfDouble( + 1, + 8, + new double[] { + 0.25, + -1.5, + 0.0017808248356550637, + .00004, + 2.179764689221826, + -0.034952777924711353, + 0.09625562194891251, + -0.1860797479660746 + }), + new double[] {}, + List.of(), + new Size(), + 1, + CameraLensModel.LENSMODEL_OPENCV); + + FrameStaticProperties frameProps = + new FrameStaticProperties( + (int) coeffs.unrotatedImageSize.width, + (int) coeffs.unrotatedImageSize.height, + 66, + coeffs); + + FrameStaticProperties rotatedFrameProps = frameProps.rotate(rot); + + Point[] originalPoints = {new Point(100, 100), new Point(200, 200), new Point(300, 100)}; + + // Distort the origional points + var distortedOriginalPoints = + OpenCVHelp.distortPoints( + List.of(originalPoints), + frameProps.cameraCalibration.getCameraIntrinsicsMat(), + frameProps.cameraCalibration.getDistCoeffsMat()); + + // and rotate them once distorted + var rotatedDistortedPoints = + distortedOriginalPoints.stream() + .map(it -> rot.rotatePoint(it, frameProps.imageWidth, frameProps.imageHeight)) + .collect(Collectors.toList()); + + // Now let's instead rotate then distort + var rotatedOrigionalPoints = + Arrays.stream(originalPoints) + .map(it -> rot.rotatePoint(it, frameProps.imageWidth, frameProps.imageHeight)) + .collect(Collectors.toList()); + + var distortedRotatedPoints = + OpenCVHelp.distortPoints( + rotatedOrigionalPoints, + rotatedFrameProps.cameraCalibration.getCameraIntrinsicsMat(), + rotatedFrameProps.cameraCalibration.getDistCoeffsMat()); + + System.out.println("Rotated distorted: " + rotatedDistortedPoints.toString()); + System.out.println("Distorted rotated: " + distortedRotatedPoints.toString()); + + for (int i = 0; i < distortedRotatedPoints.size(); i++) { + assertEquals(rotatedDistortedPoints.get(i).x, distortedRotatedPoints.get(i).x, 1e-6); + assertEquals(rotatedDistortedPoints.get(i).y, distortedRotatedPoints.get(i).y, 1e-6); + } + } + + @Test + public void testApriltagRotated() { + // matt's lifecam + CameraCalibrationCoefficients coeffs = + new CameraCalibrationCoefficients( + new Size(1270, 720), + new JsonMatOfDouble( + 3, + 3, + new double[] { + 1132.983599412085, 0.0, 610.3195830765223, + 0.0, 1138.2884596791835, 346.4121207400337, + 0.0, 0.0, 1.0 + }), + new JsonMatOfDouble( + 1, + 8, + new double[] { + 0.11508197558262527, + -1.158603446817735, + 0.0017808248356550637, + 4.3915976993589873E-4, + 2.179764689221826, + -0.034952777924711353, + 0.04625562194891251, + -0.0860797479660746 + }), + new double[] {}, + List.of(), + new Size(), + 1, + CameraLensModel.LENSMODEL_OPENCV); + + // Matt's lifecam pointing at a wall + var distortedCorners = + List.of( + new Point(834.702271, 338.878143), + new Point(1011.808899, 345.824463), + new Point(964.300476, 225.330795), + new Point(803.971191, 217.359055)); + + SolvePNPPipe pipe = new SolvePNPPipe(); + + pipe.setParams(new SolvePNPPipeParams(coeffs, TargetModel.kAprilTag6p5in_36h11)); + var ret = pipe.run(List.of(new TrackedTarget(distortedCorners))); + + // rotate and try again + var rotAngle = ImageRotationMode.DEG_90_CCW; + var rotatedDistortedPoints = + distortedCorners.stream() + .map(it -> rotAngle.rotatePoint(it, 1280, 720)) + .collect(Collectors.toList()); + pipe.setParams( + new SolvePNPPipeParams( + coeffs.rotateCoefficients(rotAngle), TargetModel.kAprilTag6p5in_36h11)); + var retRotated = pipe.run(List.of(new TrackedTarget(rotatedDistortedPoints))); + + var pose_base = ret.output.get(0).getBestCameraToTarget3d(); + // So this is ostensibly a rotation about camera +Z, + // but this is actually camera +X for our AprilTag pipe since we rotate to stay in ""WPILib""". + // Negative to return to upright + var pose_unrotated = retRotated.output.get(0).getBestCameraToTarget3d(); + + System.out.println("Base: " + pose_base); + System.out.println("rot-unrot: " + pose_unrotated); + + Assertions.assertEquals(pose_base.getX(), pose_unrotated.getX(), 0.01); + Assertions.assertEquals(pose_base.getY(), pose_unrotated.getY(), 0.01); + Assertions.assertEquals(pose_base.getZ(), pose_unrotated.getZ(), 0.01); + Assertions.assertEquals( + pose_base.getRotation().getX(), pose_unrotated.getRotation().getX(), 0.01); + Assertions.assertEquals( + pose_base.getRotation().getY(), pose_unrotated.getRotation().getY(), 0.01); + Assertions.assertEquals( + pose_base.getRotation().getZ(), pose_unrotated.getRotation().getZ(), 0.01); + } +} diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index 50607cdf2..b286adea4 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -568,8 +568,8 @@ public class RequestHandler { .stream() .filter( it -> - Math.abs(it.resolution.width - width) < 1e-4 - && Math.abs(it.resolution.height - height) < 1e-4) + Math.abs(it.unrotatedImageSize.width - width) < 1e-4 + && Math.abs(it.unrotatedImageSize.height - height) < 1e-4) .findFirst() .orElse(null); @@ -617,8 +617,8 @@ public class RequestHandler { cc.calibrations.stream() .filter( it -> - Math.abs(it.resolution.width - width) < 1e-4 - && Math.abs(it.resolution.height - height) < 1e-4) + Math.abs(it.unrotatedImageSize.width - width) < 1e-4 + && Math.abs(it.unrotatedImageSize.height - height) < 1e-4) .findFirst() .orElse(null); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 7a1f23a60..9f328d9ae 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -251,6 +251,65 @@ public final class OpenCVHelp { return trl.rotateBy(NWU_TO_EDN); } + /** + * Distort a list of points in pixels using the OPENCV5/8 models. See image-rotation.md or + * https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html for the math here. + * + * @param pointsList the undistorted points + * @param cameraMatrix standard OpenCV camera mat + * @param distCoeffs standard OpenCV distortion coefficeints. Must OPENCV5 or OPENCV8 + */ + public static List distortPoints( + List pointsList, Mat cameraMatrix, Mat distCoeffs) { + var ret = new ArrayList(); + + var cx = cameraMatrix.get(0, 2)[0]; + var cy = cameraMatrix.get(1, 2)[0]; + var fx = cameraMatrix.get(0, 0)[0]; + var fy = cameraMatrix.get(1, 1)[0]; + + var k1 = distCoeffs.get(0, 0)[0]; + var k2 = distCoeffs.get(0, 1)[0]; + var p1 = distCoeffs.get(0, 2)[0]; + var p2 = distCoeffs.get(0, 3)[0]; + var k3 = distCoeffs.get(0, 4)[0]; + + double k4 = 0; + double k5 = 0; + double k6 = 0; + if (distCoeffs.cols() == 8) { + k4 = distCoeffs.get(0, 5)[0]; + k5 = distCoeffs.get(0, 6)[0]; + k6 = distCoeffs.get(0, 7)[0]; + } + + for (Point point : pointsList) { + // To relative coordinates + double xprime = (point.x - cx) / fx; // cx, cy is the center of distortion + double yprime = (point.y - cy) / fy; + + double r_sq = xprime * xprime + yprime * yprime; // square of the radius from center + + // Radial distortion + double radialDistortion = + (1 + k1 * r_sq + k2 * r_sq * r_sq + k3 * r_sq * r_sq * r_sq) + / (1 + k4 * r_sq + k5 * r_sq * r_sq + k6 * r_sq * r_sq * r_sq); + double xDistort = xprime * radialDistortion; + double yDistort = yprime * radialDistortion; + + // Tangential distortion + xDistort = xDistort + (2 * p1 * xprime * yprime + p2 * (r_sq + 2 * xprime * xprime)); + yDistort = yDistort + (p1 * (r_sq + 2 * yprime * yprime) + 2 * p2 * xprime * yprime); + + // Back to absolute coordinates. + xDistort = xDistort * fx + cx; + yDistort = yDistort * fy + cy; + ret.add(new Point(xDistort, yDistort)); + } + + return ret; + } + /** * Project object points from the 3d world into the 2d camera image. The camera * properties(intrinsics, distortion) determine the results of this projection.