diff --git a/matlab/examples/VisualISAMGenerateData.m b/matlab/examples/VisualISAMGenerateData.m new file mode 100644 index 000000000..7ab135168 --- /dev/null +++ b/matlab/examples/VisualISAMGenerateData.m @@ -0,0 +1,50 @@ +VisualISAMGlobalVars +sprintf('vData...') + +%% Generate simulated data +points = {}; +if TRIANGLE % Create a triangle target, just 3 points on a plane + nPoints = 3; + r = 10; + for j=1:nPoints + theta = (j-1)*2*pi/nPoints; + points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]'); + end +else % 3D landmarks as vertices of a cube + nPoints = 8; + 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]')}; +end + +%% Create camera cameras on a circle around the triangle +height = 10; r = 40; +K = gtsamCal3_S2(500,500,0,640/2,480/2); +cameras = {}; +for i=1:NCAMERAS + theta = (i-1)*2*pi/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(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,'*'); + axis([1 640 1 480]); + end + end +end +odometry = cameras{1}.pose.between(cameras{2}.pose); + + +%% Set Noise parameters +poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1); +measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0); \ No newline at end of file diff --git a/matlab/examples/VisualISAMGlobalVars.m b/matlab/examples/VisualISAMGlobalVars.m new file mode 100644 index 000000000..2438a9ea0 --- /dev/null +++ b/matlab/examples/VisualISAMGlobalVars.m @@ -0,0 +1,7 @@ +global TRIANGLE NCAMERAS SHOW_IMAGES +global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE +global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES +global SAVE_FIGURES SAVE_GRAPHS +global nPoints points K cameras odometry +global poseNoise pointNoise odometryNoise measurementNoise +global frame_i isam newFactors initialEstimates result diff --git a/matlab/examples/VisualISAMInitOptions.m b/matlab/examples/VisualISAMInitOptions.m new file mode 100644 index 000000000..a9ef09bb5 --- /dev/null +++ b/matlab/examples/VisualISAMInitOptions.m @@ -0,0 +1,22 @@ +VisualISAMGlobalVars + +%% Data Options +TRIANGLE = true; +NCAMERAS = 20; +SHOW_IMAGES = false; + +%% iSAM Options +HARD_CONSTRAINT = false; +POINT_PRIORS = false; +BATCH_INIT = true; +REORDER_INTERVAL=10; +ALWAYS_RELINEARIZE = false; + +%% Display Options +SAVE_GRAPH = false; +PRINT_STATS = true; +DRAW_INTERVAL = 4; +CAMERA_INTERVAL = 1; +DRAW_TRUE_POSES = false; +SAVE_FIGURES = false; +SAVE_GRAPHS = false; \ No newline at end of file diff --git a/matlab/examples/VisualISAMInitialize.m b/matlab/examples/VisualISAMInitialize.m new file mode 100644 index 000000000..ae48f3531 --- /dev/null +++ b/matlab/examples/VisualISAMInitialize.m @@ -0,0 +1,28 @@ +VisualISAMGlobalVars + +%% Initialize iSAM +isam = visualSLAMISAM(REORDER_INTERVAL); +newFactors = visualSLAMGraph; +initialEstimates = visualSLAMValues; +i1 = symbol('x',1); +camera1 = cameras{1}; +pose1 = camera1.pose; +if HARD_CONSTRAINT % add hard constraint + newFactors.addPoseConstraint(i1,pose1); +else + newFactors.addPosePrior(i1,pose1, poseNoise); +end +initialEstimates.insertPose(i1,pose1); +% Add visual measurement factors from first pose +for j=1:nPoints + jj = symbol('l',j); + if POINT_PRIORS % add point priors + newFactors.addPointPrior(jj, points{j}, pointNoise); + end + zij = camera1.project(points{j}); + newFactors.addMeasurement(zij, measurementNoise, i1, jj, K); + initialEstimates.insertPoint(jj, points{j}); +end + +frame_i = 1 +cla; \ No newline at end of file diff --git a/matlab/examples/VisualISAMPlot.m b/matlab/examples/VisualISAMPlot.m new file mode 100644 index 000000000..549c3ac37 --- /dev/null +++ b/matlab/examples/VisualISAMPlot.m @@ -0,0 +1,42 @@ +VisualISAMGlobalVars + +if (frame_i<2) + sprintf('Cannot plot the first frame') + return +end + +%% Plot results +tic +% h=figure(2);clf +% set(1,'NumberTitle','off','Name','Visual iSAM'); +cla; +hold on; +sprintf('Computing marginals and plotting. Please wait...') +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:frame_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') +sprintf('Done!') + +% figure(2); +t=toc; +if DRAW_INTERVAL~=NCAMERAS, plot(frame_i,t,'b.'); end +if SAVE_FIGURES + print(h,'-dpng',sprintf('VisualiSAM%03d.png',frame_i)); +end +if SAVE_GRAPHS + isam.saveGraph(sprintf('VisualiSAM%03d.dot',frame_i)); +end diff --git a/matlab/examples/VisualISAMStep.m b/matlab/examples/VisualISAMStep.m new file mode 100644 index 000000000..7c86e3ce1 --- /dev/null +++ b/matlab/examples/VisualISAMStep.m @@ -0,0 +1,46 @@ +VisualISAMGlobalVars + +%% Add odometry +newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), odometry, odometryNoise); + +%% Add visual measurement factors +for j=1:nPoints + zij = cameras{frame_i}.project(points{j}); + newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), K); +end + +%% Initial estimates for the new pose. Also initialize points while in the first frame. +%TODO: this might be suboptimal since "result" is not the fully optimized result +if (frame_i==2), prevPose = cameras{1}.pose; +else, prevPose = result.pose(symbol('x',frame_i-1)); end +initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(odometry)); + +%% Update ISAM +if BATCH_INIT & (frame_i==2) % Do a full optimize for first two poses + initialEstimates + fullyOptimized = newFactors.optimize(initialEstimates) + initialEstimates = fullyOptimized; +end +% figure(1);tic; +isam.update(newFactors, initialEstimates); +% t=toc; plot(frame_i,t,'r.'); tic +result = isam.estimate(); +% t=toc; plot(frame_i,t,'g.'); +if ALWAYS_RELINEARIZE % re-linearize + isam.reorder_relinearize(); +end + + +if SAVE_GRAPH + isam.saveGraph(sprintf('VisualiSAM.dot',frame_i)); +end +if PRINT_STATS + isam.printStats(); +end +if mod(frame_i,DRAW_INTERVAL)==0 + VisualISAMPlot +end + +%% Reset newFactors and initialEstimates to prepare for the next update +newFactors = visualSLAMGraph; +initialEstimates = visualSLAMValues; \ No newline at end of file diff --git a/matlab/examples/VisualISAM_gui.fig b/matlab/examples/VisualISAM_gui.fig new file mode 100644 index 000000000..e01fc1214 Binary files /dev/null and b/matlab/examples/VisualISAM_gui.fig differ diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/examples/VisualISAM_gui.m new file mode 100644 index 000000000..baba95f79 --- /dev/null +++ b/matlab/examples/VisualISAM_gui.m @@ -0,0 +1,393 @@ +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 08-Jun-2012 16:03:16 + +% 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) + +% Choose default command line output for VisualISAM_gui +VisualISAMGlobalVars +VisualISAMInitOptions +VisualISAMGenerateData + +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 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) + sprintf('init button') + VisualISAMGlobalVars + DRAW_INTERVAL = str2num(get(handles.drawInterval,'String')) ; + NCAMERAS = str2num(get(handles.numCamEdit,'String')) ; + VisualISAMGenerateData + VisualISAMInitialize + +% --- 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) + VisualISAMGlobalVars + if (frame_i