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

simplify VisualSLAMExample code in MATLAB

release/4.3a0
Duy-Nguyen Ta 2012-06-06 09:39:55 +00:00
parent a8ffa407ae
commit 3a28baf3c8
2 changed files with 108 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
examples/matlab/VisualSLAMExample.m
View File

@ -28,61 +28,40 @@ points = {gtsamPoint3([10 10 10]'),...
gtsamPoint3([10 -10 -10]')};
% Camera poses on a circle around the cube, pointing at the world origin
nCameras = 4;
nCameras = 6;
height = 0;
r = 30;
poses = {};
for i=1:nCameras
theta = (i-1)*2*pi/nCameras;
pose_i = gtsamPose3(...
gtsamRot3([-sin(theta) 0 -cos(theta);
cos(theta) 0 -sin(theta);
0 -1 0]),...
gtsamPoint3([r*cos(theta), r*sin(theta), 0]'));
poses = [poses {pose_i}];
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
camera = gtsamSimpleCamera_lookat(t, gtsamPoint3(), gtsamPoint3([0,0,1]'), gtsamCal3_S2())
poses{i} = camera.pose();
end
% 2D visual measurements, simulated with Gaussian noise
z = {};
measurementNoiseSigmas = [0.5,0.5]';
measurementNoiseSampler = gtsamSharedDiagonal(measurementNoiseSigmas);
K = gtsamCal3_S2(50,50,0,50,50);
for i=1:size(poses,2)
zi = {};
camera = gtsamSimpleCamera(K,poses{i});
for j=1:size(points,2)
zi = [zi {camera.project(points{j}).compose(gtsamPoint2(measurementNoiseSampler.sample()))}];
end
z = [z; zi];
end
pointNoiseSigmas = [0.1,0.1,0.1]';
pointNoiseSampler = gtsamSharedDiagonal(pointNoiseSigmas);
measurementNoiseSigma = 1.0;
pointNoiseSigma = 0.1;
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
poseNoiseSampler = gtsamSharedDiagonal(poseNoiseSigmas);
%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
graph = visualSLAMGraph;
%% Add factors for all measurements
measurementNoise = gtsamSharedNoiseModel_Sigmas(measurementNoiseSigmas);
for i=1:size(z,1)
for j=1:size(z,2)
graph.addMeasurement(z{i,j}, measurementNoise, symbol('x',i), symbol('l',j), K);
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
K = gtsamCal3_S2(500,500,0,640/2,480/2);
for i=1:nCameras
camera = gtsamSimpleCamera(K,poses{i});
for j=1:size(points,2)
zij = camera.project(points{j}); % you can add noise here if desired
graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
end
end
%% Add Gaussian priors for a pose and a landmark to constrain the system
% posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas);
% graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas);
posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas);
graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas);
graph.addPointPrior(symbol('l',8), points{8}, pointPriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas);
graph.addPointPrior(symbol('l',5), points{5}, pointPriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas);
graph.addPointPrior(symbol('l',4), points{4}, pointPriorNoise);
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
@ -90,10 +69,10 @@ graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy poses and points
initialEstimate = visualSLAMValues;
for i=1:size(poses,2)
initialEstimate.insertPose(symbol('x',i), poses{i}.compose(gtsamPose3_Expmap(poseNoiseSampler.sample())));
initialEstimate.insertPose(symbol('x',i), poses{i});
end
for j=1:size(points,2)
initialEstimate.insertPoint(symbol('l',j), points{j}.compose(gtsamPoint3(pointNoiseSampler.sample())));
initialEstimate.insertPoint(symbol('l',j), points{j});
end
initialEstimate.print(sprintf('\nInitial estimate:\n '));
@ -101,10 +80,8 @@ initialEstimate.print(sprintf('\nInitial estimate:\n '));
result = graph.optimize(initialEstimate);
result.print(sprintf('\nFinal result:\n '));
%% Query the marginals
marginals = graph.marginals(result);
%% Plot results with covariance ellipses
marginals = graph.marginals(result);
figure(1);clf
hold on;
for j=1:size(points,2)

88
examples/matlab/VisualSLAMExample_triangle.m Normal file
View File

@ -0,0 +1,88 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief A simple visual SLAM example for structure from motion
% @author Duy-Nguyen Ta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a triangle target, just 3 points on a plane
r = 10;
points = {};
for j=1:3
theta = (j-1)*2*pi/3;
points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
end
%% Create camera poses on a circle around the triangle
nCameras = 6;
height = 10;
r = 30;
poses = {};
for i=1:nCameras
theta = (i-1)*2*pi/nCameras;
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
camera = gtsamSimpleCamera_lookat(t, gtsamPoint3(), gtsamPoint3([0,0,1]'), gtsamCal3_S2())
poses{i} = camera.pose();
end
%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
graph = visualSLAMGraph;
%% Add factors for all measurements
K = gtsamCal3_S2(500,500,0,640/2,480/2);
measurementNoiseSigma=1; % in pixels
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
for i=1:nCameras
camera = gtsamSimpleCamera(K,poses{i});
for j=1:3
zij = camera.project(points{j}); % you can add noise here if desired
graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
end
end
%% Add Gaussian priors for 3 points to constrain the system
pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,0.1);
for j=1:3
graph.addPointPrior(symbol('l',j), points{j}, pointPriorNoise);
end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy poses and points
initialEstimate = visualSLAMValues;
for i=1:size(poses,2)
initialEstimate.insertPose(symbol('x',i), poses{i});
end
for j=1:size(points,2)
initialEstimate.insertPoint(symbol('l',j), points{j});
end
initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result.print(sprintf('\nFinal result:\n '));
%% Plot results with covariance ellipses
marginals = graph.marginals(result);
figure(1);clf
hold on;
for j=1:size(points,2)
P = marginals.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 i=1:size(poses,2)
P = marginals.marginalCovariance(symbol('x',i))
pose_i = result.pose(symbol('x',i))
plotPose3(pose_i,P,10);
end
axis equal