From 2ebd7a8974205794849724f87e06cc164ccf6e80 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Fri, 8 Jun 2012 03:54:27 +0000 Subject: [PATCH] early prototype for visualISAM_gui. No drawing yet. --- matlab/examples/VisualISAMData_triangle.m | 32 ++++ matlab/examples/VisualISAMExample_triangle2.m | 16 ++ matlab/examples/VisualISAMInitialize.m | 33 ++++ matlab/examples/VisualISAMStep.m | 24 +++ matlab/examples/VisualISAM_gui.fig | Bin 0 -> 6094 bytes matlab/examples/VisualISAM_gui.m | 176 ++++++++++++++++++ 6 files changed, 281 insertions(+) create mode 100644 matlab/examples/VisualISAMData_triangle.m create mode 100644 matlab/examples/VisualISAMExample_triangle2.m create mode 100644 matlab/examples/VisualISAMInitialize.m create mode 100644 matlab/examples/VisualISAMStep.m create mode 100644 matlab/examples/VisualISAM_gui.fig create mode 100644 matlab/examples/VisualISAM_gui.m diff --git a/matlab/examples/VisualISAMData_triangle.m b/matlab/examples/VisualISAMData_triangle.m new file mode 100644 index 000000000..b5c4c0e11 --- /dev/null +++ b/matlab/examples/VisualISAMData_triangle.m @@ -0,0 +1,32 @@ +function [ data ] = VisualISAMData_triangle() +%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 = 3; +r = 10; +data.points = {}; +for j=1:nPoints + theta = (j-1)*2*pi/nPoints; + data.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]'); +end + +%% Create camera cameras on a circle around the triangle +nCameras = 10; +height = 10; +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/VisualISAMExample_triangle2.m b/matlab/examples/VisualISAMExample_triangle2.m new file mode 100644 index 000000000..b636ba542 --- /dev/null +++ b/matlab/examples/VisualISAMExample_triangle2.m @@ -0,0 +1,16 @@ +%% data +data = VisualISAMData_triangle(); + +%% init +[isam results] = VisualISAMInitialize(data); +sprintf('Frame 1,2:') +results + +%% Next frame index +frame_i=3; +%% All steps +while (frame_i=GI&2OsxRKnfM$L#_CV;|>WcfY{b zD*rfL!-w(@_;mx_oP*t5`TcxV_@8=)@Ee8r^7AY5%gU+9%c;o9^2&_ThTOR<8Zfb#vnUfX&-c$MBVR|}%<+Vd|!jo#YtoN}fhz08XRkWe} zI&G5d^iRj%pLLk8NNJRX=$9Cd=5@B;Io@Kb-!G zgpqf;{pj0m41VUtt*HaKr&JQtKMcD(DMco}qE>h;nvD8+#@Nq2x#)J=qV$!~-JyW( zcGEDLvLcc1wFwkKjqW45W4zFAq#BpH_Z#!;o`jqS} z0J$p`?znco(e(*pX-q8$3C4q97NG9RM>v9Naj#!4_X+z*YO6=c4-O_34C=S(D4!^y zY&(seXox6cCq6=Kuo0glG})p{FN@x5TH&RJibM(c=;5L}U=4+R#9(%1A*LbtP4sd- z7_p$_=^$^$shCnWY_(Gx!1qP+Zx+7xucm5jgbN=}bkUmrA&!2R%%PKTrb6a<0={riUp4P>jKsi0D5*X?9WNiFxpcDjJRkR}2^&{hx9S{F@N=!}iR4 zap<@fG166?0tAb4gS!aX&r2imVjI(WN2OUGT%7ThSi<(YsLI_*=pOL{g}~#T zfP?X*kt%>gimUp$PZEo`n5s15_#sr$0^}6qV4#;@+tA*H>^nEwJL?=88aiU&rpUP% zH5fai7Ay9_jn6h^t_Rm?a);!rX8jb((VnYf+}D5Qqs{Pqc)iNY|(h1gEfIqg=6kQyYNd|@BU*@H+pGaytg~{ z50Y$YxXym)qVAn>Tad7_Q#Vmd3bMM}HwNfw@8bK0k6qQbq*Tqnz&VsGa zY*j=|Stg>INt>D$&rh2>m^|n*$T^oZU?Rx*BIKXop$hIez3AYZdl0d^f(q4C{OV9g zoaCX0ncCjuyY&hVd=Fl^&+W0geDS&-M>lwTY(~SzG#U*gG#=f}l&H5=@TUY`c74sk zURM^_pqFj3QZR38w<}qv0Z!aen)epWE{Ys@){nNr+LR~f z5Q!(Yras&XY!|+bcNq++OFzdBn(J8!ebUL`*So1R=JQa3%fMjtt0;f@tm^SeN62v5 z9j4R$Svt#9^@~NWByw1xQo!bIUuv2vf1Ry<21fn{j}xA>u=Hyy5WhJmaD%JYZ(VH# z-VL;W$s1Z-tPNZcT1=pS;z21(-C7))F{9M`0L*~!&USihS(p)-;3OExCVv;$c|+Y( zZWk*vIq>GThXAEW*;W!(;WfB*2+!T-qjTQBuKtVqK}2TbK~I{4FLN31Htf8-_s+id{h@fN)crM(p59-Z`hAklNy|H@*zZGxY;uy~=IZlAkN%+grtkP#^ z5e}Y0(?L&`Ch_0qgY=PfB(-M(hvFu?^_{bwJDpRhcyvR2)UUr;o7Zc`bn_;!%Obg! zd>Er9vnu*ATPF`M$PKKGvFL(VF&!(C=Y1#=_ab6)j5B?UJc}7J5(|<$3T(8UGxAuDW6OVqJZaen(B{Am#)$TMfXJ+ z*V>v*k+LF9kCq7XK(=r{?{HmF!Zc#i5F19M?eNTj37JMuO%-h~EZk+VRn+OYKjI~x zi^f>CAw1^#0tW^OAQBZ-DwlK)!8aN!`{C?&_*Z@$D-THaap55KN@J1=x9m3^rTr=S zUg(P=q^Ka=WH#!YkYe2R%|E~VA)1V;A7D}j1bgt>=5f_zak=-bbHy-OVsrc*?NM8I z9rEAGX;0p~STMb(?_Y4G^gfb7>~8-KO|@RYBUXeLCHQg8LGEb`gQZvDB2;bpx611L zq?fqrFoWta7hG8gzb82F1K^x&cq4d8()qD+?NLcI$yEezL`$U=n8)CNiKL@*<*7(B z7e*krvGfvSEzkSifDr4p?6NIp2^otv*Yyi;ZVHLCKDUW+v?8$u(gdm>38XpiBFSS6 zhjh1ZUr9Csr@Yk`$VBRG>@=7feh(x7thN@OICMh$@u&rZu#h9cDei@*&&$H)e&tw< z*20Wy3zK5-A1G9u{xZ8dXTLv?KPFP~%oE!tkBB|9#EiRi)kcNy z=Gk45XD78N%^KXyG5B*vM@n>BL@lmNMN)NGgOM=3}-SOxuwCZsUz85Dp^-pu1?`o>$J=-eD ze%dOcQ`R;JQwYfg=7E=*K_Q z_kXJI_6??#dlpzT-+%lkDcQ&(>0uY``#sUu9~6wDnvdf6meVzSZ{W&pQC>~ox(NWM zV|*Ph1^~DJ6#OjJd)l+xB?l(UU_;d4fMld7f*Z?+%dp~_AAEb}QBiC21p3>rMa+*w zU6r#&U3Gf#;j!y-q8HJWsb!j9((!)AqlhU8aW@+)T#qur?-Z6D_*bn2;mp%QcE_FY zgy|g{$F|s_TB(u6Z&r9WB3=>xyfIXl({l-6XfLI0u3K+)ol8E76bZj6$5BBbJ@6|$ zoGXNHsIT+3eCbfXKB5Z&)!peB0UeR=InrX5`ocX3no?Nk4NIh1_8k+fEb%Y9Ib{qABGHH9Sj+L5(dyPU;8kzLA+ob#odGU>T)HdD$ zD&&ujwxT2x@0t70ZLw35d*cWCPMm0#OdXsROj{Yd#pY*Z3{3yL$dL6Mn*iq&TP4Dz z7IaO<$5vkEEp>#fVzx8|#8Gvu80Z8zx0qe;qo_S29<**|_k>>YVg0XwJFF8&CG
*sRdZ-#{{=j-0mNuM-q+kqJCR6TN}81}eeZ$Xo%52^ZpO=cpvlv1-AE&i zv|YKoMS17#C5V7iRn8!J4GDE8Gdn~f+h1YXt~Y#v{dx|Pk~{Q1>w1iS6PxW(^-Akg z6OrTd9@!o_SJJ#8G7=mdBC$?-t5`BZ`H&5m{NN#VySQ1E{kn?sF9#8-wLe|ReWwIC zDeEg-e#Mzt0=zg;8OZTFj3uYH^KH8vBzC5b^?~!DFVeu1QMfA;0uR)* z_h3Tv(MZL))geFLA$Pp43o>DLY7v^L;~&mmId$7J-!vIzyi^#594Jbhj=T+E zn)2LPpYH)l>GIpB3a?ng>ZTum3&7}ehw0;v)=;ib9&LZzb~Hx)NjUPehrJI&bj?hb zB=>A&=D;m*4}ioZ)5|8t$;ZEDb_%BVp#+dR-*;(cv}~=00$9D@{j#()3L3SR z`Jkm`m-A4s8mG{h(PHje-*@ZMqjmS+SpkTlEbnQgEPmkv&4Ab-8<*szk)o*2!X^ov`kJ{2BTzSr?2T zmM42Kd7SZ0MiLQHN81)aUPf_seBRYl5aiFmXWqK-1#KqSn)eu?mKQv=Fc9&NLP=Mn z8XAq5oWAoCDE~Ji&$RvieT2OA1>e5Uiaj!55cx=Z?-QBfmewb+xzw4L$=TKc={oVd zsbM1*Q;3O1)s^i+rNEOZ3dN^oE$U_?7fybJlF(;f>CxA+!|u^0||U zLd^c}JVn0^4?4CtXlnq7k9^s_JvHK%By!@O%hCSEw78Ub{66Yl-D9PvJK0>Y&3(l7gj2i zdXVZ9RY5z$Zly@6d!R2R9_u5@Tho+7<3HZ1(bIrq@@XqR=%(ITix6qV(VMtq{#63# zLT-i}H47fs$yZ}>Lvf){amUnJlyQ{94-A6;w4<2?I%V7HMKljMYJ7@6RI0Zc@Hv4>YKfZBS;%*2` zBj$SRGxq1|!jxb{)6=c#8FzwX1vm~@3lKv}mo=Cg}%ff^YmP3AxY zF^VuRr)KXLbWh|h?@<^oNu2yG6!zAtPZqshk~cA8w?yk@KL>a|zsvh|&0?*?hy6KI zJVRNj`tInN4bp5-gSq?0nk(ioMD?K^ntAvBme6Z~Sd%6FO(wC6Z;A>AJh2xIX)D<; zG^pK@AgU8X7dTJp{6s9{dCTUIM#P)(fQZ}mU!N&DlnTEzEe#*ppvyKPSDm7cDSple z%=@VO+&bX7z)OA!#%S7Zp|WjH8iR`H=s%I+GwRdlI~!RtMD;OMk4N9t)f|`eJ++u^r4qSH7(s~R(PX#FSWQg3vDi$((Zi!;Z^wgYL9#MDz` zung$B?+-W`w)fpb^ZM@L%cU+6uPY&4B0J(F0TrsuDODX%#r<1FDb9PiZ^rbKpgV7B z2Hb;P3P2XsBf=>SVDIljON}lV_%35v|GUCYt!wOd0=Y!QOuwBz{SngX zZ{MM#aIe}lIPasDnD5bdcUzVrNog}~3!+{?m>I$UI<`Jpy=zjln^k7B3&Fxk)XN1?^`P^p)SCI8$=yvu!Y*MP_eIXk+pJ7(Erp~!- zwA9Ruilu;$WlJqc{+e5KHWPZ;B1XuPCzhNpR*Fo1yswmn)s|!jj8$E0(d{*J|m6$cCu>EPZNp_kCzuTBPF;%J$j=sd2G7;JaAwU zv(Q-VCV2VR`^kd%m>)8?TX|y_A#kQZhS-j__?vI{af;k?tLOfQnZaoZQ@%-Y1l^zE z>BD=g#0{7=KKzoq>C9mswtdmUsNV8XAo_EZ+hPiT<7xdDX7` z`}+M0JUFtg0pK|rLM_(L8`|Y{_1KqQgz7#B(7p=z=*ufyMdy%(&fF!1I06;N=JA)n zo{taU;RPlUJ+sJ$!@C9+1s*E(WIIehkpNce+@|g>;9fv zYn%0Jv0ckGS0u>?z|vhHiZUVE4v?wmuZwR8h?>qNDgM3B-8UXil_go=?7oYgc}X2G zgx`hzj);ohx8To-OZpO3YT#Vk0r;vj9)HyA_c*ILe{jzLsfRxDl#}Axf6k@;Op|q9 zPNlaa6t~)^Au=xomVOqY!1t{u`*(7lN_}JrClZzsF%toB09 zRqbWhc=&4i%HFnjUocG)h>96a!i;8SMpHARCHaqJ_>a^Rh!!-Psx3rcglY(Z4NtzG zmhFUm(uCiPpy}MWb9zDC14e?2HeeWR6k%YZcUgw$1iSfK({BT-nC;gAUypUNOT|2o QB;r=XLZZk8Z>k#q2SW^4#sB~S literal 0 HcmV?d00001 diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/examples/VisualISAM_gui.m new file mode 100644 index 000000000..a3efe088e --- /dev/null +++ b/matlab/examples/VisualISAM_gui.m @@ -0,0 +1,176 @@ +function varargout = VisualISAM_gui(varargin) +% VISUALISAM_GUI MATLAB code for VisualISAM_gui.fig +% VISUALISAM_GUI, by itself, creates a new VISUALISAM_GUI or raises the existing +% singleton*. +% +% H = VISUALISAM_GUI returns the handle to a new VISUALISAM_GUI or the handle to +% the existing singleton*. +% +% VISUALISAM_GUI('CALLBACK',hObject,eventData,handles,...) calls the local +% function named CALLBACK in VISUALISAM_GUI.M with the given input arguments. +% +% VISUALISAM_GUI('Property','Value',...) creates a new VISUALISAM_GUI or raises the +% existing singleton*. Starting from the left, property value pairs are +% applied to the GUI before VisualISAM_gui_OpeningFcn gets called. An +% unrecognized property name or invalid value makes property application +% stop. All inputs are passed to VisualISAM_gui_OpeningFcn via varargin. +% +% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one +% instance to run (singleton)". +% +% See also: GUIDE, GUIDATA, GUIHANDLES + +% Edit the above text to modify the response to help VisualISAM_gui + +% Last Modified by GUIDE v2.5 07-Jun-2012 23:43:22 + +% Begin initialization code - DO NOT EDIT +gui_Singleton = 1; +gui_State = struct('gui_Name', mfilename, ... + 'gui_Singleton', gui_Singleton, ... + 'gui_OpeningFcn', @VisualISAM_gui_OpeningFcn, ... + 'gui_OutputFcn', @VisualISAM_gui_OutputFcn, ... + 'gui_LayoutFcn', [] , ... + 'gui_Callback', []); +if nargin && ischar(varargin{1}) + gui_State.gui_Callback = str2func(varargin{1}); +end + +if nargout + [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); +else + gui_mainfcn(gui_State, varargin{:}); +end +% End initialization code - DO NOT EDIT + + +% --- Executes just before VisualISAM_gui is made visible. +function VisualISAM_gui_OpeningFcn(hObject, eventdata, handles, varargin) +% This function has no output args, see OutputFcn. +% hObject handle to figure +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +% varargin command line arguments to VisualISAM_gui (see VARARGIN) + +% handles.data = VisualISAMData_triangle(); +% handles.frame_i = 3; +% handles.isam = visualSLAMISAM; +% handles.results = visualSLAMValues; + +% Choose default command line output for VisualISAM_gui +handles.output = hObject; + +% Update handles structure +guidata(hObject, handles); + +% UIWAIT makes VisualISAM_gui wait for user response (see UIRESUME) +% uiwait(handles.figure1); + + +% --- Outputs from this function are returned to the command line. +function varargout = VisualISAM_gui_OutputFcn(hObject, eventdata, handles) +% varargout cell array for returning output args (see VARARGOUT); +% hObject handle to figure +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Get default command line output from handles structure +varargout{1} = handles.output; + + +% --- Executes on button press in generateButton. +function generateButton_Callback(hObject, eventdata, handles) +% hObject handle to generateButton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + handles.data = VisualISAMData_triangle(); + guidata(hObject,handles) + + +% --- Executes on button press in intializeButton. +function intializeButton_Callback(hObject, eventdata, handles) +% hObject handle to intializeButton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + [handles.isam handles.results] = VisualISAMInitialize(handles.data); + handles.frame_i=3; + sprintf('Frame 1,2:') + handles.results + guidata(hObject,handles) + + +% --- Executes on button press in stepButton. +function stepButton_Callback(hObject, eventdata, handles) +% hObject handle to stepButton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + handles.frame_i + handles.results + if (handles.frame_i