-
Notifications
You must be signed in to change notification settings - Fork 18
Expand file tree
/
Copy pathconv2tum.py
More file actions
85 lines (70 loc) · 2.87 KB
/
conv2tum.py
File metadata and controls
85 lines (70 loc) · 2.87 KB
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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
#!/usr/bin/python
import argparse
import sys
def read_euroc_groundtruth(filename):
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
list = [(float(l[0])*1e-9,l[1:4]+l[5:8]+[l[4]]) for l in list if len(l)>1]
return list
def read_structvio_result(filename):
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
list = [(float(l[1])+float(l[2])*1e-9,l[7:10]+l[4:7]+[l[3]]) for l in list if len(l)>1]
return list
def read_vicon_result(filename):
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
list = [(float(l[0]),l[1:8]) for l in list if len(l)>1]
return list
def write_tum_result(list, filename):
file = open(filename, "w")
file.writelines("# timestamp tx ty tz qx qy qz qw\n")
for l in list:
ts = l[0]
pq = l[1]
file.writelines("%s %s %s %s %s %s %s %s\n"%(l[0],pq[0],pq[1],pq[2],pq[3],pq[4],pq[5],pq[6]))
def conv2tum(type, input, output, skip_secs = 0):
if type=='structvio':
res = read_structvio_result(input)
t0 = res[0][0]
res = [l for l in res if l[0] >t0 + skip_secs]
write_tum_result(res, output)
elif type=='okvis':
res = read_okvis_result(input)
t0 = res[0][0]
res = [l for l in res if l[0] >t0 + skip_secs]
write_tum_result(res, output)
elif type=='vins':
res = read_vins_result(input)
t0 = res[0][0]
res = [l for l in res if l[0] >t0 + skip_secs]
write_tum_result(res, output)
elif type=='euroc':
res = read_euroc_groundtruth(input)
t0 = res[0][0]
res = [l for l in res if l[0] >t0 + skip_secs]
write_tum_result(res, output)
def cvt_main():
example_cmd = """
%(prog)s -t structvio -i <input file> -o <output file>
"""
parser = argparse.ArgumentParser(description="convert the results into tum's format",usage=example_cmd)
parser.add_argument('-t','--type',choices=['structvio','euroc'],default='structvio')
parser.add_argument('-i', '--input', action='store', required=True, help="Path of the input file")
parser.add_argument('-o', '--output', help="Path of the output file", nargs="?")
if len(sys.argv) == 1:
parser.print_help()
sys.exit(2)
args = parser.parse_args()
type = args.type
input = args.input
output = args.output
conv2tum(type,input,output)
if __name__ == "__main__":
cvt_main()