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

Faster version if no marginals

release/4.3a0
dellaert 2015-01-22 02:10:17 +01:00
parent 10f086014b
commit 7430d0484b
1 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
matlab/+gtsam/plot3DPoints.m
View File

@ -7,7 +7,7 @@ function plot3DPoints(values, linespec, marginals)
import gtsam.*
if ~exist('linespec', 'var') || isempty(linespec)
linespec = 'g';
linespec = 'g*';
end
haveMarginals = exist('marginals', 'var');
keys = KeyVector(values.keys);
@ -15,21 +15,27 @@ keys = KeyVector(values.keys);
holdstate = ishold;
hold on
% Plot points and covariance matrices
if haveMarginals
% Plot points and covariance matrices (slow)
for i = 0:keys.size-1
key = keys.at(i);
try
p = values.atPoint3(key);
if haveMarginals
p = values.atPoint3(key)
P = marginals.marginalCovariance(key);
gtsam.plotPoint3(p, linespec, P);
else
gtsam.plotPoint3(p, linespec);
end
catch
% I guess it's not a Point3
end
end
else
% Extract all in C++ and plot all at once (fast)
P = utilities.extractPoint3(values);
if size(linespec,2)==1
plot3(P(:,1),P(:,2),P(:,3),[linespec '*']);
else
plot3(P(:,1),P(:,2),P(:,3),linespec);
end
end
if ~holdstate
hold off