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

28
matlab/+gtsam/plot3DPoints.m
View File

@ -7,7 +7,7 @@ function plot3DPoints(values, linespec, marginals)
import gtsam.* import gtsam.*
if ~exist('linespec', 'var') || isempty(linespec) if ~exist('linespec', 'var') || isempty(linespec)
linespec = 'g'; linespec = 'g*';
end end
haveMarginals = exist('marginals', 'var'); haveMarginals = exist('marginals', 'var');
keys = KeyVector(values.keys); keys = KeyVector(values.keys);
@ -15,19 +15,25 @@ keys = KeyVector(values.keys);
holdstate = ishold; holdstate = ishold;
hold on hold on
% Plot points and covariance matrices if haveMarginals
for i = 0:keys.size-1 % Plot points and covariance matrices (slow)
key = keys.at(i); for i = 0:keys.size-1
try key = keys.at(i);
p = values.atPoint3(key); try
if haveMarginals p = values.atPoint3(key)
P = marginals.marginalCovariance(key); P = marginals.marginalCovariance(key);
gtsam.plotPoint3(p, linespec, P); gtsam.plotPoint3(p, linespec, P);
else catch
gtsam.plotPoint3(p, linespec); % I guess it's not a Point3
end end
catch end
% I guess it's not a Point3 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
end end