forked from zhaoxuhui/ROS-Bag-Tools
-
Notifications
You must be signed in to change notification settings - Fork 0
/
parseEuRoCgroundtruth2TUM.py
38 lines (33 loc) · 941 Bytes
/
parseEuRoCgroundtruth2TUM.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
import os
import sys
def readFile(str_path):
time = []
x = []
y = []
z = []
qx = []
qy = []
qz = []
qw = []
fin = open(str_path,'r')
fin.readline()
line = fin.readline()
while line:
parts = line.split(",")
time.append(float(parts[0]))
x.append(float(parts[1]))
y.append(float(parts[2]))
z.append(float(parts[3]))
qw.append(float(parts[4]))
qx.append(float(parts[5]))
qy.append(float(parts[6]))
qz.append(float(parts[7]))
line = fin.readline()
return time,x,y,z,qx,qy,qz,qw
file_path = sys.argv[1]
t,x,y,z,qx,qy,qz,qw = readFile(file_path)
fout = open("groundtruth.txt",'w')
for i in range(len(t)):
line_str = str(t[i])+" "+str(x[i])+" "+str(y[i])+" "+str(z[i])+" "+str(qx[i])+" "+str(qy[i])+" "+str(qz[i])+" "+str(qw[i])+"\n"
fout.write(line_str)
fout.close()