adding scripts for processing measurement data

meas_cam_to_center.py

@@ -7,24 +7,15 @@ Created on Wed Aug 15 22:52:36 2018
 """
 
 import getpass
-import csv
-from datetime import datetime
 import math
 
 compname = getpass.getuser()
 
-datapath = "/home/"+ compname +"/full_tests/full_test1/"
+datapath = "/home/"+ compname +"/full_tests/full_test8_p/"
 f_x = open(datapath+"Robot1_Measurement_x.dat", "w+")
 f_x.write("# Time [sec] \t\t object id \t\t range [m] \t\t bearing [rad]\n")
-
-start_time = input_data[0][9]
-#start_time = "2018-08-09 07.00.28.980 AM"
 l = 0.1 #distance between cam and the center of the robot
-print("Experiment start time: ", start_time)
-dt_obj = datetime.strptime(start_time, "%Y-%m-%d %I.%M.%S.%f %p")
-time_tuple = dt_obj.timetuple()
-timestamp = dt_obj.timestamp()
-print(repr(timestamp))
+
 with open(datapath+"Robot1_Measurement_cam.dat",'r+') as measure_file:
     for line in measure_file:
         if line[0] != '#':
@@ -32,7 +23,10 @@ with open(datapath+"Robot1_Measurement_cam.dat",'r+') as measure_file:
             subject_ID = line.split()[1]
             r_cam = float(line.split()[2])
             bearing = float(line.split()[3])
-            phi = math.atan2((r_cam*math.cos(bearing)+l), (r_cam*math.sin(bearing)))
+            phi = math.atan2((r_cam*math.sin(bearing)), (r_cam*math.cos(bearing)+l),)
             r_cen = r_cam*math.sin(bearing)/math.sin(phi)
             f_x.write(str(time) + '\t\t' + subject_ID + '\t\t' + str(r_cen) + '\t\t'+ str(phi) +'\n')
-f_x.close()
\ No newline at end of file
+f_x.close()
+
+print(math.atan2(4,3))
+print(math.atan2(3,4))
\ No newline at end of file

meas_lable_to_bots.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Tue Aug 21 20:09:26 2018
+
+@author: william
+"""
+
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Wed Aug 15 22:52:36 2018
+
+@author: william
+"""
+
+import getpass
+
+compname = getpass.getuser()
+
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v2_1/"
+f = open(dataset_path+"Robot1_Measurement_x.dat", "w+")
+f.write("# Time [sec] \t\t object id \t\t range [m] \t\t bearing [rad]\n")
+l = 0.1 #distance between cam and the center of the robot
+
+lable_dict = {}
+path=dataset_path + "robot_labels.dat"
+robot_labels=open(path,'r+');
+s = robot_labels.readline()
+while(s):
+    if(s[0]!='#'):
+       print(s)
+       lable_dict.update({s.split( )[0]: s.split( )[1]})
+    s = robot_labels.readline()
+robot_labels.close()
+
+with open(dataset_path+"Robot1_Measurement.dat",'r+') as measure_file:
+    for line in measure_file:
+        if line[0] != '#':
+            time = float(line.split()[0])
+            subject_ID = line.split()[1]
+            robot_id = lable_dict.get(subject_ID)
+            if robot_id != None:
+                subject_ID = robot_id
+            print(subject_ID)
+            f.write(line.split()[0] + '\t\t' + subject_ID + '\t\t' + line.split()[2] + '\t\t'+ line.split()[3] +'\n')
+        
+f.close()
\ No newline at end of file

read_csv.py

@@ -31,9 +31,9 @@ def quaternion_to_euler_angle(w, x, y, z):
 
 compname = getpass.getuser()
 
-datapath = "/home/"+ compname +"/full_tests/full_test1/"
-f_r2 = open(datapath+"Robot1_Groundtruth.dat", "w+")
-f_r2.write("# Time [sec] \t\t\t x [m] \t\t y [m] \t\t orientation [rad]\n")
+datapath = "/home/"+ compname +"/full_tests/full_test_v2_1/"
+f_r = open(datapath+"Robot1_Groundtruth.dat", "w+")
+f_r.write("# Time [sec] \t\t\t x [m] \t\t y [m] \t\t orientation [rad]\n")
         
 input_data = list(csv.reader(open(datapath+"gt.csv", "r+")))
 num_rows = len(input_data)
@@ -45,20 +45,28 @@ dt_obj = datetime.strptime(start_time, "%Y-%m-%d %I.%M.%S.%f %p")
 time_tuple = dt_obj.timetuple()
 timestamp = dt_obj.timestamp()
 print(repr(timestamp))
+
+col_idx = 2
+while input_data[3][col_idx] != '':
+    if input_data[3][col_idx] == 'robot1':
+        break
+    col_idx+=1
+
+col_idx-=1
 row_idx = 7
-col_idx = 1
 while row_idx < num_rows:
-    cur_time = input_data[row_idx][col_idx]
-    x = input_data[row_idx][col_idx+5]  #z-positon(csv) -> y-position(dat)
-    tmp = input_data[row_idx][col_idx+7]  #z-positon(csv) -> y-position(dat)
-    x_rot = input_data[row_idx][col_idx+1]
-    y_rot = input_data[row_idx][col_idx+2]
-    z_rot = input_data[row_idx][col_idx+3]
-    w_rot = input_data[row_idx][col_idx+4]
-    if x!='':
-        roll, pitch, yaw = quaternion_to_euler_angle(float(w_rot), float(x_rot), float(y_rot), float(z_rot))
-        y = float(tmp)*-1
-        f_r2.write(str(float(cur_time)+timestamp) + '\t\t' + str(x) + '\t\t' + str(y) + '\t\t'+ str(pitch) +'\n')
+    cur_time = input_data[row_idx][1]
+    if float(cur_time) > 30:
+        x = input_data[row_idx][col_idx+5] 
+        tmp = input_data[row_idx][col_idx+7]  #z-positon(csv) -> y-position(dat)
+        x_rot = input_data[row_idx][col_idx+1]
+        y_rot = input_data[row_idx][col_idx+2]
+        z_rot = input_data[row_idx][col_idx+3]
+        w_rot = input_data[row_idx][col_idx+4]
+        if x!='':
+            roll, pitch, yaw = quaternion_to_euler_angle(float(w_rot), float(x_rot), float(y_rot), float(z_rot))
+            y = float(tmp)*-1
+            f_r.write(str(float(cur_time)+timestamp) + '\t\t' + str(x) + '\t\t' + str(y) + '\t\t'+ str(pitch) +'\n')
     row_idx+=1
     
-f_r2.close()
\ No newline at end of file
+f_r.close()
\ No newline at end of file