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

Better graphics, with points

release/4.3a0
Frank Dellaert 2012-08-25 14:26:02 +00:00
parent 9230458e06
commit 4106afdaa0
1 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
matlab/gtsam_examples/PlanarSLAMExample_graph.m
View File

@ -21,29 +21,27 @@ datafile = '/Users/dellaert/borg/gtsam/examples/Data/example.graph';
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);
[graph,initial] = load2D(datafile, model);
%% Add a Gaussian prior on pose x_1
priorMean = initial.at(0);
priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph
%% 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, 'g-*'); axis equal
plot2DTrajectory(initial, 'r-'); axis equal
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
optimizer = LevenbergMarquardtOptimizer(graph, initial);
tic
result = optimizer.optimizeSafely;
toc
hold on; plot2DTrajectory(result, 'b-*');
%% Plot Covariance Ellipses
cla;hold on
marginals = Marginals(graph, result);
P={};
for i=0:94
pose_i = result.at(i);
Pi=marginals.marginalCovariance(i);
plotPose2(pose_i,'b',Pi)
end
plot2DTrajectory(result, 'g', marginals);
plot2DPoints(result, 'b', marginals);
axis tight
axis equal
view(2)
axis tight; axis equal;