From bbf39b0964e9246358877b0d72391e9c0b010f0d Mon Sep 17 00:00:00 2001 From: catree Date: Wed, 20 Feb 2019 18:08:43 +0100 Subject: [PATCH] Add Hand-Eye calibration methods (Tsai, Park, Horaud, Andreff, Daniilidis). --- doc/opencv.bib | 65 ++ modules/calib3d/doc/pics/hand-eye_figure.png | Bin 0 -> 21200 bytes modules/calib3d/include/opencv2/calib3d.hpp | 143 +++- modules/calib3d/src/calibration_handeye.cpp | 770 ++++++++++++++++++ .../test/test_calibration_hand_eye.cpp | 381 +++++++++ 5 files changed, 1358 insertions(+), 1 deletion(-) create mode 100644 modules/calib3d/doc/pics/hand-eye_figure.png create mode 100644 modules/calib3d/src/calibration_handeye.cpp create mode 100644 modules/calib3d/test/test_calibration_hand_eye.cpp diff --git a/doc/opencv.bib b/doc/opencv.bib index aa2bac5062..e2af456532 100644 --- a/doc/opencv.bib +++ b/doc/opencv.bib @@ -17,6 +17,21 @@ number = {7}, url = {http://www.bmva.org/bmvc/2013/Papers/paper0013/paper0013.pdf} } +@inproceedings{Andreff99, + author = {Andreff, Nicolas and Horaud, Radu and Espiau, Bernard}, + title = {On-line Hand-eye Calibration}, + booktitle = {Proceedings of the 2Nd International Conference on 3-D Digital Imaging and Modeling}, + series = {3DIM'99}, + year = {1999}, + isbn = {0-7695-0062-5}, + location = {Ottawa, Canada}, + pages = {430--436}, + numpages = {7}, + url = {http://dl.acm.org/citation.cfm?id=1889712.1889775}, + acmid = {1889775}, + publisher = {IEEE Computer Society}, + address = {Washington, DC, USA}, +} @inproceedings{Arandjelovic:2012:TTE:2354409.2355123, author = {Arandjelovic, Relja}, title = {Three Things Everyone Should Know to Improve Object Retrieval}, @@ -180,6 +195,14 @@ volume = {9}, publisher = {Walter de Gruyter} } +@article{Daniilidis98, + author = {Konstantinos Daniilidis}, + title = {Hand-Eye Calibration Using Dual Quaternions}, + journal = {International Journal of Robotics Research}, + year = {1998}, + volume = {18}, + pages = {286--298} +} @inproceedings{DM03, author = {Drago, Fr{\'e}d{\'e}ric and Myszkowski, Karol and Annen, Thomas and Chiba, Norishige}, title = {Adaptive logarithmic mapping for displaying high contrast scenes}, @@ -431,6 +454,24 @@ publisher = {Cambridge university press}, url = {http://cds.cern.ch/record/1598612/files/0521540518_TOC.pdf} } +@article{Horaud95, + author = {Horaud, Radu and Dornaika, Fadi}, + title = {Hand-eye Calibration}, + journal = {Int. J. Rob. Res.}, + issue_date = {June 1995}, + volume = {14}, + number = {3}, + month = jun, + year = {1995}, + issn = {0278-3649}, + pages = {195--210}, + numpages = {16}, + url = {http://dx.doi.org/10.1177/027836499501400301}, + doi = {10.1177/027836499501400301}, + acmid = {208622}, + publisher = {Sage Publications, Inc.}, + address = {Thousand Oaks, CA, USA} +} @article{Horn81, author = {Horn, Berthold KP and Schunck, Brian G}, title = {Determining Optical Flow}, @@ -667,6 +708,18 @@ number = {2}, publisher = {Elsevier} } +@article{Park94, + author = {F. C. Park and B. J. Martin}, + journal = {IEEE Transactions on Robotics and Automation}, + title = {Robot sensor calibration: solving AX=XB on the Euclidean group}, + year = {1994}, + volume = {10}, + number = {5}, + pages = {717-721}, + doi = {10.1109/70.326576}, + ISSN = {1042-296X}, + month = {Oct} +} @inproceedings{PM03, author = {P{\'e}rez, Patrick and Gangnet, Michel and Blake, Andrew}, title = {Poisson image editing}, @@ -841,6 +894,18 @@ number = {1}, publisher = {Taylor \& Francis} } +@article{Tsai89, + author = {R. Y. Tsai and R. K. Lenz}, + journal = {IEEE Transactions on Robotics and Automation}, + title = {A new technique for fully autonomous and efficient 3D robotics hand/eye calibration}, + year = {1989}, + volume = {5}, + number = {3}, + pages = {345-358}, + doi = {10.1109/70.34770}, + ISSN = {1042-296X}, + month = {June} +} @inproceedings{UES01, author = {Uyttendaele, Matthew and Eden, Ashley and Skeliski, R}, title = {Eliminating ghosting and exposure artifacts in image mosaics}, diff --git a/modules/calib3d/doc/pics/hand-eye_figure.png b/modules/calib3d/doc/pics/hand-eye_figure.png new file mode 100644 index 0000000000000000000000000000000000000000..ff41d33f8c6a49a3b32f5c6928958881a649136b GIT binary patch literal 21200 zcmX_o1z6MH7dPGAF+h-Jbd6R*8YG0#0|Z2Bba#WajF1o%QK``#j#LIncS%Wi*E@gz z_kEu|+n(pU?>XPkse8`7_uR$5)KMiSU?4z4LnBsKQ`SdA!yrLJL-)eNL0!?@sm4To z;n--ZDx*FAdwpy#%|KnjhpL%)q9~^Sz0no9Klq_8;(DoTtKhC-FcM3%nG9*Qp`krN zQ&)a&=r^~YZb6&d!QYqMo`!@dShqf0o+2Xzf1JYk>(&MN4dLZ52FV z07L)<&?(Y`6qd%9Ck#!$x0Zb``(Ak4a-(7YrDZ+O;MIu#2;Y+^GDSSr|No&U$}oY& zrs9QhgBcvQ^bGgXk@Rl6;hOSlHuwQI2tNoHseg*k8Wj~ez{wQG5tepPizY;tjiZI# z;aSmii%`qTez zI2LAT!Z-7|!u|aeduxGwhN=nuiVQ?aS5irZ0#f7duz3>NDntV}|&i);CU4n0@d|2Sitj$Yvl@az{yeNf{s9v zg?JTPV2L#)UlRDsZ;u=+ZLTP?)iFvxIuMH|8L-zyestwN&w|TEqA$K&vzpz2&2$bU z%aD7@2jjwkl7Lki1dQTvpOas?_>>qIGuvg=W$r!ZqYb9&yX{g?H|j$IN72kR;T*+w zd0j_@D+YOCD8coxr9xvi2Cs%gIHJ0pA>GnW{gq(mINzP4xitva3L=*rKRvH4P>apF^_s98)I_+kdjo;Oy z$%=z6a0)PQ438FjHREFFpDjzfEf2wqt!`}w>5Dyl&<;o)t@gtU!~G}A74gXPBH?ji z!>{(%U~Gt0F>zstN~eGahIVuv@0ROI7{$Dp4NaYGxVWZsAgR%NZIn6DNP3I8q>Y*j z)m6%g7blVYV0=GHLT{IfP3}aT6;((Q+q@V$EgGM#H6pb6k@gQe@I4qshJS&}j3I5< zTquy;EXhR^avUF!!t2M>NxO|rU2gjmi?ByDUGzvN zKjvGs@oyosyc#snHJ&$?siI+3JO#YLd@BaTN~}=>BX3p~DMTAQz(2XjhJ8D&eqG)s zPM&elwj8XCX&rRlGxeq8;?M545jBAHhL$QmrA1uGW~`hWh(^D3=|F~ACvbz zy_fxYe=B>$29LV+$JjJDj%+D<(r8OgYm8oI8(;sW*K6+u7lvTZ9R^JwMR;(&15t%u zuX_k?<<~#A%jnBZr+%$H4RY{eCt92)!KH~K%Hj>o=Wznyf%Oj7;mR8uyAn^?c)LG? z?j2i?Oz_t-#ULwdd7MJ@5wgOybv9Rt&Q!*yi+N$QlZ}fSU9VTTfuX9tKw3p(q%oxC zmZ%#hD-D^dI1lrzeiQrFAHmd0#n-&G{E4H%46$#3indSaqDrwA6I(*20;w)S{$^wg%JzKL}CFtq-Y3>1?#dq0h z{A>j^o?$P=@}j7Z57>XJRym`{h5!WSOegXm!gOVeR~R?TvC*K(y5{BO2i%F4UBTJS z?G{n}&GAzAj4-d@hRQ8pF7ywyGyM{-w+e}XEj^8_H-aioD^E4dY%Q@Xfz?BmU3Lui zig=?)F!DLjS@-%pIAY~9#ytO@`2z-T^;t0+da}Y#Z8t>E9nl_e^T5yW{CbyqsVbYv zl|g`%S7qg})(Zd@E;;d6V3NCo_VOt#s?V4hhAErF8R2yM2alE5t!(mg0wnK>4r`Q5MWCbVghJ z{0E~yw_Ez1z0*--Y=^+}5TrG~bBuI#DkY{bY$il=Q;Up)*A}#C3#D7VP9$=_ugTu> zwWzLtv<;r88SJ7%jXn|4J2B#Yx}T+14FtJpj7q&ECL-BdBvJh80T|{CqaC^z_~G7~ z&dW5S(OV6VXezgcN~)5lo$1g!W(N2I^KM{B2i0i>cPZ=z*(w=!JKvQKDe+U*s5kcP zn@P(&5y6e8@nI==Fy3pb!WppUqT%`A12^!s=)2dJpM(e}S-a}H$e(=>xzyuZMPvRMYxS>w|L3_Gyc{@uv4jjf zczs|s@Iemv!dfTe!_<{5I{*dW4j3Pur9yqEFy`RW^c{KVwto9Cx>NJ}Z%VWiGy6Xt zYS~k!jIj#jJ{jb&=jS8k~GRzQ|{ITe;Gbibln;TFUDiTrO74H zZ*7uWtZYIEo;sRfc8@H>OR7qPN6It9`X{_mX@ps>I+s*$YvD0#WDahkcDtWg5xsR$ zmZf}#+)(b(!Iv)GsxFZbe7cYgHn2d2`l>r8JhZ3(4Tk)A+d?PX0!Oq{aPezvZ>M#y zegOHU=D@XiQgm{F6z;VoDtUY*!d_x+Axe{yIBK6sA@zR@DIN5jsIUG_Z}vx;4SlOd z=!m{yT^P^v{XN0|2+SjU+f;{w>&p3sx!3O{A*&J1ws&E63RI)x=uNm~F8WS4n1N`m z&5xmkd0z!66yuVRVO)a{o4o)+u01%GA!HOurTUd$a!poDl)MQcCYN5ku+h$(ZTO0WX_e4%hBYf}pChx^d#2b{q z)NG+#n2i&owFjDq15ENhD7k-H|MSn4*s+X#0-_H604@0yD#<9HY~uvQmEb1lP6pN9 z*E)F(ncWVrp%F*sj*~R8fWX=)iGFTATglE&_WpDj-TpGEa<(vNCwP6a5K9_TGUEFm z6A{OK%Ut)@qaPTvSN%;C^w*pey18r&8=ug7_3B5s6@5X~hU?WlG!(SBsyCJssIg~m z>yur0#Ro9}g%Zn(IHWBXRdlBJ+JapYzB*zD9r&y3_5%LA9a1Xr%tc3*u%^-BpdDh^ zPM~VwG$NYb1e4w$%WoWA)DkZ5I6F?I6}#E=kg@zF-=5@h13+eC%P%_LY)}4Vl-d22t z|6|oZKySNUyLg0|cD0rh{ga~Vd!HZ)(q%eZ#bhadVy$2sjvAtFK@bj8kcl&e9m)xA zT&mf3M9Ii)fhaY9@Td1%W-B={ilfTPo_=m%E*nWhX-U2HV^qV>fR0ny=iThpd3ffD z6h8Q@%FcO&&eY@8mPc*xO3~#g<~X&xxC%V)HEQR2#=@vMWmNu|0TobJaeh1c?=FV^ z2x|My0}f=C@fif-ONbj@S7-ghTWm)NBoZYNxM+`4@W)t=6@1J4LJsSf)%-i2IuC z9d4|bjEV-lOye+rThK5=hskXZ6#?+@+O4c8gWv+~K$WLEnv9K#2D>0`8!S0KrVDv^ zU;j#X#dkzp_S0$x!!XXUfhY>hIe>JJx!WBFvhmGRa1qyF&N6F!Y)436iJMvs{jx`F z3dLo-4f8C16{LD=*~C~{*ABb1=zmRIH1V6-kC^Wr$G_A8>zNAq^hI1~^0ek2aW7EO zw$lu=3L9q7_+|~FaTM12r{4`vUbrn$|LlyUrh@dwe#3{u0H5)f8q{UzRhWzaJdZpN zNAfh`(~F+&f~-9v$ie$Y3tHq@YU>wcu{{0$g6t8cjDJ>$s1}3zq6Pr6#E%2*uibvB z-D-a+nY1<`{x;z*4{gasrN9Vx5x4IP#2yfh$c zejC5;0T+e3>%@8&&i^B*swk5;08Lk#{N6BP*oocf zPYBtO6jc1kJYmk8{TQe|{Dc|TjdiC$C0rF{0-X+%>kfLS{{5ioMk{4Rp!N!pv;S+_ zj~Ox0ufRR!eRF)NLd@`-j%sm25T}YuK*{0Vr<=Qvt9;6`0Q^0sZgQhC>$~@!HE-IzNUtW^1Q(W`9(!CGm0bV*{RDuV(bebT!4L zR1?9Qwzix^{W5gBgBUN|6-IF1{U}yLtPqu}WfY=k&)zT+Z@k<_g{rb^1vm9Z9qFvQ z5RKyJs2^Q~!hkK2WhCV1yM|JQx9c~l7xPTn%;V+Sg=aw>8NJVGVgo+vX{T~eVICx0 z(Y1$Z%PA<(W`Mt8yJ4mThAg4|rA35DaxBh@tBBow@~gYl+w0Uqd&g&VNv{6h-f zA4}v%seE0rypVzPcK<3t2eVumKO-D+>M~;(D;mdo*RLoZns7aHItKWbkur};OL3rW ztRS^IqmM5j1pL}%Od?B_E@4%2(|6<>cI?g`G%;D+BU-I_Fr$Q^J0>49`S zJmA%CmZ0;$!b>BUz!G?OMdx(wx)<;h>6?DoGEIDxKXrh8ZXX`|r<{!x$tRZgbjuv5 zXS#4!Y?pg6+f~8NWp0Rg9w+@rAsb&p!}YZNYO^e(AwLUNed$Xz+rR`%vDOHWUA5}+ z1{?kYAvM;Co#e=`6?&dCu9y_3Jp0v;-XT0HY3IVV`a~J$O_x@_9w%vTfBTt-q<6cW zS8M;}Nn!uxWipteT$fN5tJHRsn{Z>h^}1SYnn~=vt)0+O_^mPGISN6G4b~B)ryn%z zYv2ZUaA^xayh-uwJR|@_Y~T%C9^GPk!U12bbJeDL#}fN<02==h<$99Aeo63}-desZ z)C(mS*FI?RbUEUHabC@1khWWe$NK{*+5Gc}@}90fK>?IICWC{cZVOjhi&DOh`^d9( zbsumcb7~4>?B2eKp7fp7`j2bC6WSPhyOOc~H_b-Noh0-AuA9j0?famVN&M|^tT}TJ z!G(T+5DoyU(*4S_i%F~6^cWp1c}na()u9wd7^XldyU>A7f+Ow3{iUE9+>zu%K|(>o zCjH(+nx3?k`zgTZ^ghTkzitAw0NP8?U$_`-e(r+mIf=X?#b0ov$ z$@g0+8|(`|ptq^iX|kz9u?(R&{b;(>NQh_8jAUr+M z8_X)@rbmk)$&)2|*TItN9-U|r=?+Zi4ND;I^}~fPaqly>Gcbhrr*4HTp5}`9%l~99 zujYW-N8N@-!%T=Y!fg833(zIxA2Q#V578s;m!cz<)qbd( zf2t3C*s^YxX8)(b0U`rkKOM@#5@IP1LjkHL(~Qz5sLda)V}kG2Jr8B;H~=$8uco7|z`o zLE;H6JG0zEWu}k)7SQOmo+v$ERJ2TCMR={K*isCJ1KJwfTqvdx zIhb=jCx~h<+T{HK<`S`EAuhV8z8Lcby3wR9IL_->Vl_Lyy$-$O;_JOG1ve3{_3loz zz3eiED%zs0d{y$ynBLUe0Ixn6`NKQMm}VD^IXgSZ>o^+RYY+;T^5-)Q%M?hb*_G>D zH>KAQYJ$*1kIgL9e7z`ww+zPS5j*#3I*lD${ zsyG%Gz+cc>)yO!zwy5btmlu3y^{hGhF`11detO-Lp;;bm@MQW?i7A)4E zpGXgtRAdamI!@(4y=&#PM;1vv-TeQ22jx9cN=`^wmM?1-v_KZ+1N-2}E2i5%3{qPE zur@Sjt--M=_0RlA3Z^l$;$QsQ_OY7!@gqNU6Ut%W|Q9Z^|c^cF&e=jCb6Ql;hBWJvYOyYL0dygdD5RPVU507Fw z6SNtwhaW={9P+vyw?Cx$)ZQ|P*GcaQ{2L3Rz9yH23$q@$sVpZk*ad7)f#a;I>G$$K zb45agnUBoJi6GB@Rc&1`;K#b*D_F%xMMg!EdG`iJF>Zy_AHHBB{~^`bE26K%a|F%C zUqAyE6ot$U_$H~A-UY-^T2XL6ZtW2*_5Y`g9>tmX!kZgQ9Nn0p4yNjoOkE!Hxf_D- zf-d>Wrs!6FmFJ#gotB|*lcKtbYK#rl|7TY)V;Pc`CidEO4E~+c2T=o};dQy^g0cz$ z*Y3UF9kxhnU7vfgK4_&kkKF(Fn79)u3cgtYjyBkTdx7|d_;=;6V4vO{aRv}_h znGpl7d+}7*EbZY?m-zm(eE>(l-5Y1M|CC4QOJ2az70|(Ww$6JoNeD`VH9u)9v`7~ZYUQO;-{Q|KXjI?_34ju43k_Ad1t!buE2JXLuL*>?kIM3>G`bFmmHP53u@i<+&HnUSmJpCds~McjJ4pK9gc7rZp*m<#u_u-Ct8)~^%gW9 z#&!J!m6M9Hvf3>>S^w#dG{CW^8v*{l`Sy78p-?BJ_K*Pl*P+Oi+zqWCLsD9gRYR0M zd*#}nw*2IE?lqIAQCkB}za`BK;I#fLc5t}stDw%t!>yuM2ZwwsT#NQ>{9<46*&JL~ zj3D25FGVr@dQVR$yI=(&0Kz-UGOM%{S~z#S%%bzN_r8T~*ua&L2l6tjV>6t}T|Gz% z1K?H^@`Ityn#)%($g)?y#o8JvC69nXc=Oxt6`@uoa3G0UA2g38{2(9*PfvcgmEMW2 z*0PEoijCY2l|2IVYMQ0Ik~AFZ=zODP2s|+Jm2?+*%AM3;s{R ztE-%l9D>ePgYa1hYiz#>xS_jbVlNHq81B;Bz#dN(s5dwYZ`a5P1I(v>U&A_@oy<+7Fze@MD>wt{KJg2-Sl|e_1krg zxM^OV3C;Jzs_9~h-S>4~sahGmMdnk`d#~@z=!(iCCtbwUA(u^Pqzgk6J=MZbo`ssH z7w?+H*q6?2gBR2Jzge6k^Yz&Q4Aq|@)@QzB(Mo*2A+_NOYN17vtP~@zU7fsl2Ri7? z4TRt0ZEe-U9-2h3r-db<*h3*Amtz2X0;n!ZS<{ADgseKNLR4nVdGRWeb|#>`)!kCF zGj1|AQ%*hqARpZXnR2;Ln@O23Rt6~qJK4FS-;x4!PCa^B!RNXPgwlk{{5{>*j5okvZ$y&qEH}QK1?xxtk z^Nhg3HCXOP1Q++x(V#v{v;zUTNET)CrMpqIk^&djL08aoW~_%K6;3UvAc7ih%H(AG zGn2&KD6e$U_W+%wTih!tqY9>@ya}=YVWqcryeNGdlgbCX?;Rj}H){p}}5gz$+u`PL#UL6P% zuyuAX^pPi5NaNIh$jK55OoD_^)d;pwp* zKEw-RE`nsQmaE7a1a$iL0xcaYQ+cDl3a?csgq{Ndb4^*7PRh25q>6!(Nm&Lx^4X+y+*V|Ct?qTaO$cNl_+1|IIBwR%#q}T22Q?{RkgHR3r0D?F+$VEBWKOBss4S=4KVT){3}lB=b9h(Nl5pKKaP{a zR?sSR(R39v&=aZ4sqXan$*~z=@N>&S^2@y4QHK?>{dKFuim4H90hX)c_I`9+v;)2% zyMQ*r+RWLJZJRXr`9b&~AMf;-WyDQqFEt9Ko|OnZOqTll zr5@Hzw_*)EajawBK&{Vhu@2jhvjKdt3p8gntpPBx?T@J}XMDFedrS$mnHYqN?E|8p zLc?BH-j8fxJ(DV4rAPXosgF_0=@D;PU%z0`_vj!7f0-{0yA8|42yt`#!xJr_yt$oH z;T@yG5Y=sYP?FlWaBcxFR!On#ptp9fuUaMn!Y$0W%tP4ZoE~GbsEDi}I+@M1e<_hU zj_#90@|d~ikGdL78%30xx2Mw3T1wvycqe6eELjLv8mb1P1HSTT$BV_+;r3Sd@2p6< zwvXDtl0dua9e3+&nr(T^K1}PVe{9fMp`sufhf}RG1uqA-bzMxy0_W1%m^p39h14l0 z2r)`+SAUfnk6y^Z;VUNK(XwD}7ZXawb!*9$LT@%eAEe;HgokH=#f+^fduLa6{X>q` zhCft6?R4(Z&R&`zX^4K^d1YHi#VjWZkxWATLcq zRJgN1TXZT#QMqq1YIg#ry!v~1$b4!Ds!+Mc+%Oq4S-HR4c?cI8>~vTuYT<^R1Q*`N^qXM*rH@edAx(BsF|#=3 zW9HRdvkY#4Ko13_U4D@(XYjMk+ve`+*g*TQNxw|SL%2V_!7Bs$>0|eBx<(u`Z`ukK zoQ%JcwANH)v{4^A_)#wIYpav!PBT9jdsou;h$z=r{ac^P|M~?%*3^Nu5PMHBb3%#h z@F?Z!>q=@y>)nR3mROak?I{0&&~TNMyH`bM{*m_m{VZ^fg2SD?2fx%mnN$w8-|9)( z+SuIHzi`>lnlkgL@H%pI)P!PD!H?%v5qta7B664XQc5l{uEM#f-jbj~YF|SlI{LBi}wN?ApK7De^NpvIZX{2D$jK(+qiX z!Ja}GhN8PXnD1-DoW2g#sD6@ZybbE_M4A}3O)Xvv<8B4TkGBN$z36~$8R^J67Z!(f z;tjePxDgfKmEcRcO-SKTO!%n~#Mp$k$of-Ng z9~*$AZ1q~9SX)vB%Z!`?fwFE41Z0(Ji9bqMLOZnn9P;Ewr@U@{!D(i8xuTZ z!{4_n39c3pf#s;8PIGP)9?PY;eDM}*Zwm96Thskn$w-x z5AJ;!S@v3>1*|)53&yyGn%-hNSxCY8`Th&K>XhXJ8hGVQmMrueaX;36(N zTH;|5*B8`c$3t&SY4C3F{-WBAF6A4{^+-H1eWiB0t;7P)E)hX!UqHg0&xp1mosrcv}^PQ|>oGirh{dpBtJf=r4Ep#MY7Ql0Ui1j>Nc@@w|~#1Bnq< z1P&Mp6m{tIT<}*fK0&hi=;Q0nFKPpE*hB`2mDlji2v<|fDn2@gWC}C;4J}@k{9UsQ zXnx3l0a^t!si)Nc^@B9>UJ9oo?AjU43)`YNRua0&z2EJQlnmZkw&UT3d4`>ec*!wB zbVho?REfEN_plCWIeNyq!2TX-dX_{V#Zbw3*fY}9a99UC{NRJ$usA>6-)QK*(2!j_ z=>Qff7CZ7$zSNONuV#g<9$|3JAs~XA=3m;V%tCmkn=1;Ul=z%^9100KTG3$UJAO!w zE!xN}TW@cV!pjBF?URu$?P=qsWhRp!QcPi!<~+)uC@Y)EhJvd(8f;iU=wig~y60NR zC%%cHZG`^acg<_q8GC6zHjP6^Tag!ym>+R6g0_jnxwGSk=Z+fhL>WkoOhEjpe5 z2HH1x_kY+|PMrQeM?1KF!*4|1n0|Xd$`*R4HW=kD*&0|!cll9PFsGz75Tg2g0kH}x zT~`5#E|i6R!fmP$?QF&I1YRVB`DTj}N%Uh@1{0dBOMIz0 z`fIC!hue9+YQ`0j;Lh@E?t{q-o9YcmT9PENxqGO$pug+PSNjyuNBN|U7k^FGJin=~q8aV&YZJtNizx9g9LgK~%9Jh2 z@NFjjInV0vzNA=>S;|Bjd;}grDfQ)v)e1|%JNt}d9sE)YP`Re(`3rL9`vu^)_n#J? ztr+WCJ(?@`B=rKoTrFAJtO2&KWN$g(7Cs2xWX^CLVAKgFL{THUdjTw$8lOFTM--uY z$sX>q@UA_zWIqV_wrD~WF5Y_|rGbwN7h3i(A-+n3Xif#%J`k5E>US2^*lDI=Tzt8M zm$4JX%2b~#lFNU}^ZRRTA8J*gYQ7Lwu_{1wG}D^irWHx->eKKHG3=BYSZ%I!?a%R+ z2QbxpSMDLtUm+ywJ6BAj^P~KAwB;rX){JT>(B>1vD)i~bZMio>5gKV2(jGk~#eieI zv9()*XSwJuJsOSd;Q-(?(U()mtMqTax0G>7thn5~38XRNY$4tZ6^TX+QbfP;BK^j0 z$7oK;SCy~Ku<&{%m>|~gg^Ja$5%!j-Kx>yx$|r7V9mpzeSBwld|9!D(uIrg!?QcAb zzj4>#7UdqVsVCkis;CTwj6})l6*&_wS@(arzZeOMdpWfkTN4gXRK3cCB)MWUU zh#K*DmmgkwN|hyCUPdnVQ~(m)$W~wS4B}*m@gF322E+%qKMOEOOhjvY=H)ctjvqUX z(J`ScyP!UZmpDq_(jaVDz$4;1C-vhTZ&Zr7YFoYxB)oB7zHa$`cU&aB%gH1=Q%H{v zr(1Y;?&b2~Ekw_%U+Q_I;O4v@i5>wiJOQ!9KzVAd(hAH$}E~}wEt*n{Ln%o(9vIb(|shmUBr*2JpB5H7m0tT z9${=U*fpK?V9Rcyy-j@6(}T2^vS4UqxSElz)~m-R<`Wyqs!*EL>0RO+oWP!0p>Qr1 z=fw{!XGg)L^}K3qVoMEY9nM8}y0Y9R^TWfsxE%bf?gQq&K~RON?YE%P&sPi~J$J+*P|6r~a5Qr- z>}3puI$*2!t^3~Uu;~6?gxwmzYV8y}+b+sN*mUm&%&P&|H>C`KWXN22*1z(TP*&GV z>V3e~b2TZRKpycT@egQMY%ZU;XpGhiBuTBL)i@off#~*8c;^TqY!e2GgMo#X3_Yc~cCa~Fv4tl?@ zmFR);)-54y0Vq%z-~V=BWIx^A!ziRbD9UD6h`Seld{^$k&J@A%Fl(qJr_TV_KhGo> z<;>Nb$udZ>vAxVM+QA^laiee%IOdM-cH-XSddq79R~d1aL9RR2vGC z%LFf;AAGfD;mL>Jk(A*sgea`21NpY5I|8>vf%M}UT`-jkcM@2>LEOu@ITKFaRUtAz z^+j49xiDS$Q3c$7ffyXlcbI*}V)XkBB%(5yzuZW%J@d`srt%+4hKDo}mQ6RZ&o>U0 zdp@@0e9Ec`S-zpfr%q(WZ(D3Of1-4sUk$iolMaxwmIAHc3nXW*=Mx8|l&mfS$^cD* z^}cg+bc1|CpLK5AQIpfh-~41ejMgSs508A-WrfdE>_=EI-wcdN+{y}}(!4`jY z5WS*d4})^np{&O0Zyd8K1CZXCn9T$3ZcN*cw#P_2u{2@AM(bmn=%F-tg3t@*U0 z1jWv%8}@guj7NFwS`z-8AiA3}Dx*c*H8+f}n#iNqo@FUkl@S?axr+>ui|f;?0mN97 zR3A#QR@2fT=qEIOuQVtS5h``0?a{0gd|1kUdLHmLVF1*J@5M?nG^AX3AnCmNn_QFa z{fp!;et)w3tBXZqZ$g#_hh79sUfua(xoSR>k!YfQAn$sb-8tBOP!vdKuQJa-`1BqZ zD&4SfEW9b>j`jnwqO6Hyt!d<;k?d-7ECkQVRb%$~8wX2RFO!IM;JaGFg6wjw?X)u+dw zpAegYWFvFV#+A9sMHYWk)NV>}TzC@O7tj_@IVY>Mc^MigBYt+}yp4JD5)is^hS0zN z67K`vk0LejS>nS`u+AYrtuFe|%O4Y+qay%c%s2=Sei!5R>a4GrdkH;c@bt3Ca0}~- z(a2k3&^%J=mt~UOv%GKC)Vz;99&gHt7d~y`g-!u^KPep9{TlYiH=Qa4qt9TRR_c1t zjdKGjp0Hqsy{fLiYUE}Xd=!A)Yd7QYe!|^k@q9-~;(%Ey;-HgCUF{rlYeN!V8@{|a z=-zfauiV(FuvjVw2S5|%-Q=KbN7SPZ^5v@bXlB;E-|E=eDY|oohY}Fx-$7xp5R7>l z)?-Zj9bMineUQs3YZuh_p|I5@5kGbInJ@8j4XVdWXOL<-Kg{anZ0I?d0}*6@wQs?4 zXR-pa&FlJW$hAL5K$x_stpucKKw02t1dt^DW8nw>&+&G`EU^IE)|?ee_IblOl2iAA ze)VkHbJJ|q^InyU(^iMjVp&uH8!|vVe!LkW<@+qFZ&4B^e z{tazIPy_9#{bjka0B%Qjxs@WKX!ET1ZEI6xwIr;`tzGB|-l5$M#ar5~5EU*~b0B*ss3=gK z6Yu*Wz8#@{A&pw$^tNMH6ZO8;9bnQjBK%Fd$(NAU!+bLGA0|CXl$~AXv-MlM9m~Er zUyR(_NO!bAVVSJUq?)|btf-SP1t;HQ4%EHn!3x$trv+4|Uf|=J0Djpxu5|h{-znIV zvvFZ;Sr4a&96$vlzJce~8{zM}Spq{8sLe$DC_NR=9y&}#R};aAJjLDjW!6r;(>9xD z@?sx)+JRwqsZ<+jr>aK|0E_N;{KuN^A?&$MKw>wo_snpkZH|f{TdUIv6I?C*R>OI1 z1ARsM`)Qy*MH7pPVLZbr*{G52AUVu){bzPob(ZSlUZBk7$G2t^f)F^>Wd0U z77w_0-JBojcRLtiMIyU#yVyu&85lg7KOK+#(2)@cHXJnJuqTwue^v65;_L8%^v#>U zMm;YjcV);ym}O^DjC8E4WYQLSVOH|C)8ZtFQ0_iFV2|YTSWgA`*Tq{fl^M-Wu{G!0 z9}dDyTtutC!>;lDlmv|3Z%>US+eU^2+IjDPvfT=BpmuIJ5pU`7wA?X1QjkCeWywkU zZI^a71bj6>Shnm9>(hKn(`OV7ZLfIvq@Taqr??G&t?&c-w+r0I|w#V6O}{QgR>8G6NOhxk#r zykfBFbs&pp_Py!HY3p=;*Vh;q#Wt7{{P*{R;at6h`mpx+(kCV~{`6)7W_V@=fAM7B z*-7VBZpP;=hB+*1UdE?hNPYjMzSc?|chu|HjJ0E@f>hl`0~XsG1F=L|+-y6vCQk&Q z@CB8lB-0|619|@Lf67H-oi{*GW<&04<{xZysM3=Ri%07KN-c$ z%WQB^g~>$BG8~yMY3*;U5x-G1tSF30Sbq?%!XUTxu+*o`#t^2v(6i!jc?!wRLSOPM zcE_P%mU_2|rQu{S_j7baw!m}1!DWVtu*8yLu<>QwBe`XId2M+AOI9GNAtSa8n60`W z8cp3i*C(gt>LaSon*M9Swav#zuw`qJP<*#7oZ2ZDON+Z^5hR(ol)X7|atXm7?Q1R! zY;KsRHuE^qp=iNSXnACTmAa_f()`p&_w?o#37=u|ge~^tXF4VBIWVkdvK4MC4>HYA z#EJaUK<&f(G2?`$E|07bu3&kWs-er+=;Y2f=QxD|o&9+EcdK4!dREupR|vm*YH9+C#?PaR@$*~*3~BnONf1B zImKHLQl*w1U1HO@bh#G*PdN1p`{hv-Wceqs3^K?{+fwMu54T`JjPM39z(j5R7Yk}Y z%KU%=wV%xbTHdSg6%CbiA+IFo_g3vQA*@3g@QoO&>iXNDYxx$4)9AEyvb&T(uhv}X zgq~SQbctIhh9+%cEtwePn*5-+uKO(uQW&nkRPp=Ik;tagt2p8?QenQa8mdZdVHSR+ zfo=%S=ApMRoGMzggU|%9)At7+7Zv%HRjR*5{=nap)#w|PmJ3VTde}9RASjd{7_kYH z4ZZzDGJmR5&WvR3z=wQbi|D$|d!!T{G5&RFXw-y~u5=V_ja!n&s`RMEYJZ63IlJm1o_tSVKeY!J17Q$X2LjA#0Nprh9s z1p6-K=+&qAOa?Lvh_#udjiqp1!cBAa;AlN<2g3UD*xf2>C9a82(VJ$5{*>C%WW?H{+`a^4F!D9W4=;4BW*;iGO1pljb1+G*OT-wd=iMH? zK`Y6RopY}#GD_=FT+h$ykR=#;??`=RHI#0%0RJ8%A=ZOnm`T|#4KR5m$YmIf7mbZys^tYhtb!3E-AjfW zK7&oWGpOSnceA=G{+4*^GDkAlz&B{bOqFaRFBg8O?@@Vb zVDa~ryuDGgMdd*+ZpU&;uZJyif{e0wFY97TJmhCZgk3pOgCt79yj5r7C>{D$a#hF* z3sGch*&C$XHhV~dXzON~JfbLe4(Yy==OkYk;U>a?Xr}Mln$U(qAFX7c?g#KSVAUnZ zWJf90pQ*(|-$2>89IZczE_B<;Zic#Z(&(YXl&nylCB7|NV^hM+wTXeum2v!@Z3!wK z-oIY99wc5%*P&E52}PsP1<+>RfQYaPwADW_N8T|;eT6l{^Z46&??N0#JnhYMM!q_} zmK5_5G5TRJ@aLtlqsezVjS~t5afnVD;Z@(ti>~&8&gXG&vkCG@^7Q(AZ=`>qVy=RE zt=j7xb8$J5gV3D*)NSA`!O_$3(BIDV6@l-iYD893V-kWQ!&4}IbTmtm&_o3uMI1iEW9 zn7^=cuBO^0c9Q|4J=6V|PMxx8LFi^rgDEv3qS{KDao=;Ib0!75J&KXs)%P$~Egb1< zWet{<5AW)C>A|d(p777UD62m0F~cg+XNevB3al?9W4q7Olr;fgVuzGAt1S|A$lF8UikO+@qEoCWvgbk6kBkuAZnr|Z9CbZdmj@Z*=_DsRm3%L&fP`V);YzMlMj5db6+ zENfLp&&|U-_+wwx94gb&xnXlMZ?n5TUX6?xU)(bGWx4zNaIdYM>!;J5@KPH`UeD8T zh-f7U8TM_DzXlN8F|Na+1ld)WA6af}4jt-B8w_p@n`^(B> zEnzZab@&Kgu*gO{*>g}=0OJ2P2frTS1q*~1=|I~OOPE53f|pM4sLADwTj@{UtVoHg zPToaKUd+&Fs_?HqIT_>UK(O+kizy2+gkLXWhT=oqk>Ae>0a)hFCs6V`eJ zTdtdkF2v{chU0e?gS8_|!%N|Lm;_IglTE-yUkviTQcij@Luro=eT1JB5A-FPu=PdkF9PI z;^n3iNI}Cz-)*5S_?oP{&@M&OWHrQka_j~%NA?3HJ=cXIGRLX^Pd8`&4rTlP@fo|v znz4?F2qQ+8u_qo|6xnH%WsDRxX_#zTvYWAtrHrx16A5W#mu0fXSVu}l1|dt9ghKk< zegBN_FZXZPah=C~ocDcP=lgoUUjrrJD#81LxTJ9LnI(@WR?doB;jH_u5=W3TkmWVj zn~b(60c{YmWEP&J6O^6$ER%%p)r}|-S=Ag!C-H^b996@`_kvvh2#tdhHjM|4#7l8KAeub`i`+$?DMHY!US>*9OllO486Xy@w({xoEn&?U1;Dx z$tBC67vbC^GrR82mGNMk-G`JDXZ(H|aRwOm+m7tQFn2~zfg5@7xw>UD> zEr^{?K`mWgcbWeNIb*)EuCo|K>miF`_4N9-Uw-1i6V~!y2Y+LleRrpwqh)URD8?ek z&#Rj44lP#m2gzW3Hk8xP(nh(B`aAp;WNsZgNN##e$HiQuKI&9G@nT|iaHH&il?oH3 z|4F`^&jHu03S+vPABCBggWtE<8licZ!68x)3m^ZqGObmFgKb&FTK0sqk-%xrO*BPl zP%}_lDP2>zWi_RDJtfo+r@G8DGvv)n#@#d?y7fbL+oH8PRz6T4LO0d`<2$emkZ$-Z zJS;vz62Jt2SJpkw!o95Sd16TUf4a%;!2u5>?H3yV{G;1l*iWR*x=A1L)cquMeG7(A zpXSL5*95b;5Jca~_O(ryC7+AVT&U&qxhg2%8SgU9VNpM2A6we_(Zn zhPA~MPd&mf`2RL|u*us-{B=hz?K3LiseoG2AiJj14r8LhUQEk;@?Nk@($KZ4Qq!? zS^Qxob-(m85Uo%rrfOtzs( zQHp6Zv_v@_(PJY@%V(<)IB4X@k-tI0eqd(1s;>6!C$Q{E=5zBCUFZ04<4Cm`~ zBT&5ohSn9muSc;%VSy3|w_n0oKfjB7ccZ#Y;`Z$worY&<9~I3|x{2Nw?`L5Hz9vp8 z3j(*oI0>>tuqT!esnQmopm@J!;`is<@~-?<;r7pZ!9y=jr&XtvZFfj1iz`&c{Z_7A zkU^`v!q_&-GJi0hwbwSd+n-cMhb^lHBMI!alzq+Yk@j?Q7@u~43xT`7KFi?pZno21 zOFyGoc0`)F==AzQb4VfSS8;)*i$~IGi!{n~_!O;RuEx5#*GobtTqwI%-^FaznBjSz z%1ZW}b7ErQv4fY1l5giT*gs;3s&1ky3*xszb$YY$}ZqBAvi`_vVK0vj|jC5O-JB4T940YS%cYwXa& zU4)_)^QTo}=~xP!DPR83=#d6L#}jIM2WxO&pko0ccQbr){Ji=J`p&q0dQx(3f^*Qx z%t@X{I4&D#1nhn*XR+c4gVs$mqHYcDs>Ah8NL6`BC@$Nq{OXL>BOSkwY%DP}SnTXS zIv@O2uqOHG^v_HnyJ4BoYur_$#)Bb6+ywWZOX=phMYcqVS@{`gEh77Oq8AIB+r;Q^ za)U;2v8`!|0nS~>x`949SnR|ZcF`m$A^tE4!emCXlyY)CqJjJQb9f(`R9=U~sdxgp z{NBe>9KnOlyIxAtt^jiIc1Iz$Pw5L?QXe{G7;WG7zuh7qRagz%jh3IB(q6$eS zqRrU@qz9(YBo@V)zHk>E&d>C*-FCL4L?4Gpho(dXzh1r5i30u&g-aTj<(j@aJn{~` zqJz9Y=n9jADm|@|v}mEW-I-oS3|+)&yFkin385#A=_XfQmsFWJCK{2jdqLt=hhwcg z+}_ZWHS#U*?(jp3pN4yrpQ~Z)y%nzPE5x;zEv-FkW}4%Y$cM?>}un( zl4gF%fR}e1XN_pFoAQncT5VGyb9`ohN2fKw_+U?fRmzU}$%C@S5ib8NaES2JS>Io; z{{{y+PFvR)o7z7ifGO!Tk29U5W8hyVyB%8x#rcVv^gnIcLrWZCY)YD(E^Ninugq%*cg(#R}(1 zJd&YV+)6y~5pUo*E%^j8X^o=hY z1;PJXh5w>wWl@6so!rH!7Vj$uT&pg!d7^7n~!Ar{+Nf{BRWPy=DEJ z);s)5A&}9b(lS6~Uyf%%)@XUZ zVhO<%LURUvg01r&Sq$5C8f@l&8|@2}?{QVqs_30_&T?({KaKUn+~I@Eu+*}X6_r*l zmL8yYmGj>E{ILzuQD3bnukhp_ISo5LxueG>!Y@6xK9tLL6%6;{E?gS76Dman_YVd( z0ZO?CPj%JC!2_GXQLKGG@9PN3K-F)#Gz_;0f3bYU%mMoI{6isy8}XJ93$NbVthOfx z;xo&ls4sX{e}K!)dd|_ z=uQ=dZ1-Xuw9!7C`42PraeiW|UUnqPc462g}TD zfn2641;!*o>A$1=nb#53%KS@RS%t8KuJ8oH_r)W_f#J#TiYu1?ifPlmqxj;q%o2*9 z-wy2-72-b}FQ5P;Y}Mhu>i!|Sw{DZq^yRwDM1o*uosx@vofXUIolj7knp?f%@Fh{k znDE8_GH_ov&AQeNis2>($E#m#1=_V-3M|+w;|b}%sg@~;@r75!5D$#2^jDJHqnl!W zY08=B#k0~*hnbmDYzoq*KF+Wg&1!jB$=HG-fxK_eB~~0{;NFEceiU-y+blO7b_5!* zZibry0x{?CE;6uKnp>3=YSr{-Q{5=J237lFK+z~pu%Te4lO2&m-GK_Y70m^E^3=VQ zN`2{*EX<^y=KY+6+2J5K)kb_GHG#7fzI4KP};OdTBNY4$zK@67%0= z;iiaN^vDo!#|@i5L|0lIC8WtZ$mH$uV!3?xmM@45BK5(f@Ad}X^nEF(0f*fOP0UzD zE@zO!5cmU|(C5%uFyyV42|+j|z5`{s*g>15S)j-x=PjdL@iLV{{OG0nSuQx<)rgtH z5<4e@;H@icHRsC22fX2U&}!R&;DnJ}#XDT; z{*j3X;-tOJ)|dYRVZ#`qN%jiq*BSOU}E|~mY#f@|=*3kcY4D}saeJA&6a417|g)M+2 zY$BHf87PMI9)6IcMS1X3iydS>{AO7{Z5cH4jb(j#3v}3WmKH_kq81lhtuN$*4pD}* z7GJ$9qQD4m$(HZfY8Lqibg1`_71tHQsTL0oNB!(w40sRuZg@VW);Ie1Iyk}^livug z<}R!8`!^bI|0p-3*O)7lD<#fw<>#CE46l@KKk#H26g9QK^F;`mtQjDJYw`!^SQ)qq ztu;U(mw%2eD7hU5MCAZlNJ1ANJphOTO7&G(2UuV1SNP59497DcF>6a!bW<7s3oc42otqaXCYG^c!yP0_=bX_x#J`PI zTJPJeZ8APqb)j^wS;M?r3T$qwM~7T-{Me!mhibpI`-|Ee7cBBcRkDoRDP`Zb#fD)< zGgSC;E@=WUCc`&6h-E#b30TPu-g71S>NWzq|J~qp^>+_-C|!^*P96$$<`ZHaOLF1} zCCrfED=uxP37QrjW2n*E>*ENCGlk-3I`D}FQCR!h`) that contains the rotation matrices for all the transformations +from gripper frame to robot base frame. +@param[in] t_gripper2base Translation part extracted from the homogeneous matrix that transforms a point +expressed in the gripper frame to the robot base frame (\f$_{}^{b}\textrm{T}_g\f$). +This is a vector (`vector`) that contains the translation vectors for all the transformations +from gripper frame to robot base frame. +@param[in] R_target2cam Rotation part extracted from the homogeneous matrix that transforms a point +expressed in the target frame to the camera frame (\f$_{}^{c}\textrm{T}_t\f$). +This is a vector (`vector`) that contains the rotation matrices for all the transformations +from calibration target frame to camera frame. +@param[in] t_target2cam Rotation part extracted from the homogeneous matrix that transforms a point +expressed in the target frame to the camera frame (\f$_{}^{c}\textrm{T}_t\f$). +This is a vector (`vector`) that contains the translation vectors for all the transformations +from calibration target frame to camera frame. +@param[out] R_cam2gripper Estimated rotation part extracted from the homogeneous matrix that transforms a point +expressed in the camera frame to the gripper frame (\f$_{}^{g}\textrm{T}_c\f$). +@param[out] t_cam2gripper Estimated translation part extracted from the homogeneous matrix that transforms a point +expressed in the camera frame to the gripper frame (\f$_{}^{g}\textrm{T}_c\f$). +@param[in] method One of the implemented Hand-Eye calibration method, see cv::HandEyeCalibrationMethod + +The function performs the Hand-Eye calibration using various methods. One approach consists in estimating the +rotation then the translation (separable solutions) and the following methods are implemented: + - R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/EyeCalibration \cite Tsai89 + - F. Park, B. Martin Robot Sensor Calibration: Solving AX = XB on the Euclidean Group \cite Park94 + - R. Horaud, F. Dornaika Hand-Eye Calibration \cite Horaud95 + +Another approach consists in estimating simultaneously the rotation and the translation (simultaneous solutions), +with the following implemented method: + - N. Andreff, R. Horaud, B. Espiau On-line Hand-Eye Calibration \cite Andreff99 + - K. Daniilidis Hand-Eye Calibration Using Dual Quaternions \cite Daniilidis98 + +The following picture describes the Hand-Eye calibration problem where the transformation between a camera ("eye") +mounted on a robot gripper ("hand") has to be estimated. + +![](pics/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_b\\ + Y_b\\ + Z_b\\ + 1 + \end{bmatrix} + = + \begin{bmatrix} + _{}^{b}\textrm{R}_g & _{}^{b}\textrm{t}_g \\ + 0_{1 \times 3} & 1 + \end{bmatrix} + \begin{bmatrix} + X_g\\ + Y_g\\ + Z_g\\ + 1 + \end{bmatrix} +\f] + - for each pose, the homogeneous transformation between the calibration target 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}_t & _{}^{c}\textrm{t}_t \\ + 0_{1 \times 3} & 1 + \end{bmatrix} + \begin{bmatrix} + X_t\\ + Y_t\\ + Z_t\\ + 1 + \end{bmatrix} +\f] + +The Hand-Eye calibration procedure returns the following homogeneous transformation +\f[ + \begin{bmatrix} + X_g\\ + Y_g\\ + Z_g\\ + 1 + \end{bmatrix} + = + \begin{bmatrix} + _{}^{g}\textrm{R}_c & _{}^{g}\textrm{t}_c \\ + 0_{1 \times 3} & 1 + \end{bmatrix} + \begin{bmatrix} + X_c\\ + Y_c\\ + Z_c\\ + 1 + \end{bmatrix} +\f] + +This problem is also known as solving the \f$\mathbf{A}\mathbf{X}=\mathbf{X}\mathbf{B}\f$ equation: +\f[ + \begin{align*} + ^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(1)} &= + \hspace{0.1em} ^{b}{\textrm{T}_g}^{(2)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} \\ + + (^{b}{\textrm{T}_g}^{(2)})^{-1} \hspace{0.2em} ^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c &= + \hspace{0.1em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} (^{c}{\textrm{T}_t}^{(1)})^{-1} \\ + + \textrm{A}_i \textrm{X} &= \textrm{X} \textrm{B}_i \\ + \end{align*} +\f] + +\note +Additional information can be found on this [website](http://campar.in.tum.de/Chair/HandEyeCalibration). +\note +A minimum of 2 motions with non parallel rotation axes are necessary to determine the hand-eye transformation. +So at least 3 different poses are required, but it is strongly recommended to use many more poses. + + */ +CV_EXPORTS_W void calibrateHandEye( InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base, + InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam, + OutputArray R_cam2gripper, OutputArray t_cam2gripper, + HandEyeCalibrationMethod method=CALIB_HAND_EYE_TSAI ); + /** @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 new file mode 100644 index 0000000000..18561c77fe --- /dev/null +++ b/modules/calib3d/src/calibration_handeye.cpp @@ -0,0 +1,770 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "precomp.hpp" +#include "opencv2/calib3d.hpp" + +namespace cv { + +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; +} + +// q = rot2quatMinimal(R) +// +// R - 3x3 rotation matrix, or 4x4 homogeneous matrix +// q - 3x1 unit quaternion +// q = sin(theta/2) * v +// theta - rotation angle +// v - unit rotation axis, |v| = 1 +static Mat rot2quatMinimal(const Mat& R) +{ + CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3); + + double m00 = R.at(0,0), m01 = R.at(0,1), m02 = R.at(0,2); + double m10 = R.at(1,0), m11 = R.at(1,1), m12 = R.at(1,2); + double m20 = R.at(2,0), m21 = R.at(2,1), m22 = R.at(2,2); + double trace = m00 + m11 + m22; + + double qx, qy, qz; + if (trace > 0) { + double S = sqrt(trace + 1.0) * 2; // S=4*qw + qx = (m21 - m12) / S; + qy = (m02 - m20) / S; + qz = (m10 - m01) / S; + } else if ((m00 > m11)&(m00 > m22)) { + double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx + qx = 0.25 * S; + qy = (m01 + m10) / S; + qz = (m02 + m20) / S; + } else if (m11 > m22) { + double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy + qx = (m01 + m10) / S; + qy = 0.25 * S; + qz = (m12 + m21) / S; + } else { + double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz + qx = (m02 + m20) / S; + qy = (m12 + m21) / S; + qz = 0.25 * S; + } + + return (Mat_(3,1) << qx, qy, qz); +} + +static Mat skew(const Mat& v) +{ + CV_Assert(v.type() == CV_64FC1 && v.rows == 3 && v.cols == 1); + + double vx = v.at(0,0); + double vy = v.at(1,0); + double vz = v.at(2,0); + return (Mat_(3,3) << 0, -vz, vy, + vz, 0, -vx, + -vy, vx, 0); +} + +// R = quatMinimal2rot(q) +// +// q - 3x1 unit quaternion +// R - 3x3 rotation matrix +// q = sin(theta/2) * v +// theta - rotation angle +// v - unit rotation axis, |v| = 1 +static Mat quatMinimal2rot(const Mat& q) +{ + CV_Assert(q.type() == CV_64FC1 && q.rows == 3 && q.cols == 1); + + Mat p = q.t()*q; + double w = sqrt(1 - p.at(0,0)); + + Mat diag_p = Mat::eye(3,3,CV_64FC1)*p.at(0,0); + return 2*q*q.t() + 2*w*skew(q) + Mat::eye(3,3,CV_64FC1) - 2*diag_p; +} + +// q = rot2quat(R) +// +// q - 4x1 unit quaternion +// R - 3x3 rotation matrix +static Mat rot2quat(const Mat& R) +{ + CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3); + + double m00 = R.at(0,0), m01 = R.at(0,1), m02 = R.at(0,2); + double m10 = R.at(1,0), m11 = R.at(1,1), m12 = R.at(1,2); + double m20 = R.at(2,0), m21 = R.at(2,1), m22 = R.at(2,2); + double trace = m00 + m11 + m22; + + double qw, qx, qy, qz; + if (trace > 0) { + double S = sqrt(trace + 1.0) * 2; // S=4*qw + qw = 0.25 * S; + qx = (m21 - m12) / S; + qy = (m02 - m20) / S; + qz = (m10 - m01) / S; + } else if ((m00 > m11)&(m00 > m22)) { + double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx + qw = (m21 - m12) / S; + qx = 0.25 * S; + qy = (m01 + m10) / S; + qz = (m02 + m20) / S; + } else if (m11 > m22) { + double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy + qw = (m02 - m20) / S; + qx = (m01 + m10) / S; + qy = 0.25 * S; + qz = (m12 + m21) / S; + } else { + double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz + qw = (m10 - m01) / S; + qx = (m02 + m20) / S; + qy = (m12 + m21) / S; + qz = 0.25 * S; + } + + return (Mat_(4,1) << qw, qx, qy, qz); +} + +// R = quat2rot(q) +// +// q - 4x1 unit quaternion +// R - 3x3 rotation matrix +static Mat quat2rot(const Mat& q) +{ + CV_Assert(q.type() == CV_64FC1 && q.rows == 4 && q.cols == 1); + + double qw = q.at(0,0); + double qx = q.at(1,0); + double qy = q.at(2,0); + double qz = q.at(3,0); + + Mat R(3, 3, CV_64FC1); + R.at(0, 0) = 1 - 2*qy*qy - 2*qz*qz; + R.at(0, 1) = 2*qx*qy - 2*qz*qw; + R.at(0, 2) = 2*qx*qz + 2*qy*qw; + + R.at(1, 0) = 2*qx*qy + 2*qz*qw; + R.at(1, 1) = 1 - 2*qx*qx - 2*qz*qz; + R.at(1, 2) = 2*qy*qz - 2*qx*qw; + + R.at(2, 0) = 2*qx*qz - 2*qy*qw; + R.at(2, 1) = 2*qy*qz + 2*qx*qw; + R.at(2, 2) = 1 - 2*qx*qx - 2*qy*qy; + + return R; +} + +// Kronecker product or tensor product +// https://stackoverflow.com/a/36552682 +static Mat kron(const Mat& A, const Mat& B) +{ + CV_Assert(A.channels() == 1 && B.channels() == 1); + + Mat1d Ad, Bd; + A.convertTo(Ad, CV_64F); + B.convertTo(Bd, CV_64F); + + Mat1d Kd(Ad.rows * Bd.rows, Ad.cols * Bd.cols, 0.0); + for (int ra = 0; ra < Ad.rows; ra++) + { + for (int ca = 0; ca < Ad.cols; ca++) + { + Kd(Range(ra*Bd.rows, (ra + 1)*Bd.rows), Range(ca*Bd.cols, (ca + 1)*Bd.cols)) = Bd.mul(Ad(ra, ca)); + } + } + + Mat K; + Kd.convertTo(K, A.type()); + return K; +} + +// quaternion multiplication +static Mat qmult(const Mat& s, const Mat& t) +{ + CV_Assert(s.type() == CV_64FC1 && t.type() == CV_64FC1); + CV_Assert(s.rows == 4 && s.cols == 1); + CV_Assert(t.rows == 4 && t.cols == 1); + + double s0 = s.at(0,0); + double s1 = s.at(1,0); + double s2 = s.at(2,0); + double s3 = s.at(3,0); + + double t0 = t.at(0,0); + double t1 = t.at(1,0); + double t2 = t.at(2,0); + double t3 = t.at(3,0); + + Mat q(4, 1, CV_64FC1); + q.at(0,0) = s0*t0 - s1*t1 - s2*t2 - s3*t3; + q.at(1,0) = s0*t1 + s1*t0 + s2*t3 - s3*t2; + q.at(2,0) = s0*t2 - s1*t3 + s2*t0 + s3*t1; + q.at(3,0) = s0*t3 + s1*t2 - s2*t1 + s3*t0; + + return q; +} + +// dq = homogeneous2dualQuaternion(H) +// +// H - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1] +// dq - 8x1 dual quaternion: +static Mat homogeneous2dualQuaternion(const Mat& H) +{ + CV_Assert(H.type() == CV_64FC1 && H.rows == 4 && H.cols == 4); + + Mat dualq(8, 1, CV_64FC1); + Mat R = H(Rect(0, 0, 3, 3)); + Mat t = H(Rect(3, 0, 1, 3)); + + Mat q = rot2quat(R); + Mat qt = Mat::zeros(4, 1, CV_64FC1); + t.copyTo(qt(Rect(0, 1, 1, 3))); + Mat qprime = 0.5 * qmult(qt, q); + + q.copyTo(dualq(Rect(0, 0, 1, 4))); + qprime.copyTo(dualq(Rect(0, 4, 1, 4))); + + return dualq; +} + +// H = dualQuaternion2homogeneous(dq) +// +// H - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1] +// dq - 8x1 dual quaternion: +static Mat dualQuaternion2homogeneous(const Mat& dualq) +{ + CV_Assert(dualq.type() == CV_64FC1 && dualq.rows == 8 && dualq.cols == 1); + + Mat q = dualq(Rect(0, 0, 1, 4)); + Mat qprime = dualq(Rect(0, 4, 1, 4)); + + Mat R = quat2rot(q); + q.at(1,0) = -q.at(1,0); + q.at(2,0) = -q.at(2,0); + q.at(3,0) = -q.at(3,0); + + Mat qt = 2*qmult(qprime, q); + Mat t = qt(Rect(0, 1, 1, 3)); + + Mat H = Mat::eye(4, 4, CV_64FC1); + R.copyTo(H(Rect(0, 0, 3, 3))); + t.copyTo(H(Rect(3, 0, 1, 3))); + + return H; +} + +//Reference: +//R. Y. Tsai and R. K. Lenz, "A new technique for fully autonomous and efficient 3D robotics hand/eye calibration." +//In IEEE Transactions on Robotics and Automation, vol. 5, no. 3, pp. 345-358, June 1989. +//C++ code converted from Zoran Lazarevic's Matlab code: +//http://lazax.com/www.cs.columbia.edu/~laza/html/Stewart/matlab/handEye.m +static void calibrateHandEyeTsai(const std::vector& Hg, const std::vector& Hc, + Mat& R_cam2gripper, Mat& t_cam2gripper) +{ + //Number of unique camera position pairs + int K = static_cast((Hg.size()*Hg.size() - Hg.size()) / 2.0); + //Will store: skew(Pgij+Pcij) + Mat A(3*K, 3, CV_64FC1); + //Will store: Pcij - Pgij + Mat B(3*K, 1, CV_64FC1); + + std::vector vec_Hgij, vec_Hcij; + vec_Hgij.reserve(static_cast(K)); + vec_Hcij.reserve(static_cast(K)); + + int idx = 0; + for (size_t i = 0; i < Hg.size(); i++) + { + for (size_t j = i+1; j < Hg.size(); j++, idx++) + { + //Defines coordinate transformation from Gi to Gj + //Hgi is from Gi (gripper) to RW (robot base) + //Hgj is from Gj (gripper) to RW (robot base) + Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; //eq 6 + vec_Hgij.push_back(Hgij); + //Rotation axis for Rgij which is the 3D rotation from gripper coordinate frame Gi to Gj + Mat Pgij = 2*rot2quatMinimal(Hgij); + + //Defines coordinate transformation from Ci to Cj + //Hci is from CW (calibration target) to Ci (camera) + //Hcj is from CW (calibration target) to Cj (camera) + Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); //eq 7 + vec_Hcij.push_back(Hcij); + //Rotation axis for Rcij + Mat Pcij = 2*rot2quatMinimal(Hcij); + + //Left-hand side: skew(Pgij+Pcij) + skew(Pgij+Pcij).copyTo(A(Rect(0, idx*3, 3, 3))); + //Right-hand side: Pcij - Pgij + Mat diff = Pcij - Pgij; + diff.copyTo(B(Rect(0, idx*3, 1, 3))); + } + } + + Mat Pcg_; + //Rotation from camera to gripper is obtained from the set of equations: + // skew(Pgij+Pcij) * Pcg_ = Pcij - Pgij (eq 12) + solve(A, B, Pcg_, DECOMP_SVD); + + Mat Pcg_norm = Pcg_.t() * Pcg_; + //Obtained non-unit quaternion is scaled back to unit value that + //designates camera-gripper rotation + Mat Pcg = 2 * Pcg_ / sqrt(1 + Pcg_norm.at(0,0)); //eq 14 + + Mat Rcg = quatMinimal2rot(Pcg/2.0); + + idx = 0; + for (size_t i = 0; i < Hg.size(); i++) + { + for (size_t j = i+1; j < Hg.size(); j++, idx++) + { + //Defines coordinate transformation from Gi to Gj + //Hgi is from Gi (gripper) to RW (robot base) + //Hgj is from Gj (gripper) to RW (robot base) + Mat Hgij = vec_Hgij[static_cast(idx)]; + //Defines coordinate transformation from Ci to Cj + //Hci is from CW (calibration target) to Ci (camera) + //Hcj is from CW (calibration target) to Cj (camera) + Mat Hcij = vec_Hcij[static_cast(idx)]; + + //Left-hand side: (Rgij - I) + Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1); + diff.copyTo(A(Rect(0, idx*3, 3, 3))); + + //Right-hand side: Rcg*Tcij - Tgij + diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3)); + diff.copyTo(B(Rect(0, idx*3, 1, 3))); + } + } + + Mat Tcg; + //Translation from camera to gripper is obtained from the set of equations: + // (Rgij - I) * Tcg = Rcg*Tcij - Tgij (eq 15) + solve(A, B, Tcg, DECOMP_SVD); + + R_cam2gripper = Rcg; + t_cam2gripper = Tcg; +} + +//Reference: +//F. Park, B. Martin, "Robot Sensor Calibration: Solving AX = XB on the Euclidean Group." +//In IEEE Transactions on Robotics and Automation, 10(5): 717-721, 1994. +//Matlab code: http://math.loyola.edu/~mili/Calibration/ +static void calibrateHandEyePark(const std::vector& Hg, const std::vector& Hc, + Mat& R_cam2gripper, Mat& t_cam2gripper) +{ + Mat M = Mat::zeros(3, 3, CV_64FC1); + + for (size_t i = 0; i < Hg.size(); i++) + { + for (size_t j = i+1; j < Hg.size(); j++) + { + Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; + Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); + + Mat Rgij = Hgij(Rect(0, 0, 3, 3)); + Mat Rcij = Hcij(Rect(0, 0, 3, 3)); + + Mat a, b; + Rodrigues(Rgij, a); + Rodrigues(Rcij, b); + + M += b * a.t(); + } + } + + Mat eigenvalues, eigenvectors; + eigen(M.t()*M, eigenvalues, eigenvectors); + + Mat v = Mat::zeros(3, 3, CV_64FC1); + for (int i = 0; i < 3; i++) { + v.at(i,i) = 1.0 / sqrt(eigenvalues.at(i,0)); + } + + Mat R = eigenvectors.t() * v * eigenvectors * M.t(); + R_cam2gripper = R; + + int K = static_cast((Hg.size()*Hg.size() - Hg.size()) / 2.0); + Mat C(3*K, 3, CV_64FC1); + Mat d(3*K, 1, CV_64FC1); + Mat I3 = Mat::eye(3, 3, CV_64FC1); + + int idx = 0; + for (size_t i = 0; i < Hg.size(); i++) + { + for (size_t j = i+1; j < Hg.size(); j++, idx++) + { + Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; + Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); + + Mat Rgij = Hgij(Rect(0, 0, 3, 3)); + + Mat tgij = Hgij(Rect(3, 0, 1, 3)); + Mat tcij = Hcij(Rect(3, 0, 1, 3)); + + Mat I_tgij = I3 - Rgij; + I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3))); + + Mat A_RB = tgij - R*tcij; + A_RB.copyTo(d(Rect(0, 3*idx, 1, 3))); + } + } + + Mat t; + solve(C, d, t, DECOMP_SVD); + t_cam2gripper = t; +} + +//Reference: +//R. Horaud, F. Dornaika, "Hand-Eye Calibration" +//In International Journal of Robotics Research, 14(3): 195-210, 1995. +//Matlab code: http://math.loyola.edu/~mili/Calibration/ +static void calibrateHandEyeHoraud(const std::vector& Hg, const std::vector& Hc, + Mat& R_cam2gripper, Mat& t_cam2gripper) +{ + Mat A = Mat::zeros(4, 4, CV_64FC1); + + for (size_t i = 0; i < Hg.size(); i++) + { + for (size_t j = i+1; j < Hg.size(); j++) + { + Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; + Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); + + Mat Rgij = Hgij(Rect(0, 0, 3, 3)); + Mat Rcij = Hcij(Rect(0, 0, 3, 3)); + + Mat qgij = rot2quat(Rgij); + double r0 = qgij.at(0,0); + double rx = qgij.at(1,0); + double ry = qgij.at(2,0); + double rz = qgij.at(3,0); + + // Q(r) Appendix A + Matx44d Qvi(r0, -rx, -ry, -rz, + rx, r0, -rz, ry, + ry, rz, r0, -rx, + rz, -ry, rx, r0); + + Mat qcij = rot2quat(Rcij); + r0 = qcij.at(0,0); + rx = qcij.at(1,0); + ry = qcij.at(2,0); + rz = qcij.at(3,0); + + // W(r) Appendix A + Matx44d Wvi(r0, -rx, -ry, -rz, + rx, r0, rz, -ry, + ry, -rz, r0, rx, + rz, ry, -rx, r0); + + // Ai = (Q(vi') - W(vi))^T (Q(vi') - W(vi)) + A += (Qvi - Wvi).t() * (Qvi - Wvi); + } + } + + Mat eigenvalues, eigenvectors; + eigen(A, eigenvalues, eigenvectors); + + Mat R = quat2rot(eigenvectors.row(3).t()); + R_cam2gripper = R; + + int K = static_cast((Hg.size()*Hg.size() - Hg.size()) / 2.0); + Mat C(3*K, 3, CV_64FC1); + Mat d(3*K, 1, CV_64FC1); + Mat I3 = Mat::eye(3, 3, CV_64FC1); + + int idx = 0; + for (size_t i = 0; i < Hg.size(); i++) + { + for (size_t j = i+1; j < Hg.size(); j++, idx++) + { + Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; + Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); + + Mat Rgij = Hgij(Rect(0, 0, 3, 3)); + + Mat tgij = Hgij(Rect(3, 0, 1, 3)); + Mat tcij = Hcij(Rect(3, 0, 1, 3)); + + Mat I_tgij = I3 - Rgij; + I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3))); + + Mat A_RB = tgij - R*tcij; + A_RB.copyTo(d(Rect(0, 3*idx, 1, 3))); + } + } + + Mat t; + solve(C, d, t, DECOMP_SVD); + t_cam2gripper = t; +} + +// sign function, return -1 if negative values, +1 otherwise +static int sign_double(double val) +{ + return (0 < val) - (val < 0); +} + +//Reference: +//N. Andreff, R. Horaud, B. Espiau, "On-line Hand-Eye Calibration." +//In Second International Conference on 3-D Digital Imaging and Modeling (3DIM'99), pages 430-436, 1999. +//Matlab code: http://math.loyola.edu/~mili/Calibration/ +static void calibrateHandEyeAndreff(const std::vector& Hg, const std::vector& Hc, + Mat& R_cam2gripper, Mat& t_cam2gripper) +{ + int K = static_cast((Hg.size()*Hg.size() - Hg.size()) / 2.0); + Mat A(12*K, 12, CV_64FC1); + Mat B(12*K, 1, CV_64FC1); + + Mat I9 = Mat::eye(9, 9, CV_64FC1); + Mat I3 = Mat::eye(3, 3, CV_64FC1); + Mat O9x3 = Mat::zeros(9, 3, CV_64FC1); + Mat O9x1 = Mat::zeros(9, 1, CV_64FC1); + + int idx = 0; + for (size_t i = 0; i < Hg.size(); i++) + { + for (size_t j = i+1; j < Hg.size(); j++, idx++) + { + Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; + Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); + + Mat Rgij = Hgij(Rect(0, 0, 3, 3)); + Mat Rcij = Hcij(Rect(0, 0, 3, 3)); + + Mat tgij = Hgij(Rect(3, 0, 1, 3)); + Mat tcij = Hcij(Rect(3, 0, 1, 3)); + + //Eq 10 + Mat a00 = I9 - kron(Rgij, Rcij); + Mat a01 = O9x3; + Mat a10 = kron(I3, tcij.t()); + Mat a11 = I3 - Rgij; + + a00.copyTo(A(Rect(0, idx*12, 9, 9))); + a01.copyTo(A(Rect(9, idx*12, 3, 9))); + a10.copyTo(A(Rect(0, idx*12 + 9, 9, 3))); + a11.copyTo(A(Rect(9, idx*12 + 9, 3, 3))); + + O9x1.copyTo(B(Rect(0, idx*12, 1, 9))); + tgij.copyTo(B(Rect(0, idx*12 + 9, 1, 3))); + } + } + + Mat X; + solve(A, B, X, DECOMP_SVD); + + Mat R = X(Rect(0, 0, 1, 9)); + int newSize[] = {3, 3}; + R = R.reshape(1, 2, newSize); + //Eq 15 + double det = determinant(R); + R = pow(sign_double(det) / abs(det), 1.0/3.0) * 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; +} + +//Reference: +//K. Daniilidis, "Hand-Eye Calibration Using Dual Quaternions." +//In The International Journal of Robotics Research,18(3): 286-298, 1998. +//Matlab code: http://math.loyola.edu/~mili/Calibration/ +static void calibrateHandEyeDaniilidis(const std::vector& Hg, const std::vector& Hc, + Mat& R_cam2gripper, Mat& t_cam2gripper) +{ + int K = static_cast((Hg.size()*Hg.size() - Hg.size()) / 2.0); + Mat T = Mat::zeros(6*K, 8, CV_64FC1); + + int idx = 0; + for (size_t i = 0; i < Hg.size(); i++) + { + for (size_t j = i+1; j < Hg.size(); j++, idx++) + { + Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; + Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); + + Mat dualqa = homogeneous2dualQuaternion(Hgij); + Mat dualqb = homogeneous2dualQuaternion(Hcij); + + Mat a = dualqa(Rect(0, 1, 1, 3)); + Mat b = dualqb(Rect(0, 1, 1, 3)); + + Mat aprime = dualqa(Rect(0, 5, 1, 3)); + Mat bprime = dualqb(Rect(0, 5, 1, 3)); + + //Eq 31 + Mat s00 = a - b; + Mat s01 = skew(a + b); + Mat s10 = aprime - bprime; + Mat s11 = skew(aprime + bprime); + Mat s12 = a - b; + Mat s13 = skew(a + b); + + s00.copyTo(T(Rect(0, idx*6, 1, 3))); + s01.copyTo(T(Rect(1, idx*6, 3, 3))); + s10.copyTo(T(Rect(0, idx*6 + 3, 1, 3))); + s11.copyTo(T(Rect(1, idx*6 + 3, 3, 3))); + s12.copyTo(T(Rect(4, idx*6 + 3, 1, 3))); + s13.copyTo(T(Rect(5, idx*6 + 3, 3, 3))); + } + } + + Mat w, u, vt; + SVDecomp(T, w, u, vt); + Mat v = vt.t(); + + Mat u1 = v(Rect(6, 0, 1, 4)); + Mat v1 = v(Rect(6, 4, 1, 4)); + Mat u2 = v(Rect(7, 0, 1, 4)); + Mat v2 = v(Rect(7, 4, 1, 4)); + + //Solves Eq 34, Eq 35 + Mat ma = u1.t()*v1; + Mat mb = u1.t()*v2 + u2.t()*v1; + Mat mc = u2.t()*v2; + + double a = ma.at(0,0); + double b = mb.at(0,0); + double c = mc.at(0,0); + + double s1 = (-b + sqrt(b*b - 4*a*c)) / (2*a); + double s2 = (-b - sqrt(b*b - 4*a*c)) / (2*a); + + Mat sol1 = s1*s1*u1.t()*u1 + 2*s1*u1.t()*u2 + u2.t()*u2; + Mat sol2 = s2*s2*u1.t()*u1 + 2*s2*u1.t()*u2 + u2.t()*u2; + double s, val; + if (sol1.at(0,0) > sol2.at(0,0)) + { + s = s1; + val = sol1.at(0,0); + } + else + { + s = s2; + val = sol2.at(0,0); + } + + double lambda2 = sqrt(1.0 / val); + double lambda1 = s * lambda2; + + Mat dualq = lambda1 * v(Rect(6, 0, 1, 8)) + lambda2*v(Rect(7, 0, 1, 8)); + Mat X = dualQuaternion2homogeneous(dualq); + + Mat R = X(Rect(0, 0, 3, 3)); + Mat t = X(Rect(3, 0, 1, 3)); + R_cam2gripper = R; + t_cam2gripper = t; +} + +void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base, + InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam, + OutputArray R_cam2gripper, OutputArray t_cam2gripper, + HandEyeCalibrationMethod method) +{ + CV_Assert(R_gripper2base.isMatVector() && t_gripper2base.isMatVector() && + R_target2cam.isMatVector() && t_target2cam.isMatVector()); + + std::vector R_gripper2base_, t_gripper2base_; + R_gripper2base.getMatVector(R_gripper2base_); + t_gripper2base.getMatVector(t_gripper2base_); + + std::vector R_target2cam_, t_target2cam_; + R_target2cam.getMatVector(R_target2cam_); + t_target2cam.getMatVector(t_target2cam_); + + 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); + + //Notation used in Tsai paper + //Defines coordinate transformation from G (gripper) to RW (robot base) + std::vector Hg; + Hg.reserve(R_gripper2base_.size()); + for (size_t i = 0; i < R_gripper2base_.size(); i++) + { + Mat m = Mat::eye(4, 4, CV_64FC1); + Mat R = m(Rect(0, 0, 3, 3)); + R_gripper2base_[i].convertTo(R, CV_64F); + + Mat t = m(Rect(3, 0, 1, 3)); + t_gripper2base_[i].convertTo(t, CV_64F); + + Hg.push_back(m); + } + + //Defines coordinate transformation from CW (calibration target) to C (camera) + std::vector Hc; + Hc.reserve(R_target2cam_.size()); + for (size_t i = 0; i < R_target2cam_.size(); i++) + { + Mat m = Mat::eye(4, 4, CV_64FC1); + Mat R = m(Rect(0, 0, 3, 3)); + R_target2cam_[i].convertTo(R, CV_64F); + + Mat t = m(Rect(3, 0, 1, 3)); + t_target2cam_[i].convertTo(t, CV_64F); + + Hc.push_back(m); + } + + Mat Rcg = Mat::eye(3, 3, CV_64FC1); + Mat Tcg = Mat::zeros(3, 1, CV_64FC1); + + switch (method) + { + case CALIB_HAND_EYE_TSAI: + calibrateHandEyeTsai(Hg, Hc, Rcg, Tcg); + break; + + case CALIB_HAND_EYE_PARK: + calibrateHandEyePark(Hg, Hc, Rcg, Tcg); + break; + + case CALIB_HAND_EYE_HORAUD: + calibrateHandEyeHoraud(Hg, Hc, Rcg, Tcg); + break; + + case CALIB_HAND_EYE_ANDREFF: + calibrateHandEyeAndreff(Hg, Hc, Rcg, Tcg); + break; + + case CALIB_HAND_EYE_DANIILIDIS: + calibrateHandEyeDaniilidis(Hg, Hc, Rcg, Tcg); + break; + + default: + break; + } + + Rcg.copyTo(R_cam2gripper); + Tcg.copyTo(t_cam2gripper); +} +} diff --git a/modules/calib3d/test/test_calibration_hand_eye.cpp b/modules/calib3d/test/test_calibration_hand_eye.cpp new file mode 100644 index 0000000000..d2cef969b3 --- /dev/null +++ b/modules/calib3d/test/test_calibration_hand_eye.cpp @@ -0,0 +1,381 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "test_precomp.hpp" +#include "opencv2/calib3d.hpp" + +namespace opencv_test { namespace { + +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); + std::string getMethodName(HandEyeCalibrationMethod method); + 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) +{ + Mat axis(3, 1, CV_64FC1); + for (int i = 0; i < 3; i++) + { + axis.at(i,0) = rng.uniform(-1.0, 1.0); + } + double theta = rng.uniform(min_theta, max_theta); + if (random_sign) + { + theta *= sign_double(rng.uniform(-1.0, 1.0)); + } + + Mat rvec(3, 1, CV_64FC1); + rvec.at(0,0) = theta*axis.at(0,0); + rvec.at(1,0) = theta*axis.at(1,0); + rvec.at(2,0) = theta*axis.at(2,0); + + tvec.create(3, 1, CV_64FC1); + tvec.at(0,0) = rng.uniform(min_tx, max_tx); + tvec.at(1,0) = rng.uniform(min_ty, max_ty); + tvec.at(2,0) = rng.uniform(min_tz, max_tz); + + 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)); + } + + 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) +{ + //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.05, 0.5, 0.05, 0.5, 0.05, 0.5, + R_cam2gripper, t_cam2gripper, random_sign); + + Mat R_target2base, t_target2base; + generatePose(rng, 5.0*CV_PI/180.0, 85.0*CV_PI/180.0, + 0.5, 3.5, 0.5, 3.5, 0.5, 3.5, + R_target2base, t_target2base, random_sign); + + 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); + + R_gripper2base.push_back(R_gripper2base_); + t_gripper2base.push_back(t_gripper2base_); + + Mat T_cam2gripper = Mat::eye(4, 4, CV_64FC1); + R_cam2gripper.copyTo(T_cam2gripper(Rect(0, 0, 3, 3))); + t_cam2gripper.copyTo(T_cam2gripper(Rect(3, 0, 1, 3))); + + 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_base2cam = homogeneousInverse(T_cam2gripper) * homogeneousInverse(T_gripper2base); + Mat T_target2base = Mat::eye(4, 4, CV_64FC1); + R_target2base.copyTo(T_target2base(Rect(0, 0, 3, 3))); + t_target2base.copyTo(T_target2base(Rect(3, 0, 1, 3))); + Mat T_target2cam = T_base2cam * T_target2base; + + 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 gripper and the robot base + Mat R_gripper2base_noise = T_gripper2base(Rect(0, 0, 3, 3)); + Mat rvec_gripper2base_noise; + cv::Rodrigues(R_gripper2base_noise, rvec_gripper2base_noise); + rvec_gripper2base_noise.at(0,0) += rng.gaussian(0.001); + rvec_gripper2base_noise.at(1,0) += rng.gaussian(0.001); + rvec_gripper2base_noise.at(2,0) += rng.gaussian(0.001); + + cv::Rodrigues(rvec_gripper2base_noise, R_gripper2base_noise); + + Mat t_gripper2base_noise = T_gripper2base(Rect(3, 0, 1, 3)); + t_gripper2base_noise.at(0,0) += rng.gaussian(0.001); + t_gripper2base_noise.at(1,0) += rng.gaussian(0.001); + t_gripper2base_noise.at(2,0) += rng.gaussian(0.001); + } + + R_target2cam.push_back(T_target2cam(Rect(0, 0, 3, 3))); + t_target2cam.push_back(T_target2cam(Rect(3, 0, 1, 3))); + } +} + +Mat CV_CalibrateHandEyeTest::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; +} + +std::string CV_CalibrateHandEyeTest::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; +} + +double CV_CalibrateHandEyeTest::sign_double(double val) +{ + return (0 < val) - (val < 0); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// + +TEST(Calib3d_CalibrateHandEye, regression) { CV_CalibrateHandEyeTest test; test.safe_run(); } + +}} // namespace