48 lines
1.3 KiB
Matlab
48 lines
1.3 KiB
Matlab
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% 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 Read graph from file and perform GraphSLAM
|
|
% @author Frank Dellaert
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
clear
|
|
|
|
import gtsam.*
|
|
|
|
%% Find data file
|
|
datafile = findExampleDataFile('example.graph');
|
|
|
|
%% Initialize graph, initial estimate, and odometry noise
|
|
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);
|
|
[graph,initial] = load2D(datafile, model);
|
|
|
|
%% Add a Gaussian prior on a pose in the middle
|
|
priorMean = initial.at(40);
|
|
priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]);
|
|
graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph
|
|
|
|
%% Plot Initial Estimate
|
|
cla
|
|
plot2DTrajectory(initial, 'r-'); axis equal
|
|
|
|
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
|
optimizer = LevenbergMarquardtOptimizer(graph, initial);
|
|
tic
|
|
result = optimizer.optimizeSafely;
|
|
toc
|
|
|
|
%% Plot Covariance Ellipses
|
|
cla;hold on
|
|
marginals = Marginals(graph, result);
|
|
plot2DTrajectory(result, 'g', marginals);
|
|
plot2DPoints(result, 'b', marginals);
|
|
axis tight
|
|
axis equal
|
|
view(2)
|
|
|