@@ -14,7 +14,9 @@ def __init__(self, path):
14
14
self .n_of_logs = self .parser .reader .log_count
15
15
16
16
self .gyro_scale = self .parser .headers ["gyro_scale" ] #should be already scaled in the fc
17
+ self .acc_scale = 1 / self .parser .headers ["acc_1G" ]
17
18
self .final_gyro_data = []
19
+ self .final_acc_data = []
18
20
self .extracted = False
19
21
self .camera_angle = None
20
22
@@ -76,9 +78,9 @@ def get_gyro_data(self,cam_angle_degrees=0):
76
78
77
79
return self .final_gyro_data
78
80
79
- def get_untransformed_gyro_data (self ):
81
+ def get_untransformed_imu_data (self ):
80
82
if self .extracted :
81
- return np .array (self .final_gyro_data )
83
+ return np .array (self .final_gyro_data ), np . array ( self . final_acc_data )
82
84
83
85
r = Rotation .from_euler ('x' , self .camera_angle , degrees = True )
84
86
@@ -89,32 +91,52 @@ def get_untransformed_gyro_data(self):
89
91
gy = self .parser .field_names .index ('gyroADC[2]' )
90
92
gz = self .parser .field_names .index ('gyroADC[0]' )
91
93
data_frames = []
94
+
95
+ max_index = gy
96
+
97
+ acc_index = None
98
+ if "accSmooth[0]" in self .parser .field_names :
99
+ acc_index = self .parser .field_names .index ("accSmooth[0]" )
100
+ max_index = max (acc_index + 2 , gy )
92
101
102
+ acc_frames = []
103
+
93
104
last_t = 0
94
105
for frame in self .parser .frames ():
95
- #print(len(frame.data))
96
- if len (frame .data ) > gy and ((0 < (frame .data [t ] - last_t ) < self .max_data_gab ) or (last_t == 0 )):
106
+ if len (frame .data ) > max_index and ((0 < (frame .data [t ] - last_t ) < 1000000 * self .max_data_gab ) or (last_t == 0 )):
97
107
98
108
data_frames .append ([frame .data [t ], frame .data [gx ], frame .data [gy ], frame .data [gz ]])
109
+
110
+ if acc_index :
111
+ acc_frames .append ([frame .data [t ], frame .data [acc_index + 1 ], frame .data [acc_index + 2 ], frame .data [acc_index ]])
112
+
99
113
last_t = frame .data [t ]
100
114
101
- if len (data_frames ) < 2 :
102
- return False
103
115
self .final_gyro_data .extend (data_frames )
116
+ if acc_index :
117
+ self .final_acc_data .extend (acc_frames )
104
118
105
119
106
120
self .final_gyro_data = np .array (self .final_gyro_data , dtype = np .float64 )
121
+ #if self.final_gyro_data.shape[0] < 2:
122
+ # print("No valid motion data")
123
+ # return False, False
107
124
self .final_gyro_data [:,0 ] /= 1000000
108
125
self .final_gyro_data [:,1 :] *= np .pi / 180
109
126
127
+ if acc_index :
128
+ self .final_acc_data = np .array (self .final_acc_data , dtype = np .float64 )
129
+ self .final_acc_data [:,0 ] /= 1000000
130
+ self .final_acc_data [:,1 :] *= self .acc_scale
131
+
110
132
# rough gyro rate assumed to be constant
111
133
112
134
self .gyro_rate = self .final_gyro_data .shape [0 ]/ (self .final_gyro_data [- 1 ,0 ] - self .final_gyro_data [0 ,0 ])
113
135
114
136
115
137
self .extracted = True
116
138
117
- return self .final_gyro_data
139
+ return self .final_gyro_data , self . final_acc_data
118
140
119
141
120
142
0 commit comments