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

if haveMarginals
    % Plot points and covariance matrices (slow)
    for i = 0:keys.size-1
        key = keys.at(i);
        try
            p = values.atPoint3(key)
            P = marginals.marginalCovariance(key);
            gtsam.plotPoint3(p, linespec, P);
        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
end

end