From 587ad0fad35706ab234a7a197bd0dcb68edc1719 Mon Sep 17 00:00:00 2001 From: Navid Mahabadi Date: Tue, 23 Mar 2021 16:13:13 +0100 Subject: [PATCH] update: use avialble rot2 class --- doc/Code/LocalizationFactor.cpp | 7 +++---- doc/gtsam.pdf | Bin 826123 -> 826064 bytes examples/LocalizationExample.cpp | 5 ++--- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index 623aef8cb..d298091dc 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -8,11 +8,10 @@ public: Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const { - const double cosTheta = cos(q.theta()); - const double sinTheta = sin(q.theta()); + const Rot2& R = q.rotation(); if (H) (*H) = (gtsam::Matrix(2, 3) << - cosTheta, -sinTheta, 0.0, - sinTheta, cosTheta, 0.0).finished(); + R.c(), -R.s(), 0.0, + R.s(), R.c(), 0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); } }; diff --git a/doc/gtsam.pdf b/doc/gtsam.pdf index cd125a71352875ae3cdd164fd6acd0a3583034c7..c6a39a79c4f06bb23d4b8b26d53ee5dda5912389 100644 GIT binary patch delta 8376 zcmajjWltQ;*9LH8ad#+M+`Tx9yB8?Mt++#RS=^&KOjIN1bOUGgmq1u9c1|z{A_HiGcrA8q@ZPs8&@#Xux%_&ae{b27-kQ!S zfl{eaDvFQ38QVQMg1S;})TZI63o;&pTS6MK>r8qlM!}!K?{bm6gB5E}N#g4o@OH0~ zOfa>@j&GGulQgsMnom8OA(P;k?~1L@nwPzIc~d&c%Uj2qK37lWe?)s zXx_1vmVD*V->Tq*93e~GfwB(QM@2~+VY6S%6i%0+4F zXx16BZUI45mIw}|?35W$n)v%&cC*e9^TH5A8-hDTJeUYc?%PX;+kuKT4#TO5JIhIc z^jB$@K4hbaOoX2mbxL-{`2NXl?V!9w-}^q5Tg~_M9-bhJi_P-7`qkfV5yX~-@>0{o6K#n z`GiqT7tZ^v@DA|!M_KrESieExWB5B=xSmPAhRk(IeQAJcc-MXLFH8nr5J=SGFiV83p>{JkFn!2v|Ob#z#~@PAQ4YvNY~FSdzM81jLlF{i_fFe~?! z=6zphwny%J$ZEAeUOnUT=lsWClc^4}Q5H^iM1XkPKkHsKaz36)pxjLyw(jB16J5g7 zexzl_hH^|Qw}@k=R*cpd2X^P#G%HhDaz{G>I0D%3aj#_nz!nUjI7B+(ux`ZOmq&L> zHY%jtZ2{UE*MCK}1t0*&C<)DOVp9R?l@V2-A2I2caR36$QPavK+>l|Png+z@F%>B| z@fjSP7WX+)u>VzLyBTFKMJO^pt=4{WxLPXV#A7du(Y-{)M25n_E^k|<$tslv67d~M zx0vOcY-1|a6oSR4e-5ogO*f9mMID}EUYTbc&83w=As3P-4a)kkNxvl%m-i7|XAl?@f@rS!&oGjIpqXl_3eDa3+cC8>Rfnmxkq}a-_ zV26<|bw~qkFW^8Iwq$R zuvFxsKtV&nvQ!zFTZ%yP5JeE=)Yx2hYXS}x;;}%y+*q3aGXk;FKlrRNLZ5qHKGjKb z2R1~wZ_Pum3ccG0*a%p=C-SpoDAkOJ8zKG$bWs0Q2hkadEi}^X)R(#nMRREiihUmr zM~O^ilSTw;2CBI%u7&pO$yl!^$AH7h&`e2YP)lhJm~=Ux6_Zn=Re?3+?={88Ig}W! zJLGi42fwIfB($xy=S!}&f;LAbfu^a3Bw93vj4EFXX4nTyuv6eHVxVJyS!T#LF^k3H za^b(PLH7(?)a_$uT(vV*Q>T{bc^FyVt_j6?@Z3@KAcI{E`YL zopNX=P~?~^jC5`9 zbXRiL*=AwT_x#6}=zCyE9Gg2usb1Z2U!S7UWl8jqNxLPL z=DQ`y30gdn141ODFD0&BJ4edaY@Yzeu}`Hg3AFzXRvC;NOnQJ~N=X*afiyb`KsGDh z*&Elx9*#eoTNHkBPY093QYU`BDAfRb2*9jigoLdHNZ(@r_yQ*q|}C7J%aCiE{(!oeq}N!mM4BE*C_nFr6zIAMM!1z@O3gcVeKGugDSpWL{Gc_Dxny>NRODb+gqVK+9hWRJ;m zap9E^cM!oXILRPId7U)Mt6In(@s33y}YfhMp;#pc&UHxcVD(mYn>o zL!=6?&e|56KdZr^>H)1C?FibUL#-$Wh!&f?@k0|c$JOgx&V3aXK?Q7fK!`j zM=ka2E^B9X3e}isg*;MB@Cr|q=nzieU}IMo7ANxc+w@_&h|IILuI%k5%g-6N&C~)1 zpN3A)&1W(^p;LYH7&8uAm3*~sG!H7J$?&n5oNi5)9-9|$6a9*p%+aPc@wC}LqtO*| zCD&_e9Hnbitq2^;Ux89#*YEWD>|AwiwaCUe%bNCSa}ZqiBUkwOC>QSvyWDUzRfxFJ zewN<3B9{b{^WnOVXLAp|Ol?ueb8l@Yt%U|60{VdqihX}S=kx!SBkYy1*e!z+|0N&k z%QbK=!Po8J2d`%0Pyhales}}oLfu;mF4>(1K2*BOLpw<@{{YF-ZxU!yIRs`-(O|-( zoO&Dedm+sNgt)|`1v%0GrVrwGx5*keAW*JqHp(_>9(>z)`2zVCQ?Y>QFvh-JWL!OK zzL(Y6cizzU`M^6)?DPHJ!QAqy1=Qy~u7qMvoY(4DuAf1QiG@P%E{!?`+S+Xdi)Jb5 z4s+wu9?b=W3NE0c^-Xij)CH@~(pRehXxPuTa}D2kr=+}^twh#a^vNWjJKa?2Yw(1H z8qY(pBZoV^86YWc!oP=@(3$~`6{=2%vlgnCdi8C|$IYe{3iNe0h>Ms;F+Y>+VH}2T zSBe%@x`p|QUk79NXwR#QU`Uf>s8wjzNFFn^hx5hF*P{)&g0)KTJ{ zmGQ_TE5#lWMuk{>LL_Q9%+w+zaQk}rhSp{6eqTNy+|s%&TL912|AK7`yF8xKu&@ZF z88NXqH8E|wO@;YbF8nn+82aZW<}2_aiC2g4b$~v}d?*jvrND zyDF7r0UZtMu$%9hY>;EHbCx2^xiv#y)Mzjq%HuBzH(5MX=_CuHeW%DnSt;Fg42fbZ#3 z{FbWVUHls6Sgcr&1ugx3cC!=U4#SPG#~VSmt8)j6?R(Y^;rHO4CRAK9%{eFgniC&b zEYpKwGBqa>+D8>|GEWUh=(KCGdD!rVkOrfs<-U^ZhOw#VI_@=UxfO6HF@U{;ByZ^~ zPOD0FneJ&AFyQE`*e>axOG36OydCYFu`~39YDCd>toYpf#=-zlTahe8;m{y(Or(Fa z48JsIFZgVR#~0x>7f9BeY`x;8KF}tl%Tks#ujpu4v2IBICPJUYlvo?)jic&Qp`)G~ zaKI{- zZh@;}=wP@w{=N7aK{~oFdyRO0kgDx^wpr>xCxjyd^VOdn8{AfN%W~w4si?`hW|Lp@ z#lPCLb#)@}QAmy6tw!dp{G`SBwBvlW19b?${Y$zmT)Ild^6}$zf8@9YQ%%ui!u6*o zDG`|!tsF`Ekuy<8W`kVI=%`!gcI0H5QiA#S_KuM`V&((1cTf}WpDMY<6$ABJ6?HAU z*#pgp2G`ler;U}@urW53DI(&lzUI#v5tg3;e-b{cgJi@7^G*91QKD`DAY@qy7%xU|3Cfx?8L9?6YlL+y5$hN~7;8_p?snH%b2OfH3Yc%JqitJq32R%9^TDA8SSk?Psjb*fSB+3EH%+Gk%N-D zL`CJAOax zrwWtYZh1U7!_Ad!NQv)b5PAwC3E%5dndSGlhcBS+5x!a@3LO(_%C8ZVOc_~Z{&zy9 zN9W=}_X77K*msSJCoiS}Rdx9*i>VM8nY3N6!;UOr$A_nyW`-NcN=>yP$mNbE*6SS+ zkKQ&!NSknuqO~a>DpBVWf5s+KXnGWWZwc+gTBrG`qS%v^ zZjkJYS76{*N0VdO&MCd&ZFO1r$?0p%ND%e|KO=hLy4=RwyMLSKu$)+5iPuoiwK8%v z89vqQeGQ-0>i&n}Ym1;kFx<^c5mj(P|LUWe-@Z|F-z%~n_a2`f(*7+~aF?&rG6_PZ zJ8OuLaoY}c`(@LYm5vsnKG^oM(n8;a>=?Cz^9sJBlWX$26+DR7el+r1`Y^E3YtfVWF z+IW+b_o{Bq#49=YYPRb2Z}Y2+gfrajoTpoN_u%6)L>)$vl1};FVy~X4dXY(YP5A^q2GYh~Nv zjqT+88LPHKWI6v^sXUnSqe?})qgOh?o`)(u24A|3h=7FER6vNY(}{@H9)Zh+OFhR- zFMP!=^uFF39zlHkmsro%L^bH#!LqXf*+a?zUZ4|`EEdAgFVgJo7`8J3qE)zMl5YXU zy1T5mF5MtADWxZlCG z=onnUSqzR)R>adZ!P_q@7fHXE>EFf5s9m0+1}^IP7ZD;jg^?NX!UO&Sz3c>6fA2*Ltf?!FRW;?T7!OR;F5?qAs|2d z!jeGyV~1k_K4kM6)I1oWNi1!GFQTO#bBxz_d)A3&i6pS{F<#+P(plrRMC;iC5$l$v zRUML;%u)ePX-&>O)}k6gTY}5Q!)U;A>X6MMeS5?1n#598D@KXgJ0D$KzV~$gBfMeW zvfJ2H-U4+#a|;7$@QAvomOKWyi1va7C-OpSi(oKS(NY%l8oB)8(NwnKnabw+1j0@o1HtZK{PdQk(bOiuiKfP70*rhuZ$_;N8o(k{^+GJt$t z%pThJx#&N#As-q8OudY3;M|@?lcX`78<(eO4HiY-$hsFU@VbN0fzl8u@kZKI1gn4`?< zX3^gBTK&ZlNC+%@|IGW+=3M+SyppU+}CiRG~CCeEqpbsNDjS_+HTikQ%;6chc$$!r6j_Fbe&3?2C0W@+p)hVr%0Uz^2#wv#CX3 zvtJWTRD3^az@w##xAn+%mZT|>KYk_ca$mghBAJ(Br$nt#KU;IIlVI9IEP10qUoZ2z zLXKhP)ocv;ehuAaDkqg6k+-d>*Ad74`$_00H;&dy(ef|da947{h$R^v1b)rD?-+E* z^crA}MjUW+GCmA9FBprJ9+Zi%RF%hbm1WpdfE{n}ea)yEvK-g6TB@mQXYcFn*v%#e zmjUCsa<}9@Xo@&u?p;hdu=kX8?bDs>+nu1z^=E-^*L(XLKekn_XZ?5ynKN2`3C%qWr|*f~A38EHgE zAk`O<<_E-hS8vtC{p;p~>px^i&Xq$FqU6jl*f-S&oPN3}L}5Od5;lXKtmmS7t}@Fi zjU+ZpSYN4ckUAEw`9X+Dv{z_6#pD>~?R6e7#BY`-_U)?gSDv-DKwsQU*aONa;-z!D zxaw#~hXn5wY&<)m`={etm!ir~;ZuOn#k8IIwMDl8E2|YMMeVFrbyI0UYE^v37!kKV zOC>lbGus;?Lc?HLk=J*mSX;l=kR}ubBVAeirphKjOV;EdIw;_q0Q)TQID&oeOrllVpeAHYrEJ1?9C~-wEn_Kh&bh-V5Qm_}R{#^#9z(|j64-T2YZit-ub{zPn zOS7`U6ofRw33^K_^DuNTB=TX$2*ebpO-=ZqihNyDV@?DjOVH{CnR!@i{#X)qTN3qi zDzG{Mp}NAE_`NZv9GhLFCy&jaZXph<6RJ$zzyD=I_$1XiWTnn5sPp(6`EIS=Q^O%l zoolkZs>9|oxH>_9xRcGoM9-zT!uf6>&A2ReFF@3=5ah@ztZ>Y(@4DcZX{j^%i>Z2n z85{RZEW!S)$F~+?dmg_|IHC_i8$U;+1ePeK#!HL#3pOVZ??+f8L4ZLcjX}x3?a9T8 zf9?rMX1z24&I!;u=FvDUjQ{ax{=At6Fn^GaJcTwi( zO{2j60d4wtONXZI88bS*JEl-=@F@A_@APV?jP!5CaEC!AsF>wyq%GGUrKQr(+P*}42 zm={Q(Kq18-I{X#0t(SSsl#_hakBqo@s1AGf&rlEJ{f@V>?T))4wydPO(s%Gj|HE+A z_qx}{OdzHS%r-XZ(>!mVjVj_(y!!=D*&&o8Cq_kJ$ErbFG0Obm5bnqOKvXhimV=}DEBkk8h2wVwg(B4_`bCSr5HM_}_K+hbjQSx24 z%1qvh zpNprtROEPOn3%l195%+TAEDEGD?qwlUAL-hbf$S~cR^M3__(d(;Sbz~MNIfyGFf(> z;0yMR!S40cZCrUhiJ8DAW=qkGX#@QwO%w!2V+sv-orddI4u`#|2daK3)pE1Av4M|W zE}s2#o9*7nmE+nR+A_iRmlo#_P-A9K3!TdS({lLwgm*vokXUGQx0>vST~!cO?(=OF zr+BlrOlQdfwOj7o#PxJf!gF7i!X6CP9((aI?;H+3-UMHqX9`Iyj z9${;}n;g5tCJtqOYI!TV8o9al30k5(#3PYwpD8}%_B`}=jNy;!XBhbFoI4(Zo9ib# z8i32y0`}?68t+EC5q?vjP&6{Ud-CK$)2FnJF5lBih8Ew--T1^ zG^*H%L5(_&1zW{0#OnVdc+qn9lhU1Kt_+TF+R=<;9IY4kOkM=pe@jDKGz_t9Ef3_G zcwJmuZLRmQRo~6yR_ENlxv!DBGkLFKC<8edNP6~H|EgD<>&k+FPkvj-APR3ixgxrx z(>qk&x2z+KU~Jz(@oZta8>so5p4_B%+&-$R|C*1y+A$c;-YH-plO9LLH&d3i7?q?^S)ZVD;xLLcPJ){ zj23KATHX)#&lez;Sm(_L7Cw1eaYLd)Z`jYw|Du<#%89|2^(irt!0dfizZ&Dm2no$Q zIbhQ~_J&MFkj2(yfoeB~{Qdy;qS8xJ>s?zKMxcCvMY`7V1wR__A%iy{HI0&L=P6-Jv2|fuRh0y<7qL0j`Xl8Hm(~^RZgCC5^$^fF7*SB0WBOJOug$PZpet2rf zaizCdT`ZU>*j=a{wkcPnvbiW{TCv(-hEPOWXk$umt05L<@!g1CZ_A(LCs0xsgEv(% zB;)C6RHA5Ggc|oplA6vInzs?EL5P+S!7Za|sDKCRi5bhn zGGkD#?XzP&O$v;PD69~=ruHQ{hZ#2a=<{k`CThhrwXTc6d(M(ETNg{=cyP%TE^K;9 zR;X3IZn3#2*ExLY@w9jwQs_2KR)a*k?O(`p3kXNxhpG4MPJ11j)1xKocp&RaI;m4X z+^WIRZ&E!{^{g-pBcAaf3uD&1ZW0glXQR$DS>1Ads4pvFu*XR_5)pF_G?N|id@lxy z+Gr?dMYOs4$Wb}u_8ymjU9atyIV-LR zcz~dJdZRFh_z|7_PR+JJzZ#z}_Li*$QAawDW88@QA%vk7>@a!N5)ZeL^$!nm5xVXs z17p?dZs)4?-p0vi2Oow3)aLnIRIx-@6NC9?hH3S8&bucA{Yhn5Y0c2b-{$3KoaHzJ z6JzGm7%zd*&~m)!1!hZoq??Fv$3<1U+@SC#u-l=YkdFBIy()n6GJm<{lk#9@v^U;x zhstk1!K6*6OBMS5D9Up6{I(}cs!=^jhrp6i7QY`6m4xU1=KI#ZGuXOuLSS3E^GP(v z%FrgpEia0L28OQfUdq%uxupqyk9?iaA5qQE`c*Aok9NXksS4Aj1P09_XT9 z%N6Dle)eePn<-MkOuR z$fkB}uTW3jJ5mR>pqo<8%FGdL>P7JPc+sI~RJP6Rf6j@7d_-0(md!slmT?iwJF;1FCtEVu+-oCSgf_u%d>i%ZZT!QI{6Pfndb z;9NXazl*u(o|@{K>WK!f$VRSe-atey0YSd_oFztp?7x_$=8LCS^c*DkCy>j-`$Xka z>B()jA;UPnrCeEOhWbYLiVW5hMQ8UW**Bjs99Vm;gC)yHBwn}(9^*%2uG=NKM;tNVXRZMDNqjQ}si^F4c6 zdv$zLdNa$OxKX3Z`b>ApLYonr;O)1j4MS2X7PJ1gK+IyM*%(VrmP40P=M{7y zXgy)a$g!q&jdf!M_B^YYJQHH9F<=$|HyNu37@p>OU^Jp5i{vGdAnQxS(5&)2wNoG} ze5Y8t);Tf~j4hEvg|9CC>~R&+=Xrb9>&-%7Dv(AGta3bWt7-Hpnxo}*T*?cA6K0=v zx`#}-M%<*wHhQKDd>7}7%(uVjr?$ z1WH;*9{qZ{CyIi^+}_6CJYHQCm*9RXXFwrywbBoRng<1ct z+kha0G~|r>XO^+lm#KH(;0B*e$a~pM&1H1?EzRet6!UZ~vz3;<$k)(-n)tq+zX-AY z)Fgvq8Dq52IZXO;ownh09Vp=_fCY0aQ>Z2Tc-(G6H;O6+WPYM6M*$+kj;J@%xBJwN zK45v%RlZHpUym^8{_k$Lz`Y_fW3edf0-5D#Q*lc$(0V6MH&CPw= z;wYLyWvF_BioDMzo+ZdJ5}P;nQS?mYSYFM_`4IJOtlk)K2yVfto=WCv`diXG8HaD- z(4J>qWZAZbI;OWRJb)}Kv(X`vxk*e*^`PKKeXX3I{bM1}NbPbW$aLd!Nzrrzt5=Q1 zm`u_ToswdUH6YfX$DnQWs+N><{!doaH(iID3?R*CHa#~f5a(wn?sX`bFT^^LL zF6%qLEd(G=IEof%Cyr01DL;U&Nbd4D)ru&Gc7dR1-jcuP0PGHA*`@s8=~^v-7!u(Q zt&W8wB4AAZ0s78@l=oQK^erf?>Tk0$83gy_Uo+Oo3wSQ6tQQ^A7DC{JS1=c%(0%o% zA0NkM{O?T42u9l5bs(A|<{Sx}2}Pxh5<(!*ziR`NUSV6C`?~q3bu>&stu^O59dSmA zMmixwMf?D!_yeNhR{5;bV|x4_t}#kQF}Nv5RWZXJK}E5DFJuTJFcjOORvGLyQWQwl zc0v32$o{jg%&a5q+=23YPr$2_GrAUni|#5sJjwwW=7>$dovB!KcAeX~;U_g2j#gF1 zwJIj%v}Ph zkI603(xydKE6?daGhlv#J6TgzNYRnl4?z-f&uA+7Q}B=~V`W z=f2cx23S2WeD6lhg0<_@_eYftv@?B7WxN)0E8BU9dEi>je~Y-O*jE;YCh_yb;&I;R z{1s+nkV#b)_$qxlf><)5H@5n?K+w2IBwIRSEPk}oWu79bKZ$N=8)0!XK|i*P4#009 zVC)>vimnl~e0>r$q!XQY)uQvaGHBUoP6=vjTTLCF4x6dP?A>UvId5wL)s2h#CfD)E z)ZZYsI&1f;)oHfcxlb$1|I{rM*Dv-PvvUJJ6CoI#Xg>PQE<+=EI+mODwWM16e-n!; z`yP1sj5L|bCrsLnHM_$U*^g%E2{|16lyd`D`h&* zOqR3}*oW@cycB|oOHr(L zI6|O7y~fRO7%N&LewfjTj!?qPp0Ed-Oz%#Z0ul% z^MjaK(l)F}9?B=b=-*!H23HH%2!!dsNN}YLEmFZoc5#kv`CEb_LIU4f75U6nm4&UyMQn|^V1~}X+T*75)QM+(j$ZmL@F}i* zO2sAfwarAEMD7ulqvz-CEnV~0FF}S*>}yb!BhzVUF1l|}c@Ae^2u}4=7@~|qz;vE5 z@=3zlnGx=FCM_aU8a5hv>4;d~i&D%RyzUSjM?6O~1fGXKnbQxG3D8@KUuwh!Iy(cW zI~3wk2Z+$lr`!uwTq>vYRkoBLjjgzN`lr8rj6+v^IDMS+Ai~hd?|X`^A}!+fnm|P) z5@)H-?)r&R=8wYur}^z|WM?4~qaZM)Kki51NLkl!D!jD*8Z@@#?(_wf)vteurWSQG zdDB_!4{~7R)wqBDngXd+b+tofT#WlGg|@X1YRN{O%^*{MmU>prf?8 z7PG-LeQjtmV?UzANiSHFFP+SIFj~;$%yXz}9x7Xp@1uc6=4Tth+!HD70V8qc}@ACrqgKTGhc(Fk2^J{o^5}jLNw~3YJfx+tlm> zD`!%&0tL$#)Sf9^JECHUv0x7$qqc6i63L}*o#NDwH2yg+ZaR?1a|HCzBYw%+GzII` zi#D94BJJr1qStf`Nw`-Qd^C@wKQa^dCnP^8A@@8)ZE}0Zw_X0@`99F!Cb~}17<)wm znc<4)*NEmlYAIicS=BP`{f*LBDrMPoo}ix>l6oOc0U#NoNTFH&hDE%tk~u@sDZ_GP zxHw2g!0S%9x$1qZyQ!DyuhP=0D40hh_S0Jb*&6*4SDI=pO-H-yg43#Is10^Mz)Uy9 zxsPrk0xsRcyC@72u(zx%ZMWJMckG?xvXMHY@d>aeRUoQVU~E8igG=kdNwOBo__wu+wpRPRM^6t2J+}eB0+Ez zvZF%Kz2m?odFGV*$zIg;`dopI?8rP$842PuAgJ=@PHeh|Fu;@J2C9pyRceo_v-+B$ zc2dXfbjg7$;B;T5-F<%Xl>82*(4t!ctP%%3_~iC=qd$@+HcO7MU022!R{RPHTuoB+ z^WhM&|IlmKKsar z+9>w?WFZ&KrW?xRjE*-)O*OMa3G4I$=+FiwZjO?-kG?mkrlHZ&QESg<0kx7@PHjPfMClI8MGy)+^({*niQ(WQ6BH}yad?`a0!hem9x z8gkl|7+;S2DBq{~Ih8}Eb0kiQ_=;)q@n8*wZ23*kKgAli|#Yg%=;u#xY;_Ps6Lt zbS9OEXYoqo8$!2Q*E*5~yJ`;HacI+iV_Q5Ba>Hta!mZ4WQR7tn_n02Y+pBLwOg4nW zB|SBlJDWTGzU$2#qrF{F6pyj9Zf+Fk64Z9&P8=<}LmnA=%0;=?+}(B=L7B$Kn5+n+ zU9<>A5Z`tvP!WgBI(b%~cSIisMo%hu%=x)+_NI4Q=4P1kOxIQ^R6s`R3z>`d)fv62 z{7GzZF`?C_9J&cx#_E8)iU&XEO67RDaB}OXoRk-Q3lJkDDD+l>9emh!k-jOb#3qCN zp9@D5A^sg9{*{^MzTt0_zk}rt?FPothY`E^ZESOzLKiCp9@nAGI;z!hRVK;H^qA&K zSuT`MOp}OeCa91lF^O$DTeURaUja(GrRxle+Vc~jV!Hnhdz=BR+va^FVG^mQot-0b zK)I^PvtSqXJAo4{or!&^++hIZ=AJmSRcYe(n~+>IuU=%SCUVJipdgn4oaBTx1W>>rziVTy#lFGP>~cFkN03uCVm-T zU_aQew3c+5IdqGp6VsYRZ~^$k+WKWq7xrI#`|=aHF)hAlA+6OPh|xClHBV)QXbQ?A zE%RGX?a)ug)mvGe1#S2%Y$MK@4pZ+MR&>(W74pC6eL(t**WgGQ-i^w9D*mMT4nbEh z2f3E2$}$-}&xOz$?|{X}eTC&fCy!y)-6|%5i^3I9*q%&C4o#VcH z8Z_eE5%+PsWc7U32!aEAoY>^+H$dphG6v_TN!qWE+ptppq>aFKU9Eoi zJ8BijC22WWgHPc(jlYnG-%`whoozDTFQi>ILaH+v!?V-d3io^au7`WSa`9n_uXa?| zn?vn&gft>oM17RI%WGCx_f1!faW5$Z`sVM4_FV++7$2Y3zpjc$7xxLin04R;h>yOP zdIHRiIFQtGfS&SqlzFF5>97?_iD5W%k8RD5>+9Gft>L3^v($j?yIIag=zxy%2jxH@ zMr5{vs(lJ9G3R;i%Nb)>3gF%iv_yD6#y-YT5ldhP8rFHNb+K79aq;jwZGAMd0Q)N1V)T|Ul z(C9b5fRu(Wr79(-5`(N2iXx@K?es>>DstM+E}w#t7Zkx`uJqia*v66IDEFUrw5-2J zJt{?|%~x&+2vvzij0ZSn4MZJ9Hx9xg zVqntzRR}d4#2zFEPEN^{qzdDR0kdfvQ%ke%lGfqJXoW27Ck32&6vPR#aedjXCiR7Z z%^G44n1O#Z=z$Nw3#PJ=(Mrz}5}vVg|JLkHl6z%QLyh!$BDREgkYIlvO83|T79Ugx z^P13czJG)ggwmzq;&1ueuTQWE@%5ViI213R>S~Z=@%VIlzz+Oj- zAoUh6vcgKmJK+4aM04`a;fm~;e%7J3MW48!QRTfoGf-ej^3Dnk-^O~$s`%6T3@|yu$6mkeaj0JMj-gv;59~|wra43_>Vm1k)Tk&);G%OOH^#`lGpsV2?!s;gZ^7zNvy zKxD^tB>?-}W9v@+(vh#|#Qdvcc#_C2;>205vU*~9j_Eq5>Jfx?^1I48{ zopca_r^mg%oFEjF4f?y5pF&Ss+}cD*LM4(Ztc;8UM5#O>*bK7XzWqql}OM3A|h*s9u7=?mr)7>p_s&HU;Zx$;= z?8{oE@t)!jwa24jI7^Si=SN{oUq5eL1D3=H3-|f>nOXdiHj^S^%TP&0l%4|a4*F|n zR7_f#p}!bDUjPXk_MLEdX$+ZrmQMug;Efe*JxxtB4xEuA?@}^M@~`gRQTUa+KYKXL zi!t-7@X?sW;tW*q&cnZG&zR#3BZ<}bbOR3E=-&*V=g+<4r~AT*%DeWjKGq-O+|yK{m69acfM8BMksaH=%_CKfImL(@hl+&mf} zr}Dvp#83r_Gmhlo+>=-`LsL1s zT?#u3)oy1p%83^&c)d~xehlP$2rld7q5hgKY(J}2Nt^8v9e{QQ_|4MGgTB9$kBjW~ zzw)A~kL&4V_tLo@C*J}dNuJViwcxb*#Eqd%pcv=E?{u|J9V-*H7m=7+MqN~t39GwO zVq%>Y!N%AfdZvbcPjXvE&(C>6+hqqQ_Oz83zSKA(i~Gxd_21HU?YOPC(hYoF94i;)p-9r%i5SnI#^zWq&z8FK0wKM$<9P=3FgX?_f%&d*VHT;)=Pq}|| z_Uj;uVq;$|9|?vaSZQ=0Olgm@5u2Z=;~I*@we3+ZL=OKlXTvn>9fYrv%N@*xW86Xf zAD}?{d7jzNi}@!eUzrA%Yk_q_5ZJ#ui5V zRG3k%*>TTAio8_i$tDYM1Yzx?aR0#ngYXaHKS=)||AX=m>OW}zpf|OTV(<g*kdxu%mgAI?=I7<(mf`1@mXhQbqZ0Xl zj~Jq}f3$G0a-Un#2Pacd4mUj%-5jYeL>P6tHTJ^f&ev5Q6l! za}8MR8L3!}iGG4qc#e74IB5I&g8P;V2e4SK6Z?>e@bi#!$!IuJJcE$nO~K)DRI$xd zR{(!!hSm7PIKRuTprz&8&L~&FP`Rz(XOzhz;vM3X_>M;w`pa?*=USj#P;}B0g%m&X zs}hSy+PpKk=0V?V1>sYy=0Nx>`NuBWB8hSh;{3J z%_yfKrR95f|9)pYzaMvHGtep~7|7D1*{?;8cbfWthUK7Ha@T}RN^^wWv*e-Vq~2rZ zwM?=oF9ZNo|d+t7=4ucKVj|kNg~)W zU3t~SltHo+SQsX7*OFDopY%oA5N&Il#tXImoCG*uOg8UbH|Gy@YLSzMSH2+|vWsp&g zJLVmy|C!b@dL!14>egByVwWKgxfc&LA^XuKV3(XJo{|MC?3+pQG=&_;zFe#K!K#(r z4@RIooOzifZllH4e2f+MBY50+;>&?b3G@yUt%MCTxGl6KvcrYY!r>!dPU?d>SEB2Y z7AV%I*p4=eB1r(2*(kiXiObk9CyiU&K5e>ZD?s5PbgKF5PV95*cWT&{rPbz{=0z^U z5zE2AFjU=wEtg*^-WlPg(BLgA#Xih($Ch0#zmd|na6xq`lWg4p%TcrAW=m(T@cHMN z%EMwdu1&57!x~Hs>~^i1QuSQxQrI)&%Q=bkWuc^Y+7vj`+IS2|DdxF~5&#rq~_f(%8jWGVsmQ+mZto_;)H>{bFOC(iIQwjkl|V)urC^YYs?xhM^j5`&k6haa7ePF770{eJ+>)-P)S diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index aa8e0db19..d9b205359 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -92,9 +92,8 @@ class UnaryFactor: public NoiseModelFactor1 { // Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix // H = [ cos(q.theta) -sin(q.theta) 0 ] // [ sin(q.theta) cos(q.theta) 0 ] - const double cosTheta = cos(q.theta()); - const double sinTheta = sin(q.theta()); - if (H) (*H) = (gtsam::Matrix(2, 3) << cosTheta, -sinTheta, 0.0, sinTheta, cosTheta, 0.0).finished(); + const Rot2& R = q.rotation(); + if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); }