first commit

svo_analysis/scripts/analyse_dataset.py

@@ -0,0 +1,42 @@
+# -*- coding: utf-8 -*-
+
+import associate
+import numpy as np
+import matplotlib.pyplot as plt
+import yaml
+import transformations
+
+def loadDataset(filename):
+  file = open(filename)
+  data = file.read()
+  lines = data.replace(","," ").replace("\t"," ").split("\n")
+  D = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
+  return D
+  
+dataset_dir = '/home/cforster/Datasets/SlamBenchmark/px4_r2'
+trajectory_data = dataset_dir+'/groundtruth.txt'
+stepsize = 10
+
+# load dataset
+data = loadDataset(trajectory_data)
+n = data.shape[0]
+steps = np.arange(0,n,stepsize)
+
+# compute trajectory length
+last_pos = data[0,1:4]
+trajectory_length = 0
+
+for i in steps[1:]:
+  new_pos = data[i,1:4]
+  trajectory_length += np.linalg.norm(new_pos-last_pos)
+  last_pos = new_pos
+  
+
+print 'trajectory lenght = ' + str(trajectory_length) + 'm'
+print 'height mean = ' + str(np.mean(data[:,3])) + 'm'
+print 'height median = ' + str(np.median(data[:,3])) + 'm'
+print 'height std = ' + str(np.std(data[:,3])) + 'm'
+print 'duration = ' + str(data[-1,0]-data[0,0]) + 's'
+print 'speed = ' + str(trajectory_length/(data[-1,0]-data[0,0])) + 'm/s'
+
+