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