From 417d7a38defb26cce3680add0f03807dddd181ef Mon Sep 17 00:00:00 2001 From: catree Date: Thu, 27 Aug 2020 07:24:07 +0200 Subject: [PATCH] Add Robot-World/Hand-Eye calibration function. --- doc/opencv.bib | 18 +- .../doc/pics/robot-world_hand-eye_figure.png | Bin 0 -> 23978 bytes modules/calib3d/include/opencv2/calib3d.hpp | 172 +++- modules/calib3d/src/calibration_handeye.cpp | 246 ++++- .../test/test_calibration_hand_eye.cpp | 854 ++++++++++++------ 5 files changed, 980 insertions(+), 310 deletions(-) create mode 100644 modules/calib3d/doc/pics/robot-world_hand-eye_figure.png diff --git a/doc/opencv.bib b/doc/opencv.bib index 6045a8434d..54396d6a10 100644 --- a/doc/opencv.bib +++ b/doc/opencv.bib @@ -217,7 +217,7 @@ hal_id = {inria-00350283}, hal_version = {v1}, } -@article{Collins14 +@article{Collins14, year = {2014}, issn = {0920-5691}, journal = {International Journal of Computer Vision}, @@ -612,6 +612,14 @@ number = {8}, publisher = {IOP Publishing Ltd} } +@article{Li2010SimultaneousRA, + title = {Simultaneous robot-world and hand-eye calibration using dual-quaternions and Kronecker product}, + author = {Aiguo Li and Lin Wang and Defeng Wu}, + journal = {International Journal of Physical Sciences}, + year = {2010}, + volume = {5}, + pages = {1530-1536} +} @article{LibSVM, author = {Chang, Chih-Chung and Lin, Chih-Jen}, title = {LIBSVM: a library for support vector machines}, @@ -922,6 +930,14 @@ number = {2}, publisher = {Springer} } +@article{Shah2013SolvingTR, + title = {Solving the Robot-World/Hand-Eye Calibration Problem Using the Kronecker Product}, + author = {Mili Shah}, + journal = {Journal of Mechanisms and Robotics}, + year = {2013}, + volume = {5}, + pages = {031007} +} @inproceedings{Shi94, author = {Shi, Jianbo and Tomasi, Carlo}, title = {Good features to track}, diff --git a/modules/calib3d/doc/pics/robot-world_hand-eye_figure.png b/modules/calib3d/doc/pics/robot-world_hand-eye_figure.png new file mode 100644 index 0000000000000000000000000000000000000000..867bc81c7332365ed1699848aea5cb3669651665 GIT binary patch literal 23978 zcmXtg2Q=H?`@e)3QJdKMp;n2YX4PtHE2@gvL0g-ks2#*s9kwb;)e4f@J9bO$U0Z9{ zXzl$=zvuryIq#E`d+vCi=RWuK-22?uJF$=SHEF;cU=k7%8f`7L$0Q_x0pjCBNlq-W z-2(snBDd4kR3o|m_sMN8Od^(0p|s4rNl0ir|2=>dQCdzClK)7w)$Tue`Q!HozjS`% z@#uw}?54W}SRat{>)k>Se+2rS!0r2ns_G03_ZbY|h4bJ4ru{CQ?Cp(rqrk>rS>D!D zT!O)&Sw;(E2fkSwkH#+pa|&}JGqN`~EsT%-2J40R=?yLU=Lso{Zm>!ZgDs!&Gcx)2oc)j*KDSQ_u?}Nnw6y zZ*+B-pcR-{%kVQ`s$Hzt6nzW*R`m;mr~(3e8&Z@qOJa&Nv?U$LgcsvC@z6Pp_v zk^@40MfZ_;Ta0?ZpJA1_^*JfLunp)96)&cUO_!vW^mQW(D+Pm&?QmT!X$5fM!Fdjx zl2?SVNqDko4(++)UPW?l&(Jza+Shx=Oze=B5wY`Yi%_g;@x8T=k?@RQK~P2*-y-Ug z_Y__!04X@T=PU%TW;lVFG_TtslVR0FDIdqIenvdq9K>t=Fh39Jvk%M}Jn_@p{6fu( zSr^^12Tg<;N93`8rjZAq(91Kg1QSiOPTFT5ITdOU{2TBY2S2C)umU@Ls400d!pU9e z9`t@uMb#@ZoHXj@o`lKRePa1nZBEoJ6B@@y9}?RnY^0kKeQbwY3Woemg zPZ839R)Prac0}(XQGKmjfZw!D6xh-w64pK?+(>w<#{CCG-Qm#J>O(uPOEx}<3(Ize zM-7mZJ8EI*;Wx;3I`g!R)rF))fkx<3bQ=dP(S|nf2$BRN0v|_WRssbNz@0?HQJ!cE zWivY6i-m^|4C(FBjd;Q7d>JmH@oa98^8l|fc^dJy{3xkCk_ib5N?tYYH=VuTm10-K z;+EOss!anQE#Jn_AGXCIGP`6s;*_PxPHHW(?WO;9&?s#SfU!QF>6#IJ=06{kvSIPC zgJ}BdZ{-N4wE}5o0b4c{6H0t@i`auq4?XneABiXHxyd&u+}i>Iod*$=yv`W1JzeO3 zTG|%{4h)N-h@l}>nrM1>XHJ(S>$W~e+!s8V1pADN`#6dyS&%kR$iBSuIgO^78Tm_E zd2W^{R(QIfRq8==f`mJnj$Ti^iTDl)Fl^xZ4GtlN3s^C_bDil$O1 z!-Hi-^?33`v6RX0?2rL98M20d^TZX5eQ2A#@H0u0pVJw(dwgNILUbSCZB&}_k zFu9)fEY6~B%CTqYG`uv+p(+bJhEnpzOVE@U`C4ZC5z?S7JpGD1DE@~S4~{C;sPSH0 z6|u_KJyLA?IXWiC;TDQZf^(g=d6je$slMUzia&Mc6!yP5T0=m^8yC=Jh#qiiQ2ooj zj#4K1h#xPUWJ!chmo@Zo=PIdQfz-TbU8{t9YOQilsU>9M9my*y(0G}E59B1*fbs{R zrE}+caSBT+u7ufncuWYkrGjjL|LneOJRTP>ds$5~RUTC_iC8pSJpPgyXGeyD?%f@V ztZ(jdMqucJZGk4%9*1mMF1+p204{wuKlW*QYRC_D6C>+Pd~OM=glPyW=Y=iHm#Pzb zIJ5ejaIe6Jn9rb>g0E4&r2FXiVPeitRFeqD_%A`C8?pmhU<6a$cr$&*PErOTfJ1~= zP4Whc_sl&3b477iu>B!0K-6UD;{;jNu-In`{1d)m@7D=!B1k>)MV|Um)~7^oq@N+J z=y6?k1MCRw&#&G}r>>NWKws68gI*L-uT2-J@u_m6{@a7B#)&=kBO*@aRBv1guGcj%04N2zBfk|(z5P)tOi%fstkkIR03YX!K}UHvy?or{dJsUV4W z)S-v}vWM>R*3dB*QvGdzBO}R#h>j#xWJLToj7Jovy#RcO-wty(Crvd6PIa(?UB;%B zvIyZbFyQLJeTk0K)}H*;C3uYD1+(aXJcY|kE{zCs!?&z;>8qKDeX4aDf`%mP5V@+>@tJq?=Z~tQylS+WVdwa$Z=N3+>XfC2gQ=hvm}if-fC`ht!K)_tM;>rungC6LEQWrh+6l(`Ifiy4B^T+Q`W}LS z3b&sFGYNn1EeN-ByzcPh*qC%S&mfb!$OPV)kmkiuj>fIH@37>1&%er^?bl!Y%4qgP zn|1MLCFMF-ElM`#Em{_khEx|L@4T>fYkRD2-M`2!JXuP5BA{cy0LM(Rd9S@n_;yN5 zmJQZ(t+4aa)YUGdS{Gh)!iqfMx}_cwQtJ2UB|iRfi!!j=rGy?%2J}=w-TqTFoIw(z zWk9bLeI1&Y@RNyhU5IE)J-lksEBlu@Fw!7HP}_}3d_OB*3{%8J4iqQ7*ZnIfx-pXd zp`*d6Ur{H}9y)P-=RMk*8q2x#O(s(2BIJ(7d?cED@s3W?^O4C(S0s}jJ3K}V=k@e~ zot@iNW_EKKXv*55K@}`n@l7)%k`XKCswYatDVF;gAAYv6PBbN&Nx8)COuA*5~FvtnkGaqk)l_BBLMZ zH9SwEXS(z?7h{pXyO`!uQ!1JM0B^3T=0jB`0xv>oLcq>IDmdQp2CJ9Nz4la37~liQl7RtWMLx4I|Ndn?&wDgysjE+j-X+Tr zYu=7i8{gyO*tYs|^PpPV{oA&oo)vN4VqZ^SYhL zcF1VsPyA&`k1>M9bok_Cd7~J#bzD84s6Z~2YtFvjvatyaz=bY|d7>+SFKi(%Y7#!C zXpdL>%fc6u-8j0U3td=cX z6m8)#qjAC&ZEx(>{t81e@$K-NK&}-F{ZOt%ySij$ZntgV;`obcWfRTgVtSvhD8z>m zTZS6h*Pr}&kLR|=q}~{)ibIpo<%`E(eI+?6$RNKb4Aqx@lZ3==PJs=L_;HkkP^LK` z&>npptHFCLOtrp%!(6)p8|Js z7L7rgd>8y}&OF3yBC;3SS?a$JcA~~PH|^xL)UsiUsKP&wuQBzD2)X>5LUpy)H-S&c zUZ}U0j}ifB0e{v~EZygt+v;U@7=L0Xs()Wxdr`h*W-kEnr!Sf%?-xJr(d8-^-2*9; zFRpME>z{r6Z2gGgh+PeoD`%L?hSk?#|2eN<#v$r2luv=uCZt2C%xr3YD4)Pha6=?c zLXnBBYXI!mvk!!)Sh9In=w3coo+OVbjVvtuXCr$vb$w2pbLf$#s;L*?LtA^8sMNPv zvH%{4=ZD@exO$-IH`YR>wdVEP(}*&Knw~9d$l+IKf)rtCDqEdt&m|`rru*K}*Za@8 z7_P{|U+;?Jch5#QIF0VcWjQ;7%e!EJTo%@Gd{Q3oH`e=G#{T?%^^>nkBESZb)RH?9 z?IoMv$z{KKHN_HU+=II#AvWnIqnlY8FCzY6gsE>?{VvzFaH`l%{mJXJtHS1e_qlnB zTs`Q=E1!zNUl5nBzYsr>KgU{j=avp?sKZ9@ufR>RM2&O!3jL7oMRvd3a8Ak!k2Z-4 zX|^Cw{fB;cPi0M?#=;j4=_;5a?RUzCK{vhMTmORPpvN!6UP^E8M%lSfgMu$+>s#Qk z!_F}>>=wkq6w~dWOZ`luxM>CQxYoVx-SEb;CnZW3u$!ax+|R8xe%BTHNVsIDCwXB@ z4v=Pl-_XPKH2X|;80L9f?Hi_Y*yE(a64g$($`8Hy_pplP^wHu*!@Tl6nW{o zkys{r!G1;tJ|wqQBi(K9d-q+q#(Q(I)~GfQzJQ9hM70dM|=l2gc8Y zJ5Mo{W775;us{t**HP;Fh3FFOleo82)+^B3a0NL0$4dJsf>8O#yM$FZnc$Tz?t|YY z$tD~zcz-y5&J6j_ctD|sHLsnAh|DBM-F!3#guikgB=et67@v+a4&bNeConMyPK@%g z^O3XTBz1p}591JVBO!>8=O+QKHUvp>;A>=HcFMui8(kx9%D%6=C}7JG;EVaR$8%AW z-nbwSUgvC$0G;G#KNg^cC|Xiq&tc^vy6{G+)W=LL@$T%4`$KA8x4`2SvbuI{QcIVs zyr7G@RhXry@gmoI9J0A^u01QV03oLCc^;)}-=$Hq@g*4ja{P)V59!-xS#29O3!&~= zXnaic(ii&u2!#i0ibX)S_&YAANI_NpTdwWsDY>Cb7J6 z5r45WclA=(#MfXjNTuf-$Hz+(d{M4xIHypvO$ZZyq&4Whd6cSqppCh z*QLG;P`$-Lb!FH4*ZmLp?tFF`G9oYU16iFb4I@b=LZ#AYRXTn&t=RLK^!PU3i8;+{ z*?YeVKNiHPT<-2jC1;CRj|VhIru=2j`+QQ|7_Pq;eGJ9Q``XOKT`~-~&5q3BzYcPd z2Vs18ie+E9XJB;SmgUYsAG5=&4&O6gzYQ*ZS;=Zr?C|g= zYq2g8r1rc)=137TWimj(kI!b7{hD9BJO~aop`>yM3zY+UyR@vQu9YC(jCdn9S}jYD zOL1V4aauTKW0wMcM-rS6Q(m|tgg`%{7k=>+0I`vpfbWj+HA_X{+}<8v&Yv)QU(%Km z{PciAMpvpIVXj1A7S1$@$&1{V41MD+0++9CvwSWT1??fn<_(wK#eRd>RPR1o*;OJD zCD1?QtFYHT;cNJ0I(^Eld#ksH9AQxhkBgADJ8gX3C_f0g3Ip*Qk3_Z>EhMcl_HPh2 zrexzG@o;9&Dlt74gZ1u?KPpO&BYdUCC>oi)REL7`=vV#io6eSaAK|Y?= z?nE2y2#f_S`!_k}$ zr;Qq}*ZV^v+2?i#u%AaHmj;%sDK-}u$@5|GRuR?T8pt+R6@picM_YIY1UWpu#h`Tj zZ1MSyF87RodSJk3acSCDXcjTGxe-hX>$#QVmrQiMV`z^3Uv_kL@Q*s3wGW-)8SSU& znRT!3cYrCA`~NZKylLIG0-i*+`bAk}R3L+G^JhUqbM3HCJ*z)MJm}gihuQ{n2$nM? z9Z)rvsj;%^etdBLy;)%Yp=$YZl1a6thVCeF%c7U5YNr>FJEZcJ=j$jqXo1yz?#fRH z`RrCc4>P1nck92<0PUfaXUO%y(5x;-D|VK1`n+YjeR*hG5G%AzapA#_tqEE_>=j8GW<2N z&COHi+tfs)O7rI69D?5P0Yvw_S$y{WzGhHcvMzI) zU#V2EWExqEy0or@Aa1b%UVG1c_KsN9)Oj#LL!B#>3h61P7AZ^@Sa1AjgOJNj+!tQ5 zF>52@$skY5RX@IXOmLbCagc8a(6d3hnUnq;0X99f?V6{{J;_=P4!R!%SiLea=Hu0= z(@|#desApcOo61&fm$#|4tGFm_&(v5ckUxtDsaB-=!g*^dAx?tqg*IvSPJ5eb_~(G z&h(`F`v(jWJq8AjGUW@J6Cu=IVrZ1|;H_XiT%PkxC`jjw-7wxqx1G>uUniZgbcE(y=Mn@o(u zIrk%Hx55roKN{dTdTqPy&fn_-wvaNe5hH=No_AX`F#Apr*irU;LZiy^UHN$98LFVz z@z2?q**Z9Lmps;PFdb~p(@I$u&Wa4g<|$8Q5!YVYXp-Gq0*9Qz@$vUtf$G`LD_m*j z@;QG!PsUiOHu~#k(E00u96A!8>qR*GeM&=Ypki9tH+lc!NLOJcPn3p+5u3VuL&`DW z`bnk?U}U>YEN9p)v$}NEvn`Ef8~*(1-D}ZrvkCWv(^a_HjLA!-jDGEiB8IX%jbgaC zs4PuCY758KPleam?h9jW@{7c}?TZE5FqdS_R);h@De3R+--V{1Oy1FNKt8V&OzRY( z?k^1OX9eRG)*8o8nZD~0&#-W?sE1!rq)9r{#s>K?&1?cumRlQPr{Hlb|c)oy*#sk_KF(;sWhc_f+mrw2N}B|oEEhkxe1r@>a} zKB~RZ+52?`%&Y^T7#@8lpk$n%dTy$p=a=+*^sK&4?*#GL;=^|OmSx^;HHOlm*Eqhd zmFF@>0KwB;H+{_iyEAYKMBG`{BSTkzcTOX=J2adl=l}*&mZ2xvD+zQO{&an&~bXH-cKeX@w{zT0>CaZLiNjS8)`T3$OQ4!Oz zY|4e`4I)7Qw;y87NNso8yHIQM>XkZpi*`(VwP;dIM|z$j=`P643|evU*g5bT5u-ec zF8Ii0wmNt!OWc56)3SK6HnVH(Hgw&z-R}O%mwnH6`y064&!|&q?-_SMWB#b<<;5q= z&*tFNHB_EWeCZ<@ab8FsC_PT#URu0tfTn&+?BQ)@z7JyS+XJo1zuWAou#LU?{?Jl9 z;V$tg+bj8!$3w_3EWA>*YMm=OMWZQoo~AppC{P1=H$^mg`^#Ov#)>5i;7T+mD^cvKyql>$ zVaK-@th*lt2wAF>j+cJzAPb-XE9tMQesukWuEr}Ja^j^+KT&UdK(u%1<$_Tm|k5!%BeNpYwAXpeRUVieAN+IB)_oF^6W03vlNThg2e+ExXyz%AE)~9 zpw)s|o<07?oKS>MJwPJzYCK4c^(Q{ng(=w6UuD9}O(JzG;FZOq_eFEc(<+OG+K(ai za3{Z(FcI_3S z6z{iTMc?`u3Z7juITxPG%d>VOxr(e76_L?ie0_i!8`O-PRre&ij#WF+|FoftYYI6- za>AG8}lkG*z0h)cQqQr_q?`{hI`YVivv%<5d$$SMcH23-|XTr6TknbO}U?-hq#;(27v|8`FLcO@Y;vO zy)GP40PfNT6z%N}&km~DYgM8S3z5H2|KSZgt77yZs4uw+T*l2YSxqx==I zi9jQXLF&tAGcm@TRJFn8W)H;9+WvFZ5K|xt@>ez04#*6>5kCDZy@G*Y97Puk~(=!`Jg!m-7W{He-D8_F@^`sp~Hn@`>kw2ZqEpY*e+6uEIfp zZDX`>Ip~5eelc-BIJL8vxz$L zMqjK*JpC!O%oxWXyPD_J#EGs z-&FqO<~Kp8`LO_0_1WW;-9cHb&Lk&6aLl7*LJb;z1{@o+?o7~T&WU?R(gF%%VeH%6 z(*G6CFs*GnoWExBh!B1;q+)OE4{HmL7O{GY)P{aJaDrZbn%{q)2FeBvj*@{om+Qnq;Cc0cSa$=!wkY2aJ6&| z?Bl*R^{Y9+0WEp&K>+QxSVYEd*+PyHIykGP)s z4ZlmfmE)gzy?u``eZ?(67U|C`+r{@PG$G(PE@lL%NEcN3we>woP{sr4M%*G@H3L&5 z<%B1pfPpcM+?CE(4tsj`0QM=T?!PNKQM#8(5aTNe2=6%vvhKyZ$IWV|MA{Xn?90*G2~eF=e$e#B^k(xX-QkxW~31c-yjGk5gJ{KM*~WFatAK<9=_a zt{pX~s3lMFc#9p%^m@FDf3}#d1W+FV^XfbRpD^t4;4(TZLt+9{3GdSHeX3OJIR|?$ zfDJ_lJw|d~A2@u%lf`%HBeSFKuANwhgPvKD{SGtcU}PL9c}qCr)xDhs9|Lya2)Qfh z`YY^g__)dT(J|Mn@l2Y|U0q^U)z2^>K!2bk0OMOXlt^%{l(h@fus~XKzRjS;mJB_@ zI(E4um}}^bc~|6?@t5)%4W&nE93~9*j0lxMAxq|HyjY;l%m>+Zq3 zgcUABgdd$ZJcQ$zaRMlDldu1M?;%tTT?@@-Uew*pnOs^f{?;o0oP@&8LXY*UT^hhdVJ_OmJS^pM)OFMg~|cM9uE={W>KdUWjZCn z!IVxj?x!UYSq=f-muuXF{9g+T-+MR_3hC>d74|E1$L&YT#+x{e7%!f+<@EP8xN>e%96TOQLB#Qfo%a10=k zpihrou`l5;d9gvh(-F{+y|L_1F`b_giK!g?gi|bX@QI)rj;BR7w5kPl?rT7G7Ztzz zoEJq2wD8{wvd*7dW`@VqjmC~wHc#1zI&1M~t>EM}^9f|UYHt42R|zZ2)`WF;?YA*S0;~2uQ45~bPf{}YvjpsKK+LaGU~lQ)KBdRq#feJr#HT&V3$^{;9p03Q z{~IgvNqh-P6;GOz%cakoC+`r%RL73^ku%zX`++?kF>bY>9HY3SUi?*;am)Q=RkE)7HW4OhW+niG~jvsvKUr+!3RR$?{0yTCVMLy z$>Bmvze(@Y7DDxTbvnEd(hi(8a(CxJ<)oz=RV)w#3D!D}x;uTIz8y{Xmhvy&`A?B){-$B$#&4T&y5-J^Y}d2i8%&ucrv)P+A{ zsSG(*PcaiV9QfSS!3QcXq}{wUT$qF2Sx#k+s`uG03WavYU-+^Y9e!JVW?pzehK}?Q2=C0(y`p>7cOMAxn*y zeAkaVMAdRmsZ9D^937XMTdDVN-i8fnJ$+7T(s!g#d*fVJYJwSgXZ4Fg!-_xkDx*8t zET4{~wvk@;ERgAHmv#q_K!ai;LnmXN!Dp#vZz??RXY6{B95$t#K(uRKmkJTD7JxIz zbFaqJ-KthIj?ebz1G=QGg`rL&+Sm`%VTj`_(>XLHu%n#LEJ`~kF!X19zHg`p@�D zABhQ_+?m)I4oUa~vtKa-&HQ(hKi(+j>oBH%yQV9%{~DAY#UjC4U{3?JTFDI|(j%Vw z>yiW9AKr84oahp;V2Ce!09i9Jn;+eTI9Qa~WC`zhCUytyZ3p;bV#X|o!;PCWeyg|u zS^$cX4b{n+4mfoAe%P@jt1}*QDTo-HXuR{iaN~L*-2e2fQf_()J_mnw9Q7(sTZv;P z%S0rOemFxYgrbS<3p66Q;#siQB)qojMI$1cGK&=ZBibIiyuYps`hMMkCw#p6-uMCb zx8td&Nwy(EIs;=ANW+;i!|kmeB#}CG!1OG+u(@+)CzqONv z7>bri9jSn>;_JV^{)lV-dWyj8v|RaBS}HF1Z2h%orm~W1ucs8bD}bT5RVD_k3Ai4et`BhI)qlv{Zx&92uCAtdctCkQd(;{chR^UwS1NEBlcZ%zJ*eiPY)2BQr zHk0BtwKLPyipL5@MH8R+v$`sUCw4Txjln73eh|kW0r$>4z z9w}k`EGa#}RLfCUyZKHWI5L#~7=>b7 z`(2ilFh+Qc9g*lqcEXxk;$@_=Hhc}B`^0_gWLIt7f$9sZ@p|`>bS?R38fwyP10OJcZ}#_%t$@t(E+B>YL=@|_OhDWxuz9^(n{IT5|61pPXq|BNoc z>5FlH%+VvW{_IEI#gCnI`PEpeW+>k}bSaLZS08;Yo=BaYINn8-*`ZH$8g9_V&})uXmz-0`3l}ANpNY+T$r_HySWq z42bsy*jD-2?4eZ{R`m-#d`!UwunTA$2{ia(UIh>ZmX3}ZRe?m~=&v$_->q26N9)K( z(&yRmzzt9K@fQT4sU{7o4kD978cjFKHHliH$nfNB_eI%7^Tij*d5@(ag&3l1^=eX}_)Na7p_20$U6vkx z{pTcc5;UcGM-Kwd?BdwK4os<^?&+ne5vva56Vwrzy(k(@#Ypd>ocRB>HxQ~Byh}$7 zaUOcpX&@hMyY29N>m8ALsGc6S?Y%?g`gtj)5T&o#MMqr3?;e5%$geOfBk}1}Ym%{ah#liNG!~Awdc?35csDm%X2_0c}q00#KBWv(>Q|Il0z|8efo->4Z9T6f+D}0 zwV6O%fa4#jl%;n6Xt511 zNUwp`6p8Sa6F$+b2>c6XV(GX8&)z*GE0UB(^8ix`VEgZ%&9(kOQS2zLp^~MTR21#s z{WmBuuiA@s-~bRpWQPGG`2?~aTmbvWR*2KA_2({11jR4$xPw7#DeG0_eIl1OhIWo1 z1Ivxhi-W&q9AGax2_p*s_AcQjdY81%Sw8PpmefB3F_`fXH)rOr$AbfOm@(cs>Oyc6 zk(Md#M4=C;0Zk;MG&+`(OHC+w^ZIoOhX_NAAl3ioTFqSnKVk1ey6^^Na=cHv^#AHK z5(?aXCftsbov9s$*q=50vto>*6``IWk947OIK+6&6Hvmy+||Ml~J+@DthFlE_pb~>2WMTEdV zRSFeAS^$SYR!TnA*1q6ByS*F+Spu;jEO8Ug6(Q+Y_Rq1Fdm0I4gf>;3cC))nzdL`# z5gETRtRD#e2$~rx#(`c5dKa_N;&TH^Ub}Dmgr`WA?-9aF*f*#W=Ql()Y?AMy7t#;Q zn1!hXw%@h*hrRe!9;7m^8DsSC2cHU&!3tc+r!`MBwveaLBCZa;qZg!;B&9%69r->F zPv_VFWEearbP!o;dK6UOj75pA!2E(_5^xXbR(+w)S#mN>_1*ZMZu@O#N8ZdU=gwXo z+Jn#;oKo)Mw9AphM`{FpK=pZ^S3~QMOQku)nF*`&6)dkuiI9WVCRi~J{H0t|#S$7Z zMxE+xoEHnSNGLA3Rf9C1mV&#)X?cOX)S3Z;5uK!`ye0b8G@y-R7=39aMBQjk#~{) zp?WXcEIWlFUt8gn0m>BCN1-YdEhNwFtW&08sdG-pfqp;MqOSGMmc3$HqZ24b4Db&5k#h}zbjlVbXYOx^nguUDVw6| zHSfWJ?4a=G0e#k8|B&p#DC{`-cVor*)`iV<>h0RA;B0c99zNcjq&IPlk`#4xQ?zxB zkY=tg^6JfqWK#TuCg+*XOBvRz+k_+n2&P~Yt#)Ss6Y(m0e{$SCm=?u5M>0hzKelj8 zRwM2sPTY2&YYf%?ys$gZ5+V_UePSG71UtOP7$tAaI|EY*4Wk|a>~6SxSJ%?ZTH&p4 zy_>YL8xdDv%nKT%5z=N+&Gq$xb!}5lPk17KUfv^~_M=~>dk*^Tx8se}o8~{ z`q2gW=BB`~$}opF_wIb~t^Kk~xRq{3o_(eUl-jjJX%z9py_ z5#fZ}l*Zyf`$0@o6=gr&e11Y`IhAutmt+<)d^)?Ny0)kscvaKdAuMG;5Qn? zcF8rv*+O1NquXd$5>L4>NC#BuuKD%cQA?*IkC2A=`?5Y=?X^WQ^+`dLQjN52SN`j+ zk=yg+Frcq*sj%_KeWLk0B7dqwE2NOpkjQhT>+9!G=R-ZL^CLu%foEG_zv-`5GTJ|} ztH1y+*v)HucdR1vHAXXpjwc}~Tte_70S2ABcLw?H~E`Y1V(3YK+? zKZ}q0cI^%=;cuIA+tKvVpL0Ru$k;_j=(?@gacYpz!TB7HvRw?mcCA_4tx*tMp^Iyt zG=y481%z9aeSoEAKzHo%%hcmoDvl@DKZeByU(eo|E7u8!<*DC7MT=9a$09(~3jYFw zleE3lt+ms*L&-%-)?{aMX$xD!LaIeJsE1e9HH<1NvR*bi^6I%Tgljlc_M;`8>-!b& z?s^hGpvt$u*}Mqj+_wz%&uI|P3XE`;Q_(o!jfmnu`s{pb?VARKNMppW2`DFWr^bY; z5Vgb}tL~|ct`=?}@qSgK_Vlmbuu;L98MX>xuk}0UfW;Rv8(r_io+#wleguj>d?3$> z7R7m?4I2~l(H%hv*XWs1Kc6~fNG-z_P zwoyE!^xO=vtqr` z_@KT|AYGBa~eJpNgIF4@|!c9ZkwfG70} z>P0Ra#n~{Bw#RTdsw~VW%N-V(^P9b_fQ=5u*>aucJ4?Mf9Y83rToQ(# zOy)oNe)7t%?}8LdxZrcVu1q~4g_vcR6hyaVLJE)MU5H5xP+~jK!M8meF57YR(;0v4 z{CRcU%DAqgF)R=l^XqUfZ#VASN%JevjqUCDxpVZ~+w&Te6W~mV_Czmh$pkBvf|=v% zN?$T>VH1uWUf01*Eo|a%AK{by$KN~ijPGr5|KX%i!2S3F2--T&5QpAmCcQai9*xg#(6=T|P*L2qj8rzI-{vwJDb!!_gQ zzFI#NUBbe}jL5QC&yEx8cQ@qOo>;bCji;w}-JXCcNO-u5ER2Xmq`j-0O`` z<2%RGCa_PncIvy&g1h6H|LDArt3uWMXKFpWlR`zPN8CBDMEuudgBzn3LzfJaw|udo znz2ykanZP>L-wRjC)wOYKiKrJF zr8<(kxd+laStDKFIA`}un~o;X;n7tpr+&(b^98i(RcuR>h0nnjPrR!IP2FB-+gd5< z6NyiS_QiloY3XP05xzc#E)>s6LTN5Tj9HtXV`psym$>K>Kv$`PiK4cF;Wx*JI;%;% z?;CU-p**_(s^%n7@OzPsceEsO^r0@+bf?O?nY1so)8JZ-O{=__l#scTs5 z!#Jh<#|S|`jF@A5izHb!oW@NE_B+r0`tdWNTQ6ANgV0PEMFeQ) zM->P<`fgRGaEO#x@w*|AGmV7Au}7wfY+fXgjk54d0i-HlLy)-_a&oupR)kW`7(QfItPonfB{QUd- z<;L&iSm1|FK@GLNA;FUr?mgh(Pzq`;!xR=nakBcdd$xKL*V?B_R_&|Vb2uG)DgLYi zCVd}`+v0AB&fkYqs~1NISkB)&>PmkT#km}8^}vFTTD`rGkQB9lW!92oJdiEBUbPe@ znx<~RzP=_F(fc(lKl+&YkP;JOt4_T)$89S3an>aUkMqxa$G^y1NXV5z2S!H7e!s4N zuJU5I$wE|__bP{-(3XC@Vh$D4O-=>CvRHF1FW>zYLxNMi1>-BFp|Qr-{@mW$w=b+? zeoK=Io$r6ij>`Wuu}X&%F?k@vRq#v>OIw1ey#9ofeIjmmp21{T&CCn_Nadn^q+s$) z>mcVXK{tI(cmGlB^!wf?Nku+OuB3@=vtKlkFch!alC<5NF@JV}S?4uX>1%A(^c|c^ z&9UIpZ%!(x$Q}e+JtA0iCz_sdaPx~~s;KpT`L45GO~0^wQH3<}P;QsV)tarMd;Fkz z2U8@mO^8J#uCWlDIo{>5(`13NUaODB-9ZVXm{#bJ#WL~U602bUoAEcEEIsAb(PTn_ zQ@g9PqVkdE=u!zfgjJm*XUWTm-eqVO8RWpbWg-2Ztyr>j2s}9vgt&EV>+Zjkn3A9? z1aghqUhx?_O_w=KFH7)mUmfd6OD&Z>`-J2BR%`$2vmfEHzqBtgz2Hv}zc6d$5*l&Rc(D^u;KRCQgj!m!xHgw%0Gxm5P*>iLa7IOsGU3k@Pp>vYj`&X3v$Y~f zD31j`e6O{=CbfNHyqB0`N>X4?VhQtJ*NzP|uGLx5vK#&=^klJe`B#ZshqFTq>2E`I z4>bMZfE~j2ZAEa{!iKljnG0Z*p~NQ>y?Z+Q0^m$;&2k+So?T= zqb)<8ND%OX@+GNv*Yo#6oH&g?)eSvqo#xEeuL`obUV(1wC!tI<<=^vXU8_J!0dnW* zzbO?XEvMj5lH8GBERxdJ(n}c$3S-%R!T>1n0#ZkvPURYFY~nn*ZOF}m-a>bX?wJ|- zj(4cSMB=i;Q{TyuC=ktf95RtCguE#s)*%@3PgwLTsmCM;*m+uW22Gddu!*4Ob(9;Y>RS;kLNx3|GG@9&K#-Mwh;(FCGZ+O zRQ$IL(j__Pwwfhd6w93)R>lE+k)KMUJ$AV)qSh!$lj}2gY*rze6y5kk&Eg)#0X!iR zA@J^lgkQ{&RAdZ=&FS(v)#4MZmNuhH{DfC3a#e>=i}tW*_)=^4_QL21H??;GOZb(B1>~MGpjBuTPMEd?rdzl5Rl)Nm>h&>xyGjq|J)* zzi#pEGwUQQV_BCTp7aNVEqu}>t@PQVVz6kyKTf}ASFjdBl5m`39PwmbhS?>y28bhv zRIAW>cPU31s|JvyCqACH6UsV#{8=QjT2O(dw9b=Ur-LESBt22awyv+N#@Q@|;#9Fx zel_5dqwjw3z*<%zPPG$Mv=rBEXbzN3 z=GKf7#cww%BYFif}nX>LJOk9VvYch@uFP25@w%_2Rj|;2a3CIhCOJ!XN}jLc)FL z-~R&i{9u_97z}J757FPjhVi5R!bRVJ{}*}(rpnMQz}xHAg`rXjg~zei|t9f2m{-&sQN@1%DQ~C{pIAs`z^$SFb8V+iBnbL%hog$ z%HoTNo|U`$)DjG(HK|+j+byaklwXK>p6Z9J6ESx)dd&UN7qAXM2-dR75FvGTAH?ON zC?PY5XCy2F6RVZ?USuEhTPp5zy9m|(+UG9CqwTrLg5(AR-@y{N{mZ?(O88_>SY6A9 zqd(v7ICd)@-u;m%!bJ5}zc*4~A;j+XdqX<0C45xc9^#hni>x+6o`9ms?e$OLZRIpO zCNFa}Kn0Q6g2Pmcl&&5hC)NoZ5eudMkBLcE@vPg(@c#?!8x!Qa7;2r7C0uny&DtEG z+gS)9%8&9mg%CoBA}!8N;7zi-YLl2hdm8x*vnQ~9U@W|P2Jq7|o<||$QOtQ9 zh=pGV=b>JELM%)p79^O^P}DTtSn6Tbn{%hIE+0<=s}%75)qrPwGS643V=!%h4;yi>9Lezk0UKLZ(jGK51A%v)qXdYN*(Y#WkS@!b~Li9X4%^9A@ zCKleN%J$wUk@Ry^0h<_$=F`}oU#Ue?;w{k>^+OA=I&c718uL(K2VhhQL~Iw}31Ig$ z(YzYVeGUgkZs+gaw~AvC6sFG>(DV&I$Acqq-4+8jDB`sy@e0&RlBd9Y$|w9nC+C$T zB$^U$A>zAO55#INFwg%D{`)hmSooNJe8vy4V7o&?KWoMcpMOU09QN<}dw*5MFUM60}c6Rf5dYx=H?*Z7}S)rmir9iP7 z2Hrrp9VjW1EL`Dvm7tsVl>a^iN?_J6M4=({JX|HpOfpnbNx*rY=x!A z-9}Hz(_Ggr-}xmnT#HQKw<&E7PUQI>4WZ*DvwZ#^{eIXbd_Fko1)j$!;G-hWV_D#PzzdpVMUBw_ z{K6NLCja~|>nEEhXGv5f(Cbc<`ePnsVAcl*M0MuDGy7*>!yuqx77!Nqnpf3_b6 z{tkSSCWia?JaCpTr1t@q1|9{D#$1!zVSAIy`*;2dcpSKl-ud8L_}tR~tOcA(Pg}%m zu+A$hyXAHdaC_Q0FX9XTO@R?uft36Gb3RX-yLNKR{sOE}%a8m!&jtR6<=KB-!fQSX z_%^ViTg}ZrmiM}ze3kb84cr59qyO$eUsQBr```2Y`W@(EJrH;exY*x+0B|30YX(DV zH@_xJ_;H>BE&^Wn@osRdervzR2LKyqnxumqwQ3wPYew#PZu#U#@n=apE&mlu5Ee%^mRh2D*xQ!du?v1ZF(WBYiI zVZld@z?N=VzXROqi-Z^aZ(avh3LWqJn2WQ^MQ^Hq=99qo{+@69cTL~pT-rTU3;Z1Z z>;EqB<4jDOx6bt6{lyK7XMFMT5$0;Wj=puy%4TsM?$_-lzh<}i7%#vYjf_Y;&cQzB z7Xk12>tX*+1F$MA1qj zqRZbqCG-+Ph>G(KpS)h6_+XKZ+`%AYJKj3}^Q-+B&Gb%@$t0XjF326g zpRui=M*lZB@0;jxa|XiM?E<}rpHm|+77Nyz=ay@!gmXPCs5yqbU${M^11 zx(>~He1#CzU^kzT?qT6Qt6~ZEo<_M9dwbf1-523}g@t=`4t8s4Iu`jF7S1l<6betZ z2uhva?dLo!^t*h)L79Yebu5T#stZe)LBO;A`_O%E`r(W zMYjO!`rnQ4a}63bv|{;?U-jdB*LBgkgM}MD%<`YFDmF zO}V~AaHFk_`NzX2I}do@S6*}aIdDM}>4ULI(R<4DFa|ec2gMaUALE0XOdEqaz*|0` z^EKBAQ8K3aGd7aq65TkY?dOR*gb2Ab)=Yoo_S+E=>W0!p7ly0Cq)z`9NU-Dg>j zHy#UES>MHImY;K{3uYpNMO7?;z`dVguI|S62eX}breLa$G>s^Qa0Y=$yS#iEa*O2WgYw)eN>Imh(zRf>wJ1=_JZ z><-{4EN3y1<^Eq2+s}laK%Vxri7$yohPIXH{I+Cax8@>PUf)*~?$HX8?AtR;)T?6K zWa$K!*pMRT(?_wr&Fxuu|8AuJe73)@8U*h1^l?aFUczBT9`8#m^<*V1VmXm^o%YCZ zKi!A9hIh{))amJkb3#T`UUx*;9KOrdeCZ66_IO`KO~y8ppY)aE@;O#Z4Om3oZP@)$k0fs1vc=te3F8 z(1)=Q<}q1TpNIJSU&Z!Mr}*k;YZk5yvJ4igJ=u>v#m8b<&AXxoxYM88k-*Ksk^W2# z_GfL9-~V@o?)eveGTXzwORd;8(B3Y2boTY%-AV7z;+1_c7GP0XfA<@FKDH6PewqlJ7Y5{_mn`7V&BRyXXD9&h(r4HQ@BLh0MnO-DmkC<8N*a&vLiu!ztVLeJ6fLce?6-IQMg$N01KCcT4gf_%wvH<4znTFj?u zR_Z?f-ZOn}n&!v++RyJs%q1LtuCMU(&DJA#TP(n3fj`r4xrqGBpa0oD>4)dbAGu{a z$A3Q^+cpC$@;=Y!pLEOe2LIiR(C5ta`F2E_(R7B7<7OGoc_aVqzfycRldb%?w}dn! z>DM{9u5Vy&UoaWM)fr#87*w9`{kDsUYNjtF znl$f<8X)1%(Vjl3UFZwpBd}rsO&PpaTl+%vLac=4`t;Pb-961awk(zuzkHhDecPY& z@EkGG-@CF4(ZPN$=g?CmApP7o!M3u7<@kOp`@C?GTQxiSu_n23GBSfGEamUp&tF01 zpC$bN*U0c$&F+>x#Xsv5pWm7@oYQ*#*^6a3&M5!gdK4eHG{*mSt2EJVzzQmy>gRT% z3v`n&2KG+77RzP0Zkfh*QJ)w0^aV&0JynM?wO&j4^*F)}o{O*muJ2-7a)}I!yS4oI z`v(SQAe>!T8Kjf_Am_T7y1>OVd|_90F?ouG_Y|5#7TV?+g!7*n#B4ElO}yhGg0F(+ zV^MR>8LYsmzG7HCbfcY__PK=;2xmuVB|F!T)#)qViFu9@^lfYKvHIM{p(%8;&vEN9 zU!|1q`$hj_hSgq!uTtmGyHuADwIZUa0_MdH z=^{TibYVZo+{;_~D)|PtsK2E5hItC}z2okK^)j62*FH8YVEZRR)C%1co-$F)d;3FI z6~0@XKC4>i)`WRH@5ACy+Fd^mbK~iD@;B<})3pr?XD7C~{x3hFrLgMo-*PcIh@RY; z5iS~^kU6CHWB>VxG$rrCe8`zj{! z=8F$Xg(ci|bCwJ22fhH95n9PKhra(jY~SJ>cO`wt7X;5>8`b~t1wqo?Rt@gT=*;43 z=m=ToLI_cBq_C~EBV9u}>1qFVA6DdGK{-A+-DUa}wpj}1Hhsp0WN$Zp|LMP55K{b1 z1cI@^MJ|!HO31c)AoMqdd zOvj>I=|AtLcb~ang|Hk8mosYgpSNRCXV6mzas?Jn=uKfE z!Cj%x2q&s0$}~=mq5odpt=%)Re4_C88STHz6tX_o-~X6ftIPVh?uCVAf8c@`eCNmX zde02YqEOJ-Q8 zwEHn%rl)mLHx|j;j=6TTMd~%`xe-E$>X0vw6D*)*9#&O9=tOV~J^7a#`2@c%7R=Lu zxiM2A!92+=s4uY8j^$lIx4{Y_%*DdP!vb_VJvpT7_-Ab5R#>~ALs_@bA5+ zyA=jvVcQ2{`=Za%6QSIQg-Ew!X(wB`1v?B2i25;>tJI>~R|p}hBb?{BaP6DrJx!Q9 zaZczxtyn(NO#j>2p?@z(vnmsq8*(-l@ifK%Zk7x3bl{g6EV99Vtl4RwG1EUg-KuND z>gvv*Ct3A%@^>EC*55ym-svd8e?Nr<^Q6+o?T3Eu^XaMd|FMhjOKG3Istan6NE=S_ zeu3TvBk%>vX_(JG`0nA?!>FOE5JFU(U_WqKESK_CvUe$13UiTuin$iM=}C#$1gj4? z%`MO%G&sxztnAbC{s~SmbHRR&xp7}6b9t=>|1L|pvxeNH4K)Dv%KMZUi(?d|Sc)Ywv7B>pbf){yV2hnx-$wS~p8u zSDDT+DRg3OZ2MuTQ@bJ-rl?;w(FrOOe8IIK_`z2;L{UN4IZ##vMPXwToVrSex@LA8 zUDH}yI_q3kH)`wNT)HGDIexgVoAcy3&y$?AN!GOg3l440^LF3QIp@Bf|J(I{E!W=; zk9Bk!b4;Spd?ti1_n8Hx`*J%_My3o*R;ylRo z+e}W0w)njT0)css3Q{+F6DpfaN@Ig4)beH&x;dJ&Cb!ty>Og8;59iMN4llZoFn_}$ zRnKo95_8Fin?`9?X1fsZDk}H;B}(etMQi;xgE#|$z(s*YsC4T%6D2-JQUA%SOpICL zOk~-QT7_+<5fe*fyxGM1FwODAHgBslc^oadi0e!&NoyshS@HfA6w^ExnYu$Jl2*n^ zZmc5(W2#Pb?%CsNG&i0u+gA24kj6IIec6L58$J!(O{2GF5N99|xahIT}YgAx|+L5X);O~5aEd%KURnSARRlKwD~BE6hO*8NXS%vr1E8ssYdJ?ew` zDO<5spJOr$XqorliljA)5^aAsD$DzYiSTArTmL+g79B%niSI#)l*^Hu`G863 zK#o8=P$k8)z|W9dd>bmMJB);54*GuHgj~=oPz$%zf4N@3S6h%+%v=5UlSs03Pr<{@ z8KnAl3yS6T#ZM&=2n0kV-?3Hnfr9OCq0vk_8|S>y3+sDPS=kVc<_e&sn&**Lc8ZZL z0elU$bAK4QyqH;ulbfALt>P#)v#rXlnBBGxZWaemx{ z@)IT(yxHmV?lRf^(+?ewn%q{Rc<8gX>hGoD!an7DpPl<+KZHDi`dOyq_WL?dnV9eP zy!vSLGX6Iz-^-#Mso1^Z>pkdu^qfDtuap>q+kMX4I6phZ1OkD;oTCGka}C)7%%jsp zHDnTnhUm*_lh-%HLK??tzz62@4_4xfZ zl!SF1DpCBK_sJ9Is7Y>ZX71nim<|JfMlt9P&+%4oyOW98z8~{_`~xb{YxnDNq;B^L z$~eyBjh<5<(Z4tl2n4{^&$%KBmk2 zyhL+j^6B8C8-p}IahlDzTd<)=7srPUR;Nn7~jNJ&T1rRay)m98t@wvbFWD>o3Qeb zEz(wyuG2@e7|k&1R`tHKxKI5w;+F|^CN`0%Q{4YJq21Q`JCVpt(#u|N!dgpnQ;L5f zH+2@Lkl;%Nbu!6f$PYo6(r~S=H2EI%y++Fk{uny83+XC zGm`q*WQjI=k=u2%iFT7MrPS4E%>Tc^#BdA=AN<#rDX5KL)s{<97U!TB>9k*VpOdt% zUUS(L*Ezx2DdGGbxu+*PcAQ3&jXb)6AJRx_8Zse`+DfdHSadIzd^X7f&d)Jr>b7Ty zbCtR3?G(3btk^1DN^u->JYyY)#Tf_$E_#e0w_n+!2@)_kj{56&SWfCy!aMfXF+C>E?T%-#dzj?Mc1j$^8FkuA*RovddS@<(I!jap9j9l>?hwm zj$*}S_cO&*^J8CTi+{#?HwSSB0)hFAF_Zzi&V+ZSgk1Nc*yOb)#{;&Q5{@91prnbN z>NiI-mfje0@h+gb^x;asKghY&TP92M3YvQzS2EY1OSGddRw;4lKRF^_JRnP-ev?RiF=Ksn$uKI1*n(;rv+>PD zXh%@2?QOa9cc4TdP=@chh>D-^bvB^d)GWGCdBt3pRU|j;c)>%#>nSb+nn|3rN8oA{ z->sC4vFL$foxdB81#t!ffk1;4AEcr5Q${NNwpy<90-F^zz>8`>>U+Bp#Wx3X#QJ@& znRj@fMc)6bCXfB7w)0#t4EzYi?C$Wxz#E*d-;1GY|;Ocl6m(+eUe<-qWbeqmE*t zkCu?@u=ja{i4tvfl$Clra+f}Y5^jzHJ5U|t%_zGwd(7Ar3wGLG&m8k}lja7L#XI2h zzaN!;j7)<(TNwixYqDN4#_s|D0KVio57~M=zgSYy1H z7GW>a@jW&L+5XPs_-o1bBS>!VqsY}ggi1;7L!~K4a`%12b4!`G`Ojj9=R8_q-F_Xp zrAKnaelJp9+J;I`mZ5~8V}Ac|?pzy?tM<0s15F?h2n0~A-*MCpqkRspm&zNeb99~a z3DxspOev;WHjZ|G7Y1pr`wRpEfq)m%TWo=BjN5V1W23(dUt|`Nss0ari(Uyk?#i`) that contains the rotation, `(3x3)` rotation matrices or `(3x1)` rotation vectors, +for all the transformations from world frame to the camera frame. +@param[in] t_world2cam Translation part extracted from the homogeneous matrix that transforms a point +expressed in the world frame to the camera frame (\f$_{}^{c}\textrm{T}_w\f$). +This is a vector (`vector`) that contains the `(3x1)` translation vectors for all the transformations +from world frame to the camera frame. +@param[in] R_base2gripper Rotation part extracted from the homogeneous matrix that transforms a point +expressed in the robot base frame to the gripper frame (\f$_{}^{g}\textrm{T}_b\f$). +This is a vector (`vector`) that contains the rotation, `(3x3)` rotation matrices or `(3x1)` rotation vectors, +for all the transformations from robot base frame to the gripper frame. +@param[in] t_base2gripper Rotation part extracted from the homogeneous matrix that transforms a point +expressed in the robot base frame to the gripper frame (\f$_{}^{g}\textrm{T}_b\f$). +This is a vector (`vector`) that contains the `(3x1)` translation vectors for all the transformations +from robot base frame to the gripper frame. +@param[out] R_base2world Estimated `(3x3)` rotation part extracted from the homogeneous matrix that transforms a point +expressed in the robot base frame to the world frame (\f$_{}^{w}\textrm{T}_b\f$). +@param[out] t_base2world Estimated `(3x1)` translation part extracted from the homogeneous matrix that transforms a point +expressed in the robot base frame to the world frame (\f$_{}^{w}\textrm{T}_b\f$). +@param[out] R_gripper2cam Estimated `(3x3)` rotation part extracted from the homogeneous matrix that transforms a point +expressed in the gripper frame to the camera frame (\f$_{}^{c}\textrm{T}_g\f$). +@param[out] t_gripper2cam Estimated `(3x1)` translation part extracted from the homogeneous matrix that transforms a point +expressed in the gripper frame to the camera frame (\f$_{}^{c}\textrm{T}_g\f$). +@param[in] method One of the implemented Robot-World/Hand-Eye calibration method, see cv::RobotWorldHandEyeCalibrationMethod + +The function performs the Robot-World/Hand-Eye calibration using various methods. One approach consists in estimating the +rotation then the translation (separable solutions): + - M. Shah, Solving the robot-world/hand-eye calibration problem using the kronecker product \cite Shah2013SolvingTR + +Another approach consists in estimating simultaneously the rotation and the translation (simultaneous solutions), +with the following implemented method: + - A. Li, L. Wang, and D. Wu, Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product \cite Li2010SimultaneousRA + +The following picture describes the Robot-World/Hand-Eye calibration problem where the transformations between a robot and a world frame +and between a robot gripper ("hand") and a camera ("eye") mounted at the robot end-effector have to be estimated. + +![](pics/robot-world_hand-eye_figure.png) + +The calibration procedure is the following: + - a static calibration pattern is used to estimate the transformation between the target frame + and the camera frame + - the robot gripper is moved in order to acquire several poses + - for each pose, the homogeneous transformation between the gripper frame and the robot base frame is recorded using for + instance the robot kinematics +\f[ + \begin{bmatrix} + X_g\\ + Y_g\\ + Z_g\\ + 1 + \end{bmatrix} + = + \begin{bmatrix} + _{}^{g}\textrm{R}_b & _{}^{g}\textrm{t}_b \\ + 0_{1 \times 3} & 1 + \end{bmatrix} + \begin{bmatrix} + X_b\\ + Y_b\\ + Z_b\\ + 1 + \end{bmatrix} +\f] + - for each pose, the homogeneous transformation between the calibration target frame (the world frame) and the camera frame is recorded using + for instance a pose estimation method (PnP) from 2D-3D point correspondences +\f[ + \begin{bmatrix} + X_c\\ + Y_c\\ + Z_c\\ + 1 + \end{bmatrix} + = + \begin{bmatrix} + _{}^{c}\textrm{R}_w & _{}^{c}\textrm{t}_w \\ + 0_{1 \times 3} & 1 + \end{bmatrix} + \begin{bmatrix} + X_w\\ + Y_w\\ + Z_w\\ + 1 + \end{bmatrix} +\f] + +The Robot-World/Hand-Eye calibration procedure returns the following homogeneous transformations +\f[ + \begin{bmatrix} + X_w\\ + Y_w\\ + Z_w\\ + 1 + \end{bmatrix} + = + \begin{bmatrix} + _{}^{w}\textrm{R}_b & _{}^{w}\textrm{t}_b \\ + 0_{1 \times 3} & 1 + \end{bmatrix} + \begin{bmatrix} + X_b\\ + Y_b\\ + Z_b\\ + 1 + \end{bmatrix} +\f] +\f[ + \begin{bmatrix} + X_c\\ + Y_c\\ + Z_c\\ + 1 + \end{bmatrix} + = + \begin{bmatrix} + _{}^{c}\textrm{R}_g & _{}^{c}\textrm{t}_g \\ + 0_{1 \times 3} & 1 + \end{bmatrix} + \begin{bmatrix} + X_g\\ + Y_g\\ + Z_g\\ + 1 + \end{bmatrix} +\f] + +This problem is also known as solving the \f$\mathbf{A}\mathbf{X}=\mathbf{Z}\mathbf{B}\f$ equation, with: + - \f$\mathbf{A} \Leftrightarrow \hspace{0.1em} _{}^{c}\textrm{T}_w\f$ + - \f$\mathbf{X} \Leftrightarrow \hspace{0.1em} _{}^{w}\textrm{T}_b\f$ + - \f$\mathbf{Z} \Leftrightarrow \hspace{0.1em} _{}^{c}\textrm{T}_g\f$ + - \f$\mathbf{B} \Leftrightarrow \hspace{0.1em} _{}^{g}\textrm{T}_b\f$ + +\note +At least 3 measurements are required (input vectors size must be greater or equal to 3). + + */ +CV_EXPORTS_W void calibrateRobotWorldHandEye( InputArrayOfArrays R_world2cam, InputArrayOfArrays t_world2cam, + InputArrayOfArrays R_base2gripper, InputArrayOfArrays t_base2gripper, + OutputArray R_base2world, OutputArray t_base2world, + OutputArray R_gripper2cam, OutputArray t_gripper2cam, + RobotWorldHandEyeCalibrationMethod method=CALIB_ROBOT_WORLD_HAND_EYE_SHAH ); + /** @brief Converts points from Euclidean to homogeneous space. @param src Input vector of N-dimensional points. diff --git a/modules/calib3d/src/calibration_handeye.cpp b/modules/calib3d/src/calibration_handeye.cpp index 79c8b58a23..077de110f6 100644 --- a/modules/calib3d/src/calibration_handeye.cpp +++ b/modules/calib3d/src/calibration_handeye.cpp @@ -514,10 +514,32 @@ static void calibrateHandEyeHoraud(const std::vector& Hg, const std::vector t_cam2gripper = t; } -// sign function, return -1 if negative values, +1 otherwise -static int sign_double(double val) +static Mat_ normalizeRotation(const Mat_& R_) { - return (0 < val) - (val < 0); + // Make R unit determinant + Mat_ R = R_.clone(); + double det = determinant(R); + if (std::fabs(det) < FLT_EPSILON) + { + CV_Error(Error::StsNoConv, "Rotation normalization issue: determinant(R) is null"); + } + R = std::cbrt(std::copysign(1, det) / std::fabs(det)) * R; + + // Make R orthogonal + Mat w, u, vt; + SVDecomp(R, w, u, vt); + R = u*vt; + + // Handle reflection case + if (determinant(R) < 0) + { + Matx33d diag(1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0); + R = u*diag*vt; + } + + return R; } //Reference: @@ -573,29 +595,8 @@ static void calibrateHandEyeAndreff(const std::vector& Hg, const std::vecto int newSize[] = {3, 3}; R = R.reshape(1, 2, newSize); //Eq 15 - double det = determinant(R); - if (std::fabs(det) < FLT_EPSILON) - { - CV_Error(Error::StsNoConv, "calibrateHandEye() with CALIB_HAND_EYE_ANDREFF method: determinant(R) is null"); - } - R = cubeRoot(static_cast(sign_double(det) / abs(det))) * R; - - Mat w, u, vt; - SVDecomp(R, w, u, vt); - R = u*vt; - - if (determinant(R) < 0) - { - Mat diag = (Mat_(3,3) << 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, -1.0); - R = u*diag*vt; - } - - R_cam2gripper = R; - - Mat t = X(Rect(0, 9, 1, 3)); - t_cam2gripper = t; + R_cam2gripper = normalizeRotation(R); + t_cam2gripper = X(Rect(0, 9, 1, 3)); } //Reference: @@ -708,7 +709,7 @@ void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gr CV_Assert(R_gripper2base_.size() == t_gripper2base_.size() && R_target2cam_.size() == t_target2cam_.size() && R_gripper2base_.size() == R_target2cam_.size()); - CV_Assert(R_gripper2base_.size() >= 3); + CV_Check(R_gripper2base_.size(), R_gripper2base_.size() >= 3, "At least 3 measurements are needed"); //Notation used in Tsai paper //Defines coordinate transformation from G (gripper) to RW (robot base) @@ -779,4 +780,195 @@ void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gr Rcg.copyTo(R_cam2gripper); Tcg.copyTo(t_cam2gripper); } + +//Reference: +//M. Shah, "Solving the robot-world/hand-eye calibration problem using the kronecker product" +//Journal of Mechanisms and Robotics, vol. 5, p. 031007, 2013. +//Matlab code: http://math.loyola.edu/~mili/Calibration/ +static void calibrateRobotWorldHandEyeShah(const std::vector>& cRw, const std::vector>& ctw, + const std::vector>& gRb, const std::vector>& gtb, + Matx33d& wRb, Matx31d& wtb, Matx33d& cRg, Matx31d& ctg) +{ + Mat_ T = Mat_::zeros(9, 9); + for (size_t i = 0; i < cRw.size(); i++) + { + T += kron(gRb[i], cRw[i]); + } + + Mat_ w, u, vt; + SVDecomp(T, w, u, vt); + + Mat_ RX(3,3), RZ(3,3); + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + RX(j,i) = vt(0, i*3+j); + RZ(j,i) = u(i*3+j, 0); + } + } + + wRb = normalizeRotation(RX); + cRg = normalizeRotation(RZ); + Mat_ Z = Mat(cRg.t()).reshape(1, 9); + + const int n = static_cast(cRw.size()); + Mat_ A = Mat_::zeros(3*n, 6); + Mat_ b = Mat_::zeros(3*n, 1); + Mat_ I3 = Mat_::eye(3,3); + + for (int i = 0; i < n; i++) + { + Mat cRw_ = -cRw[i]; + cRw_.copyTo(A(Range(i*3, (i+1)*3), Range(0,3))); + I3.copyTo(A(Range(i*3, (i+1)*3), Range(3,6))); + + Mat ctw_ = ctw[i] - kron(gtb[i].t(), I3) * Z; + ctw_.copyTo(b(Range(i*3, (i+1)*3), Range::all())); + } + + Mat_ t; + solve(A, b, t, DECOMP_SVD); + + for (int i = 0; i < 3; i++) + { + wtb(i) = t(i); + ctg(i) = t(i+3); + } +} + +//Reference: +//A. Li, L. Wang, and D. Wu, "Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product" +//International Journal of Physical Sciences, vol. 5, pp. 1530–1536, 2010. +//Matlab code: http://math.loyola.edu/~mili/Calibration/ +static void calibrateRobotWorldHandEyeLi(const std::vector>& cRw, const std::vector>& ctw, + const std::vector>& gRb, const std::vector>& gtb, + Matx33d& wRb, Matx31d& wtb, Matx33d& cRg, Matx31d& ctg) +{ + const int n = static_cast(cRw.size()); + Mat_ A = Mat_::zeros(12*n, 24); + Mat_ b = Mat_::zeros(12*n, 1); + Mat_ I3 = Mat_::eye(3,3); + + for (int i = 0; i < n; i++) + { + //Eq 19 + kron(cRw[i], I3).copyTo(A(Range(i*12, i*12 + 9), Range(0, 9))); + kron(-I3, gRb[i].t()).copyTo(A(Range(i*12, i*12 + 9), Range(9, 18))); + + kron(I3, gtb[i].t()).copyTo(A(Range(i*12 + 9, (i+1)*12), Range(9, 18))); + Mat cRw_ = -cRw[i]; + cRw_.copyTo(A(Range(i*12 + 9, (i+1)*12), Range(18, 21))); + I3.copyTo(A(Range(i*12 + 9, (i+1)*12), Range(21, 24))); + + ctw[i].copyTo(b(Range(i*12 + 9, i*12+12), Range::all())); + } + + Mat_ x; + solve(A, b, x, DECOMP_SVD); + + Mat_ RX = x(Range(0,9), Range::all()).reshape(3, 3); + wRb = normalizeRotation(RX); + x(Range(18,21), Range::all()).copyTo(wtb); + + Mat_ RZ = x(Range(9,18), Range::all()).reshape(3, 3); + cRg = normalizeRotation(RZ); + x(Range(21,24), Range::all()).copyTo(ctg); +} + +void calibrateRobotWorldHandEye(InputArrayOfArrays R_world2cam, InputArrayOfArrays t_world2cam, + InputArrayOfArrays R_base2gripper, InputArrayOfArrays t_base2gripper, + OutputArray R_base2world, OutputArray t_base2world, + OutputArray R_gripper2cam, OutputArray t_gripper2cam, + RobotWorldHandEyeCalibrationMethod method) +{ + CV_Assert(R_base2gripper.isMatVector() && t_base2gripper.isMatVector() && + R_world2cam.isMatVector() && t_world2cam.isMatVector()); + + std::vector R_base2gripper_tmp, t_base2gripper_tmp; + R_base2gripper.getMatVector(R_base2gripper_tmp); + t_base2gripper.getMatVector(t_base2gripper_tmp); + + std::vector R_world2cam_tmp, t_world2cam_tmp; + R_world2cam.getMatVector(R_world2cam_tmp); + t_world2cam.getMatVector(t_world2cam_tmp); + + CV_Assert(R_base2gripper_tmp.size() == t_base2gripper_tmp.size() && + R_world2cam_tmp.size() == t_world2cam_tmp.size() && + R_base2gripper_tmp.size() == R_world2cam_tmp.size()); + CV_Check(R_base2gripper_tmp.size(), R_base2gripper_tmp.size() >= 3, "At least 3 measurements are needed"); + + // Convert to double + std::vector> R_base2gripper_, t_base2gripper_; + std::vector> R_world2cam_, t_world2cam_; + + R_base2gripper_.reserve(R_base2gripper_tmp.size()); + t_base2gripper_.reserve(R_base2gripper_tmp.size()); + R_world2cam_.reserve(R_world2cam_tmp.size()); + t_world2cam_.reserve(R_base2gripper_tmp.size()); + + // Convert to rotation matrix if needed + for (size_t i = 0; i < R_base2gripper_tmp.size(); i++) + { + { + Mat rot = R_base2gripper_tmp[i]; + Mat R(3, 3, CV_64FC1); + if (rot.size() == Size(3,3)) + { + rot.convertTo(R, CV_64F); + R_base2gripper_.push_back(R); + } + else + { + Rodrigues(rot, R); + R_base2gripper_.push_back(R); + } + Mat tvec = t_base2gripper_tmp[i]; + Mat t; + tvec.convertTo(t, CV_64F); + t_base2gripper_.push_back(t); + } + { + Mat rot = R_world2cam_tmp[i]; + Mat R(3, 3, CV_64FC1); + if (rot.size() == Size(3,3)) + { + rot.convertTo(R, CV_64F); + R_world2cam_.push_back(R); + } + else + { + Rodrigues(rot, R); + R_world2cam_.push_back(R); + } + Mat tvec = t_world2cam_tmp[i]; + Mat t; + tvec.convertTo(t, CV_64F); + t_world2cam_.push_back(t); + } + } + + CV_Assert(R_world2cam_.size() == t_world2cam_.size() && + R_base2gripper_.size() == t_base2gripper_.size() && + R_world2cam_.size() == R_base2gripper_.size()); + + Matx33d wRb, cRg; + Matx31d wtb, ctg; + switch (method) + { + case CALIB_ROBOT_WORLD_HAND_EYE_SHAH: + calibrateRobotWorldHandEyeShah(R_world2cam_, t_world2cam_, R_base2gripper_, t_base2gripper_, wRb, wtb, cRg, ctg); + break; + + case CALIB_ROBOT_WORLD_HAND_EYE_LI: + calibrateRobotWorldHandEyeLi(R_world2cam_, t_world2cam_, R_base2gripper_, t_base2gripper_, wRb, wtb, cRg, ctg); + break; + } + + Mat(wRb).copyTo(R_base2world); + Mat(wtb).copyTo(t_base2world); + + Mat(cRg).copyTo(R_gripper2cam); + Mat(ctg).copyTo(t_gripper2cam); +} } diff --git a/modules/calib3d/test/test_calibration_hand_eye.cpp b/modules/calib3d/test/test_calibration_hand_eye.cpp index 919c6ad28e..50bcbd7aff 100644 --- a/modules/calib3d/test/test_calibration_hand_eye.cpp +++ b/modules/calib3d/test/test_calibration_hand_eye.cpp @@ -7,241 +7,12 @@ namespace opencv_test { namespace { -static std::string getMethodName(HandEyeCalibrationMethod method) -{ - std::string method_name = ""; - switch (method) - { - case CALIB_HAND_EYE_TSAI: - method_name = "Tsai"; - break; - - case CALIB_HAND_EYE_PARK: - method_name = "Park"; - break; - - case CALIB_HAND_EYE_HORAUD: - method_name = "Horaud"; - break; - - case CALIB_HAND_EYE_ANDREFF: - method_name = "Andreff"; - break; - - case CALIB_HAND_EYE_DANIILIDIS: - method_name = "Daniilidis"; - break; - - default: - break; - } - - return method_name; -} - -class CV_CalibrateHandEyeTest : public cvtest::BaseTest -{ -public: - CV_CalibrateHandEyeTest() { - eps_rvec[CALIB_HAND_EYE_TSAI] = 1.0e-8; - eps_rvec[CALIB_HAND_EYE_PARK] = 1.0e-8; - eps_rvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8; - eps_rvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8; - eps_rvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8; - - eps_tvec[CALIB_HAND_EYE_TSAI] = 1.0e-8; - eps_tvec[CALIB_HAND_EYE_PARK] = 1.0e-8; - eps_tvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8; - eps_tvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8; - eps_tvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8; - - eps_rvec_noise[CALIB_HAND_EYE_TSAI] = 2.0e-2; - eps_rvec_noise[CALIB_HAND_EYE_PARK] = 2.0e-2; - eps_rvec_noise[CALIB_HAND_EYE_HORAUD] = 2.0e-2; - eps_rvec_noise[CALIB_HAND_EYE_ANDREFF] = 1.0e-2; - eps_rvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-2; - - eps_tvec_noise[CALIB_HAND_EYE_TSAI] = 5.0e-2; - eps_tvec_noise[CALIB_HAND_EYE_PARK] = 5.0e-2; - eps_tvec_noise[CALIB_HAND_EYE_HORAUD] = 5.0e-2; - eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 5.0e-2; - eps_tvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 5.0e-2; - } -protected: - virtual void run(int); - void generatePose(RNG& rng, double min_theta, double max_theta, - double min_tx, double max_tx, - double min_ty, double max_ty, - double min_tz, double max_tz, - Mat& R, Mat& tvec, - bool randSign=false); - void simulateData(RNG& rng, int nPoses, - std::vector &R_gripper2base, std::vector &t_gripper2base, - std::vector &R_target2cam, std::vector &t_target2cam, - bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper); - Mat homogeneousInverse(const Mat& T); - double sign_double(double val); - - double eps_rvec[5]; - double eps_tvec[5]; - double eps_rvec_noise[5]; - double eps_tvec_noise[5]; -}; - -void CV_CalibrateHandEyeTest::run(int) -{ - ts->set_failed_test_info(cvtest::TS::OK); - - RNG& rng = ts->get_rng(); - - std::vector > vec_rvec_diff(5); - std::vector > vec_tvec_diff(5); - std::vector > vec_rvec_diff_noise(5); - std::vector > vec_tvec_diff_noise(5); - - std::vector methods; - methods.push_back(CALIB_HAND_EYE_TSAI); - methods.push_back(CALIB_HAND_EYE_PARK); - methods.push_back(CALIB_HAND_EYE_HORAUD); - methods.push_back(CALIB_HAND_EYE_ANDREFF); - methods.push_back(CALIB_HAND_EYE_DANIILIDIS); - - const int nTests = 100; - for (int i = 0; i < nTests; i++) - { - const int nPoses = 10; - { - //No noise - std::vector R_gripper2base, t_gripper2base; - std::vector R_target2cam, t_target2cam; - Mat R_cam2gripper_true, t_cam2gripper_true; - - const bool noise = false; - simulateData(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise, R_cam2gripper_true, t_cam2gripper_true); - - for (size_t idx = 0; idx < methods.size(); idx++) - { - Mat rvec_cam2gripper_true; - cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true); - - Mat R_cam2gripper_est, t_cam2gripper_est; - calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]); - - Mat rvec_cam2gripper_est; - cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est); - - double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2); - double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2); - - vec_rvec_diff[idx].push_back(rvecDiff); - vec_tvec_diff[idx].push_back(tvecDiff); - - const double epsilon_rvec = eps_rvec[idx]; - const double epsilon_tvec = eps_tvec[idx]; - - //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds? - if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec) - { - ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n", - getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec); - ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); - } - } - } - - { - //Gaussian noise on transformations between calibration target frame and camera frame and between gripper and robot base frames - std::vector R_gripper2base, t_gripper2base; - std::vector R_target2cam, t_target2cam; - Mat R_cam2gripper_true, t_cam2gripper_true; - - const bool noise = true; - simulateData(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise, R_cam2gripper_true, t_cam2gripper_true); - - for (size_t idx = 0; idx < methods.size(); idx++) - { - Mat rvec_cam2gripper_true; - cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true); - - Mat R_cam2gripper_est, t_cam2gripper_est; - calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]); - - Mat rvec_cam2gripper_est; - cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est); - - double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2); - double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2); - - vec_rvec_diff_noise[idx].push_back(rvecDiff); - vec_tvec_diff_noise[idx].push_back(tvecDiff); - - const double epsilon_rvec = eps_rvec_noise[idx]; - const double epsilon_tvec = eps_tvec_noise[idx]; - - //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds? - if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec) - { - ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n", - getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec); - ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); - } - } - } - } - - for (size_t idx = 0; idx < methods.size(); idx++) - { - { - double max_rvec_diff = *std::max_element(vec_rvec_diff[idx].begin(), vec_rvec_diff[idx].end()); - double mean_rvec_diff = std::accumulate(vec_rvec_diff[idx].begin(), - vec_rvec_diff[idx].end(), 0.0) / vec_rvec_diff[idx].size(); - double sq_sum_rvec_diff = std::inner_product(vec_rvec_diff[idx].begin(), vec_rvec_diff[idx].end(), - vec_rvec_diff[idx].begin(), 0.0); - double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / vec_rvec_diff[idx].size() - mean_rvec_diff * mean_rvec_diff); - - double max_tvec_diff = *std::max_element(vec_tvec_diff[idx].begin(), vec_tvec_diff[idx].end()); - double mean_tvec_diff = std::accumulate(vec_tvec_diff[idx].begin(), - vec_tvec_diff[idx].end(), 0.0) / vec_tvec_diff[idx].size(); - double sq_sum_tvec_diff = std::inner_product(vec_tvec_diff[idx].begin(), vec_tvec_diff[idx].end(), - vec_tvec_diff[idx].begin(), 0.0); - double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / vec_tvec_diff[idx].size() - mean_tvec_diff * mean_tvec_diff); - - std::cout << "\nMethod " << getMethodName(methods[idx]) << ":\n" - << "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff - << ", Std rvec error: " << std_rvec_diff << "\n" - << "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff - << ", Std tvec error: " << std_tvec_diff << std::endl; - } - { - double max_rvec_diff = *std::max_element(vec_rvec_diff_noise[idx].begin(), vec_rvec_diff_noise[idx].end()); - double mean_rvec_diff = std::accumulate(vec_rvec_diff_noise[idx].begin(), - vec_rvec_diff_noise[idx].end(), 0.0) / vec_rvec_diff_noise[idx].size(); - double sq_sum_rvec_diff = std::inner_product(vec_rvec_diff_noise[idx].begin(), vec_rvec_diff_noise[idx].end(), - vec_rvec_diff_noise[idx].begin(), 0.0); - double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / vec_rvec_diff_noise[idx].size() - mean_rvec_diff * mean_rvec_diff); - - double max_tvec_diff = *std::max_element(vec_tvec_diff_noise[idx].begin(), vec_tvec_diff_noise[idx].end()); - double mean_tvec_diff = std::accumulate(vec_tvec_diff_noise[idx].begin(), - vec_tvec_diff_noise[idx].end(), 0.0) / vec_tvec_diff_noise[idx].size(); - double sq_sum_tvec_diff = std::inner_product(vec_tvec_diff_noise[idx].begin(), vec_tvec_diff_noise[idx].end(), - vec_tvec_diff_noise[idx].begin(), 0.0); - double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / vec_tvec_diff_noise[idx].size() - mean_tvec_diff * mean_tvec_diff); - - std::cout << "Method (noise) " << getMethodName(methods[idx]) << ":\n" - << "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff - << ", Std rvec error: " << std_rvec_diff << "\n" - << "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff - << ", Std tvec error: " << std_tvec_diff << std::endl; - } - } -} - -void CV_CalibrateHandEyeTest::generatePose(RNG& rng, double min_theta, double max_theta, - double min_tx, double max_tx, - double min_ty, double max_ty, - double min_tz, double max_tz, - Mat& R, Mat& tvec, - bool random_sign) +static void generatePose(RNG& rng, double min_theta, double max_theta, + double min_tx, double max_tx, + double min_ty, double max_ty, + double min_tz, double max_tz, + Mat& R, Mat& tvec, + bool random_sign) { Mat axis(3, 1, CV_64FC1); for (int i = 0; i < 3; i++) @@ -251,7 +22,7 @@ void CV_CalibrateHandEyeTest::generatePose(RNG& rng, double min_theta, double ma double theta = rng.uniform(min_theta, max_theta); if (random_sign) { - theta *= sign_double(rng.uniform(-1.0, 1.0)); + theta *= std::copysign(1.0, rng.uniform(-1.0, 1.0)); } Mat rvec(3, 1, CV_64FC1); @@ -266,18 +37,33 @@ void CV_CalibrateHandEyeTest::generatePose(RNG& rng, double min_theta, double ma if (random_sign) { - tvec.at(0,0) *= sign_double(rng.uniform(-1.0, 1.0)); - tvec.at(1,0) *= sign_double(rng.uniform(-1.0, 1.0)); - tvec.at(2,0) *= sign_double(rng.uniform(-1.0, 1.0)); + tvec.at(0,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0)); + tvec.at(1,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0)); + tvec.at(2,0) *= std::copysign(1.0, rng.uniform(-1.0, 1.0)); } cv::Rodrigues(rvec, R); } -void CV_CalibrateHandEyeTest::simulateData(RNG& rng, int nPoses, - std::vector &R_gripper2base, std::vector &t_gripper2base, - std::vector &R_target2cam, std::vector &t_target2cam, - bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper) +static Mat homogeneousInverse(const Mat& T) +{ + CV_Assert( T.rows == 4 && T.cols == 4 ); + + Mat R = T(Rect(0, 0, 3, 3)); + Mat t = T(Rect(3, 0, 1, 3)); + Mat Rt = R.t(); + Mat tinv = -Rt * t; + Mat Tinv = Mat::eye(4, 4, T.type()); + Rt.copyTo(Tinv(Rect(0, 0, 3, 3))); + tinv.copyTo(Tinv(Rect(3, 0, 1, 3))); + + return Tinv; +} + +static void simulateDataEyeInHand(RNG& rng, int nPoses, + std::vector &R_gripper2base, std::vector &t_gripper2base, + std::vector &R_target2cam, std::vector &t_target2cam, + bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper) { //to avoid generating values close to zero, //we use positive range values and randomize the sign @@ -348,7 +134,7 @@ void CV_CalibrateHandEyeTest::simulateData(RNG& rng, int nPoses, t_gripper2base_noise.at(2,0) += rng.gaussian(0.001); } - // test rvec represenation + //Test rvec representation Mat rvec_target2cam; cv::Rodrigues(T_target2cam(Rect(0, 0, 3, 3)), rvec_target2cam); R_target2cam.push_back(rvec_target2cam); @@ -356,40 +142,173 @@ void CV_CalibrateHandEyeTest::simulateData(RNG& rng, int nPoses, } } -Mat CV_CalibrateHandEyeTest::homogeneousInverse(const Mat& T) +static void simulateDataEyeToHand(RNG& rng, int nPoses, + std::vector &R_base2gripper, std::vector &t_base2gripper, + std::vector &R_target2cam, std::vector &t_target2cam, + bool noise, Mat& R_cam2base, Mat& t_cam2base) { - CV_Assert( T.rows == 4 && T.cols == 4 ); + //to avoid generating values close to zero, + //we use positive range values and randomize the sign + const bool random_sign = true; + generatePose(rng, 10.0*CV_PI/180.0, 50.0*CV_PI/180.0, + 0.5, 3.5, 0.5, 3.5, 0.5, 3.5, + R_cam2base, t_cam2base, random_sign); - Mat R = T(Rect(0, 0, 3, 3)); - Mat t = T(Rect(3, 0, 1, 3)); - Mat Rt = R.t(); - Mat tinv = -Rt * t; - Mat Tinv = Mat::eye(4, 4, T.type()); - Rt.copyTo(Tinv(Rect(0, 0, 3, 3))); - tinv.copyTo(Tinv(Rect(3, 0, 1, 3))); + Mat R_target2gripper, t_target2gripper; + generatePose(rng, 5.0*CV_PI/180.0, 85.0*CV_PI/180.0, + 0.05, 0.5, 0.05, 0.5, 0.05, 0.5, + R_target2gripper, t_target2gripper, random_sign); - return Tinv; + Mat T_target2gripper = Mat::eye(4, 4, CV_64FC1); + R_target2gripper.copyTo(T_target2gripper(Rect(0, 0, 3, 3))); + t_target2gripper.copyTo(T_target2gripper(Rect(3, 0, 1, 3))); + + for (int i = 0; i < nPoses; i++) + { + Mat R_gripper2base_, t_gripper2base_; + generatePose(rng, 5.0*CV_PI/180.0, 45.0*CV_PI/180.0, + 0.5, 1.5, 0.5, 1.5, 0.5, 1.5, + R_gripper2base_, t_gripper2base_, random_sign); + + Mat R_base2gripper_ = R_gripper2base_.t(); + Mat t_base2gripper_ = -R_base2gripper_ * t_gripper2base_; + + Mat T_gripper2base = Mat::eye(4, 4, CV_64FC1); + R_gripper2base_.copyTo(T_gripper2base(Rect(0, 0, 3, 3))); + t_gripper2base_.copyTo(T_gripper2base(Rect(3, 0, 1, 3))); + + Mat T_cam2base = Mat::eye(4, 4, CV_64FC1); + R_cam2base.copyTo(T_cam2base(Rect(0, 0, 3, 3))); + t_cam2base.copyTo(T_cam2base(Rect(3, 0, 1, 3))); + + Mat T_target2cam = homogeneousInverse(T_cam2base) * T_gripper2base * T_target2gripper; + + if (noise) + { + //Add some noise for the transformation between the target and the camera + Mat R_target2cam_noise = T_target2cam(Rect(0, 0, 3, 3)); + Mat rvec_target2cam_noise; + cv::Rodrigues(R_target2cam_noise, rvec_target2cam_noise); + rvec_target2cam_noise.at(0,0) += rng.gaussian(0.002); + rvec_target2cam_noise.at(1,0) += rng.gaussian(0.002); + rvec_target2cam_noise.at(2,0) += rng.gaussian(0.002); + + cv::Rodrigues(rvec_target2cam_noise, R_target2cam_noise); + + Mat t_target2cam_noise = T_target2cam(Rect(3, 0, 1, 3)); + t_target2cam_noise.at(0,0) += rng.gaussian(0.005); + t_target2cam_noise.at(1,0) += rng.gaussian(0.005); + t_target2cam_noise.at(2,0) += rng.gaussian(0.005); + + //Add some noise for the transformation between the robot base and the gripper + Mat rvec_base2gripper_noise; + cv::Rodrigues(R_base2gripper_, rvec_base2gripper_noise); + rvec_base2gripper_noise.at(0,0) += rng.gaussian(0.001); + rvec_base2gripper_noise.at(1,0) += rng.gaussian(0.001); + rvec_base2gripper_noise.at(2,0) += rng.gaussian(0.001); + + cv::Rodrigues(rvec_base2gripper_noise, R_base2gripper_); + + t_base2gripper_.at(0,0) += rng.gaussian(0.001); + t_base2gripper_.at(1,0) += rng.gaussian(0.001); + t_base2gripper_.at(2,0) += rng.gaussian(0.001); + } + + R_base2gripper.push_back(R_base2gripper_); + t_base2gripper.push_back(t_base2gripper_); + + //Test rvec representation + Mat rvec_target2cam; + cv::Rodrigues(T_target2cam(Rect(0, 0, 3, 3)), rvec_target2cam); + R_target2cam.push_back(rvec_target2cam); + t_target2cam.push_back(T_target2cam(Rect(3, 0, 1, 3))); + } } -double CV_CalibrateHandEyeTest::sign_double(double val) +static std::string getMethodName(HandEyeCalibrationMethod method) { - return (0 < val) - (val < 0); + std::string method_name = ""; + switch (method) + { + case CALIB_HAND_EYE_TSAI: + method_name = "Tsai"; + break; + + case CALIB_HAND_EYE_PARK: + method_name = "Park"; + break; + + case CALIB_HAND_EYE_HORAUD: + method_name = "Horaud"; + break; + + case CALIB_HAND_EYE_ANDREFF: + method_name = "Andreff"; + break; + + case CALIB_HAND_EYE_DANIILIDIS: + method_name = "Daniilidis"; + break; + + default: + break; + } + + return method_name; } -/////////////////////////////////////////////////////////////////////////////////////////////////// +static std::string getMethodName(RobotWorldHandEyeCalibrationMethod method) +{ + std::string method_name = ""; + switch (method) + { + case CALIB_ROBOT_WORLD_HAND_EYE_SHAH: + method_name = "Shah"; + break; -TEST(Calib3d_CalibrateHandEye, regression) { CV_CalibrateHandEyeTest test; test.safe_run(); } + case CALIB_ROBOT_WORLD_HAND_EYE_LI: + method_name = "Li"; + break; -TEST(Calib3d_CalibrateHandEye, regression_17986) + default: + break; + } + + return method_name; +} + +static void printStats(const std::string& methodName, const std::vector& rvec_diff, const std::vector& tvec_diff) +{ + double max_rvec_diff = *std::max_element(rvec_diff.begin(), rvec_diff.end()); + double mean_rvec_diff = std::accumulate(rvec_diff.begin(), + rvec_diff.end(), 0.0) / rvec_diff.size(); + double sq_sum_rvec_diff = std::inner_product(rvec_diff.begin(), rvec_diff.end(), + rvec_diff.begin(), 0.0); + double std_rvec_diff = std::sqrt(sq_sum_rvec_diff / rvec_diff.size() - mean_rvec_diff * mean_rvec_diff); + + double max_tvec_diff = *std::max_element(tvec_diff.begin(), tvec_diff.end()); + double mean_tvec_diff = std::accumulate(tvec_diff.begin(), + tvec_diff.end(), 0.0) / tvec_diff.size(); + double sq_sum_tvec_diff = std::inner_product(tvec_diff.begin(), tvec_diff.end(), + tvec_diff.begin(), 0.0); + double std_tvec_diff = std::sqrt(sq_sum_tvec_diff / tvec_diff.size() - mean_tvec_diff * mean_tvec_diff); + + std::cout << "Method " << methodName << ":\n" + << "Max rvec error: " << max_rvec_diff << ", Mean rvec error: " << mean_rvec_diff + << ", Std rvec error: " << std_rvec_diff << "\n" + << "Max tvec error: " << max_tvec_diff << ", Mean tvec error: " << mean_tvec_diff + << ", Std tvec error: " << std_tvec_diff << std::endl; +} + +static void loadDataset(std::vector& R_target2cam, std::vector& t_target2cam, + std::vector& R_base2gripper, std::vector& t_base2gripper) { - const std::string camera_poses_filename = findDataFile("cv/hand_eye_calibration/cali.txt"); - const std::string end_effector_poses = findDataFile("cv/hand_eye_calibration/robot_cali.txt"); + const std::string camera_poses_filename = findDataFile("cv/robot_world_hand_eye_calibration/cali.txt"); + const std::string end_effector_poses = findDataFile("cv/robot_world_hand_eye_calibration/robot_cali.txt"); - std::vector R_target2cam; - std::vector t_target2cam; - // Parse camera poses + // Parse camera poses, the pose of the chessboard in the camera frame { - std::ifstream file(camera_poses_filename.c_str()); + std::ifstream file(camera_poses_filename); ASSERT_TRUE(file.is_open()); int ndata = 0; @@ -418,17 +337,15 @@ TEST(Calib3d_CalibrateHandEye, regression_17986) } } - std::vector R_gripper2base; - std::vector t_gripper2base; - // Parse end-effector poses + // Parse robot poses, the pose of the robot base in the robot hand frame { - std::ifstream file(end_effector_poses.c_str()); + std::ifstream file(end_effector_poses); ASSERT_TRUE(file.is_open()); int ndata = 0; file >> ndata; - R_gripper2base.reserve(ndata); - t_gripper2base.reserve(ndata); + R_base2gripper.reserve(ndata); + t_base2gripper.reserve(ndata); Matx33d R; Matx31d t; @@ -438,11 +355,112 @@ TEST(Calib3d_CalibrateHandEye, regression_17986) R(1,0) >> R(1,1) >> R(1,2) >> t(1) >> R(2,0) >> R(2,1) >> R(2,2) >> t(2) >> last_row(0) >> last_row(1) >> last_row(2) >> last_row(3)) { - R_gripper2base.push_back(Mat(R)); - t_gripper2base.push_back(Mat(t)); + R_base2gripper.push_back(Mat(R)); + t_base2gripper.push_back(Mat(t)); + } + } +} + +static void loadResults(Matx33d& wRb, Matx31d& wtb, Matx33d& cRg, Matx31d& ctg) +{ + const std::string transformations_filename = findDataFile("cv/robot_world_hand_eye_calibration/rwhe_AA_RPI/transformations.txt"); + std::ifstream file(transformations_filename); + ASSERT_TRUE(file.is_open()); + + std::string str; + //Parse X + file >> str; + Matx44d wTb; + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + { + file >> wTb(i,j); } } + //Parse Z + file >> str; + int cam_num = 0; + //Parse camera number + file >> cam_num; + Matx44d cTg; + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + { + file >> cTg(i,j); + } + } + + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + wRb(i,j) = wTb(i,j); + cRg(i,j) = cTg(i,j); + } + wtb(i) = wTb(i,3); + ctg(i) = cTg(i,3); + } +} + +class CV_CalibrateHandEyeTest : public cvtest::BaseTest +{ +public: + CV_CalibrateHandEyeTest(bool eyeToHand) : eyeToHandConfig(eyeToHand) { + eps_rvec[CALIB_HAND_EYE_TSAI] = 1.0e-8; + eps_rvec[CALIB_HAND_EYE_PARK] = 1.0e-8; + eps_rvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8; + eps_rvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8; + eps_rvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8; + + eps_tvec[CALIB_HAND_EYE_TSAI] = 1.0e-8; + eps_tvec[CALIB_HAND_EYE_PARK] = 1.0e-8; + eps_tvec[CALIB_HAND_EYE_HORAUD] = 1.0e-8; + eps_tvec[CALIB_HAND_EYE_ANDREFF] = 1.0e-8; + eps_tvec[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-8; + + eps_rvec_noise[CALIB_HAND_EYE_TSAI] = 2.0e-2; + eps_rvec_noise[CALIB_HAND_EYE_PARK] = 2.0e-2; + eps_rvec_noise[CALIB_HAND_EYE_HORAUD] = 2.0e-2; + eps_rvec_noise[CALIB_HAND_EYE_ANDREFF] = 1.0e-2; + eps_rvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 1.0e-2; + + eps_tvec_noise[CALIB_HAND_EYE_TSAI] = 5.0e-2; + eps_tvec_noise[CALIB_HAND_EYE_PARK] = 5.0e-2; + eps_tvec_noise[CALIB_HAND_EYE_HORAUD] = 5.0e-2; + if (eyeToHandConfig) + { + eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 7.0e-2; + } + else + { + eps_tvec_noise[CALIB_HAND_EYE_ANDREFF] = 5.0e-2; + } + eps_tvec_noise[CALIB_HAND_EYE_DANIILIDIS] = 5.0e-2; + } +protected: + virtual void run(int); + + bool eyeToHandConfig; + double eps_rvec[5]; + double eps_tvec[5]; + double eps_rvec_noise[5]; + double eps_tvec_noise[5]; +}; + +void CV_CalibrateHandEyeTest::run(int) +{ + ts->set_failed_test_info(cvtest::TS::OK); + + RNG& rng = ts->get_rng(); + + std::vector > vec_rvec_diff(5); + std::vector > vec_tvec_diff(5); + std::vector > vec_rvec_diff_noise(5); + std::vector > vec_tvec_diff_noise(5); + std::vector methods; methods.push_back(CALIB_HAND_EYE_TSAI); methods.push_back(CALIB_HAND_EYE_PARK); @@ -450,15 +468,291 @@ TEST(Calib3d_CalibrateHandEye, regression_17986) methods.push_back(CALIB_HAND_EYE_ANDREFF); methods.push_back(CALIB_HAND_EYE_DANIILIDIS); - for (size_t idx = 0; idx < methods.size(); idx++) { - SCOPED_TRACE(cv::format("method=%s", getMethodName(methods[idx]).c_str())); + const int nTests = 100; + for (int i = 0; i < nTests; i++) + { + const int nPoses = 10; + if (eyeToHandConfig) + { + { + //No noise + std::vector R_base2gripper, t_base2gripper; + std::vector R_target2cam, t_target2cam; + Mat R_cam2base_true, t_cam2base_true; + + const bool noise = false; + simulateDataEyeToHand(rng, nPoses, R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, noise, + R_cam2base_true, t_cam2base_true); - Matx33d R_cam2gripper_est; - Matx31d t_cam2gripper_est; - calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]); + for (size_t idx = 0; idx < methods.size(); idx++) + { + Mat rvec_cam2base_true; + cv::Rodrigues(R_cam2base_true, rvec_cam2base_true); + + Mat R_cam2base_est, t_cam2base_est; + calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, methods[idx]); + + Mat rvec_cam2base_est; + cv::Rodrigues(R_cam2base_est, rvec_cam2base_est); + + double rvecDiff = cvtest::norm(rvec_cam2base_true, rvec_cam2base_est, NORM_L2); + double tvecDiff = cvtest::norm(t_cam2base_true, t_cam2base_est, NORM_L2); + + vec_rvec_diff[idx].push_back(rvecDiff); + vec_tvec_diff[idx].push_back(tvecDiff); + + const double epsilon_rvec = eps_rvec[idx]; + const double epsilon_tvec = eps_tvec[idx]; + + //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds? + if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec) + { + ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n", + getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec); + ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); + } + } + } + + { + //Gaussian noise on transformations between calibration target frame and camera frame and between robot base and gripper frames + std::vector R_base2gripper, t_base2gripper; + std::vector R_target2cam, t_target2cam; + Mat R_cam2base_true, t_cam2base_true; + + const bool noise = true; + simulateDataEyeToHand(rng, nPoses, R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, noise, + R_cam2base_true, t_cam2base_true); + + for (size_t idx = 0; idx < methods.size(); idx++) + { + Mat rvec_cam2base_true; + cv::Rodrigues(R_cam2base_true, rvec_cam2base_true); + + Mat R_cam2base_est, t_cam2base_est; + calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, methods[idx]); + + Mat rvec_cam2base_est; + cv::Rodrigues(R_cam2base_est, rvec_cam2base_est); + + double rvecDiff = cvtest::norm(rvec_cam2base_true, rvec_cam2base_est, NORM_L2); + double tvecDiff = cvtest::norm(t_cam2base_true, t_cam2base_est, NORM_L2); + + vec_rvec_diff_noise[idx].push_back(rvecDiff); + vec_tvec_diff_noise[idx].push_back(tvecDiff); + + const double epsilon_rvec = eps_rvec_noise[idx]; + const double epsilon_tvec = eps_tvec_noise[idx]; + + //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds? + if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec) + { + ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n", + getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec); + ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); + } + } + } + } + else + { + { + //No noise + std::vector R_gripper2base, t_gripper2base; + std::vector R_target2cam, t_target2cam; + Mat R_cam2gripper_true, t_cam2gripper_true; + + const bool noise = false; + simulateDataEyeInHand(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise, + R_cam2gripper_true, t_cam2gripper_true); + + for (size_t idx = 0; idx < methods.size(); idx++) + { + Mat rvec_cam2gripper_true; + cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true); + + Mat R_cam2gripper_est, t_cam2gripper_est; + calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]); + + Mat rvec_cam2gripper_est; + cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est); + + double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2); + double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2); + + vec_rvec_diff[idx].push_back(rvecDiff); + vec_tvec_diff[idx].push_back(tvecDiff); + + const double epsilon_rvec = eps_rvec[idx]; + const double epsilon_tvec = eps_tvec[idx]; + + //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds? + if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec) + { + ts->printf(cvtest::TS::LOG, "Invalid accuracy (no noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n", + getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec); + ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); + } + } + } + + { + //Gaussian noise on transformations between calibration target frame and camera frame and between gripper and robot base frames + std::vector R_gripper2base, t_gripper2base; + std::vector R_target2cam, t_target2cam; + Mat R_cam2gripper_true, t_cam2gripper_true; + + const bool noise = true; + simulateDataEyeInHand(rng, nPoses, R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, noise, + R_cam2gripper_true, t_cam2gripper_true); + + for (size_t idx = 0; idx < methods.size(); idx++) + { + Mat rvec_cam2gripper_true; + cv::Rodrigues(R_cam2gripper_true, rvec_cam2gripper_true); + + Mat R_cam2gripper_est, t_cam2gripper_est; + calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]); + + Mat rvec_cam2gripper_est; + cv::Rodrigues(R_cam2gripper_est, rvec_cam2gripper_est); + + double rvecDiff = cvtest::norm(rvec_cam2gripper_true, rvec_cam2gripper_est, NORM_L2); + double tvecDiff = cvtest::norm(t_cam2gripper_true, t_cam2gripper_est, NORM_L2); + + vec_rvec_diff_noise[idx].push_back(rvecDiff); + vec_tvec_diff_noise[idx].push_back(tvecDiff); + + const double epsilon_rvec = eps_rvec_noise[idx]; + const double epsilon_tvec = eps_tvec_noise[idx]; + + //Maybe a better accuracy test would be to compare the mean and std errors with some thresholds? + if (rvecDiff > epsilon_rvec || tvecDiff > epsilon_tvec) + { + ts->printf(cvtest::TS::LOG, "Invalid accuracy (noise) for method: %s, rvecDiff: %f, epsilon_rvec: %f, tvecDiff: %f, epsilon_tvec: %f\n", + getMethodName(methods[idx]).c_str(), rvecDiff, epsilon_rvec, tvecDiff, epsilon_tvec); + ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); + } + } + } + } + } + + for (size_t idx = 0; idx < methods.size(); idx++) + { + std::cout << std::endl; + printStats(getMethodName(methods[idx]), vec_rvec_diff[idx], vec_tvec_diff[idx]); + printStats("(noise) " + getMethodName(methods[idx]), vec_rvec_diff_noise[idx], vec_tvec_diff_noise[idx]); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// + +TEST(Calib3d_CalibrateHandEye, regression_eye_in_hand) +{ + //Eye-in-Hand configuration (camera mounted on the robot end-effector observing a static calibration pattern) + const bool eyeToHand = false; + CV_CalibrateHandEyeTest test(eyeToHand); + test.safe_run(); +} + +TEST(Calib3d_CalibrateHandEye, regression_eye_to_hand) +{ + //Eye-to-Hand configuration (static camera observing a calibration pattern mounted on the robot end-effector) + const bool eyeToHand = true; + CV_CalibrateHandEyeTest test(eyeToHand); + test.safe_run(); +} + +TEST(Calib3d_CalibrateHandEye, regression_17986) +{ + std::vector R_target2cam, t_target2cam; + // Dataset contains transformation from base to gripper frame since it contains data for AX = ZB calibration problem + std::vector R_base2gripper, t_base2gripper; + loadDataset(R_target2cam, t_target2cam, R_base2gripper, t_base2gripper); + + std::vector methods = {CALIB_HAND_EYE_TSAI, + CALIB_HAND_EYE_PARK, + CALIB_HAND_EYE_HORAUD, + CALIB_HAND_EYE_ANDREFF, + CALIB_HAND_EYE_DANIILIDIS}; + + for (auto method : methods) { + SCOPED_TRACE(cv::format("method=%s", getMethodName(method).c_str())); + + Matx33d R_cam2base_est; + Matx31d t_cam2base_est; + calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base_est, t_cam2base_est, method); + + EXPECT_TRUE(checkRange(R_cam2base_est)); + EXPECT_TRUE(checkRange(t_cam2base_est)); + } +} + +TEST(Calib3d_CalibrateRobotWorldHandEye, regression) +{ + std::vector R_world2cam, t_worldt2cam; + std::vector R_base2gripper, t_base2gripper; + loadDataset(R_world2cam, t_worldt2cam, R_base2gripper, t_base2gripper); + + std::vector rvec_R_world2cam; + rvec_R_world2cam.reserve(R_world2cam.size()); + for (size_t i = 0; i < R_world2cam.size(); i++) + { + Mat rvec; + cv::Rodrigues(R_world2cam[i], rvec); + rvec_R_world2cam.push_back(rvec); + } - EXPECT_TRUE(checkRange(R_cam2gripper_est)); - EXPECT_TRUE(checkRange(t_cam2gripper_est)); + std::vector methods = {CALIB_ROBOT_WORLD_HAND_EYE_SHAH, + CALIB_ROBOT_WORLD_HAND_EYE_LI}; + + Matx33d wRb, cRg; + Matx31d wtb, ctg; + loadResults(wRb, wtb, cRg, ctg); + + for (auto method : methods) { + SCOPED_TRACE(cv::format("method=%s", getMethodName(method).c_str())); + + Matx33d wRb_est, cRg_est; + Matx31d wtb_est, ctg_est; + calibrateRobotWorldHandEye(rvec_R_world2cam, t_worldt2cam, R_base2gripper, t_base2gripper, + wRb_est, wtb_est, cRg_est, ctg_est, method); + + EXPECT_TRUE(checkRange(wRb_est)); + EXPECT_TRUE(checkRange(wtb_est)); + EXPECT_TRUE(checkRange(cRg_est)); + EXPECT_TRUE(checkRange(ctg_est)); + + //Arbitrary thresholds + const double rotation_threshold = 1.0; //1deg + const double translation_threshold = 50.0; //5cm + + //X + //rotation error + Matx33d wRw_est = wRb * wRb_est.t(); + Matx31d rvec_wRw_est; + cv::Rodrigues(wRw_est, rvec_wRw_est); + double X_rotation_error = cv::norm(rvec_wRw_est)*180/CV_PI; + //translation error + double X_t_error = cv::norm(wtb_est - wtb); + SCOPED_TRACE(cv::format("X rotation error=%f", X_rotation_error)); + SCOPED_TRACE(cv::format("X translation error=%f", X_t_error)); + EXPECT_TRUE(X_rotation_error < rotation_threshold); + EXPECT_TRUE(X_t_error < translation_threshold); + + //Z + //rotation error + Matx33d cRc_est = cRg * cRg_est.t(); + Matx31d rvec_cMc_est; + cv::Rodrigues(cRc_est, rvec_cMc_est); + double Z_rotation_error = cv::norm(rvec_cMc_est)*180/CV_PI; + //translation error + double Z_t_error = cv::norm(ctg_est - ctg); + SCOPED_TRACE(cv::format("Z rotation error=%f", Z_rotation_error)); + SCOPED_TRACE(cv::format("Z translation error=%f", Z_t_error)); + EXPECT_TRUE(Z_rotation_error < rotation_threshold); + EXPECT_TRUE(Z_t_error < translation_threshold); } }