Example #1
0
#!/usr/bin/python

import associate

import numpy as np
import cv2
from matplotlib import pyplot as plt

rgbPicsLoc = "rgb/"
depthPicsLoc = "depth/"

accelData = associate.read_file_list('accelerometer.txt')
depthData = associate.read_file_list('depth.txt')
rgbData = associate.read_file_list('rgb.txt')

pairedData = associate.associate(rgbData, depthData, 0.0, 0.02)
truthData = associate.read_file_list('groundtruth.txt')
rgbData = associate.read_file_list('rgb.txt')

fast = cv2.FastFeatureDetector_create()

img = cv2.imread(rgbData.items()[0][1][0], 0)
kp = fast.detect(img, None)
img2 = img
img2 = cv2.drawKeypoints(img, kp, img2, color=(255,0,0))
#cv2.imshow('image', img2)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#cv2.imwrite('drawn_img.png', img2)

def proj(x, F, C, B):
urllib.urlretrieve(freiburg1URL, 'rgbd_dataset_freiburg1_xyz.tgz', reporthook)
os.system('setterm -cursor on')

#Extract downloaded data
print "Extracting benchmark dataset"
try:
    tar = tarfile.open('rgbd_dataset_freiburg1_xyz.tgz', 'r:gz')
    for item in tar:
        tar.extract(item)
    print 'Done.'
except:
    print "Error opening downloaded file"

#Run assoc
print "Associating timestamps"
first_list = associate.read_file_list(
    "rgbd_dataset_freiburg1_xyz/groundtruth.txt")
second_list = associate.read_file_list("rgbd_dataset_freiburg1_xyz/depth.txt")
third_list = associate.read_file_list("rgbd_dataset_freiburg1_xyz/rgb.txt")

#Match Groundtruth with depth
matchesDepth = associate.associate(first_list, second_list, 0.0, 0.02)

#Convert matched groundtruth-depth timestamps to full data tuples (gtTime, qx, qy, qz, qw, tx, ty, tz, depthTime, depthFile)
matchedDepthVals = []
for a, b in matchesDepth:
    matchedDepthVals.append([
        val for sublist in [[a], first_list[a], [b], second_list[b]]
        for val in sublist
    ])

#Convert tuple to dict of form {gtTime: [qx, qy, qz, qw, tx, ty, tz], depthTime: [depthFile])
Example #3
0
    def __init__(self):
        self.use_cuda = True
        self.display = True
        self.config = yaml.load(open('config.yaml', 'r'))
        self.dataset = self.config['DATASET']['NAME']
        self.detectron2 = Detectron2()
        # ros
        self.image_pub = rospy.Publisher("/segmentation_mask",
                                         Image_ros,
                                         queue_size=10,
                                         latch=True)
        self.pose_sub = rospy.Subscriber("/camera_pose", Image_ros,
                                         self.pose_callback)
        # self.pose_true = False
        self.pose = np.eye(4)
        self.bridge = CvBridge()

        if (self.dataset == 'kitti'):

            self.sequence_list = os.listdir(
                self.config['DATASET']['KITTI']['DATA_PATH'])
            self.velo2cam = np.array(
                self.config['DATASET']['KITTI']['TRANSFORMS']['Velo2cam'])

            self.kitti_timestamps = open(
                self.config['DATASET']['KITTI']['TIMESTAMPS']).readlines()
            self.kitti_odom = open(
                self.config['DATASET']['KITTI']['ODOM_PATH']).readlines()

            self.matches = sorted(self.sequence_list)
            self.max_vel = self.config['DATASET']['KITTI']['MAX_VEL']

            self.fx = self.config['DATASET']['KITTI']['CAMERA'][
                'focal_length_x']
            self.fy = self.config['DATASET']['KITTI']['CAMERA'][
                'focal_length_y']
            self.cx = self.config['DATASET']['KITTI']['CAMERA'][
                'optical_center_x']
            self.cy = self.config['DATASET']['KITTI']['CAMERA'][
                'optical_center_y']

        elif (self.dataset == 'tum'):
            #self.deepsort = DeepSort(args.deepsort_checkpoint, use_cuda=use_cuda)
            self.first_list = read_file_list(
                self.config['DATASET']['TUM']['RGB_PATH'] + '.txt')
            self.second_list = read_file_list(
                self.config['DATASET']['TUM']['DEPTH_PATH'] + '.txt')
            self.third_list = read_file_list(
                self.config['DATASET']['TUM']['ODOM_PATH'] + '.txt')

            self.matches = associate(self.first_list, self.second_list,
                                     self.third_list, 0.0, 0.02)

            self.max_vel = self.config['DATASET']['TUM']['MAX_VEL']
            self.fx = self.config['DATASET']['TUM']['CAMERA']['focal_length_x']
            self.fy = self.config['DATASET']['TUM']['CAMERA']['focal_length_y']
            self.cx = self.config['DATASET']['TUM']['CAMERA'][
                'optical_center_x']
            self.cy = self.config['DATASET']['TUM']['CAMERA'][
                'optical_center_y']

        self.class_names = self.config['CLASSES']['ALL']
        self.rigid = self.config['CLASSES']['RIGID']
        self.not_rigid = self.config['CLASSES']['NON_RIGID']
