12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

GenerateData is now a function, no globals

release/4.3a0
Frank Dellaert 2012-06-10 04:25:05 +00:00
parent 17a2793a38
commit 7eb449c205
8 changed files with 90 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
matlab/examples/VisualISAMDemo.m
View File

@ -3,11 +3,10 @@
% Make sure global variables are visible on command prompt
% so you can examine how they change as you step through
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 SHOW_TIMING
global points K cameras odometry
global data
global poseNoise pointNoise odometryNoise measurementNoise
global frame_i isam newFactors initialEstimates result

18
matlab/examples/VisualISAMExample.m
View File

@ -11,16 +11,11 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Global variables used in VisualISAMExample
global TRIANGLE NCAMERAS SHOW_IMAGES
global data
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
%% Setting data options
TRIANGLE = false;
NCAMERAS = 20;
SHOW_IMAGES = false;
%% iSAM Options
HARD_CONSTRAINT = false;
POINT_PRIORS = false;
@ -37,14 +32,19 @@ DRAW_TRUE_POSES = false;
SAVE_FIGURES = false;
SAVE_GRAPHS = false;
%% Generate data and initialize iSAM with the first pose and points
VisualISAMGenerateData
%% Generate data
options.triangle = false;
options.nrCameras = 20;
showImages = false;
data = VisualISAMGenerateData(options,showImages);
%% Initialize iSAM with the first pose and points
VisualISAMInitialize
figure;
VisualISAMPlot
%% Main loop for iSAM: stepping through all poses
for frame_i=2:NCAMERAS
for frame_i=2:options.nrCameras
VisualISAMStep
if mod(frame_i,DRAW_INTERVAL)==0
VisualISAMPlot

18
matlab/examples/VisualISAMExample_triangle.m
View File

@ -11,16 +11,11 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Global variables used in VisualISAMExample
global TRIANGLE NCAMERAS SHOW_IMAGES
global data
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
%% Setting data options
TRIANGLE = true;
NCAMERAS = 10;
SHOW_IMAGES = false;
%% iSAM Options
HARD_CONSTRAINT = false;
POINT_PRIORS = false;
@ -37,14 +32,19 @@ DRAW_TRUE_POSES = false;
SAVE_FIGURES = false;
SAVE_GRAPHS = false;
%% Generate data and initialize iSAM with the first pose and points
VisualISAMGenerateData
%% Generate data
options.triangle = true;
options.nrCameras = 10;
showImages = false;
data = VisualISAMGenerateData(options,showImages);
%% Initialize iSAM with the first pose and points
VisualISAMInitialize
figure;
VisualISAMPlot
%% Main loop for iSAM: stepping through all poses
for frame_i=2:NCAMERAS
for frame_i=2:options.nrCameras
VisualISAMStep
if mod(frame_i,DRAW_INTERVAL)==0
VisualISAMPlot

48
matlab/examples/VisualISAMGenerateData.m
View File

@ -1,25 +1,20 @@
function data = VisualISAMGenerateData(options, showImages)
% VisualISAMGenerateData: create data for viusalSLAM::iSAM examples
% Authors: Duy Nguyen Ta and Frank Dellaert
% options
global TRIANGLE NCAMERAS SHOW_IMAGES
% global outputs
global points cameras K odometry
global poseNoise odometryNoise pointNoise measurementNoise % data ?
if nargin<2, showImages=false; end
%% Generate simulated data
points = {};
if TRIANGLE % Create a triangle target, just 3 points on a plane
data.points = {};
if options.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]');
data.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]'),...
data.points = {gtsamPoint3([10 10 10]'),...
gtsamPoint3([-10 10 10]'),...
gtsamPoint3([-10 -10 10]'),...
gtsamPoint3([10 -10 10]'),...
@ -31,29 +26,28 @@ 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 = {};
gui = gcf;
for i=1:NCAMERAS
theta = (i-1)*2*pi/NCAMERAS;
data.K = gtsamCal3_S2(500,500,0,640/2,480/2);
data.cameras = {};
for i=1:options.nrCameras
theta = (i-1)*2*pi/options.nrCameras;
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
data.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), data.K);
end
%% show images if asked
if showImages
gui = gcf;
for i=1:options.nrCameras
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});
zij = data.cameras{i}.project(data.points{j});
plot(zij.x,zij.y,'*');
axis([1 640 1 480]);
end
end
figure(gui);
end
figure(gui);
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);
%% Calculate odometry between cameras
data.odometry = data.cameras{1}.pose.between(data.cameras{2}.pose);

