1# Open3D: www.open3d.org 2# The MIT License (MIT) 3# See license file or visit www.open3d.org for details 4 5import numpy as np 6 7class CameraPose: 8 def __init__(self, meta, mat): 9 self.metadata = meta 10 self.pose = mat 11 def __str__(self): 12 return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \ 13 "Pose : " + "\n" + np.array_str(self.pose) 14 15def read_trajectory(filename): 16 traj = [] 17 with open(filename, 'r') as f: 18 metastr = f.readline(); 19 while metastr: 20 metadata = list(map(int, metastr.split())) 21 mat = np.zeros(shape = (4, 4)) 22 for i in range(4): 23 matstr = f.readline(); 24 mat[i, :] = np.fromstring(matstr, dtype = float, sep=' \t') 25 traj.append(CameraPose(metadata, mat)) 26 metastr = f.readline() 27 return traj 28 29def write_trajectory(traj, filename): 30 with open(filename, 'w') as f: 31 for x in traj: 32 p = x.pose.tolist() 33 f.write(' '.join(map(str, x.metadata)) + '\n') 34 f.write('\n'.join(' '.join(map('{0:.12f}'.format, p[i])) for i in range(4))) 35 f.write('\n') 36