From 8644f81dc8cb4ce7175bde27a1b69f628f4cf797 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 7 Jun 2012 13:16:28 +0000 Subject: [PATCH] VisualiSAM, going crazy. MATLAB is amazing! --- examples/matlab/VisualISAMExample.m | 77 +++++++++---- examples/matlab/iSAMgui.fig | Bin 0 -> 5766 bytes examples/matlab/iSAMgui.m | 171 ++++++++++++++++++++++++++++ 3 files changed, 225 insertions(+), 23 deletions(-) create mode 100644 examples/matlab/iSAMgui.fig create mode 100644 examples/matlab/iSAMgui.m diff --git a/examples/matlab/VisualISAMExample.m b/examples/matlab/VisualISAMExample.m index 89947cd40..e1116552d 100644 --- a/examples/matlab/VisualISAMExample.m +++ b/examples/matlab/VisualISAMExample.m @@ -12,15 +12,25 @@ clear -%% Set Options here +%% Data Options TRIANGLE = false; -NCAMERAS = 10; +NCAMERAS = 20; SHOW_IMAGES = false; + +%% iSAM Options HARD_CONSTRAINT = false; POINT_PRIORS = false; BATCH_INIT = true; +REORDER_INTERVAL=10; ALWAYS_RELINEARIZE = false; -DRAW_TRUE_POSES = true; + +%% Display Options +SAVE_GRAPH = false; +DRAW_INTERVAL = 20; +CAMERA_INTERVAL = 1; +DRAW_TRUE_POSES = false; +SAVE_FIGURES = false; +SAVE_GRAPHS = false; %% Generate simulated data if TRIANGLE % Create a triangle target, just 3 points on a plane @@ -50,7 +60,8 @@ for i=1:NCAMERAS t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K); if SHOW_IMAGES % show images - figure(i);clf;hold on + figure(2+i);clf;hold on + set(2+i,'NumberTitle','off','Name',sprintf('Camera %d',i)); for j=1:nPoints zij = cameras{i}.project(points{j}); plot(zij.x,zij.y,'*'); @@ -68,7 +79,7 @@ pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1); measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0); %% Initialize iSAM -isam = visualSLAMISAM; +isam = visualSLAMISAM(REORDER_INTERVAL); newFactors = visualSLAMGraph; initialEstimates = visualSLAMValues; i1 = symbol('x',1); @@ -92,6 +103,8 @@ for j=1:nPoints end %% Run iSAM Loop +figure(1);clf;hold on; +set(1,'NumberTitle','off','Name','iSAM timing'); for i=2:NCAMERAS %% Add odometry @@ -115,33 +128,51 @@ for i=2:NCAMERAS fullyOptimized = newFactors.optimize(initialEstimates) initialEstimates = fullyOptimized; end + figure(1);tic; isam.update(newFactors, initialEstimates); + t=toc; plot(i,t,'r.'); tic result = isam.estimate(); + t=toc; plot(i,t,'g.'); if ALWAYS_RELINEARIZE % re-linearize isam.reorder_relinearize(); end - %% Plot results - figure(NCAMERAS+1);clf - hold on; - for j=1:size(points,2) - P = isam.marginalCovariance(symbol('l',j)); - point_j = result.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],P); + if SAVE_GRAPH + isam.saveGraph(sprintf('VisualiSAM.dot',i)); end - for ii=1:i - P = isam.marginalCovariance(symbol('x',ii)); - pose_ii = result.pose(symbol('x',ii)); - plotPose3(pose_ii,P,10); - if DRAW_TRUE_POSES % show ground truth - plotPose3(cameras{ii}.pose,0.001*eye(6),10); + if mod(i,DRAW_INTERVAL)==0 + %% Plot results + tic + h=figure(2);clf + set(1,'NumberTitle','off','Name','Visual iSAM'); + hold on; + for j=1:size(points,2) + P = isam.marginalCovariance(symbol('l',j)); + point_j = result.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],P); + end + for ii=1:CAMERA_INTERVAL:i + P = isam.marginalCovariance(symbol('x',ii)); + pose_ii = result.pose(symbol('x',ii)); + plotPose3(pose_ii,P,10); + if DRAW_TRUE_POSES % show ground truth + plotPose3(cameras{ii}.pose,0.001*eye(6),10); + end + end + axis([-40 40 -40 40 -10 20]);axis equal + view(3) + colormap('hot') + figure(2); + t=toc; + if DRAW_INTERVAL~=NCAMERAS, plot(i,t,'b.'); end + if SAVE_FIGURES + print(h,'-dpng',sprintf('VisualiSAM%03d.png',i)); + end + if SAVE_GRAPHS + isam.saveGraph(sprintf('VisualiSAM%03d.dot',i)); end end - axis([-40 40 -40 40 -10 20]);axis equal - view(2) - colormap('hot') - %print(h,'-dpng',sprintf('VisualISAM_%03d.png',i)); %% Reset newFactors and initialEstimates to prepare for the next update newFactors = visualSLAMGraph; diff --git a/examples/matlab/iSAMgui.fig b/examples/matlab/iSAMgui.fig new file mode 100644 index 0000000000000000000000000000000000000000..5f6154b49e54aa88b51c014d3ee2c3fa8d17f47f GIT binary patch literal 5766 zcma)AXE+;Z4~Py$$bhXC|L0ssJcfP|!ytgMo>JU~i9QVQ_@g#r`F z|A6$V5#7I4Dy^L>@k`!^1Oq-kpT@8^Yt7?o!!AYXuz3pn?tKehY>2x|y1gYtB zhYf0KYCm8L|4#K&PSm4^MNwVcEeDdV(UtM`^}SSiw=7lnPy4zp-!o(4S?^IrP%w|n zy&K0OQr`?v4b;56bD^d$g*&SoIR$L13jE=@@*3!GCX12?p#PO2p+8+OA?B7KHb7r5 zQGJh@DNDs}Y&ow@r(KTf5E@|5o+tU{{VByC3-&B5%dR=?B4&Z9fYL`#x=fA}<@W2C zrEi^Uf|nAujrh)KJ48F>ny^aKx&es1)!iz^>uE=RkX5&N6K;E_b8}Lcey$8odcW92 z=Rn(AYgN0$;ju=_==LbiPI=FseBSDsYUwg}!r> z)(;^kIBqz`6YpP&$79LYm@m~gVk!C_A8%?Up_6WacZS=T-&u^TiUFr$r(2$5#4!L2 zFNPoU2qTCQ#)x3n{vD`|u8zlI<4V}IO^LurCUUs6&OZx_fB8>S?(4x%pGTh4fU`rZ41-?N}<4SaV$T3mS)PC^x2a`8w}mGAiF7tvm=pJQ+!%i8HQ-On+K z+t`}i!=Z4?n9MgWF|18mn!%>dG|N`QWKp*By@s@B@-}p3jAuSq&@KttlM&A}s}x$m zbPzMo0PaP>k27(sY)!1`wDWB8d=OtQ>VpjG?;#t!)U}#Tj%3& z%ghDEDxIl`3W~VBDDheNNAr)M3*cAkAL)Xdeq}UMPOT5KkFKtqG(z>TygGq33vFy1 zj;=p9xAz4{eV?!fKyeG;L*U&$_rnIpX9`CmzFxmw45a-+kqMJhw|`at{UkI<#sAKj zO>&hQ8DSfpcUV_8dbLlU0TNr-R0q$wiq%)PGNnslYuc#eu(}gR|e8Z#Hb$bmF_Fc?PY<1bnYO8Zos^9QS7;MMa zBJ7$9q@=e=G=jMT_DyHXpBODWRcm`d6c^N}n8--6z8>cYeT>g~GxP^;GNlTRvc1Xf zWFT*Uz8sG0KWEBkt;&miXg{U>-oa&JwZ&?e0s={boKhW zyl=z!4?ZCM#5Lldx!8MJ3mJ(eNf*iX;eSVzZraefM6vYfs&C!SYpqV)m*?A}hi@Tk z#3MqQv#*U@S?>ir-{=sfc-#54EcfIEQ`<}lIzdAow!O=Ty4p3lqbA@$%o^wzISE_} z4_qTMo*j;d4WSxG>qFgmlxLH65QQ)zDG3RYh_Z!eGHNq6`-I9jKH*JzVw?ops3s*J zcp|Mlk)Z4!B9(Xmk%!Qkl!JAr@}3W{A~RRlmpH#ggM5h z35EI-zMM$sqG7*ID+@Yca`33yUAhY5qT%{gPu-!P{G(7d3gv?Idtrt0=iRl4VGhLb z(mld{NYFhdPkDKwvrIt20>J~njt(jsaTYoLmI2{WeOsSiwi*cE$3~~^&{8_Y8Zf_K!}4)07OM11{>!_vYZ5-W6IO%F#xbUfal z2B%8f4T-xDf0`aAQurHpvAE{;q&DU#dBa|`2CoLz7ChqB1W0u#u5?)&FMf_RO)IAB>dbWKq zn{Y$^O~o@daA!(L6+mR-m9k+%3VH3i!iG2Ei=@~zgy)Mig6 zLB=-fr$9ekc;LJaLyxXoFxZqgneU$W*8#pQ=cLU`6<&EjM*7mvXrTOyapBA3>p;y5wx7*$cbhKGBnlX{#hfgZ9Au?+60gbsD$vBxd3diz_jJ0(VNCRYBNM?wISpSY&VeKc!rtOU$-=*}09X*qC_3 zW&HF{N9SRsC%Xd1KyF=&JWOn-9 zIUz%sCvsSwBE#M{FN?t9KMj8z!$$1QqK`f=_FZ?(g!Iss>&q(n8K5E7yqo4;oRZxE z%6q2YTGJ-t>*DQh?NE$&u8cWb=meKnHx;ipkZ z%^Zu$jAZjI%3+!Psd;ZoASHy-6PV<^vI8Eug2L`T5LFkE7q<@c8td5_?Gu*uD83l9UH2@T7$)-#BkHV;YB?@yN<<@Op zjM-JK=h8uq+m6jFAF?n#QuSJ-bC_bA6O#H4N(pJ{{(P|_+OW15?7bE=01YdBrYa&$ z`8uG<4IujL?wo*&OEwjwb^hkqkQ_Qw35j_m%l#P( z^R1p~Js&@(G)i#$x<1acV&V>I6jrDX$a_|Pt8-K%CbX69WxW5Y(%d?8uFWQ{9WKr8jhOOR7EcXM`!xx+qpYN%P%=aUT5gi5c zz<7COLj8F7pwRd%qH-5~5$+lisO9@%lSJr1^*BdBg!3_oD_ixB4l0Vt-r%$E_62>j zO|22zQehXvo7MDzcapX|w=J&fft7I$SWEr|%zH>GnprfGo#T}Xe=2oG)E|x0Z=KLN z_m8*P<;S3TW`DrWAA_XWUsU|`N@|l%jrGp*%JRV2(+ucH2KLd#&Tde4uasp1QtqkT z%%fL+j5k<#w<>)q!#~}>)BODy5Cea5$@*GxQ?w~`Fo`%d~hOoqGQKQ zJcy#+Q9syn_Hk@<_i-$qAO2s7Y7GhESijAd)U|ES3^RpQz_QEGsz=;K-3oZvWgx+| zIqiRKW;+?kAF`;G{2*Sg2>F+9^{U3bqu=ruQ7K-nG#0ZC@KY94egx zEs4dF)LSM7Z}gL*p04yq|BdFq{|^Z{DB zJ_X&|!`{TiNb`}zX+fVs!8?NX`F#2$3L`ChAnts))eb(JEXP7Lhq_2ezL3{ucIRQE zUbDv!J9|1}OB|U0p*wpxi_)dmoIkxLtH;M2YA?;+Fb%n02Kuxh3V+^N$44sr{&aaS zF4lh3VmpW296E#))1ndac)0`RJKhmQlOVC33+HBz+{a`~2Q)it76I?EZ0NGm z8ze>B*tJ5dLs^3thYm6w!|2!)?}Le{CPJ8ji4vixaE&)HfMh+ZCi-Y;!U;eF(HlC2rt>0~3^H&k{3!#b4|?Mm?0OU3K1? zhbWc^LKC9r%Mpk=_N|BKZ90^nCV_@>d^lTkOem>Rd1&3<<^z|= zeuXzMM0qtwBEc`NW^`8~WmMaR7v$HUn_XHGTv=22Nf+AyBDg6!b0qc_KjB*?JtQ%LIQqDNfSb_gLPVu0b>`Jpvrfyi^VgHAh7v zj=}vg=-*uaDu`of9{=SdA@e3=z2J{te@QwcL}Wl5phU#a%|_R z1s-m|;H=V#Gr^}Z$3XG^2at&MtLx#+5g2!|J4B}UL%9vsarP?rBEPvtSh6NiPmWW@ zP}Roml*xsXGcQaHU7h_r)|%LWH$T3_Om3rD3G@mKnGJ>fWZBHX(uwW;2ytNZXynJ} z5KA{nrD-oHa>&t60olY{arj{In{&j3WI7}to$nM+#*wqfwI*&qKMJ|M=M=)P#$AYJ zc-+d6%^mn0A25%7gQjA|U6&-luy0`v@Dx~J2Q#{@R7ny3(F>V2HXL33Yl?rSK z6bau~fA9`^UUS#I6#dQz3bToP!C=y%i!*@ir3`t+l`^SIC3`pPI_^lmk8%lgtvTR1 z;T#IfPIuK)qy%wUHko|KxE#g7-+V5O?K{9>X=160Be)v7Qc{K-pL;8hf(y1B^i-J{ z5qrB94?b8(OG^)4hOK0AgHMGUUpT_A@GiDJ6DM&X4qIiOOmRk_)A8l~n41>R4cVac zts)@d&VO?I&_=_TL4&ch(rBqZ8z$(DPR=pX+o(^7_klUo-~U zi}m-Q0rMoi(N`@gf#k&#ip#wf9{bMofAD?9T3fGeYyXG4eu@$OLjcIXp6?IlIaP3^ zTqqgPP~VX-OZCq4mY*?pF-TnG`K;nsMeijym(^jZ^HjdpiIn0s{H!OM(eK+*TBkct? z8MSlwzXtsD4h}I2lBEA2Hxx#HzT^s0aHZpjiG6Td&63=r$>98^-dwC7XFW=hc%DEP zVA)^QQ8jnx&vRleTs1Q8nOFI!IR8G3Wznv5Yhvm_2wUpv?O&~e4gLAhMlCWo4~e2G zOj%`!qm_Qvhkei4*B4zOohJ(CFe~60Zkcmn3!E|!uy&nMzq5=r;&I9~q|kl^;Xkv& z;}u)+xDt}|J>5gsfNkVUImeI2nUjtxYB`1v8B7t;dx@Pb3SDfs37FxZCT=mkn+`bo7(c|V?c7BN zaU;=YJuxCiJ8!Svvfm8GSzAnZEt}H15Rc<+99+_V)2%mfiB=`b_yO_ki z5+)JyPrPI)lzTNi-^=Vm<3*Z^HG~Vk@Wy)Vi)@T5EE+l4g^sfsK7J(jdHB|>zjCtS zaH(>~jwFvmhTC(a#SI?qz6ZG40;uw7bn?WXC%zS6@87pabD`JLh4U27@ z^1{ShRQI++_W$|gKsZSr7pg13o)TO`1um+<&Natts-L&>Da+PlE=#WzuB9S~S33JU z`T3>k&S>+p7_l3!u5;nvhOsA|L2kQkjp#?Q#3DFd!pK#n9Kz|-Z-uS>CwH0SfI7^br3#1Ogo>uWv-!^{hHMjzDQqui$HpsMRoml;N7(^`k>}hj|z^&SJ6vn<)~NAb|?B+`0$xepUAA8^@`}}nWvo!@WY}^&D9p0QJ>nk z3cKTsjrfi7jqHupq+-=%hsWj8fPGH&IWqM*E%iA*^|_w7S}ZwNyIa4kERps^`IJC@ zqqUitxPF}N7&G3Rw~T&&ajiBE^e4#07QrUhkwj+#L=PkZcqwq S#A7^nFj4xPMleAk<-Y)|5uDBd literal 0 HcmV?d00001 diff --git a/examples/matlab/iSAMgui.m b/examples/matlab/iSAMgui.m new file mode 100644 index 000000000..71cd53855 --- /dev/null +++ b/examples/matlab/iSAMgui.m @@ -0,0 +1,171 @@ +function varargout = iSAMgui(varargin) +% ISAMGUI MATLAB code for iSAMgui.fig +% ISAMGUI, by itself, creates a new ISAMGUI or raises the existing +% singleton*. +% +% H = ISAMGUI returns the handle to a new ISAMGUI or the handle to +% the existing singleton*. +% +% ISAMGUI('CALLBACK',hObject,eventData,handles,...) calls the local +% function named CALLBACK in ISAMGUI.M with the given input arguments. +% +% ISAMGUI('Property','Value',...) creates a new ISAMGUI or raises the +% existing singleton*. Starting from the left, property value pairs are +% applied to the GUI before iSAMgui_OpeningFcn gets called. An +% unrecognized property name or invalid value makes property application +% stop. All inputs are passed to iSAMgui_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 iSAMgui + +% Last Modified by GUIDE v2.5 07-Jun-2012 01:55:37 + +% Begin initialization code - DO NOT EDIT +gui_Singleton = 1; +gui_State = struct('gui_Name', mfilename, ... + 'gui_Singleton', gui_Singleton, ... + 'gui_OpeningFcn', @iSAMgui_OpeningFcn, ... + 'gui_OutputFcn', @iSAMgui_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 iSAMgui is made visible. +function iSAMgui_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 iSAMgui (see VARARGIN) + +% Choose default command line output for iSAMgui +handles.output = hObject; + +% Update handles structure +guidata(hObject, handles); + +% This sets up the initial plot - only do when we are invisible +% so window can get raised using iSAMgui. +if strcmp(get(hObject,'Visible'),'off') + plot(rand(5)); +end + +% UIWAIT makes iSAMgui wait for user response (see UIRESUME) +% uiwait(handles.figure1); + + +% --- Outputs from this function are returned to the command line. +function varargout = iSAMgui_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 pushbutton1. +function pushbutton1_Callback(hObject, eventdata, handles) +% hObject handle to pushbutton1 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +axes(handles.axes1); +cla; + +popup_sel_index = get(handles.popupmenu1, 'Value'); +switch popup_sel_index + case 1 + plot(rand(5)); + case 2 + plot(sin(1:0.01:25.99)); + case 3 + bar(1:.5:10); + case 4 + plot(membrane); + case 5 + surf(peaks); +end + + +% -------------------------------------------------------------------- +function FileMenu_Callback(hObject, eventdata, handles) +% hObject handle to FileMenu (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + + +% -------------------------------------------------------------------- +function OpenMenuItem_Callback(hObject, eventdata, handles) +% hObject handle to OpenMenuItem (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +file = uigetfile('*.fig'); +if ~isequal(file, 0) + open(file); +end + +% -------------------------------------------------------------------- +function PrintMenuItem_Callback(hObject, eventdata, handles) +% hObject handle to PrintMenuItem (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +printdlg(handles.figure1) + +% -------------------------------------------------------------------- +function CloseMenuItem_Callback(hObject, eventdata, handles) +% hObject handle to CloseMenuItem (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +selection = questdlg(['Close ' get(handles.figure1,'Name') '?'],... + ['Close ' get(handles.figure1,'Name') '...'],... + 'Yes','No','Yes'); +if strcmp(selection,'No') + return; +end + +delete(handles.figure1) + + +% --- Executes on selection change in popupmenu1. +function popupmenu1_Callback(hObject, eventdata, handles) +% hObject handle to popupmenu1 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: contents = get(hObject,'String') returns popupmenu1 contents as cell array +% contents{get(hObject,'Value')} returns selected item from popupmenu1 + + +% --- Executes during object creation, after setting all properties. +function popupmenu1_CreateFcn(hObject, eventdata, handles) +% hObject handle to popupmenu1 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: popupmenu 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 + +set(hObject, 'String', {'plot(rand(5))', 'plot(sin(1:0.01:25))', 'bar(1:.5:10)', 'plot(membrane)', 'surf(peaks)'}); + + +% --- Executes on button press in pushbutton4. +function pushbutton4_Callback(hObject, eventdata, handles) +% hObject handle to pushbutton4 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA)