|
a |
|
b/utils/trajectory_io.py |
|
|
1 |
import numpy as np |
|
|
2 |
|
|
|
3 |
|
|
|
4 |
class CameraPose: |
|
|
5 |
|
|
|
6 |
def __init__(self, meta, mat): |
|
|
7 |
self.metadata = meta |
|
|
8 |
self.pose = mat |
|
|
9 |
|
|
|
10 |
def __str__(self): |
|
|
11 |
return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \ |
|
|
12 |
"Pose : " + "\n" + np.array_str(self.pose) |
|
|
13 |
|
|
|
14 |
|
|
|
15 |
def 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 |
|
|
|
29 |
|
|
|
30 |
def write_to_file(f_name, i, rot_mat, translation): |
|
|
31 |
file1 = open(f_name, 'a') # 'a' means append |
|
|
32 |
L1 = str(i) + ' ' + str(i) + ' ' + str(i + 1) + '\n' |
|
|
33 |
L2 = str(rot_mat[0, 0]) + ' ' + str(rot_mat[0, 1]) + ' ' + str(rot_mat[0, 2]) + ' ' + str( |
|
|
34 |
translation[0]) + '\n' |
|
|
35 |
L3 = str(rot_mat[1, 0]) + ' ' + str(rot_mat[1, 1]) + ' ' + str(rot_mat[1, 2]) + ' ' + str( |
|
|
36 |
translation[1]) + '\n' |
|
|
37 |
L4 = str(rot_mat[2, 0]) + ' ' + str(rot_mat[2, 1]) + ' ' + str(rot_mat[2, 2]) + ' ' + str( |
|
|
38 |
translation[2]) + '\n' |
|
|
39 |
L5 = "0 0 0 1\n" |
|
|
40 |
file1.write(L1) |
|
|
41 |
file1.write(L2) |
|
|
42 |
file1.write(L3) |
|
|
43 |
file1.write(L4) |
|
|
44 |
file1.write(L5) |
|
|
45 |
file1.close() |