Skip to content
This repository was archived by the owner on Jul 8, 2023. It is now read-only.

Commit 0a90c21

Browse files
committed
Acceleration parsing for raw blackbox
1 parent 05afe7e commit 0a90c21

File tree

3 files changed

+36
-13
lines changed

3 files changed

+36
-13
lines changed

blackbox_extract.py

Lines changed: 29 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,9 @@ def __init__(self, path):
1414
self.n_of_logs = self.parser.reader.log_count
1515

1616
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"]
1718
self.final_gyro_data = []
19+
self.final_acc_data = []
1820
self.extracted = False
1921
self.camera_angle = None
2022

@@ -76,9 +78,9 @@ def get_gyro_data(self,cam_angle_degrees=0):
7678

7779
return self.final_gyro_data
7880

79-
def get_untransformed_gyro_data(self):
81+
def get_untransformed_imu_data(self):
8082
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)
8284

8385
r = Rotation.from_euler('x', self.camera_angle, degrees=True)
8486

@@ -89,32 +91,52 @@ def get_untransformed_gyro_data(self):
8991
gy = self.parser.field_names.index('gyroADC[2]')
9092
gz = self.parser.field_names.index('gyroADC[0]')
9193
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)
92101

102+
acc_frames = []
103+
93104
last_t = 0
94105
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)):
97107

98108
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+
99113
last_t = frame.data[t]
100114

101-
if len(data_frames) < 2:
102-
return False
103115
self.final_gyro_data.extend(data_frames)
116+
if acc_index:
117+
self.final_acc_data.extend(acc_frames)
104118

105119

106120
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
107124
self.final_gyro_data[:,0] /= 1000000
108125
self.final_gyro_data[:,1:] *= np.pi/180
109126

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+
110132
# rough gyro rate assumed to be constant
111133

112134
self.gyro_rate = self.final_gyro_data.shape[0]/(self.final_gyro_data[-1,0] - self.final_gyro_data[0,0])
113135

114136

115137
self.extracted = True
116138

117-
return self.final_gyro_data
139+
return self.final_gyro_data, self.final_acc_data
118140

119141

120142

gyro_integrator.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -137,12 +137,13 @@ def integrate_all(self, use_acc = False):
137137
if apply_complementary:
138138
if complementary_mask[i]:
139139
avec = self.acc[i][1:]
140+
avec /= np.linalg.norm(avec)
140141

141142
accWorldVec = quat.rotate_vector_fast(self.orientation, avec)
142143
correctionWorld = np.cross(accWorldVec, self.grav_vec)
143144

144145
# high weight for first two seconds to "lock" it, then
145-
weight = 10 if this_time - start_time < 1.5 else 0.6
146+
weight = 10 if this_time - start_time < 1.5 else 0.5
146147
correctionBody = weight * quat.rotate_vector_fast(quat.conjugate(self.orientation), correctionWorld)
147148
omega = omega + correctionBody
148149

gyrolog.py

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -626,10 +626,10 @@ def extract_log_internal(self, filename):
626626

627627
try:
628628
bbe = BlackboxExtractor(filename)
629-
self.gyro = bbe.get_gyro_data(cam_angle_degrees=self.angle_setting)
629+
self.gyro, self.acc = bbe.get_untransformed_imu_data()
630630

631631
return True
632-
except:
632+
except: # TODO: change
633633
print("Error reading raw blackbox file. Try converting to CSV in blackbox explorer")
634634
return False
635635

@@ -1152,9 +1152,9 @@ def guess_log_type_from_log(logfile, check_data = False):
11521152
#"test_clips/RC_0038_210813211513.MP4",
11531153
#"test_clips/RC_0031_210722220523.MP4",
11541154
#"test_clips/Runcam/RC_0036_filtered.MP4",
1155-
"test_clips/DJIG0043wiebe.mp4",
1156-
"test_clips/GX016015.MP4",
1157-
"test_clips/nivim_insta360.mp4",
1155+
#"test_clips/DJIG0043wiebe.mp4",
1156+
#"test_clips/GX016015.MP4",
1157+
#"test_clips/nivim_insta360.mp4",
11581158
"test_clips/Tiago_Ferreira_5_inch.mp4",
11591159
"test_clips/MasterTim17_caddx.mp4",
11601160
"test_clips/starling2.MOV",

0 commit comments

Comments
 (0)