28
matlab/examples/VisualISAMInitialize.m
View File

@ -3,36 +3,46 @@ function VisualISAMStep
% Authors: Duy Nguyen Ta and Frank Dellaert
% global variables, input
global cameras points pointNoise poseNoise measurementNoise K
global data pointNoise poseNoise measurementNoise
% global variables, output
global isam newFactors initialEstimates frame_i result
global poseNoise odometryNoise pointNoise measurementNoise
% options
global REORDER_INTERVAL HARD_CONSTRAINT POINT_PRIORS
%% Initialize iSAM
isam = visualSLAMISAM(REORDER_INTERVAL);
%% 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);
%% Add constraints/priors
newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;
i1 = symbol('x',1);
camera1 = cameras{1};
camera1 = data.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:size(points,2)
initialEstimates.insertPose(i1,pose1); % TODO: should not be from ground truth!
%% Add visual measurement factors from first pose
for j=1:size(data.points,2)
jj = symbol('l',j);
if POINT_PRIORS % add point priors
newFactors.addPointPrior(jj, points{j}, pointNoise);
newFactors.addPointPrior(jj, data.points{j}, pointNoise);
end
zij = camera1.project(points{j});
newFactors.addMeasurement(zij, measurementNoise, i1, jj, K);
initialEstimates.insertPoint(jj, points{j});
zij = camera1.project(data.points{j});
newFactors.addMeasurement(zij, measurementNoise, i1, jj, data.K);
initialEstimates.insertPoint(jj, data.points{j}); % TODO: should not be from ground truth!
end
frame_i = 1;

9
matlab/examples/VisualISAMPlot.m
View File

@ -2,17 +2,16 @@
% Authors: Duy Nguyen Ta and Frank Dellaert
% global variables, input
global points cameras frame_i isam result
global data frame_i isam result
% options
global CAMERA_INTERVAL DRAW_TRUE_POSES SAVE_FIGURES SAVE_GRAPHS
%% Plot results
tic
h=gca;
cla(h);
hold on;
for j=1:size(points,2)
for j=1:size(data.points,2)
point_j = result.point(symbol('l',j));
plot3(point_j.x, point_j.y, point_j.z,'marker','o');
if (frame_i>1)
@ -29,15 +28,13 @@ for ii=1:CAMERA_INTERVAL:frame_i
end
plotPose3(pose_ii,P,10);
if DRAW_TRUE_POSES % show ground truth
plotPose3(cameras{ii}.pose,0.001*eye(6),10);
plotPose3(data.cameras{ii}.pose,0.001*eye(6),10);
end
end
axis([-40 40 -40 40 -10 20]);axis equal
view(3)
colormap('hot')
t=toc;
% if DRAW_INTERVAL~=NCAMERAS, plot(frame_i,t,'b.'); end
if SAVE_FIGURES
fig2 = figure('visible','off');
newax = copyobj(h,fig2);

18
matlab/examples/VisualISAMStep.m
View File

