Got rid of redundant examples (clutter!)
Frank Dellaert
parent
991d8f3c5f
commit
25b4a15e94
|
|
@ -1,48 +0,0 @@
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% GTSAM Copyright 3510, 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
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
|
|
||||||
% Data Options
|
|
||||||
options.triangle = true;
|
|
||||||
options.nrCameras = 10;
|
|
||||||
options.showImages = false;
|
|
||||||
|
|
||||||
% iSAM Options
|
|
||||||
options.hardConstraint = false;
|
|
||||||
options.pointPriors = false;
|
|
||||||
options.batchInitialization = true;
|
|
||||||
options.reorderInterval = 10;
|
|
||||||
options.alwaysRelinearize = false;
|
|
||||||
|
|
||||||
% Display Options
|
|
||||||
options.saveDotFile = false;
|
|
||||||
options.printStats = false;
|
|
||||||
options.drawInterval = 5;
|
|
||||||
options.cameraInterval = 1;
|
|
||||||
options.drawTruePoses = false;
|
|
||||||
options.saveFigures = false;
|
|
||||||
options.saveDotFiles = false;
|
|
||||||
|
|
||||||
%% Generate data
|
|
||||||
[data,truth] = VisualISAMGenerateData(options);
|
|
||||||
|
|
||||||
%% Initialize iSAM with the first pose and points
|
|
||||||
[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options);
|
|
||||||
figure(1);
|
|
||||||
VisualISAMPlot(truth, data, isam, result, options)
|
|
||||||
|
|
||||||
%% Main loop for iSAM: stepping through all poses
|
|
||||||
for frame_i=3:options.nrCameras
|
|
||||||
[isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
|
|
||||||
if mod(frame_i,options.drawInterval)==0
|
|
||||||
VisualISAMPlot(truth, data, isam, result, options)
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
@ -1,87 +0,0 @@
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% 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 cameras on a circle around the triangle
|
|
||||||
nCameras = 6;
|
|
||||||
height = 10;
|
|
||||||
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
|
|
||||||
|
|
||||||
%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
|
|
||||||
graph = visualSLAMGraph;
|
|
||||||
|
|
||||||
%% Add factors for all measurements
|
|
||||||
measurementNoiseSigma=1; % in pixels
|
|
||||||
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
|
|
||||||
for i=1:nCameras
|
|
||||||
for j=1:3
|
|
||||||
zij = cameras{i}.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 cameras and points
|
|
||||||
initialEstimate = visualSLAMValues;
|
|
||||||
for i=1:size(cameras,2)
|
|
||||||
initialEstimate.insertPose(symbol('x',i), cameras{i}.pose);
|
|
||||||
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(cameras,2)
|
|
||||||
P = marginals.marginalCovariance(symbol('x',i))
|
|
||||||
pose_i = result.pose(symbol('x',i))
|
|
||||||
plotPose3(pose_i,P,10);
|
|
||||||
end
|
|
||||||
axis([-20 20 -20 20 -1 15]);
|
|
||||||
axis equal
|
|
||||||
view(-37,40)
|
|
||||||
Loading…
Reference in New Issue