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

Use GenerateData

release/4.3a0
Frank Dellaert 2012-06-13 12:07:02 +00:00
parent 25b4a15e94
commit 0724bd73f3
1 changed files with 21 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
matlab/examples/VisualSLAMExample.m
View File

@ -16,28 +16,13 @@
% - Each camera sees all landmarks. % - Each camera sees all landmarks.
% - Visual measurements as 2D points are given, corrupted by Gaussian noise. % - Visual measurements as 2D points are given, corrupted by Gaussian noise.
%% Generate simulated data % Data Options
% 3D landmarks as vertices of a cube options.triangle = false;
points = {gtsamPoint3([10 10 10]'),... options.nrCameras = 10;
gtsamPoint3([-10 10 10]'),... options.showImages = false;
gtsamPoint3([-10 -10 10]'),...
gtsamPoint3([10 -10 10]'),...
gtsamPoint3([10 10 -10]'),...
gtsamPoint3([-10 10 -10]'),...
gtsamPoint3([-10 -10 -10]'),...
gtsamPoint3([10 -10 -10]')};
% Camera cameras on a circle around the cube, pointing at the world origin %% Generate data
nCameras = 6; [data,truth] = VisualISAMGenerateData(options);
height = 0;
r = 30;
cameras = {};
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]');
cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K);
end
measurementNoiseSigma = 1.0; measurementNoiseSigma = 1.0;
pointNoiseSigma = 0.1; pointNoiseSigma = 0.1;
@ -48,29 +33,28 @@ graph = visualSLAMGraph;
%% Add factors for all measurements %% Add factors for all measurements
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
for i=1:nCameras for i=1:size(data.z,1)
for j=1:size(points,2) for j=1:size(data.z,2)
zij = cameras{i}.project(points{j}); % you can add noise here if desired graph.addMeasurement(data.z{i,j}, measurementNoise, symbol('x',i), symbol('l',j), data.K);
graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
end end
end end
%% Add Gaussian priors for a pose and a landmark to constrain the system %% Add Gaussian priors for a pose and a landmark to constrain the system
posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas); posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas);
graph.addPosePrior(symbol('x',1), cameras{1}.pose, posePriorNoise); graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma); pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise); graph.addPointPrior(symbol('l',1), truth.points{1}, pointPriorNoise);
%% Print the graph %% Print the graph
graph.print(sprintf('\nFactor graph:\n')); graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy cameras and points %% Initialize cameras and points to ground truth in this example
initialEstimate = visualSLAMValues; initialEstimate = visualSLAMValues;
for i=1:size(cameras,2) for i=1:size(truth.cameras,2)
initialEstimate.insertPose(symbol('x',i), cameras{i}.pose); initialEstimate.insertPose(symbol('x',i), truth.cameras{i}.pose);
end end
for j=1:size(points,2) for j=1:size(truth.points,2)
initialEstimate.insertPoint(symbol('l',j), points{j}); initialEstimate.insertPoint(symbol('l',j), truth.points{j});
end end
initialEstimate.print(sprintf('\nInitial estimate:\n ')); initialEstimate.print(sprintf('\nInitial estimate:\n '));
@ -82,18 +66,19 @@ result.print(sprintf('\nFinal result:\n '));
marginals = graph.marginals(result); marginals = graph.marginals(result);
cla cla
hold on; hold on;
for j=1:size(points,2) for j=1:result.nrPoints
P = marginals.marginalCovariance(symbol('l',j)); P = marginals.marginalCovariance(symbol('l',j));
point_j = result.point(symbol('l',j)); point_j = result.point(symbol('l',j));
plot3(point_j.x, point_j.y, point_j.z,'marker','o'); plot3(point_j.x, point_j.y, point_j.z,'marker','o');
covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
end end
for i=1:size(cameras,2) for i=1:result.nrPoses
P = marginals.marginalCovariance(symbol('x',i)) P = marginals.marginalCovariance(symbol('x',i));
pose_i = result.pose(symbol('x',i)) pose_i = result.pose(symbol('x',i));
plotPose3(pose_i,P,10); plotPose3(pose_i,P,10);
end end
axis([-35 35 -35 35 -15 15]); axis([-35 35 -35 35 -15 15]);
axis equal axis equal
view(-37,40) view(-37,40)
colormap hot