def evaluate_ate(_args):
    # 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_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(_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))
    if len(matches) < 2:
        raise ValueError(
            "Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?"
        )

    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()
    rot, trans, trans_error = align(second_xyz, first_xyz)

    second_xyz_aligned = rot * second_xyz + trans

    first_stamps = list(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 = list(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.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)

    return {
        "compared_pose_pairs": (len(trans_error)),
        "absolute_translational_error.rmse":
        numpy.sqrt(numpy.dot(trans_error, trans_error) / len(trans_error)),
        "absolute_translational_error.mean":
        numpy.mean(trans_error),
        "absolute_translational_error.median":
        numpy.median(trans_error),
        "absolute_translational_error.std":
        numpy.std(trans_error),
        "absolute_translational_error.min":
        numpy.min(trans_error),
        "absolute_translational_error.max":
        numpy.max(trans_error),
    }
Example #5
0
import matplotlib
import matplotlib.pyplot as plt
from matplotlib import rc

# config -------------------------------------------------------------------------

trace_name='ptam_i7_rig3'
n_align_frames = 300

img_prefix = '../trace/'+trace_name+'/'+trace_name+'_'
traj_groundtruth = '../trace/'+trace_name+'/traj_groundtruth.txt'
traj_estimate = '../trace/'+trace_name+'/traj_estimate.txt'

#---------------------------------------------------------------------------------

first_list = associate.read_file_list(traj_groundtruth)
second_list = associate.read_file_list(traj_estimate)

matches = associate.associate(first_list, second_list,float(0.0),float(0.02))
if len(matches)<2:
    sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")

# read data in nupmy matrix
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) for value in second_list[b][0:3]] for a,b in matches]).transpose()

# align Sim3
scale,rot,trans,trans_error = compare_two_trajectories.alignSim3(first_xyz,second_xyz, n_align_frames)

second_xyz_aligned = scale*rot*second_xyz+trans
alignment_error = second_xyz_aligned - first_xyz
Example #6
0
dname = os.path.dirname(abspath)
os.chdir(dname)

if not os.path.exists(filename_zip):
    print 'Downloading dataset file ', filename_zip
    testfile = urllib.URLopener()
    testfile.retrieve(dataset_url, filename_zip)

if not os.path.exists(filename):
    print 'Extracting dataset ', filename
    tar = tarfile.open(filename_zip, "r:gz")
    tar.extractall()
    tar.close()