@ -3,8 +3,8 @@ function VisualISAMStep
% Authors: Duy Nguyen Ta and Frank Dellaert
% global variables, input
global frame_i odometry odometryNoise newFactors initialEstimates
global points cameras measurementNoise K
global frame_i odometryNoise measurementNoise newFactors initialEstimates
global data
% global variables, input/output
global isam
@ -25,18 +25,18 @@ if frame_i > 2
end
%% Add odometry
newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), odometry, odometryNoise);
newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), data.odometry, odometryNoise);
%% Add visual measurement factors
for j=1:size(points,2)
zij = cameras{frame_i}.project(points{j});
newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), K);
for j=1:size(data.points,2)
zij = data.cameras{frame_i}.project(data.points{j});
newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), data.K);
end
%% Initial estimates for the new pose. Also initialize points while in the first frame.
if (frame_i==2), prevPose = cameras{1}.pose;
%% Initial estimates for the new pose. Also initialize data.points while in the first frame.
if (frame_i==2), prevPose = data.cameras{1}.pose;
else, prevPose = result.pose(symbol('x',frame_i-1)); end
initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(odometry));
initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(data.odometry));
%% Update ISAM
if BATCH_INIT & (frame_i==2) % Do a full optimize for first two poses

33
matlab/examples/VisualISAM_gui.m
View File

@ -71,21 +71,15 @@ end
function initOptions(handles)
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
% Setting data options
TRIANGLE = chooseDataset(handles);
NCAMERAS = str2num(get(handles.numCamEdit,'String'));
SHOW_IMAGES = get(handles.showImagesCB,'Value');
global SAVE_FIGURES SAVE_GRAPHS SHOW_TIMING
% iSAM Options
HARD_CONSTRAINT = get(handles.hardConstraintCB,'Value');
POINT_PRIORS = get(handles.pointPriorsCB,'Value');
BATCH_INIT = get(handles.batchInitCB,'Value');
REORDER_INTERVAL = str2num(get(handles.numCamEdit,'String'));
REORDER_INTERVAL = str2num(get(handles.reorderIntervalEdit,'String'));
ALWAYS_RELINEARIZE = get(handles.alwaysRelinearizeCB,'Value');
% Display Options
@ -96,7 +90,7 @@ CAMERA_INTERVAL = str2num(get(handles.cameraIntervalEdit,'String'));
DRAW_TRUE_POSES = get(handles.drawTruePosesCB,'Value');
SAVE_FIGURES = get(handles.saveFiguresCB,'Value');
SAVE_GRAPHS = get(handles.saveGraphsCB,'Value');
SHOW_TIMING = false;
%----------------------------------------------------------
% Callback functions for GUI elements
@ -227,8 +221,19 @@ function saveGraphsCB_Callback(hObject, ~, handles)
% --- Executes on button press in intializeButton.
function intializeButton_Callback(hObject, ~, handles)
global data
% initialize global options
initOptions(handles)
VisualISAMGenerateData
% Generate Data
options.triangle = chooseDataset(handles);
options.nrCameras = str2num(get(handles.numCamEdit,'String'));
showImages = get(handles.showImagesCB,'Value');
data = VisualISAMGenerateData(options, showImages);
% Initialize and plot
VisualISAMInitialize
VisualISAMPlot
showFramei(hObject, handles)
@ -236,8 +241,8 @@ showFramei(hObject, handles)
% --- Executes on button press in runButton.
function runButton_Callback(hObject, ~, handles)
global frame_i NCAMERAS DRAW_INTERVAL
while (frame_i<NCAMERAS)
global frame_i data DRAW_INTERVAL
while (frame_i<size(data.cameras,2))
frame_i = frame_i+1;
showFramei(hObject, handles)
VisualISAMStep
@ -251,8 +256,8 @@ end
% --- Executes on button press in stepButton.
function stepButton_Callback(hObject, ~, handles)
global frame_i NCAMERAS DRAW_INTERVAL
if (frame_i<NCAMERAS)
global frame_i data DRAW_INTERVAL
if (frame_i<size(data.cameras,2))
frame_i = frame_i+1;
showFramei(hObject, handles)
VisualISAMStep