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