if not os.path.exists(filename + '/depth_gt.txt'):
    first_list = associate.read_file_list(filename + '/depth.txt')
    second_list = associate.read_file_list(filename + '/groundtruth.txt')

    matches = associate.associate(first_list, second_list, 0.0, 0.02)

    f = open(filename + '/depth_gt.txt', 'w')
    for a, b in matches:
        f.write(
            "%f %s %f %s\n" %
            (a, " ".join(first_list[a]), b - 0.0, " ".join(second_list[b])))
    f.close()

print 'Dataset is ready.'

os.system(
    'rosrun ewok_ring_buffer tum_rgbd_ring_buffer_example rgbd_dataset_freiburg2_pioneer_slam res.txt'
def align_sim3(model, data):
    """Implementation of the paper: S. Umeyama, Least-Squares Estimation
       of Transformation Parameters Between Two Point Patterns,
       IEEE Trans. Pattern Anal. Mach. Intell., vol. 13, no. 4, 1991.
       Input:
           model -- first trajectory (3xn)
           data -- second trajectory (3xn)
       Output:
           s -- scale factor (scalar)
           R -- rotation matrix (3x3)
           t -- translation vector (3x1)
           t_error -- translational error per point (1xn)
    """
    # substract mean
    mu_M = model.mean(0).reshape(model.shape[0],1)
    mu_D = data.mean(0).reshape(data.shape[0],1)
    model_zerocentered = model - mu_M
    data_zerocentered = data - mu_D
    n = np.shape(model)[0]
    
    # correlation
    C = 1.0/n*np.dot(model_zerocentered.transpose(), data_zerocentered)
    sigma2 = 1.0/n*np.multiply(data_zerocentered,data_zerocentered).sum()
    U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
    D_svd = np.diag(D_svd)
    V_svd = np.transpose(V_svd)
    S = np.eye(3)

    if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
    S[2,2] = -1

    R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
    s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
    t = mu_M-s*np.dot(R,mu_D)
    # TODO:
    model_aligned = s * R * model + t
    alignment_error = model_aligned - data
    t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0]

    #return s, R, t #, t_error
    return s, R, t, t_erro

