gtsam/matlab/+gtsam/plotPose3.m

function plotPose3(pose, P, axisLength)
% plotPose3 shows a Pose, possibly with covariance matrix
if nargin<3,axisLength=0.1;end

% get rotation and translation (center)
gRp = pose.rotation().matrix();  % rotation from pose to global
C = pose.translation();

if ~isempty(axisLength)
    % draw the camera axes
    xAxis = C+gRp(:,1)*axisLength;
    L = [C xAxis]';
    line(L(:,1),L(:,2),L(:,3),'Color','r');
    
    yAxis = C+gRp(:,2)*axisLength;
    L = [C yAxis]';
    line(L(:,1),L(:,2),L(:,3),'Color','g');
    
    zAxis = C+gRp(:,3)*axisLength;
    L = [C zAxis]';
    line(L(:,1),L(:,2),L(:,3),'Color','b');
end

% plot the covariance
if (nargin>2) && (~isempty(P))
    pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame    
    gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
    gtsam.covarianceEllipse3D(C,gPp);  
end
    
end