Added matlab utility functions for plotting 3D points

release/4.3a0
Richard Roberts 2012-07-24 13:35:04 +00:00
parent e2e780de4d
commit 1b935dbdc5
2 changed files with 49 additions and 0 deletions

36
matlab/plot3DPoints.m Normal file
View File

@ -0,0 +1,36 @@
function plot3DPoints(values, linespec, marginals)
%PLOT3DPOINTS Plots the Point3's in a values, with optional covariances
% Finds all the Point3 objects in the given Values object and plots them.
% If a Marginals object is given, this function will also plot marginal
% covariance ellipses for each point.
import gtsam.*
if ~exist('linespec', 'var') || isempty(linespec)
linespec = 'g';
end
haveMarginals = exist('marginals', 'var');
keys = KeyVector(values.keys);
holdstate = ishold;
hold on
% Plot points and covariance matrices
for i = 0:keys.size-1
key = keys.at(i);
p = values.at(key);
if isa(p, 'gtsam.Point3')
if haveMarginals
P = marginals.marginalCovariance(key);
plotPoint3(p, linespec, P);
else
plotPoint3(p, linespec);
end
end
end
if ~holdstate
hold off
end
end

13
matlab/plotPoint3.m Normal file
View File

@ -0,0 +1,13 @@
function plotPoint3(p, color, P)
%PLOTPOINT3 Plot a Point3 with an optional covariance matrix
if size(color,2)==1
plot3(p.x,p.y,p.z,[color '*']);
else
plot3(p.x,p.y,p.z,color);
end
if exist('P', 'var')
covarianceEllipse3D([p.x;p.y;p.z],P);
end
end