# 欧式变换误差
"""
由于真实轨迹录制时的坐标系和算法一开始的坐标系存在差异,
所以算法估计的相机轨迹和真实轨迹之间存在一个欧式变换,
所以按照对估计值和真实值进行配准后,
需要求解真实值和匹配的估计值之间的一个欧式变换。

对估计值进行变换后再与真实值计算差值。

"""
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])
        # outer() 前一个参数表示 后一个参数扩大倍数
        # https://blog.csdn.net/hqh131360239/article/details/79064592
    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
    # err = sqrt((x-x')^2 + (y-y')^2 + (z-z')^2) 
    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):
    """
    Plot a trajectory using matplotlib. 
    2D图
    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)
 
def plot_traj3D(ax,stamps,traj,style,color,label):
    """
    Plot a trajectory using matplotlib. 
    3D图
    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 = []
    z = []
    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])
            z.append(traj[i][2])
        elif len(x)>0:
            ax.plot(x,y,z,style,color=color,label=label)
            
            label=""
            x=[]
            y=[]
            z=[]
        last= stamps[i]
    if len(x)>0:
        ax.plot(x,y,z,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_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('--plot3D', help='plot the first and the aligned second trajectory to as interactive 3D plot (format: png)', action = 'store_true')
    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)
    
    # 按照时间戳进行匹配,最大差值不能超过 max_difference 0.02
    # 
    matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))    
    if len(matches)<2:
        sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
    
    # 按照匹配对 仅取出x、y、z位置信息
    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()
    
    # 对匹配后的位置坐标求解误差====
    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.plot:
        # 绘制2D图=====
        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)
        
    if args.plot3D:
        # 绘制3D图======
        import matplotlib as mpl
        mpl.use('Qt4Agg')
        from mpl_toolkits.mplot3d import Axes3D
        import numpy as np
        import matplotlib.pyplot as plt
        fig = plt.figure()
        ax = fig.gca(projection='3d')
        #        ax = fig.add_subplot(111)
        plot_traj3D(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","groundTruth")
        plot_traj3D(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","orb-slam2-flow")
        
        # 误差线
        #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],[z1,z2],'-',color="red",label=label)
        #    label=""            
        
        ax.legend()
        ax.set_xlabel('x [m]')
        ax.set_ylabel('y [m]')
        print "Showing"
        plt.show(block=True)
        plt.savefig("./test.png",dpi=90)
Example #8
0
#!/usr/bin/env python

import sys
import numpy as np
import associate
import matplotlib.pyplot as plt

if len(sys.argv) < 3:
    print 'Usage: python evalute_imu_count <imu_count_file> <ground_truth>'
    exit(0)

imu_file = sys.argv[1]
groundtruth_file = sys.argv[2]

imu_count_dict = associate.read_file_list(imu_file)

imu_count_tuple_list = [tuple([i] + imu_count_dict[i]) for i in imu_count_dict]
imu_count_tuple_list.sort()

print imu_count_tuple_list[:5]

count = [0 for i in range(len(imu_count_dict))]
for idx in range(len(imu_count_tuple_list)):
    count[idx] = round(
        imu_count_tuple_list[idx][1] /
        (imu_count_tuple_list[idx][0] - imu_count_tuple_list[idx][2]) * 0.05,
        3)

plt.plot(sorted(imu_count_dict.keys()), count)
plt.show()
            relative_T = np.dot(np.linalg.inv(groundtruth_T2), groundtruth_T1)
            # convert groundtruth to camera frame
            relative_T = np.dot(np.dot(T_cb, relative_T), T_bc)
            relative_ground_truth.append((matches_dict[pose_t2], relative_T))
        except KeyError:
            pass

    return dict(relative_ground_truth)


groundtruth_file = sys.argv[1]
imu_file = sys.argv[2]
cmv_file = sys.argv[3]

groundtruth = associate.read_file_list(groundtruth_file, timestamp_scale=1e-9)
imu_prior = associate.read_file_list(imu_file)
cvm_prior = associate.read_file_list(cmv_file)

# convert from list to pose dictionary
groundtruth_poses = parseGroundtruth(groundtruth)

# get only the ones contained in both imu and cvm
imu_cvm_matches = associate.associate(imu_prior,
                                      cvm_prior,
                                      offset=0.0,
                                      max_difference=0.007)
imu_prior = {i[0]: imu_prior[i[0]] for i in imu_cvm_matches}
cvm_prior = {i[0]: cvm_prior[i[1]] for i in imu_cvm_matches}
# get only the groundtruth contained in both imu and cvm (and the one previous to it!)
# imu_groundtruth_matches = associate.associate(imu_prior, groundtruth, offset=0.0, max_difference=0.02)
def evaluator(first_file, second_file):
    first_list = associate.read_file_list(first_file)
    second_list = associate.read_file_list(second_file)

    matches = associate.associate(first_list, second_list, 0.0, 0.02)

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

    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(1.0) for value in second_list[b][0:3]]
         for a, b in matches]).transpose()

    first_quat = numpy.matrix([[float(value) for value in first_list[a][3:7]]
                               for a, b in matches])
    second_quat = numpy.matrix(
        [[float(value) * float(1.0) for value in second_list[b][3:7]]
         for a, b in matches])

    first_quats = []

    rot, trans, trans_error = align(second_xyz, first_xyz)

    for quat_1 in first_quat:
        quat_1_array = numpy.squeeze(numpy.asarray(quat_1))

        quat_constr = quat(quat_1_array[3], quat_1_array[0], quat_1_array[1],
                           quat_1_array[2])
        first_quats.append(quat_constr)

    counter = 0

    rot_errors = []

    for quat_1 in second_quat:
        quat_1_array = numpy.squeeze(numpy.asarray(quat_1))
        quat_constr = quat(quat_1_array[3], quat_1_array[0], quat_1_array[1],
                           quat_1_array[2])
        orientation_aligned = first_quats[
            counter].inverse.rotation_matrix * rot * quat_constr.rotation_matrix
        rot_errors.append(angle(orientation_aligned))
        counter += 1

    first_stamps = sorted(first_list)
    second_stamps = sorted(second_list)
    second_xyz_full = numpy.matrix(
        [[float(value) for value in second_list[b][0:3]]
         for b in second_stamps]).transpose()

    print("compared_pose_pairs " + str(len(trans_error)) + " pairs")

    print("alignment transformation R + t is")
    print(rot)
    print(trans)

    print("absolute_translational_error.rmse " + str(
        numpy.sqrt(numpy.dot(trans_error, trans_error) / len(trans_error))) +
          " m")
    print("absolute_translational_error.mean " + str(numpy.mean(trans_error)) +
          " m")
    print("absolute_translational_error.median " +
          str(numpy.median(trans_error)) + " m")
    print("absolute_translational_error.std " + str(numpy.std(trans_error)) +
          " m")
    print("absolute_translational_error.min " + str(numpy.min(trans_error)) +
          " m")
    print("absolute_translational_error.max " + str(numpy.max(trans_error)) +
          " m")

    print()

    print("absolute_rotational_error.rmse " +
          str(numpy.sqrt(numpy.dot(rot_errors, rot_errors) /
                         len(rot_errors))) + " rad")
    print("absolute_rotational_error.mean " + str(numpy.mean(rot_errors)) +
          " rad")
    print("absolute_rotational_error.median " + str(numpy.median(rot_errors)) +
          " rad")
    print("absolute_rotational_error.std " + str(numpy.std(rot_errors)) +
          " rad")
    print("absolute_rotational_error.min " + str(numpy.min(rot_errors)) +
          "  rad")
    print("absolute_rotational_error.max " + str(numpy.max(rot_errors)) +
          " rad")

    return numpy.mean(trans_error), numpy.mean(rot_errors)
    def associate_identitiy(self):
        self.first_list = associate.read_file_list(self.resultsfile)
        self.second_list = associate.read_file_list(self.groundtruthfile)

        self.matches = zip(self.first_list, self.second_list)
