From cafac0b7a1b5a73c7b2cd95288a3b3362e1fbb09 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Fri, 8 Jun 2012 08:32:07 +0000 Subject: [PATCH] VisualISAMgui with figure for 2 datasets. --- matlab/examples/VisualISAMData_cube.m | 34 ++++ matlab/examples/VisualISAMInitialize.m | 10 +- matlab/examples/VisualISAMPlot.m | 22 +++ matlab/examples/VisualISAMStep.m | 13 +- matlab/examples/VisualISAM_gui.fig | Bin 6094 -> 8272 bytes matlab/examples/VisualISAM_gui.m | 264 +++++++++++++++++++++++-- 6 files changed, 318 insertions(+), 25 deletions(-) create mode 100644 matlab/examples/VisualISAMData_cube.m create mode 100644 matlab/examples/VisualISAMPlot.m diff --git a/matlab/examples/VisualISAMData_cube.m b/matlab/examples/VisualISAMData_cube.m new file mode 100644 index 000000000..3f4f015cf --- /dev/null +++ b/matlab/examples/VisualISAMData_cube.m @@ -0,0 +1,34 @@ +function [ data ] = VisualISAMData_cube() +%VISUALISAMDATA_TRIANGLE Generate data for visual ISAM triangle example. +% Landmarks include 3 points around the world's origin on the z=0 plane. +% Cameras are on a circle at a certain height, looking at the origin. +%% Create a triangle target, just 3 points on a plane +nPoints = 8; +data.points = {gtsamPoint3([10 10 10]'),... + gtsamPoint3([-10 10 10]'),... + gtsamPoint3([-10 -10 10]'),... + gtsamPoint3([10 -10 10]'),... + gtsamPoint3([10 10 -10]'),... + gtsamPoint3([-10 10 -10]'),... + gtsamPoint3([-10 -10 -10]'),... + gtsamPoint3([10 -10 -10]')}; + +%% Create camera cameras on a circle around the triangle +nCameras = 10; +height = 0; +r = 30; +data.cameras = {}; +data.K = gtsamCal3_S2(500,500,0,640/2,480/2); +for i=1:nCameras + theta = (i-1)*2*pi/nCameras; + t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); + data.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), data.K); +end + +data.posePriorNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 5.0 5.0 5.0]'); +data.odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 2.0 2.0 2.0]'); +data.pointPriorNoise = gtsamSharedNoiseModel_Sigma(3, 0.1); +data.measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0); + +end + diff --git a/matlab/examples/VisualISAMInitialize.m b/matlab/examples/VisualISAMInitialize.m index 4ccab5f5c..da0bb735f 100644 --- a/matlab/examples/VisualISAMInitialize.m +++ b/matlab/examples/VisualISAMInitialize.m @@ -28,6 +28,14 @@ function [ isam, results ] = VisualISAMInitialize( data, reorderInterval ) %% Update ISAM isam.update(newFactors, initials); - results = isam.estimate(); + results.frame_i = 2; + results.estimates = isam.estimate(); + for i=1:2 + results.Pposes{i} = isam.marginalCovariance(symbol('x',i)); + end + + for j=1:size(data.points,2) + results.Ppoints{j} = isam.marginalCovariance(symbol('l',j)); + end end diff --git a/matlab/examples/VisualISAMPlot.m b/matlab/examples/VisualISAMPlot.m new file mode 100644 index 000000000..0459ee201 --- /dev/null +++ b/matlab/examples/VisualISAMPlot.m @@ -0,0 +1,22 @@ +function VisualISAMPlot( results, data ) +%VISUALISAMPLOT Plot results of a step in visual ISAM + hold on; + + for i=1:results.frame_i + pose_ii = results.estimates.pose(symbol('x',i)); + plotPose3(pose_ii,results.Pposes{i},10); + end + + for j=1:size(data.points,2) + point_j = results.estimates.point(symbol('l',j)); + plot3(point_j.x, point_j.y, point_j.z,'marker','o'); + covarianceEllipse3D([point_j.x;point_j.y;point_j.z],results.Ppoints{j}); + end + + axis([-35 35 -35 35 -35 35]) + view([36 34]) + colormap('hot') + + hold off; +end + diff --git a/matlab/examples/VisualISAMStep.m b/matlab/examples/VisualISAMStep.m index 3a0aafb0b..fefc795dd 100644 --- a/matlab/examples/VisualISAMStep.m +++ b/matlab/examples/VisualISAMStep.m @@ -14,11 +14,18 @@ function [ isam, results ] = VisualISAMStep( frame_i, isam, data, prevResults ) %% Initial estimates for new variables initials = visualSLAMValues; - prevPose = prevResults.pose(symbol('x',frame_i-1)); + prevPose = prevResults.estimates.pose(symbol('x',frame_i-1)); initials.insertPose(symbol('x',frame_i), prevPose.compose(odometry)); isam.update(newFactors, initials); - results = isam.estimate(); - + results.frame_i = frame_i; + results.estimates = isam.estimate(); + for i=1:frame_i + results.Pposes{i} = isam.marginalCovariance(symbol('x',i)); + end + + for j=1:size(data.points,2) + results.Ppoints{j} = isam.marginalCovariance(symbol('l',j)); + end end diff --git a/matlab/examples/VisualISAM_gui.fig b/matlab/examples/VisualISAM_gui.fig index 4fb6f6bde9066a3c8c118e14c1764fe498286463..7cb8a50e801fa00d342d1c1d64113c773361e537 100644 GIT binary patch literal 8272 zcma)hcQhMd7q?MWySlVCtyP;EwfCs4YLB9*y=RQps9jpCc2Sff_6QMsZ?#EC5PJnd z68ZYR?{~hxzx&s7$8*m;=Q+Q7fA>5(ibmRsN~~f6LaaK9M*NP>ZVr5``fj#cjn z{}Vz@9h!e9&VPDP-hr8-Cxhyvg2jro<~JJeUJ%i-v!&lrZCcR+jB+|Fv(rA9)8r6n zMO-WvID*@kv#GrGuta@zkk4=RRuyD@HVF)ASl``y_q+Hpori`)fs-RxdTG&BT8C#O zZQc`h(9v=nCMu1B2GO&fZ}~=(N9rZ;#=gjZduDn7Vy$s3vE;E5B%8qvOsigCL9Ib6 zTq7zE+r`w$!`H80tfvcokf?Y|5yxwut>>1IubblRy{2F#KkydI%}xEPWK+sLn(%d{ z%?>T;RPY@2`5A4ULd0dho8(TbAkTHq)siod6Stac0fI!G-h45Ie!fCkuAl#bm>VeX zl}0V?M)NN7y}#mK`VF!We){G(h8p;frReZ-Hn(@0wOVaT%!ltkW4{h^mlv!!$E^kK zh})irua49pU&pspk=*gE_fVIb>6cEQe(BHcSt6?znfn8w;n0s}a}>S z9uty+AxUQpOg-eJt{vCx0tJZcAD>Q^Y+g+!>|>5U6$s`s9%PUYt~3}PsOCe1LR$H* zpZ7=TW7Yd346$`^2=}eZDeMv6>a;uoUvwqfk2k``z$0*jR}{UIPHK#L$&&u7B{R=% zH6eHVZ4f`brzM?NUwx%FNp2VjGm`4HIa`Rq?>}FZpf)(h9Xxq+2-0!1hpFW~aCtx_N!E=zp?JUGrp^&EuNdR<#cIWpRlQ==P4D4a_dWc5!Q1h(?}MC7Du;f$7k!%U!Ae0XKc*d0?eqf zuD0&(EOahN<~aCT#6BV}uOq9qJHCCl*8n+8A#4eMY?i-Q%xZ4Fd3AYu>`N?|YKH6m z-ZZsZ1AQ)T!%k{v>cT#0qa9vIfl9@S_V5~z2}Ar;oHfty)Qs~BIR2rSU4J3&u!XHsRrZ7~H};J|~qq_03zktyaY~Q29+raSruI9V$%)M)oq!8N-fy1I;?7 zMyt&r7F|lk*S$~u8K^fZ^BmnBm9quW@JhrI2Hy5J>iy`|r@udsjy1dxl|DtC)D>Cs4weFiA_X5cz!i_cif79v zV1eA*J={9JGFHDC6EJu_*2| z0+Gpual}QMj7fyhd)$J|AH43b+g$0=8$oO2uWNP64uuJGyn(PKdoM)KSh~$#brO^0 zux`c_H?yx0CVv3BoYVSAqb3bpmUhy-ZHA*V4=yIpYWFA0Ou4qAcj1ra3!nai-tox% zVTKJEJ)!tu&s_MM=`d?MYCB&)m`a-FmV|%+k3TRS|4(aWS<(FX#^N!3g)D)Rk!5P- zWh9Xy2XOyalPWoz?kWx2^zc!ltes3letwKpx%Ep)V=PG=#beTm)mC=htuL+jJ>tlD zxFmbVLKX`0tLy=34-&AyhBt8mu-q0;M`)~picPv+1%$?lHs>u$SS$fd`cZtFVqj-O zbVqv(UpjF2xrhRf`4-`~Xi?%yIQ6}}kkl_VzBKw7Zv|#R>^rg>-2{-4Y`0W*A*lI= z%qJ-dh0CH`K*riK#Tt~N8u6t5Hyo4RYP!-E&GA*}P=b^SD?W>!?D8C(?(!_Rhq~K; zwjyH>$eC|&*~uQA61CsSHbJ}->zqFya43(RkYvcoJU-v{TAB`VeSV(F2S(o5biCEw zh6xJX2H71pX~0TpVXn);=H8O&tCIU91N^EDNdYB>EEL;5ZS4jb*&bKW-A>nSa zuxDqT<6J(?x3>E)&Lu&Co<%sE9kl)WDIVpigUs(hZf}w5LGhOv6uMUps5#Bxo(dcY zd@|m&kY64525=dihnw+xsyx;xd9@$j4Zg+|?Lxpc!){adp3 zJ1X39aFcrEt6GBW{P@h1M*qFqz2%C3c~AdLB}k3=u5k%_K;b6a1E*#(M> z-c8JPvKn~CXe?L1pf%Je0gQMcjk=2Uo}OmIu=+14F8HerO@97zNKY5h&FbtrBcFR~n3cm$V?OA@8+2u4fV<39*n1tUl~W z4)9(2JT(ipDNVQ3T;OO!2dDk9oO>1~=MG2%ZE13R>Hu3doQJ>YTvVJs7Dktcoi+-p z&32m3HVPJ8_Rn?(Ez`01+h9L#bR!$|!g*l9yENJzf)KzH3|w=|@<0!>5r#$aUG(Dr zF}Qy%#zar=q2i;$Or(rwXo`9k?-f#oizaK(p{ z;RN;_JL*EGUBf{q+jO7Wg}X~6oLW`_0_siM)8z-khK0i9FC|cy;}f~p$?VZ(9~i#7 zn>JIrEWckb|DZ(*cCw#AP!=KJO;x?FITH+fJzBgSxfRrc)u)DUT;b3BUY~$FDXF{D z4?bl(B~kF*bIM>C_eL9dBCIJxfi?sbKg{ebRx7TfUY>UDd9PA@KEB|HYIp`;ek3tc ztVqa%C@A&U4ZZkG^Se6ys5Q3upb%0v+nt*YYfDhA7yC8x#K{2!&#kJ$!oPMG7 z3@e$v`(u1wBTOv&*gex)kGrD7p=5%kS{+EO$f{x#0QD`T_CE?}>jHTN(yW2@u#k!rT<@>t-0 zM_2yfl*s%8R`-4qr@ItYcEKxUBBao`!-(tYnUj{fW(5}x$0^X+_-bO;{PS9+JDDk6 zL=l?IhU<6sjehnuH_g4Ep;-JKSrI|wy%))2V-BP+=qS1m++E2N@DwK1l~vjNw2$%(TsFJ!p_1q?bZ^sOa`_SMfRpza#03sq72>%g_SN67 z1EU2I90q6}_(M0(sw$au%JW_UR{+#5$Gg`ld-}0)!GN|D=7o@3Ui7&N9T!M)BcBmi z2)fck<~o&ECIY{5fK0m|Smo`&|LdLZFaMKR;urleeFLvbCm(^oUdR=+36u~Rm$zs? zLur7X;#8sL0n4CPHSJm6*?0k6Kkwqi$pV2}s{hS!Mc2Oyj|#>e=`22)DcU~nCgE`} zYP2~0slV)IVNQ{m>0!~~?$og4;g%Eqp}5Mr1F&01S1Qphzo98}#n1=&DM4c3BrZCB z;OAcNY)ro0Erj7jb&hqQw*d`iZ~1-A0^Nz5|NM6U_-TaO!(sKT)v8;k4Dq_s$t&%n z-mgz$qe+fghxMaGf0h_`Fox~7@RQVr(bz?*4dcGUtrZ%1XejnP{P<=saZm!n|H#B* zg!shYHhlttIeUF2((icnfO%NMEPIiM4fAYoIRyxi()HRjy^FrZuwf7|d6hwFLr|ID z0qQgOkzHXgFTeiUfcxOzZxlpkkzWjA_XuZAPF()npk{qro!g{lG*!YX*wrP_#Fzwk z1`zphxZuo^liJ9aKb>mizoUZcMLWz@(kg$ysYD;|caN(=^xtsmcTdA64{BD`>+3k3080^WV&iS*6qNA}82aJJ3U9*3S zjI>4Jmu9A2O9zmb==-}W(kaFHN{0!C5#b+B^VYFM1THJD{^Up@uAMS{*O&D#aQ=_# zJ7v3VR04DD9UO|A3?hmvKE44Y^v-kzOSb)KXMQzbnQ^h)S+1AVuF*gz8|@WsI+CK7 zFPz)st2s%)#f2yUd^C5&Or0&7QJ~}>GRH14`39K81V2k^>})jPxS{gmq;dTmODt1h zqaN+Q858sXyo1N`c~3qi9o%YIfW)sF^!d(=T0c3^Du?D6L;Y3C!W2&jBLw;?EKsO* zZDzG$G;L)X^pn7H%opfSm8V(w$k2<-1fr*SGj=AhSoaKCR+G7li=H(COPh_#2xRT3@a*jTHY|55g_WJ>-lf-uu;!aSgu0Ir0($&4%VRgwH%iF{SUF5wWS=uzZ=jT0c6%X2h6UnZdJG{ zcHST~;c;P4kGl5Fi_+)CQs%7-GR(Qo;w#)&m*-`aCc(--8?A|kz_H&a9+E+6%+MdPfg`K<4VF0pk~etWui;Cx zz>!;acO1DJL+Yh9bX24e(YWbS%uU*xx;Zk`9E^yhZ;q!OF}6rg{|=n^3OB-6FBx`%uD z^M*1nP}7y+%0i3SE7q$j$ZqS)dr7G4K2V>2<9;Pi%g;|;Zpesg21x!TwrtajMdT@Y zUl0RwH;AG9`)5Rr#W+OXdQ5KwO3cmP+Ix@o=C!&J{fm3V#$QRkep~xTj0g&WX8YkV zifd?d?In&WR{&M@}i-Q*gGSXLuvV%oj`aN%9wG zrMaYt$zjz#GK~*&1h_R6@eFIc|MyZNkol+nSdqGqMSuh2gok}W*G_=FLrGiK`m%$- zy}3J{%c&3Zs)1;&pCZyKT+Ob5 zOu(C4R&VU|8idahLv8P!k@ueb-OWC*AneBxUQl*B3;)L{9Jk!Vt~B8bk-NTd73)ct zdOQJ)h^lf@#a@>5=E_JCs@ywcq+BQ$p-Nbinjg&z(ha6z%R zlC0J9yceJrne#wd9eEgRCU|GW=jX8=X>Whaa-65^jXRjM6O3Fc+FY5jnbXPXoN!p0 zwhy&o5b3m%=IJc$oh&4|*{x?T94p1VYQTly7iE2LsG4tCpD~%_W4RqJ>9h$yy5dH= zdZq=GUn;y-W&3jT@V#Uw4e-}Ke)ryWK3aD1ek>DKe&V<{{MzaT^KsedE5#F4`jjjp zjCG|GnD0EIbmh3oC)WDez4K(rc&rei6k2_FBvX&{7fpxoP`7kQ!LAHyFwSXkKy=M| zes*Jc@N*`Rp%Z6zyd5!0;u-eFx;H8DB?sd>p5Ewpu1Rsh-S5Yo4ntsm%mP#zET3!< z8*{G;!Wm9a7K46YUjlH!H$}vh_$SgqtpyF8&j0R16;inHDE8iKf&6gF`>z+X}o}lw8Wmb?eCeM<4kc$xBe=Yih{H?()oY!K$;UtnclzK*`kW z6W$C`fOHLa9^Kcy4!F(#h_TtU?uyVO|034+`u?6L^+h`OQt$st<2+T9;_Uk+e4oTx zTEf3Vy|$!pt*_HACA9a}DFu@Z))~`1m=}AdQ%sb$&1-g~>8qFLdk6q7m$TLxMhHYj zt@VEC(hBtp>9haw-I4Blb4rfQ#~@mgDdn?Uau!UeHcGg6FtS6AmcrmFa!kk<*0X+upR7pEQvGTL<*9V_7-6*gn2$RgUVqKJSDTJ( z_aB}u-n|*?o%uppQ@HV)6c?C$qloH=T`(4;O_I}REZX*nV^sz)o2|8OHFqCBZeX{x z0S@837C?^4{$!M7941+fbi?~4=O$$+ZiwsnvCXjF%K^vL`hygb`2K_5xO_#H7QOX0 za~oPP=5!9~YbmpB8Ql^q`^HowEV*36I>o*03DQ5X#x(rlsT9YL3NViw-Ay<}2p#$H z>74k92?w!cF|F=|w;k0JJ0H0d&sts;A@Xkybtok<+%Y8J*HvukvUANh?JzOqIgpTB-c4!tljp!v#Co zT81!XWimX)eFFCw)!{XJV-{xR?42ue{$z2UW%s@y!X|EJn)nM?d%(hGdx)Za)Fcpu&HXnMrN4W0yS0fv#z;`CB1a*}( zim^;J)pWZTN?X{$J<8={c znf&Rz=K6+YN|t&x@BYmZ7-*$?V?KD^FcDA^N-rnO?I^ zU|wJz+>;}+{5Dnl5a11tdu6rD$*)(Rmvw`K%V5w@U#Yu9!!rR<&6Mtl4kyD+sm8jn z+7xMKNJN@=ax-JgTkq&0$)2Nz4}liXvWhfZNB70b^k(Mw7Vj!9*0dC@HICf?4vvDU zUulzhzU$%TMh$tpeV#mzUq4%kJ73|4igu`y3DRw_JoC(h*~mJa={sG{J;ez3clvEt z148^0hktd>&Ed@wP!hcihi7Q+sqN9a{a+L18e$FcaS+{-A}iq@-AiHySmr=VU()0Z z@=;sOY%@(f34_4Ei>ozJrX8h7D^|Z;)|JNS^7Jr5BZxW}88!uG$?nwmW~6wQY8y=9 znlYZ0;dGJAW!|>>BlhcT8eYVz^;sIG+_!y3LqvYNO^*MXgurLr>w9BrKTzrN zw@gwFixIZp4|t(5TfVZN+)}2vJr?L^mXq3*?Y`;%^P9*?jk zF*acnN`T8z-Wns*SjcBd(cZ$r?ef8~W2ph2?q^>D?|eT@BU&)?R7`PGrVx|Z@BrRU zM0v;y%UU8&WJzO6F1=d~Fhx<6ZV%}!9_Y%S<_}ju&k&fDXzU}`?Wtym=j@l9X^*Aa z>2J83$$s0>kYK5PsabwF{(^Ab>=NMs@Px{i%MgXWp8N&75L$kPrSJ0tCte>G2cNch z;=^`Fg)^4EhaZgcO(sGzjjUC{F!wFokIqQTlyt0>iBFIFf=TC#ZBls~=E&|dgUi6b zGW8<>;rww2Z-q+&OX1tYJ6LJWYl%(J`R8yr;*sQWeTWy{NzdOIaxwi5xim|-MC{cc zbjh=B#sg!SG_@)|b9-iPOiT|@f^Z*CB)qAiNsCn{n^cwFZTrZIERJW(Puv7axr75B zMF@izd}%%4RN8xy)Uqt`GbsJ@V2!^4Ss5%*p{DEp&T?n%I`Y?wxeJXVJA!%%#}dwY zAgK3V6I`&2EBUP^WbK#tODWwoLfbt009+37TDV^D- zr}!V_1+*B7|5D_la%TOUaNj)f<+SD_Z;MLZSxN2ukL|7whAm>h0@5Xu&L|{(HYH9T zN}jT`dMPe{dyb9V$m&aWt#q)6T_)D6x9;paz6MO*=E;n(Qn2~G{W{JO?K1-$Vq;Nm ze}YI7FGidBJs(hg9YWYZvPH=z?4$XrTs{A4_Xof;ly2pPCGz>3`C(nN@{;PblsXu| zs4gLD;Lk$e6Nj@*q3M32tgpXW-Xm2T0;YCJ8(aI^JGN}M1NWG_BM+9`{$Ndv%ydbB zXBltU1bJf?D!VC!9P^UD$ebY1spURu%nl*UkI|Zm782VIAIhRaO31Fh&L2+@G}I_d zX~w37V$3_mqSPlA^}2GW5h)ypD{2R9FjBudRNWK%6Y9%;+JKKOF_;7xDm@(db6Aeh z7z22Gnh<+t{HD_x!=j%ny5X%pT7nuWJ$HW0frOvCPDWl8(*3oS8>nG)SBsHKd=L0; zmDXu8NW|n*s3w(M#UnFvGmI^J`dTFs*jA(LPRZ78D9jD*tjjO=+$+!zBX*;yjdfR) ziD>RTER2+;#030!P$&kcK)eh;>~M>6H>IsHXLG|tg;BbHj%y59*446T;!o5_1}gN( zRZ|kVh^Vc3D$rGcp3U4WjnqIpW~s5F;Tr5rKxoiH zXb?zX03;|rA}Y2fQ32nL3t_r{ahG*ZN8zuJ5|uvM7Sp_pA(o*Urt@pMb67^=!!lfG(~!;&SqKNgm^bKPbdC{68h) p6!Lls?};R7lJC31Vy#OC0tN9Nt-4TAyFMy zLPD7Fdc2-yy}R0(4KWAK9Jy4=k#kR+xPf}$z_AAo96&+{Rj7ih7tS!>Gqc{EIPuy} z8dBv;nSvortye@TSU*AEaH6a0#3IV9?2}<{+F+1!$Hscnv&lVKV^?{h7z>uyMB16f1`Qdf9Wk$kVnB!PwIS5>8W3Vgxcd&!H5#Vg0 z=nn3wr6#*7Y{o3^vzAl zA1VH$;*S;oN%5Z*|3&fN6#rfEKNNqW_@9bDRs1i-pDF(Qf5x8~em*tk@x+*?p7*eH zfq&Yse(~2hXk0W-8aIuj##Q62aUYg0@R$6o@IO0(dt)CRe}A9dM(O(u0IY|s7V`2QK`TpYGDJ6r`a+8L> z&*lG&1R_x5ap=p4$EinLjbuIYY={yoDmyUo>o(6fQF>p1Tu4NmtgyMf^TF~;Xy_cb>w^Qpy3oCC}*98y{kBY|sk^Yhu_`m2UtAY*Yj zw3>dTbEjVTYf9SDdIt%;YsR<{VQF)9s|51v=+Hi_bP(>?OXe37h95c?)%HQ2mXE*Y zPhf2*!Ko=Uif zt2a39^ZlUD^Fr5IY}BcHfTNW0st-X33j81nn#7C5W5gQOh{H@H8enw1R$n<<5!Tr5F6Z{5ip8P(3uD-b5F@e&Wj0K37eD^?ClX zf9fHWEw5QMa0Pkzyr!A?{imH*_1agLjrW7B;-?eQM_(L$KP1;q_McoG-s<6Zw>U1M zw(s%K*Ma^p_al1sdAlCF-e+8)^AaeroAvlr^}TC(O?X>ML=a|Gyu`ye5&$v_+-80I zBqQTdHE0}kN9`B0uFy}O2;58!2d(#|_wUKt z(SBw1@6_+Pak*~WXL!&32LJ&7|Lj>!Y#cQd_HMEv4M_;1m4d`iL{%k`LX#jwe-T2; zY&I<$NJz=1DIlcOJL~Mab;hF^5AEjE3sR0f_ShSU1LvMNao`wk2nnfhLgB&<4j`~+ z>>ZDHlKoYBAWs^3-y6Sg-aOmyy?I+=9(JmrL3!6D*oF*KX9Y3{wxD*C(>T8@ehu+! zm|ufZo|AlrUn6o9G$DDr20dsqf8_Zw&clHiXCehBqv448HEn*j#rzz%Y(nzF-aW_& zEPIr)=)R9!ZV;m$P`tyigNS<1-N3Ph6LxCQsSp%C|9Xq}e${wy!Ov?B_2<{~TWk5{ zoy|8E78lExq3edY3oURn3ET{rm;UHf&yspm+0Oi^)^kSl3jnO{BIzqkf8?*b)Vt%m z(2o4|u!Vid@R#8I-AfDBOl_ll8`*3Qea@Gq!uR3qHueKZR!jNyooaw*rZHbGNbo*d z4jALhRDgt(p?zn1u1R-}D~M5{&d!4N_VGExKEXqWI`uso$Zgzm9Te#}N>BIC^Txak zM_Mmzlm!1GXxfiEv8Q9af1-vrX4o-gtgGNBqn|%#wR;0{6nJlIcta`v(echn`FH;c zx@ei_B~5?O(4XSh*C7pAH$u87$7w5?_4iV@@n_Y+a>K;_dH^~*2V;^KBY{5K=6Dqu zAltF)Im0f&8~eDgILaW##nzEw!_f|9>tw{cYW!hunDpI)0e@{bi zc(k*1d{**TfL>JqS$PR^J?bKlVdV1zfJwze;u!iavU$*JJl6szuUA?=53tR{hDTN5 z*Hc`(&%*rl*GPAD>Lbt`*Hm4jcr(bEQOT_5ORS!Vfc%UEe>z|A5M*JPis&@!{|6n` z|F_QS|Gw1!lc!Yw>-ANdeYXsM3BKL>-=%Kg?ja{w?5y-ZtNYXIewCBDjen1Otox%n zKPU6uuS*gCrWMV~hPaP3+>C*%zl-uqH~D|d!@O=IsGajQn9v^tg`kYO-CvFUtl583 z{x#Pj%YUi$e>>}|{J;Au(D0YwyNRo+G-$V{0>D{c<+%8o195yk?Y=`=Jr=KX{GR>7 zId1GUp2B;`@1-7}%1wrO<9@EYKEt0Nwa<{!2q?fkJ>5h&H?m zisdP7j5SJqwCaQ(+LAtQ2K%I0^l7uGz5YjE>-?_!e}G5tTak2Fj%(3qyR>q{gH9D?f~GzSi`8-PZmL7;Ps{>vm*0m&s&8zx;TD-ZLroe+7=CKbDgG zZ%fJX(76BPnE58J^i2HYo}=h+soQh=Pmguv%U}Hd(c@QMw}PLnFUQ!ZRs3cD{5S7i zvUWone`wi9Nn0;LPeZR&{h9;G^Cx%_GZ&@)0qAd@>D8X^-#y+QwU)X){pt&KdafC| z$9n1m}sWo z%(+)R=%X-OX}a61h9t5jFFuqES>gd8HJgR9f13zbYjHXQMe5;wE{7yfaMj@z1p{mR z!@=xd3E53tVKq&*4!tVY(=kX*(qNM^Xv)lfnV?mN=P%boOv}pKVw>5zb=>#iKO!si zt`qmq%zm1n6-1CJTMqns-cDLm3^z8f$Z;7H%<7>G5qFEa*!}?38}l&RLcZoaWwRH! ze+-u!{%ZUeQRKcRk-tmG3;!pAa@v0Y00960>{m}u6fqR<3_`>M0b`;kL%f@K^Unp= z1>B8b3=4}I6H{g$vyI($oavy;iNp(^g0m;Tf*(aVapfb}?sl0@GdS$_+j%;33wuQ=Y&%zVq%XP9T1`Y;}Ke=#i= z7bk*WMBMoT{-niMego(>2)QdxsaxJUImlcm2i?-V^X6ZM?*S8P`<$ijPKNKGhdFr{ z&36)+zq^CEGWBwG+(rS<+z%@5J^5_!ANBT+e$IXS`b_>E&rQGR%s)lPrSd@Na`)F* zhBt-;d?~%on_LLqE&hGl`MRg%e|r_YqIr&6^Hlz)1QN)JdzSk0t*uL4PdCGJk-Ckp z3-7FT8EO5Ul;>JJdq0lLjP7b*y;~3wOvw)THO9E;H!bbQgK-bgXye}2zIrc1AX|-;*jEAOZ3u)>Tg@;ig)q6)6SubzTcFYfA3NIEP4Nk z^W68l?(Dqc?@o=jPwr^{y`1nmBdU$g*b-;L#-C&FV-WF#`E^dY)(0p5rKb1XE8I=B z#iAd8{oTa~$FRZ;1tPiariBc0sj(oVh!mCyps|)bo@{IbNYD`}*5DD@kvOI`N&?oE z?HXDp3^Ip=G=*&tUvvy?e+5SYVJKs^4pO$N&qot$iO5n?N^{9pF^oV!TA4OPs{1vv zleJG|ufZ1hXSU=G2dzhd1*E>R1)yMK&y%=U^Rn4AF!L$&l_sG?^!gakaE*t=dkMXh z?-8*Eu~EEX&QTHyL#4K4+IJoS`>wtS5QOygnrP z)`bCQ$FXiXL;tj)FZmiG_YlW6JR)xMDRv5)9ZS5OQF_z*f1gL+MwBCOyto%e^b6pP z6u-0fdm89@_v~t(cv))QT83WHSa(r8UwA0 zeicXbih!ffa-VaXanAbL`DyxV9pUfap_jGKIb)xa|1>}WN?CWD__9vtxt^yrqGtg{ z|2ul)#$}@Oe=j6@mZ4|f$2sBA1;xv;i98Gmet?_`rHn(48Qps)vil57X7>ff%W(^J z5q0XcBkQg-s`>lHgm%v>UXFE}(M{~T1P+>fKoHRE{+rHurg!{$N7bR@{=P}l$4UDv z@%`b`eD6QEUM+|B^tDE;8p{^PrC!%t5;DUfqyHzEo(JAVW5?fRZ&uc`{|U5bpMPBC9bEKM-wBm_y>E$r}~CTl>iizyhPo6N^r*$Qb})kj^{!7 zu5p^URfTpBIc7fOsSA$Sbl$$yWthr~I}P5<>f7}?Im5-1uUpR0Kk2wQ4M%vY4F6wn z@$Xlj{&wZ*&->p$dh*?)C%;aGi@_Lii|L=n#eM8JvP{o#@eCKwaPbTm=gt3Q~o5zCO3C^cJnj;uxRjvKAKr_`p?J)xmL{zt6n8z6M*9;1`B%|BaV!$9t_XY{KvHj38m(@CmzR8w+~|Lce^#9G zORI;}R>fR7#s*hb^4@zWK;fXzg7~gY(?@)%>76&`6@;Zm^+r?jFM&gO*h(Ut*wgFl zS;LRGhmP$_eldFe!P0(<=jE+uKm8%;V%fYWY5FBY{}u7z9$+me@7L(4nDn z8z(AFsO?m-b-Qp|xaAv362&esP8>P*xX-|+*vH_)g%jd{#2o~7 zVk@o}txX8i4z*;xyu|O9=l4Gcf;G?fO|~;^F9^z4QSgK!cWX$gV+ZeVe}jwEU^_2x zqHqfuF6W2XbZqK5mV)z8!O2J9WE7l&c+b~|Y{lU6+xbvFJ3)S)X<8J#%l5vFDFNYX zLd)tw^H}mNrfmnmN0Q%l2&PzC1%M9quzeKDH(b}kCgevRK?=bhR6b5=w-t~Paw5m&vP6ad?(0d3Q@-clD8?gk#s9K9NH!~LU#b| zzJ>h%?@qY)J?-9{UsViBR@;rer;V-dv-P!lz4rjRu8+Gm0XH*-n*poB9&zOiAy>-A zv!nE9MX?J2^wbqmb%-tT^w#l?i3`oZUS|{ZgPOfKPY&;|8I{3KfA2LiX$`$$Zt1f) ztR7%8gw~Tzquo6k;!4@H5cGls@5-1j*DlD_*kMj3Vzny%mo)#O?bxR$&X6cLm-#_1 z6-Uac_PL{-%P^#NbEm`kyWm1Rc0)^tc-sozqNYb4jQf@IOTpuptn%%Kph@f6*FStM z8+yJ($>%irk~G$Nf5X`K*Rr5xBlz%ZQ`alXZ}XbHIp5^*BJMQ;@#23`@SIwZ&;w}U zz9@{-#L+EHAI>ZB3)05X=_K6$-{)5Rc=#N%?p)UA;`X%;J++V`f0cbMiQRtuV;%*0 zQ!xCO=NrZ+f31jo`nfsH{fc1xetfsEM`+1)2r9WAcD!l&e^71=CkH5cN4eZbkZv9( zsS8|}^_1iRxL$cT%GUh&dA^V=?|7X|d%o23u<0R4QOlG9SDBZinDghW+5DNx{JC^R z^QYS9qx5^B*^Bej&Y!4{X+6Q@Ih#A9=T3R|r1kDwD*dt)^vUma(I+c{{`UK&tKeoL zaP@j&fcSoXf6U^lw8xr?f)lsD^4Of!w^@CAso9J3G3%S`&Qlk-S$&JHZ}QI9W$yL; zo02?;pnm~B_UUgA0000;0RRAaob6UYPQpMG{WG+sh%|BMl4H0xF?B&)xX@i~pavo- zP1T4u@F*O?%XkNuE}da!^V&5N5lud1TX zGCMzn`OSHTAF~|Vzm)BtJC(ov(rVJJRZnfePt{+-Ja?6suXO)hj-O_WGMnb*WL{)1 zi~8ZQf6B)NHJ5DA`1=!ES~A{YJYYOzJYu}dc-)fP<>G6>bP0%h2N3lD5cLoc^#~C4 zE+FbLAnHAa|4|9xJGK9A@jG??6#N6`kC size(handles.results,2)) + [handles.isam handles.results{handles.frame_i}] = ... + VisualISAMStep(handles.frame_i, handles.isam, ... + handles.data, handles.results{handles.frame_i-1}); + end + handles.results{handles.frame_i}.estimates + cla(handles.resultAxes); + VisualISAMPlot(handles.results{handles.frame_i}, handles.data) guidata(hObject,handles) else + sprintf('Frame %d:', handles.frame_i) sprintf('No more frame!') end -% --- Executes on button press in clearButton. -function clearButton_Callback(hObject, eventdata, handles) -% hObject handle to clearButton (see GCBO) -% eventdata reserved - to be defined in a future version of MATLAB -% handles structure with handles and user data (see GUIDATA) - clear; - - function edit1_Callback(hObject, eventdata, handles) % hObject handle to edit1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB @@ -154,7 +168,11 @@ function popupmenu1_Callback(hObject, eventdata, handles) % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu1 contents as cell array % contents{get(hObject,'Value')} returns selected item from popupmenu1 - + str = cellstr(get(hObject,'String')); + sel = get(hObject,'Value'); + handles.selectedDataset = str{sel} + handles=initialize(handles) + guidata(hObject, handles); % --- Executes during object creation, after setting all properties. function popupmenu1_CreateFcn(hObject, eventdata, handles) @@ -170,7 +188,211 @@ end % --- Executes during object creation, after setting all properties. -function figure1_CreateFcn(hObject, eventdata, handles) -% hObject handle to figure1 (see GCBO) +function Dataset_CreateFcn(hObject, eventdata, handles) +% hObject handle to Dataset (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called + + +% --- Executes on button press in backButton. +function backButton_Callback(hObject, eventdata, handles) +% hObject handle to backButton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + if (handles.frame_i>2) + handles.frame_i = handles.frame_i-1; + sprintf('Frame %d:', handles.frame_i) + handles.results{handles.frame_i}.estimates + cla(handles.resultAxes); + VisualISAMPlot(handles.results{handles.frame_i}, handles.data) + guidata(hObject,handles) + else + sprintf('No more frame!'); + end + +% --- Executes on button press in runButton. +function runButton_Callback(hObject, eventdata, handles) +% hObject handle to runButton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + for i=handles.frame_i+1:size(handles.data.cameras,2) + if (i > size(handles.results,2)) + [handles.isam handles.results{i}] = ... + VisualISAMStep(i, handles.isam, ... + handles.data, handles.results{i-1}); + end + handles.results{i}.estimates + cla(handles.resultAxes); + VisualISAMPlot(handles.results{i}, handles.data) + end + handles.frame_i = size(handles.data.cameras,2); + sprintf('Frame %d:', handles.frame_i) + guidata(hObject,handles) + + +% --- Executes on button press in stopButton. +function stopButton_Callback(hObject, eventdata, handles) +% hObject handle to stopButton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + sprintf('Not yet implemented') + + +function edit8_Callback(hObject, eventdata, handles) +% hObject handle to edit8 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: get(hObject,'String') returns contents of edit8 as text +% str2double(get(hObject,'String')) returns contents of edit8 as a double + + +% --- Executes during object creation, after setting all properties. +function edit8_CreateFcn(hObject, eventdata, handles) +% hObject handle to edit8 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: edit controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + + + +function odoTrans_Callback(hObject, eventdata, handles) +% hObject handle to odoTrans (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: get(hObject,'String') returns contents of odoTrans as text +% str2double(get(hObject,'String')) returns contents of odoTrans as a double + + +% --- Executes during object creation, after setting all properties. +function odoTrans_CreateFcn(hObject, eventdata, handles) +% hObject handle to odoTrans (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: edit controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + + + +function measNoise_Callback(hObject, eventdata, handles) +% hObject handle to measNoise (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: get(hObject,'String') returns contents of measNoise as text +% str2double(get(hObject,'String')) returns contents of measNoise as a double + + +% --- Executes during object creation, after setting all properties. +function measNoise_CreateFcn(hObject, eventdata, handles) +% hObject handle to measNoise (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: edit controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + + + +function posePriorRot_Callback(hObject, eventdata, handles) +% hObject handle to posePriorRot (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: get(hObject,'String') returns contents of posePriorRot as text +% str2double(get(hObject,'String')) returns contents of posePriorRot as a double + + +% --- Executes during object creation, after setting all properties. +function posePriorRot_CreateFcn(hObject, eventdata, handles) +% hObject handle to posePriorRot (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: edit controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + + + +function posePriorTrans_Callback(hObject, eventdata, handles) +% hObject handle to posePriorTrans (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: get(hObject,'String') returns contents of posePriorTrans as text +% str2double(get(hObject,'String')) returns contents of posePriorTrans as a double + + +% --- Executes during object creation, after setting all properties. +function posePriorTrans_CreateFcn(hObject, eventdata, handles) +% hObject handle to posePriorTrans (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: edit controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + + + +function pointPrior_Callback(hObject, eventdata, handles) +% hObject handle to pointPrior (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: get(hObject,'String') returns contents of pointPrior as text +% str2double(get(hObject,'String')) returns contents of pointPrior as a double + + +% --- Executes during object creation, after setting all properties. +function pointPrior_CreateFcn(hObject, eventdata, handles) +% hObject handle to pointPrior (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: edit controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + + + +function odoRot_Callback(hObject, eventdata, handles) +% hObject handle to odoRot (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: get(hObject,'String') returns contents of odoRot as text +% str2double(get(hObject,'String')) returns contents of odoRot as a double + + +% --- Executes during object creation, after setting all properties. +function odoRot_CreateFcn(hObject, eventdata, handles) +% hObject handle to odoRot (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: edit controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end