function err = calcPoseError( resPose, gtPose )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%calcPoseError
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%Huijun Di
        %2020.7.3
    	%Jacky Lin
    	%2020.7.9
    	%每个时刻的位移误差和旋转角度误差
        %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));