Example #12
0
          color='r',
          label='ground truth')
ax13.plot(euler_est_time_relative,
          euler_estimate[:, 2],
          linestyle='-',
          linewidth=0.8,
          color='b',
          label='estimate')
ax13.set_ylabel('yaw [deg]', fontsize=10)
ax13.set_xlabel('t [s]', fontsize=10)
ax13.tick_params(labelsize=9)
ax13.legend(loc='upper right', fontsize=6, edgecolor='w')
ax13.grid(linestyle="--")

########  plot orientation error  ########
first_list = associate.read_file_list(file_traj_gt)
second_list = associate.read_file_list(file_traj_estimate)
matches = associate.associate(first_list, second_list, 0.0, 0.02)
time = []
for a, b in matches:
    time.append(b - euler_gt_time[0])
diff_euler = []
diff_euler = np.loadtxt(file_euler_error, usecols=(1, 2, 3))

fig2, ax2 = plt.subplots(figsize=(6, 3))
ax2.plot(time, diff_euler[:, 0], linewidth=1.0, color='r', label='roll')
ax2.plot(time, diff_euler[:, 1], linewidth=1.0, color='g', label='pitch')
ax2.plot(time, diff_euler[:, 2], linewidth=1.0, color='b', label='yaw')
ax2.set_xlabel('t [s]', fontsize=10)
ax2.set_ylabel('rotation err. [deg]', fontsize=10)
ax2.tick_params(labelsize=9)
        "color_sensor": rgb_sensor,  # RGB sensor
        "depth_sensor": depth_sensor,  # Depth sensor
        "semantic_sensor": semantic_sensor,  # Semantic sensor
        "seed": 1,  # used in the random navigation
        "enable_physics": False,  # kinematics only
    }
    cfg = make_cfg(sim_settings)
    sim = habitat_sim.Simulator(cfg)
    #height = sim.pathfinder.get_bounds()[0][1]
    height = 0.1
    meters_per_pixel = 0.01

    hablab_topdown_map = maps.get_topdown_map(
        sim.pathfinder, height, meters_per_pixel=meters_per_pixel)

    first_list = read_file_list(first_file)
    second_list = read_file_list(second_file)
    matches = first_list.keys()
    first_xyz = [[float(value) for value in first_list[a][0:3]]
                 for a in matches]
    second_xyz = [[float(value) * float(1.0) for value in second_list[b][0:3]]
                  for b in matches]
    first_rot = [[float(value) for value in first_list[a][3:]]
                 for a in matches]
    grid_dimensions = (hablab_topdown_map.shape[0],
                       hablab_topdown_map.shape[1])
    trajectory = [
        maps.to_grid(
            -(path_point[2]) + 19.25,
            (path_point[0] - 2.04),
            grid_dimensions,
    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
    # err = sqrt((x-x')^2 + (y-y')^2 + (z-z')^2)
    trans_error = numpy.sqrt(
        numpy.sum(numpy.multiply(alignment_error, alignment_error), 0)).A[0]

    return rot, trans, trans_error


# 读取数据=================================================================================
ground_truth_list = associate.read_file_list("./groundtruth.txt")  # 真实轨迹数据
src_list = associate.read_file_list("./src.txt")  # 原 orb-slam2 预测
flow_list = associate.read_file_list("./flow3.txt")  # flow 光流加持
geom_list = associate.read_file_list("./geom.txt")  # geom 多视角几何加持
offset = 0.0  # 时间偏移量
max_difference = 0.02  # 最大时间差值
scale = 1.0  # 数据大小尺度

# 真实值与 原版本算法预测值 进行对比====================================
# 按照时间戳进行匹配,最大差值不能超过 max_difference 0.02
matches_src = associate.associate(ground_truth_list, src_list, float(offset),
                                  float(max_difference))
if len(matches_src) < 2:
    sys.exit(
        "Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?"
    )
Example #15
0
        '--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')
    """
    save  all evaluation data
    """
    parser.add_argument('--save_evaluation', help='save all evaluation data')
    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))
    if len(matches) < 2:
        sys.exit(
            "Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?"
        )

    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) for value in second_list[b][0:3]]
                               for a, b in matches]).transpose()
    second_zerocentered = second_xyz - second_xyz.mean(1)
    first_zerocentered = first_xyz - first_xyz.mean(1)
