VisualSLAM matlab example

release/4.3a0
Duy-Nguyen Ta 2012-06-05 05:15:26 +00:00
parent 22e71e4374
commit 715d663e4f
3 changed files with 162 additions and 0 deletions

View File

@ -0,0 +1,119 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Assumptions
% - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc...
% - Cameras are on a circle around the cube, pointing at the world origin
% - Each camera sees all landmarks.
% - Visual measurements as 2D points are given, corrupted by Gaussian noise.
%% Generate simulated data
% 3D landmarks as vertices of a cube
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]')};
% Camera poses on a circle around the cube, pointing at the world origin
nCameras = 8;
r = 30;
poses = {};
for i=1:nCameras
theta = i*2*pi/nCameras;
posei = 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 {posei}];
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);
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
poseNoiseSampler = gtsamSharedDiagonal(poseNoiseSigmas);
hold off;
%% 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);
end
end
%% Add Gaussian priors for a pose and a landmark to constraint the system
posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas);
graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas);
graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise);
%% 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}.compose(gtsamPose3_Expmap(poseNoiseSampler.sample())));
end
for j=1:size(points,2)
initialEstimate.insertPoint(symbol('l',j), points{j}.compose(gtsamPoint3(pointNoiseSampler.sample())));
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 '));
%% Query the marginals
marginals = graph.marginals(result);
%% Plot results with covariance ellipses
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));
posei = result.pose(symbol('x',i))
plotCamera(posei,10);
posei_t = posei.translation()
covarianceEllipse3D([posei_t.x;posei_t.y;posei_t.z],P(4:6,4:6));
end

View File

@ -0,0 +1,25 @@
function covarianceEllipse3D(c,P)
% covarianceEllipse3D: plot a Gaussian as an uncertainty ellipse
% Based on Maybeck Vol 1, page 366
% k=2.296 corresponds to 1 std, 68.26% of all probability
% k=11.82 corresponds to 3 std, 99.74% of all probability
%
% Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
[e,s] = eig(P);
k = 11.82;
radii = k*sqrt(diag(s));
% generate data for "unrotated" ellipsoid
[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3));
% rotate data with orientation matrix U and center M
data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc);
n = size(data,2);
x = data(1:n,:)+c(1); y = data(n+1:2*n,:)+c(2); z = data(2*n+1:end,:)+c(3);
% now plot the rotated ellipse
sc = mesh(x,y,z);
shading interp
alpha(0.5)
axis equal

View File

@ -0,0 +1,18 @@
function plotCamera(pose, axisLength)
C = pose.translation().vector();
R = pose.rotation().matrix();
xAxis = C+R(:,1)*axisLength;
L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r');
yAxis = C+R(:,2)*axisLength;
L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g');
zAxis = C+R(:,3)*axisLength;
L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b');
axis equal
end