function err = calcPoseError( resPose, gtPose )

if size(resPose, 1) < 4
    resPose(4, 1:4) = [0 0 0 1];
end

if size(gtPose, 1) < 4
    gtPose(4, 1:4) = [0 0 0 1];
end

errPose = inv(resPose)*gtPose;

d = 0.5*(errPose(1,1)+errPose(2,2)+errPose(3,3)-1.0);
err(1) = acos(max(min(d,1.0),-1.0))/pi*180;
err(2) = sqrt(sum(errPose(1:3, 4).^2));