Example #16
0
def ate(first_file,
        second_file,
        offset=0,
        max_diff=0.02,
        scale=1.0,
        plot=1,
        save_png=None):
    first_list = associate.read_file_list(first_file)
    second_list = associate.read_file_list(second_file)

    matches = associate.associate(first_list, second_list, float(offset),
                                  float(max_diff))
    if len(matches) < 2:
        sys.exit(
            "Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?"
        )

    first_xyz = np.matrix([[float(value) for value in first_list[a][0:3]]
                           for a, b in matches]).transpose()
    second_xyz = np.matrix(
        [[float(value) * float(scale) for value in second_list[b][0:3]]
         for a, b in matches]).transpose()
    rot, trans, trans_error = align(second_xyz, first_xyz)

    first_stamps = first_list.keys()
    first_stamps.sort()
    first_xyz_full = np.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 = np.matrix(
        [[float(value) * float(scale) for value in second_list[b][0:3]]
         for b in second_stamps]).transpose()
    second_xyz_full_aligned = rot * second_xyz_full + trans

    rmse = np.sqrt(np.dot(trans_error, trans_error) / len(trans_error))
    emean = np.mean(trans_error)
    emedian = np.median(trans_error)
    estd = np.std(trans_error)
    emin = np.min(trans_error)
    emax = np.max(trans_error)

    name = os.path.basename(first_file)[:-4]

    if save_png is not None or plot:
        fig = plt.figure()
        ax = fig.add_subplot(111)
        plot_traj(ax, first_stamps,
                  first_xyz_full.transpose().A, '-', "red", "ground truth")
        plot_traj(ax, second_stamps,
                  second_xyz_full_aligned.transpose().A, '-', "blue",
                  "estimated")
        ax.legend()
        ax.set_xlabel('x [m]')
        ax.set_ylabel('y [m]')
        plt.title(name)
    if save_png is not None:
        plt.savefig(save_png)
    if plot:
        plt.show()
    return len(trans_error), rmse, emean, emedian, estd, emin, emax
