SLAM结果的评价

时间戳对齐

SLAM结果的评价主要是指计算估计的轨迹与Groundtruth的ATE、RTE等指标。
由于一般情况下，估计的轨迹不会包括所有时间戳，需要找到与它最近的一帧真值作为对应帧。
一种简单的思路就是对所有帧的时间戳作差并从小到大排序，依次获得对应帧。

def associate(first_list, second_list,offset,max_difference):
    """
    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 
    to find the closest match for every input tuple.
    
    Input:
    first_list -- first dictionary of (stamp,data) tuples
    second_list -- second dictionary of (stamp,data) tuples
    offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
    max_difference -- search radius for candidate generation

    Output:
    matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
    
    """
    first_keys = first_list.keys()
    second_keys = second_list.keys()
    potential_matches = [(abs(a - (b + offset)), a, b) 
                         for a in first_keys 
                         for b in second_keys 
                         if abs(a - (b + offset)) < max_difference]
    potential_matches.sort()
    matches = []
    for diff, a, b in potential_matches:
        if a in first_keys and b in second_keys:
            first_keys.remove(a)
            second_keys.remove(b)
            matches.append((a, b))
    
    matches.sort()
    return matches

时间戳处理

对于Euroc数据集，使用的是长整型时间戳(ns),可以先转化为(s)

# file state_groundtruth_estimate0/data.csv
def format_euroc(file, save_file):
    fout = open(save_file, 'w')
    for line in open(file).readlines():
        if line[0] != '#' and len(line)>0:
            v = line.strip().split(',')
            fout.write('%f %s %s %s %s %s %s %s\n'%(float(v[0])/1e9,v[1],v[2],v[3],v[5],v[6],v[7],v[4]))

位姿对齐

对于单目SLAM的轨迹或者存在漂移的轨迹，需要先对齐轨迹，这一过程类似于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)
    
    """
    np.set_printoptions(precision=3,suppress=True)
    model_zerocentered = model - model.mean(1)
    data_zerocentered = data - data.mean(1)
    
    W = np.zeros( (3,3) )
    for column in range(model.shape[1]):
        W += np.outer(model_zerocentered[:,column],data_zerocentered[:,column])
    U,d,Vh = np.linalg.linalg.svd(W.transpose())
    S = np.matrix(np.identity( 3 ))
    if(np.linalg.det(U) * np.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 = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0]
        
    return rot,trans,trans_error

计算ATE、RTE

代码来源： https://github.com/symao/vio_evaluation