Initial commit

Author: James Clarkson

.gitignore

@@ -0,0 +1,5 @@
+target/
+conf/kfusion.env
+.classpath
+.project
+.settings

bin/checkPos.py

@@ -0,0 +1,176 @@
+#!/usr/bin/python
+# Copyright (c) 2014 University of Edinburgh, Imperial College, University of Manchester.
+# Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1
+#
+# This code is licensed under the MIT License.
+
+import sys
+import re
+import math
+import numpy
+
+
+kfusion_log_regex  =      "([0-9]+[\s]*)\\t" 
+kfusion_log_regex += 8 *  "([0-9.]+)\\t" 
+kfusion_log_regex += 3 *  "([-0-9.]+)\\t" 
+kfusion_log_regex +=      "([01])\s+([01])" 
+
+nuim_log_regex =      "([0-9]+)" 
+nuim_log_regex += 7 * "\\s+([-0-9e.]+)\\s*" 
+
+
+# open files
+
+if len(sys.argv) != 3 :
+    print "I need two parameters, the benchmark log file and the original scene camera position file."
+    exit (1)
+
+# open benchmark log file first
+print "Get KFusion output data." 
+framesDropped = 0
+validFrames = 0
+lastFrame = -1
+untracked = -4;
+kfusion_traj = []
+fileref = open(sys.argv[1],'r')
+data    = fileref.read()
+fileref.close()
+lines = data.split("\n") # remove head + first line
+headers = lines[0].split("\t")
+fulldata = {}
+if len(headers) == 15 :
+    if headers[14] == "" :
+        del headers[14]
+if len(headers) != 14 :
+    print "Wrong KFusion log  file. Expected 14 columns but found " + str(len(headers))
+    exit(1)
+for variable in  headers :
+    fulldata[variable] = []
+
+for line in lines[1:] :
+    matching = re.match(kfusion_log_regex,line)
+    if matching :
+        dropped =  int( matching.group(1)) - lastFrame - 1
+        if dropped>0:     		
+    		framesDropped = framesDropped + dropped    		
+    		for pad in range(0,dropped) :
+    	         kfusion_traj.append( lastValid )     	            
+    	         
+        kfusion_traj.append( (matching.group(10),matching.group(11),matching.group(12),matching.group(13),1 ) )
+        lastValid = (matching.group(10),matching.group(11),matching.group(12),matching.group(13), 0)
+        if int(matching.group(13)) == 0 :
+            untracked = untracked+1
+        validFrames = validFrames +1
+        for elem_idx in  range(len(headers)) :
+            fulldata[headers[elem_idx]].append(float(matching.group(elem_idx+1)))
+        
+        lastFrame = int(matching.group(1))
+    else :
+        #print "Skip KFusion line : " + line
+        break
+
+# open benchmark log file first
+nuim_traj = []
+fileref = open(sys.argv[2],'r')
+data    = fileref.read()
+fileref.close()
+lines = data.split("\n") # remove head + first line
+for line in lines :
+    matching = re.match(nuim_log_regex,line)
+    if matching :
+        nuim_traj.append( (matching.group(2),matching.group(3),matching.group(4)) )
+    else :
+        #print "Skip nuim line : " + line
+        break
+
+working_position = min ( len(kfusion_traj) , len(nuim_traj) )
+print "KFusion valid frames " + str(validFrames) + ",  dropped frames: " + str(framesDropped)
+print "KFusion result        : " + str(len(kfusion_traj)) + " positions."
+print "NUIM  result        : " + str(len(nuim_traj)) + " positions."
+print "Working position is : " + str(working_position) 
+print "Untracked frames: " +str(untracked)
+nuim_traj=nuim_traj[0:working_position]
+kfusion_traj=kfusion_traj[0:working_position]
+
+print "Shift KFusion trajectory..."
+
+first = nuim_traj[0]
+fulldata["ATE"] = []
+#ATE_wrt_kfusion does not consider the ATE for frames which were dropped if we are running in non process-every-frame mode
+fulldata["ATE_wrt_kfusion"] = []
+distance_since_valid=0;
+#print "Frame  speed(m/s)   dlv(m) ATE(m)   valid   tracked"
+for p in range(working_position) :
+    kfusion_traj[p] = (float(kfusion_traj[p][0]) + float(first[0]) , - (float(kfusion_traj[p][1]) + float(first[1]) ) , float(kfusion_traj[p][2]) + float(first[2]), int(kfusion_traj[p][3]),int(kfusion_traj[p][4]) )
+    diff = (abs( kfusion_traj[p][0] - float(nuim_traj[p][0])) ,  abs( kfusion_traj[p][1] - float(nuim_traj[p][1] )) ,  abs( kfusion_traj[p][2] - float(nuim_traj[p][2] )) )
+    ate=math.sqrt(sum(( diff[0] * diff[0],  diff[1] * diff[1],  diff[2] * diff[2])))
+      
+    if( p==1 ): 
+        lastValid = nuim_traj[p]
+    if 1 ==1 :  
+        dx = float(nuim_traj[p][0]) - float(lastValid[0])
+        dy = float(nuim_traj[p][1]) - float(lastValid[1]) 
+        dz = float(nuim_traj[p][2]) - float(lastValid[2])
+        distA = math.sqrt((dx*dx) + (dz*dz))
+        dist = math.sqrt( (dy*dy) + (distA *distA))
+        speed = dist/0.0333
+        if (kfusion_traj[p][3]==0):
+            tracked = "untracked"
+        else:
+            tracked = ""
+        if (kfusion_traj[p][4]==0):
+            valid = "dropped"
+        else:
+            valid = "-"
+        distance_since_valid = distance_since_valid + dist
+#        print "%4d %6.6f %6.6f %6.6f %10s %10s"% (p, speed, distance_since_valid, ate, valid, tracked )
+        lastValid =  nuim_traj[p]
+        if kfusion_traj[p][4]==1:
+            distance_since_valid= 0
+
+    if (kfusion_traj[p][4] == 1 ):
+        fulldata["ATE_wrt_kfusion"].append(ate)        
+    fulldata["ATE"].append(ate)                
+        
+#print "The following are designed to enable easy macchine readability of key data" 
+#print "MRkey:,logfile,ATE,computaion,dropped,untracked"
+#print ("MRdata:,%s,%6.6f,%6.6f,%d,%d") % ( sys.argv[1], numpy.mean(fulldata["ATE"]), numpy.mean(fulldata["computation"]), framesDropped, untracked)
+
+print "\nA detailed statistical analysis is provided."
+print "Runtimes are in seconds and the absolute trajectory error (ATE) is in meters." 
+print "The ATE measure accuracy, check this number to see how precise your computation is."
+print "Acceptable values are in the range of few centimeters."
+
+for variable in sorted(fulldata.keys()) :
+    if "X" in variable or "Z" in variable or "Y" in variable or "frame" in variable  or "tracked" in variable      or "integrated" in variable  :  
+        continue
+
+    if (framesDropped == 0)  and (str(variable) == "ATE_wrt_kfusion"):
+        continue
+		
+    print "%20.20s" % str(variable),
+    print "\tMin : %6.6f" % min(fulldata[variable]),
+    print "\tMax : %0.6f"  % max(fulldata[variable]),
+    print "\tMean : %0.6f" % numpy.mean(fulldata[variable]),
+    print "\tTotal : %0.8f" % sum(fulldata[variable])
+
+#first2 = []S
+#derive = []
+
+#for row_idx in range(len(rows1)) :
+#    col1 = rows1[row_idx].split("\t")
+#    col2 = rows2[row_idx].split(" ")
+#    v1 = col1[8:11]
+#    v2 = col2[1:4]
+#    if first2 == [] :
+#        first2 = v2
+#    v1 = [float(v1[0]) + float(first2[0]) , - (float(v1[1]) + float(first2[1]) ) , float(v1[2]) + float(first2[2]) ]
+#    derive.append([abs(float(v1[0]) - float(v2[0])) , abs (float(v1[1]) - float(v2[1]) ) , abs(float(v1[2]) - float(v2[2])) ])
+
+#maxderive = reduce(lambda a,d:  [max(a[0] , d[0]),max(a[1] , d[1]),max(a[2] , d[2])], derive, [0.0,0.0,0.0])
+#minderive = reduce(lambda a,d:  [min(a[0] , d[0]),min(a[1] , d[1]),min(a[2] , d[2])], derive, [1000000,10000000.0,10000000.0])
+#total = map(lambda x: x/len(rows1), reduce(lambda a,d: [a[0] + d[0],a[1] + d[1],a[2] + d[2]], derive, [0.0,0.0,0.0])) 
+
+#print "Min derivation : " + str(min(minderive))
+#print "Max derivation : " + str(max(maxderive))
+#print "Average derivation : " + str(total[0])