Example #17
0
        #print(i,orb_num)
        if orb_num < min_orb:
            delete_arr.append(i)
    #print("delete:",delete_arr)
    for i in delete_arr:
        del cloud_dict[i]

    orb_dict = {"kp": kp, "des": des, "assign": assign}

    return cloud_dict, orb_dict


if __name__ == "__main__":
    data_dir = "/data/datasets/yurouy/rgbd_dataset_freiburg1_xyz/"
    depth_path = data_dir + "depth.txt"
    rgb_path = data_dir + "rgb.txt"
    depth_list = read_file_list(depth_path)
    rgb_list = read_file_list(rgb_path)
    asso_list = associate(rgb_list,
                          depth_list,
                          offset=0.0,
                          max_difference=0.02)

    for i in range(1):
        rgb = data_dir + rgb_list[asso_list[i][0]][0]
        print(rgb)
        depth = data_dir + depth_list[asso_list[i][1]][0]
        rgb_im = Image.open(rgb)
        depth_im = Image.open(depth)
        get_clustered_point_cloud(np.array(rgb_im), np.array(depth_im))
Example #18
0
def main(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))    
    if len(matches)<2:
        sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")


    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()
    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.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        import matplotlib.pylab as pylab
        from matplotlib.patches import Ellipse
	matplotlib.rcParams.update({'font.size': 22})
        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]',fontsize=40)
        #ax.set_ylabel('y [m]',fontsize=40)
	title=args.plot.split('/')[0][13:].replace('_','/').replace('freiburg','fr')
        ax.set_title(title,fontsize=40)
        plt.savefig(args.plot,dpi=250)
    
    return trans_error
Example #19
0
    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('--interpolate', help='interpolate and resample the ground truth trajectory', action='store_true')
    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_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)
    
    if args.interpolate:
       first_list = interpolate_and_resample(first_list,list(second_list.keys()),float(args.offset),float(args.max_difference)*2)

    matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))    
    if len(matches)<2:
        sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")

    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()
    rot,trans,trans_error = align(second_xyz,first_xyz)

   
Example #20
0
import numpy as np
# pip install numpy-quaternion
import quaternion
import argparse
from associate import read_file_list

if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='Convert tum .freiburg to redwood .log')
    parser.add_argument('filename', type=str, help='input filename[.txt]')

    args = parser.parse_args()

    poses_dict = read_file_list(args.filename)

    with open(''.join(args.filename.split('.')[:-1]) + '.log', 'w') as f:
        for i, (_, v) in enumerate(poses_dict.items()):
            tq = np.array(list(map(float, v)))
            t = tq[:3]
            q_tum = tq[3:]
            # xyzw to wxyz
            q = np.quaternion(q_tum[3], q_tum[0], q_tum[1], q_tum[2])
            R = quaternion.as_rotation_matrix(q)
            T = np.identity(4)
            T[:3, :3] = R
            T[:3, 3] = t

            f.write('{} {} -1\n'.format(i, i + 1))
            for i in range(4):
                f.write(' '.join(list(map(str, T[i]))) + '\n')