TUM evaluate_ate.py评测工具

绝对轨迹误差脚本直接测量真实轨迹和估计轨迹的点之间的差异。

作为预处理步骤，我们使用时间戳将估计的姿势与地面真实姿势相关联。 基于此关联，我们使用奇异值分解来对齐真实轨迹和估计轨迹。

最后，我们计算每对姿势之间的差异，并输出这些差异的均值/中值/标准差。

此外，脚本还可以将两个轨迹绘制到png或pdf文件，这样一来可以更加直观的看到差异。

接下来，我们分别看一下相应的脚本执行命令

（注：需要将evaluate_ate.py、groundtruth.txt、CameraTrajectory.txt、associate.py放在同一位置）

（1）仅输出RMSE/cm误差，执行如下命令：

python evaluate_ate.py groundtruth.txt CameraTrajectory.txt

（2）输出真实轨迹和预测轨迹以及误差，并直观显示，执行如下命令：

 python evaluate_ate.py groundtruth.txt CameraTrajectory.txt --plot result.png

（3）输出所有误差，包含平均值，中值等, 执行如下命令：

 python evaluate_ate.py groundtruth.txt CameraTrajectory.txt --verbose

主要功能：
修改轨迹名称，修改图例位置，修改图例字体大小，
下图参考：https://blog.csdn.net/wannna/article/details/102751689
下面代码图例位置设置为 右上角：

plt.legend(loc="upper right")   # 与plt.legend(loc=1)等价

下面代码图例位置设置为 右下角：

ax.legend(loc="lower right")

设置图例文字大小

ax.legend(loc="lower right",fontsize=12)

设置图片保存分辨率：

plt.savefig(args.plot,dpi=800)

以及取消图中difference计算，修改见下面代码

#!/usr/bin/python
"""
This script computes the absolute trajectory error from the ground truth
trajectory and the estimated trajectory.
"""

import sys
import numpy
import argparse
import associate

def align(model,data):
    """Align two trajectories using the method of Horn (closed-form).
    
    Input:
    model -- first trajectory (3xn)
    data -- second trajectory (3xn)
    
    Output:
    rot -- rotation matrix (3x3)
    trans -- translation vector (3x1)
    trans_error -- translational error per point (1xn)
    
    """
    numpy.set_printoptions(precision=3,suppress=True)
    model_zerocentered = model - model.mean(1)
    data_zerocentered = data - data.mean(1)
    
    W = numpy.zeros( (3,3) )
    for column in range(model.shape[1]):
        W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
    U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
    S = numpy.matrix(numpy.identity( 3 ))
    if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
        S[2,2] = -1
    rot = U*S*Vh
    trans = data.mean(1) - rot * model.mean(1)
    
    model_aligned = rot * model + trans
    alignment_error = model_aligned - data
    
    trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
        
    return rot,trans,trans_error

def plot_traj(ax,stamps,traj,style,color,label)