added script to transfer cam-ref measurement to center-ref measurment

meas_cam_to_center.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Wed Aug 15 22:52:36 2018
+
+@author: william
+"""
+
+import getpass
+import csv
+from datetime import datetime
+import math
+
+compname = getpass.getuser()
+
+datapath = "/home/"+ compname +"/full_tests/full_test1/"
+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] != '#':
+            time = float(line.split()[0])
+            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)))
+            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

read_csv.py

@@ -31,11 +31,11 @@ def quaternion_to_euler_angle(w, x, y, z):
 
 compname = getpass.getuser()
 
-datapath = "/home/"+ compname +"/full_tests/"
-f_r2 = open(datapath+"/full_test5/Robot1_Groundtruth.dat", "w+")
+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")
         
-input_data = list(csv.reader(open(datapath+"/full_test5/gt.csv", "r+")))
+input_data = list(csv.reader(open(datapath+"gt.csv", "r+")))
 num_rows = len(input_data)
 start_time = input_data[0][9]
 #start_time = "2018-08-09 07.00.28.980 AM"

untitled0.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Wed Aug 15 22:52:36 2018
+
+@author: william
+"""
+
+import getpass
+import csv
+from datetime import datetime
+import math
+
+compname = getpass.getuser()
+
+datapath = "/home/"+ compname +"/full_tests/"
+f_r2 = open(datapath+"/full_test5/Robot1_Groundtruth.dat", "w+")
+f_r2.write("# Time [sec] \t\t\t x [m] \t\t y [m] \t\t orientation [rad]\n")
+        
+input_data = list(csv.reader(open(datapath+"/full_test5/gt.csv", "r+")))
+num_rows = len(input_data)
+start_time = input_data[0][9]
+#start_time = "2018-08-09 07.00.28.980 AM"
+
+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(meas_path,'r+') as measure_file:
+                for line in measure_file:
+                    if line[0] != '#' and (self.end_time+2 >= float(line.split()[0]) >= self.start_time):
+                        time = round(float(line.split()[0]), 3)
+                        subject_ID = int(line.split()[1])
+                        measurment_range = float(line.split()[2])
+                        bearing = float(line.split()[3])
+    
+f_r2.close()
\ No newline at end of file