#!/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])
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), }
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
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)
#!/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)
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?" )
'--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)
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
#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))
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
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)
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')