66 lines
1.5 KiB
Matlab
Executable File
66 lines
1.5 KiB
Matlab
Executable File
function plot3DTrajectory(values,linespec,frames,scale,marginals)
|
|
% plot3DTrajectory plots a 3D trajectory
|
|
% plot3DTrajectory(values,linespec,frames,scale,marginals)
|
|
|
|
if ~exist('scale','var') || isempty(scale), scale=1; end
|
|
if ~exist('frames','var'), scale=[]; end
|
|
|
|
import gtsam.*
|
|
|
|
Pose3Values = gtsam.utilities.allPose3s(values);
|
|
|
|
haveMarginals = exist('marginals', 'var');
|
|
keys = KeyVector(Pose3Values.keys);
|
|
|
|
holdstate = ishold;
|
|
hold on
|
|
|
|
% Plot poses and covariance matrices
|
|
lastIndex = [];
|
|
for i = 0:keys.size-1
|
|
key = keys.at(i);
|
|
try
|
|
x = Pose3Values.atPose3(key);
|
|
if ~isempty(lastIndex)
|
|
% Draw line from last pose then covariance ellipse on top of
|
|
% last pose.
|
|
lastKey = keys.at(lastIndex);
|
|
try
|
|
lastPose = Pose3Values.atPose3(lastKey);
|
|
plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
|
|
if haveMarginals
|
|
P = marginals.marginalCovariance(lastKey);
|
|
else
|
|
P = [];
|
|
end
|
|
gtsam.plotPose3(lastPose, P, scale);
|
|
catch err
|
|
% warning(['no Pose3 at ' lastKey]);
|
|
end
|
|
end
|
|
lastIndex = i;
|
|
catch
|
|
warning(['no Pose3 at ' key]);
|
|
end
|
|
end
|
|
|
|
% Draw final pose
|
|
if ~isempty(lastIndex)
|
|
lastKey = keys.at(lastIndex);
|
|
try
|
|
lastPose = Pose3Values.atPose3(lastKey);
|
|
if haveMarginals
|
|
P = marginals.marginalCovariance(lastKey);
|
|
else
|
|
P = [];
|
|
end
|
|
gtsam.plotPose3(lastPose, P, scale);
|
|
catch
|
|
% warning(['no Pose3 at ' lastIndex]);
|
|
end
|
|
end
|
|
|
|
if ~holdstate
|
|
hold off
|
|
end
|