def __init__(self, videopath, calibrationfile, hero = 8, fov_scale = 1.6):
# General video stuff
self.undistort_fov_scale = fov_scale
self.cap = cv2.VideoCapture(videopath)
self.width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
self.height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
self.fps = self.cap.get(cv2.CAP_PROP_FPS)
self.num_frames = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT))
# Camera undistortion stuff
self.undistort = FisheyeCalibrator()
self.undistort.load_calibration_json(calibrationfile, True)
self.map1, self.map2 = self.undistort.get_maps(self.undistort_fov_scale,new_img_dim=(self.width,self.height))
# Get gyro data
self.gpmf = Extractor(videopath)
self.gyro_data = self.gpmf.get_gyro(True)
# Hero 6??
if hero == 6:
self.gyro_data[:,1] = self.gyro_data[:,1]
self.gyro_data[:,2] = -self.gyro_data[:,2]
self.gyro_data[:,3] = self.gyro_data[:,3]
elif hero == 5:
self.gyro_data[:,1] = -self.gyro_data[:,1]
self.gyro_data[:,2] = self.gyro_data[:,2]
self.gyro_data[:,3] = -self.gyro_data[:,3]
self.gyro_data[:,[2, 3]] = self.gyro_data[:,[3, 2]]
elif hero == 8:
# Hero 8??
self.gyro_data[:,[2, 3]] = self.gyro_data[:,[3, 2]]
#gyro_data[:,1] = gyro_data[:,1]
#gyro_data[:,2] = -gyro_data[:,2]
#gyro_data[:,3] = gyro_data[:,3]
#gyro_data[:,1:] = -gyro_data[:,1:]
#points_test = np.array([[[0,0],[100,100],[200,300],[400,400]]], dtype = np.float32)
#result = self.undistort.undistort_points(points_test, new_img_dim=(self.width,self.height))
#print(result)
#exit()
# Other attributes
initial_orientation = Rotation.from_euler('xyz', [0, 0, 0], degrees=True).as_quat()
self.integrator = GyroIntegrator(self.gyro_data,initial_orientation=initial_orientation)
self.integrator.integrate_all()
self.times = None
self.stab_transform = None
self.initial_offset = 0
def check_log_type(self, filename):
#gyro_data = gpmf.get_gyro(True)
if self.filename_matches(filename):
try:
self.gpmf = GPMFExtractor(filename)
return True
except:
# Error if it doesn't contain GPMF data
return False
return False
def __init__(self, videopath, calibrationfile):
# General video stuff
self.cap = cv2.VideoCapture(videopath)
self.width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
self.height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
self.fps = self.cap.get(cv2.CAP_PROP_FPS)
self.num_frames = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT))
# Camera undistortion stuff
self.undistort = FisheyeCalibrator()
self.undistort.load_calibration_json(calibrationfile, True)
self.map1, self.map2 = self.undistort.get_maps(
1.6, new_img_dim=(self.width, self.height))
# Get gyro data
self.gpmf = Extractor(videopath)
self.gyro_data = self.gpmf.get_gyro(True)
# Hero 6??
#self.gyro_data[:,1] = self.gyro_data[:,1]
#self.gyro_data[:,2] = -self.gyro_data[:,2]
#self.gyro_data[:,3] = self.gyro_data[:,3]
# Hero 8??
self.gyro_data[:, [2, 3]] = self.gyro_data[:, [3, 2]]
#gyro_data[:,1] = gyro_data[:,1]
#gyro_data[:,2] = -gyro_data[:,2]
#gyro_data[:,3] = gyro_data[:,3]
#gyro_data[:,1:] = -gyro_data[:,1:]
# Other attributes
initial_orientation = Rotation.from_euler('xyz', [0, 0, 0],
degrees=True).as_quat()
self.integrator = GyroIntegrator(
self.gyro_data, initial_orientation=initial_orientation)
self.integrator.integrate_all()
self.times = None
self.stab_transform = None
class GPMFStabilizer(Stabilizer):
def __init__(self, videopath, calibrationfile, hero = 8, fov_scale = 1.6):
# General video stuff
self.undistort_fov_scale = fov_scale
self.cap = cv2.VideoCapture(videopath)
self.width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
self.height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
self.fps = self.cap.get(cv2.CAP_PROP_FPS)
self.num_frames = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT))
# Camera undistortion stuff
self.undistort = FisheyeCalibrator()
self.undistort.load_calibration_json(calibrationfile, True)
self.map1, self.map2 = self.undistort.get_maps(self.undistort_fov_scale,new_img_dim=(self.width,self.height))
# Get gyro data
self.gpmf = Extractor(videopath)
self.gyro_data = self.gpmf.get_gyro(True)
# Hero 6??
if hero == 6:
self.gyro_data[:,1] = self.gyro_data[:,1]
self.gyro_data[:,2] = -self.gyro_data[:,2]
self.gyro_data[:,3] = self.gyro_data[:,3]
elif hero == 5:
self.gyro_data[:,1] = -self.gyro_data[:,1]
self.gyro_data[:,2] = self.gyro_data[:,2]
self.gyro_data[:,3] = -self.gyro_data[:,3]
self.gyro_data[:,[2, 3]] = self.gyro_data[:,[3, 2]]
elif hero == 8:
# Hero 8??
self.gyro_data[:,[2, 3]] = self.gyro_data[:,[3, 2]]
#gyro_data[:,1] = gyro_data[:,1]
#gyro_data[:,2] = -gyro_data[:,2]
#gyro_data[:,3] = gyro_data[:,3]
#gyro_data[:,1:] = -gyro_data[:,1:]
#points_test = np.array([[[0,0],[100,100],[200,300],[400,400]]], dtype = np.float32)
#result = self.undistort.undistort_points(points_test, new_img_dim=(self.width,self.height))
#print(result)
#exit()
# Other attributes
initial_orientation = Rotation.from_euler('xyz', [0, 0, 0], degrees=True).as_quat()
self.integrator = GyroIntegrator(self.gyro_data,initial_orientation=initial_orientation)
self.integrator.integrate_all()
self.times = None
self.stab_transform = None
self.initial_offset = 0
def stabilization_settings(self, smooth = 0.95):
v1 = 20 / self.fps
v2 = 900 / self.fps
d1 = 0.042
d2 = -0.396
err_slope = (d2-d1)/(v2-v1)
correction_slope = err_slope + 1
gyro_start = (d1 - err_slope*v1)
interval = 1/(correction_slope * self.fps)
print("Start {}".format(gyro_start))
print("Interval {}, slope {}".format(interval, correction_slope))
self.times, self.stab_transform = self.integrator.get_interpolated_stab_transform(smooth=smooth,start=-gyro_start,interval = interval) # 2.2/30 , -1/30
def extract_log_internal(self, filename):
try:
if self.gpmf:
if self.gpmf.videopath == filename:
pass
else:
self.gpmf = GPMFExtractor(filename)
else:
self.gpmf = GPMFExtractor(filename)
self.gyro = self.gpmf.get_gyro(True)
self.gpmf.parse_accl()
self.acc = self.gpmf.get_accl(True)
minlength = min(self.gyro.shape[0], self.acc.shape[0])
maxlength = max(self.gyro.shape[0], self.acc.shape[0])
# Make sure they match
if maxlength - minlength == 0: #
pass
elif maxlength - minlength < 10:
# probably just some missing datapoints
self.gyro = self.gyro[0:minlength]
self.acc = self.acc[0:minlength]
self.acc[:,
0] = self.gyro[:,
0] # same timescale, acceleration less time-sensitive
else:
to_interp = interpolate.interp1d(self.acc[:, 0],
self.acc[:, 1:],
axis=0,
fill_value=np.array([0, 1,
0]),
bounds_error=False)
new_acc = np.copy(self.gyro)
new_acc[:, 1:] = to_interp(self.gyro[:, 0])
self.acc = new_acc
# resample acc to gyro timescale
except Exception as e:
print(e)
print("Failed to extract GPMF gyro")
return False
hero = int(self.variant.lstrip("hero"))
# Hero 6??
if hero == 6:
pass
# Identity
#self.gyro[:,1] = self.gyro[:,1]
#self.gyro[:,2] = self.gyro[:,2]
#self.gyro[:,3] = self.gyro[:,3]
elif hero == 7:
pass
#self.gyro[:,1] = self.gyro[:,1]
#self.gyro[:,2] = self.gyro[:,2]
#self.gyro[:,3] = self.gyro[:,3]
elif hero == 5:
pass
# equivalent to matrix index 13
#self.gyro[:,1] = -self.gyro[:,1]
#self.gyro[:,2] = self.gyro[:,2]
#self.gyro[:,3] = self.gyro[:,3]
#self.gyro[:,[2, 3]] = self.gyro[:,[3, 2]]
elif hero == 8:
pass
# Hero 8??
# equal matrix index 1
#self.gyro[:,[2, 3]] = self.gyro[:,[3, 2]]
#self.gyro[:,2] = -self.gyro[:,2]
elif hero == 9:
pass
#self.gyro[:,1] = -self.gyro[:,1]
#self.gyro[:,2] = self.gyro[:,2]
#self.gyro[:,3] = self.gyro[:,3]
#self.gyro[:,[2, 3]] = self.gyro[:,[3, 2]]
return True
class GPMFLog(GyrologReader):
def __init__(self):
super().__init__("GoPro GPMF metadata")
self.filename_pattern = "(?i).*\.mp4"
self.variants = {
"hero5": [13],
"hero6": [0],
"hero7": [0],
"hero8": [1],
"hero9": [13]
}
self.variant = "hero6"
self.default_filter = -1
self.default_search_size = 4
self.gpmf = None
self.post_init()
def check_log_type(self, filename):
#gyro_data = gpmf.get_gyro(True)
if self.filename_matches(filename):
try:
self.gpmf = GPMFExtractor(filename)
return True
except:
# Error if it doesn't contain GPMF data
return False
return False
def guess_log_from_videofile(self, videofile):
if self.check_log_type(videofile):
return videofile
else:
return False
def extract_log_internal(self, filename):
try:
if self.gpmf:
if self.gpmf.videopath == filename:
pass
else:
self.gpmf = GPMFExtractor(filename)
else:
self.gpmf = GPMFExtractor(filename)
self.gyro = self.gpmf.get_gyro(True)
self.gpmf.parse_accl()
self.acc = self.gpmf.get_accl(True)
minlength = min(self.gyro.shape[0], self.acc.shape[0])
maxlength = max(self.gyro.shape[0], self.acc.shape[0])
# Make sure they match
if maxlength - minlength == 0: #
pass
elif maxlength - minlength < 10:
# probably just some missing datapoints
self.gyro = self.gyro[0:minlength]
self.acc = self.acc[0:minlength]
self.acc[:,
0] = self.gyro[:,
0] # same timescale, acceleration less time-sensitive
else:
to_interp = interpolate.interp1d(self.acc[:, 0],
self.acc[:, 1:],
axis=0,
fill_value=np.array([0, 1,
0]),
bounds_error=False)
new_acc = np.copy(self.gyro)
new_acc[:, 1:] = to_interp(self.gyro[:, 0])
self.acc = new_acc
# resample acc to gyro timescale
except Exception as e:
print(e)
print("Failed to extract GPMF gyro")
return False
hero = int(self.variant.lstrip("hero"))
# Hero 6??
if hero == 6:
pass
# Identity
#self.gyro[:,1] = self.gyro[:,1]
#self.gyro[:,2] = self.gyro[:,2]
#self.gyro[:,3] = self.gyro[:,3]
elif hero == 7:
pass
#self.gyro[:,1] = self.gyro[:,1]
#self.gyro[:,2] = self.gyro[:,2]
#self.gyro[:,3] = self.gyro[:,3]
elif hero == 5:
pass
# equivalent to matrix index 13
#self.gyro[:,1] = -self.gyro[:,1]
#self.gyro[:,2] = self.gyro[:,2]
#self.gyro[:,3] = self.gyro[:,3]
#self.gyro[:,[2, 3]] = self.gyro[:,[3, 2]]
elif hero == 8:
pass
# Hero 8??
# equal matrix index 1
#self.gyro[:,[2, 3]] = self.gyro[:,[3, 2]]
#self.gyro[:,2] = -self.gyro[:,2]
elif hero == 9:
pass
#self.gyro[:,1] = -self.gyro[:,1]
#self.gyro[:,2] = self.gyro[:,2]
#self.gyro[:,3] = self.gyro[:,3]
#self.gyro[:,[2, 3]] = self.gyro[:,[3, 2]]
return True
from crisp.l3g4200d import post_process_L3G4200D_data
# Note: currently uses a modified crisp package with support for the OpenCV Fisheye module
import crisp.rotations
from crisp.calibration import PARAM_ORDER
import crisp.videoslice
import cv2
import logging
logging.basicConfig(level=logging.DEBUG)
logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)
video_file_path = "test_clips/chessboard.mp4"
testgyro = Extractor(video_file_path)
gyro_data = testgyro.get_gyro()
gyro_rate = testgyro.gyro_rate
print("Rate: {}".format(gyro_rate))
fps = testgyro.fps
img_size = testgyro.size
CAMERA_MATRIX = np.array([[847.6148226238896, 0.0, 960.0],
[0.0, 852.8260246970873, 720.0], [0.0, 0.0, 1.0]])
# Scale to match resolution
CAMERA_MATRIX *= img_size[0] / 1920
CAMERA_MATRIX[2][2] = 1.0
CAMERA_DIST_COEFS = [
class GPMFStabilizer:
def __init__(self, videopath, calibrationfile):
# General video stuff
self.cap = cv2.VideoCapture(videopath)
self.width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
self.height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
self.fps = self.cap.get(cv2.CAP_PROP_FPS)
self.num_frames = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT))
# Camera undistortion stuff
self.undistort = FisheyeCalibrator()
self.undistort.load_calibration_json(calibrationfile, True)
self.map1, self.map2 = self.undistort.get_maps(
1.6, new_img_dim=(self.width, self.height))
# Get gyro data
self.gpmf = Extractor(videopath)
self.gyro_data = self.gpmf.get_gyro(True)
# Hero 6??
#self.gyro_data[:,1] = self.gyro_data[:,1]
#self.gyro_data[:,2] = -self.gyro_data[:,2]
#self.gyro_data[:,3] = self.gyro_data[:,3]
# Hero 8??
self.gyro_data[:, [2, 3]] = self.gyro_data[:, [3, 2]]
#gyro_data[:,1] = gyro_data[:,1]
#gyro_data[:,2] = -gyro_data[:,2]
#gyro_data[:,3] = gyro_data[:,3]
#gyro_data[:,1:] = -gyro_data[:,1:]
# Other attributes
initial_orientation = Rotation.from_euler('xyz', [0, 0, 0],
degrees=True).as_quat()
self.integrator = GyroIntegrator(
self.gyro_data, initial_orientation=initial_orientation)
self.integrator.integrate_all()
self.times = None
self.stab_transform = None
def stabilization_settings(self, smooth=0.95):
v1 = 20 / self.fps
v2 = 900 / self.fps
d1 = 0.042
d2 = -0.396
err_slope = (d2 - d1) / (v2 - v1)
correction_slope = err_slope + 1
gyro_start = (d1 - err_slope * v1)
interval = 1 / (correction_slope * self.fps)
print("Start {}".format(gyro_start))
print("Interval {}, slope {}".format(interval, correction_slope))
self.times, self.stab_transform = self.integrator.get_interpolated_stab_transform(
smooth=smooth, start=-gyro_start,
interval=interval) # 2.2/30 , -1/30
def auto_sync_stab(self,
smooth=0.8,
sliceframe1=10,
sliceframe2=1000,
slicelength=50):
v1 = (sliceframe1 + slicelength / 2) / self.fps
v2 = (sliceframe2 + slicelength / 2) / self.fps
d1, times1, transforms1 = self.optical_flow_comparison(
sliceframe1, slicelength)
d2, times2, transforms2 = self.optical_flow_comparison(
sliceframe2, slicelength)
print("v1: {}, v2: {}, d1: {}, d2: {}".format(v1, v2, d1, d2))
err_slope = (d2 - d1) / (v2 - v1)
correction_slope = err_slope + 1
gyro_start = (d1 - err_slope * v1) - 0.4 / self.fps
interval = 1 / (correction_slope * self.fps)
print("Start {}".format(gyro_start))
print("Interval {}, slope {}".format(interval, correction_slope))
plt.plot(times1, transforms1[:, 2])
plt.plot(times2, transforms2[:, 2])
plt.plot((self.integrator.get_raw_data("t") + gyro_start) *
correction_slope, self.integrator.get_raw_data("z"))
plt.plot((self.integrator.get_raw_data("t") + d2),
self.integrator.get_raw_data("z"))
plt.plot((self.integrator.get_raw_data("t") + d1),
self.integrator.get_raw_data("z"))
plt.show()
self.times, self.stab_transform = self.integrator.get_interpolated_stab_transform(
smooth=smooth, start=-gyro_start,
interval=interval) # 2.2/30 , -1/30
def optical_flow_comparison(self, start_frame=0, analyze_length=50):
frame_times = []
frame_idx = []
transforms = []
self.cap.set(cv2.CAP_PROP_POS_FRAMES, start_frame)
# Read first frame
_, prev = self.cap.read()
prev_gray = cv2.cvtColor(prev, cv2.COLOR_BGR2GRAY)
for i in range(analyze_length):
prev_pts = cv2.goodFeaturesToTrack(prev_gray,
maxCorners=200,
qualityLevel=0.01,
minDistance=30,
blockSize=3)
succ, curr = self.cap.read()
frame_id = (int(self.cap.get(cv2.CAP_PROP_POS_FRAMES)))
frame_time = (self.cap.get(cv2.CAP_PROP_POS_MSEC) / 1000)
if i % 10 == 0:
print("Analyzing frame: {}/{}".format(i, analyze_length))
if succ:
# Only add if succeeded
frame_idx.append(frame_id)
frame_times.append(frame_time)
print(frame_time)
curr_gray = cv2.cvtColor(curr, cv2.COLOR_BGR2GRAY)
# Estimate transform using optical flow
curr_pts, status, err = cv2.calcOpticalFlowPyrLK(
prev_gray, curr_gray, prev_pts, None)
idx = np.where(status == 1)[0]
prev_pts = prev_pts[idx]
curr_pts = curr_pts[idx]
assert prev_pts.shape == curr_pts.shape
# TODO: Try getting undistort + homography working for more accurate rotation estimation
#src_pts = undistort.undistort_points(prev_pts, new_img_dim=(int(width),int(height)))
#dst_pts = undistort.undistort_points(curr_pts, new_img_dim=(int(width),int(height)))
#H, mask = cv2.findHomography(src_pts, dst_pts)
#retval, rots, trans, norms = undistort.decompose_homography(H, new_img_dim=(int(width),int(height)))
m, inliers = cv2.estimateAffine2D(prev_pts, curr_pts)
dx = m[0, 2]
dy = m[1, 2]
# Extract rotation angle
da = np.arctan2(m[1, 0], m[0, 0])
transforms.append([dx, dy, da])
prev_gray = curr_gray
else:
print("Frame {}".format(i))
transforms = np.array(transforms) * self.fps
estimated_offset = self.estimate_gyro_offset(frame_times, transforms)
return estimated_offset, frame_times, transforms
# Test stuff
v1 = 20 / self.fps
v2 = 1300 / self.fps
d1 = 0.042
d2 = -0.604
err_slope = (d2 - d1) / (v2 - v1)
correction_slope = err_slope + 1
gyro_start = (d1 - err_slope * v1)
interval = correction_slope * 1 / self.fps
plt.plot(frame_times, transforms[:, 2])
plt.plot((self.integrator.get_raw_data("t") + gyro_start) *
correction_slope, self.integrator.get_raw_data("z"))
plt.show()
def estimate_gyro_offset(self, OF_times, OF_transforms):
# Estimate offset between small optical flow slice and gyro data
gyro_times = self.integrator.get_raw_data("t")
gyro_roll = self.integrator.get_raw_data("z")
OF_roll = OF_transforms[:, 2]
costs = []
offsets = []
for i in range(800):
offset = 0.8 - i / 500
cost = self.gyro_cost_func(OF_times, OF_roll, gyro_times + offset,
gyro_roll)
offsets.append(offset)
costs.append(cost)
final_offset = offsets[np.argmin(costs)]
print("Estimated offset: {}".format(final_offset))
plt.plot(offsets, costs)
plt.show()
return final_offset
def gyro_cost_func(self, OF_times, OF_roll, gyro_times, gyro_roll):
sum_squared_diff = 0
gyro_idx = 0
for OF_idx in range(len(OF_times)):
while gyro_times[gyro_idx] < OF_times[OF_idx]:
gyro_idx += 1
diff = gyro_roll[gyro_idx] - OF_roll[OF_idx]
sum_squared_diff += diff**2
#print("Gyro {}, OF {}".format(gyro_times[gyro_idx], OF_times[OF_idx]))
#print("DIFF^2: {}".format(sum_squared_diff))
#plt.plot(OF_times, OF_roll)
#plt.plot(gyro_times, gyro_roll)
#plt.show()
return sum_squared_diff
def renderfile(self, outpath="Stabilized.mp4", out_size=(1920, 1080)):
out = cv2.VideoWriter(outpath, -1, 29.97, (1920 * 2, 1080))
crop = (int((self.width - out_size[0]) / 2),
int((self.height - out_size[1]) / 2))
self.cap.set(cv2.CAP_PROP_POS_FRAMES, 21 * 30)
i = 0
while (True):
# Read next frame
success, frame = self.cap.read()
frame_num = int(self.cap.get(cv2.CAP_PROP_POS_FRAMES))
print("FRAME: {}, IDX: {}".format(frame_num, i))
if success:
i += 1
if i > 1000:
break
if success and i > 0:
frame_undistort = cv2.remap(frame,
self.map1,
self.map2,
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT)
frame_out = self.undistort.get_rotation_map(
frame_undistort, self.stab_transform[frame_num - 1])
# Fix border artifacts
frame_out = frame_out[crop[1]:crop[1] + out_size[1],
crop[0]:crop[0] + out_size[0]]
frame_undistort = frame_undistort[crop[1]:crop[1] +
out_size[1],
crop[0]:crop[0] +
out_size[0]]
#out.write(frame_out)
#print(frame_out.shape)
# If the image is too big, resize it.
#%if(frame_out.shape[1] > 1920):
# frame_out = cv2.resize(frame_out, (int(frame_out.shape[1]/2), int(frame_out.shape[0]/2)));
size = np.array(frame_out.shape)
frame_out = cv2.resize(frame_out, (int(size[1]), int(size[0])))
frame = cv2.resize(frame_undistort,
((int(size[1]), int(size[0]))))
concatted = cv2.resize(cv2.hconcat([frame_out, frame], 2),
(1920 * 2, 1080))
out.write(concatted)
cv2.imshow("Before and After", concatted)
cv2.waitKey(5)
# When everything done, release the capture
out.release()
def release(self):
self.cap.release()
out_size = (1920, 1080)
crop_start = (int((width - out_size[0]) / 2), int((height - out_size[1]) / 2))
out = cv2.VideoWriter(outpath, -1, 29.97, (1920 * 2, 1080))
undistort = FisheyeCalibrator()
undistort.load_calibration_json("camera_presets/Hero_7_2.7K_60_4by3_wide.json",
True)
map1, map2 = undistort.get_maps(1.6, new_img_dim=(int(width), int(height)))
# Here
gpmf = Extractor("test_clips/GX016017.MP4")
#bb = BlackboxExtractor("test_clips/GX015563.MP4_emuf_004.bbl")
#gyro_data = bb.get_gyro_data()
gyro_data = gpmf.get_gyro(True)
#gyro_data[:,[2, 3]] = gyro_data[:,[3, 2]]
gyro_data[:, 1] = gyro_data[:, 1]
gyro_data[:, 2] = -gyro_data[:, 2]
gyro_data[:, 3] = gyro_data[:, 3]
#gyro_data[:,1:] = -gyro_data[:,1:]
initial_orientation = Rotation.from_euler('xyz', [0, 0, 0],
degrees=True).as_quat()