TUM评测工具及自动调用Python脚本

1、TUM evaluate_ate.py, 在原有基础上，增加了输出每一帧ATE

以下代码增加了额外打注释，可以辅助理解ATE的求解方式。

#!/usr/bin/python
# -*- coding: utf-8 -*-

import sys
import numpy
import argparse
import associate

#一般数据的第一帧的相机的位姿设定为单位矩阵，但是grounftruth的第一帧未必是算法估计位姿打第一帧，
#因此两个轨迹很有不能不在同一个坐标系，因此，首先需要将估计的轨迹和grounftruth轨迹align到统一坐标系下，方法类似ICP.
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 维度:3 * n, 每一列是一个向量，即轨迹中两个对应点的绝对估计误差。
    alignment_error = model_aligned - data
    
    #trans_error 维度:3 * n, 每一列是一个向量，即轨迹中两个对应点的距离。
    trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
        
    return rot,trans,trans_error

#分别绘制groundtruth和估计位姿的曲线，只是绘制了每个点的x,y坐标，省去了第三维，因此是一个平面图
def plot_traj(ax,stamps,traj,style,color,label):
    """
    Plot a trajectory using matplotlib. 
    
    Input:
    ax -- the plot
    stamps -- time stamps (1xn)
    traj -- trajectory (3xn)
    style -- line style
    color -- line color
    label -- plot legend
    
    """
    stamps.sort()
    interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])

    x = []
    y = []
    last = stamps[0]
    for i in range(len(stamps)):
        if stamps[i]-last < 2*interval:
            x.append(traj[i][0])
            y.append(traj[i][1])
        elif len(x)>0:
            ax.plot(x,y,style,color=color,label=label)
            label=""
            x=[]
            y=[]
        last= stamps[i]
    if len(x)>0:
        ax.plot(x,y,style,color=color,label=label)
            

if __name__=="__main__":
    # parse command line
    parser = argparse.ArgumentParser(description='''
    This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. 
    ''')
    parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
    parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
    parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
    parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0)
    parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
    parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
    parser.add_argument('--save_per_transerr', help='save tanserror of each frame (format: stamp error)')
    parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
    parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
    parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true')
    args = parser.parse_args()

    first_list = associate.read_file_list(args.first_file)
    second_list = associate.read_file_list(args.second_file)

    matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))

    #print(matches)
    if len(matches)<2:
        sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")

    #shape:3*n
    first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
    second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
   
    #the 1st frmae is Identity Matrix, but the 1st frame may be different in groundth and estimate
    rot,trans,trans_error = align(second_xyz,first_xyz) 
    
    second_xyz_aligned = rot * second_xyz + trans
    
    first_stamps = first_list.keys()
    first_stamps.sort()
    first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
    
    second_stamps = second_list.keys()
    second_stamps.sort()
    second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
    second_xyz_full_aligned = rot * second_xyz_full + trans
    
    if args.verbose:
        print "compared_pose_pairs %d pairs"%(len(trans_error))
        print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
        print "absolute_translational_error.mean %f m"%numpy.mean(trans_error)
        print "absolute_translational_error.median %f m"%numpy.median(trans_error)
        print "absolute_translational_error.std %f m"%numpy.std(trans_error)
        print "absolute_translational_error.min %f m"%numpy.min(trans_error)
        print "absolute_translational_error.max %f m"%numpy.max(trans_error)
    else:
        print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
        
    if args.save_associations:
        file = open(args.save_associations,"w")
        file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)]))
        file.close()
        
    if args.save:
        file = open(args.save,"w")
        file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)]))
        file.close()

    #分别输出每一帧的绝对位姿误差
    if args.save_per_transerr:
        file = open(args.save_per_transerr,"w")
        for i in range(0,len(matches)):
            stamp = "%.6f"% matches[i][1]
            tanserr = trans_error[i]
            file.write(stamp+"\t"+"%f"%tanserr+"\n")
        file.close()

    #绘制groundtruth和估计位姿误差
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        import matplotlib.pylab as pylab
        from matplotlib.patches import Ellipse
        fig = plt.figure()
        ax = fig.add_subplot(111)

        plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth")
        plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated")

        label="difference"
        for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
            ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
            label=""
            
        ax.legend()
        ax.set_xlabel('x [m]')
        ax.set_ylabel('y [m]')
        plt.savefig(args.plot,dpi=90)

2、python自动调用C++可执行程序脚本

import os
import shutil


voca_path = "../../Vocabulary/ORBvoc.txt"

tum_path = "./TUM3.yaml"

data_path = ["",
	"1-freiburg3_walking_xyz",
	"2-freiburg3_walking_halfsphere",
    	"3-freiburg3_walking_static",
    	"4-freiburg3_walking_rpy",
    	"5-freiburg3_sitting_xyz"]

gtruth = ["",
	"results/groundtruth/groundtruth1.txt",
	"results/groundtruth/groundtruth2.txt",
	"results/groundtruth/groundtruth3.txt",
	"results/groundtruth/groundtruth4.txt",
	"results/groundtruth/groundtruth5.txt"]

for i in range(1,6) :
	for j in range(1,21) :
		os.system(r'./rgbd_tum '+ voca_path + ' ' + tum_path +' ../../data/' + data_path[i] + '  ../../data/' + data_path[i] + '/associate.txt')
		os.system(r'python results/evaluate_ate.py ' + gtruth[i] + ' results/cameraTrajectory.txt  --verbose --save_per_transerr err.txt')
		os.system(r'python statistic_dyna_err.py  dynapoints_cnt.txt  err.txt')

		dir_name = str(i)+"-"+str(j);
		os.mkdir("dyna_affect_err/"+dir_name)

		shutil.copy("err.png", "dyna_affect_err/" + dir_name)
		shutil.copy("dynapoints_cnt.txt", "dyna_affect_err/" + dir_name)
		shutil.copy("err.txt", "dyna_affect_err/" + dir_name)
		shutil.copy("err.png", "dyna_affect_err/err" + dir_name + ".png")