function plotPoint3(p, color, P)
%PLOTPOINT3 Plot a Point3 with an optional covariance matrix
if size(color,2)==1
    plot3(p(1),p(2),p(3),[color '*']);
else
    plot3(p(1),p(2),p(3),color);
end
if exist('P', 'var')
    gtsam.covarianceEllipse3D([p(1);p(2);p(3)],P);
end

end