def __init__(self, input_data, initial_orientation=None):
"""Initialize instance of FrameRotationIntegrator for getting orientation from frame change data
Args:
input_data (numpy.ndarray): Nx4 array, where each row is [frame num, gyroX,gyroY,gyroZ]
initial_orientation (float[4]): Quaternion representing the starting orientation, Defaults to [1, 0.0001, 0.0001, 0.0001].
"""
self.data = np.copy(input_data)
self.num_data_points = self.data.shape[0]
# initial orientation quaternion
if type(initial_orientation) != type(None):
self.orientation = np.array(initial_orientation)
else:
self.orientation = np.array([1, 0.0001, 0.0001, 0.0001])
# Variables to save integration data
self.orientation_list = None
self.time_list = None
# IMU reference vectors
self.imuRefX = quat.vector(1, 0, 0)
self.imuRefY = quat.vector(0, 1, 0)
self.imuRefY = quat.vector(0, 0, 1)
self.already_integrated = False
def __init__(self,
input_data,
time_scaling=1,
gyro_scaling=1,
zero_out_time=True,
initial_orientation=None,
acc_data=None):
"""Initialize instance of gyroIntegrator for getting orientation from gyro data
Args:
input_data (numpy.ndarray): Nx4 array, where each row is [time, gyroX,gyroY,gyroZ]
time_scaling (int, optional): time * time_scaling should give time in second. Defaults to 1.
gyro_scaling (int, optional): gyro<xyz> * gyro_scaling should give angular velocity in rad/s. Defaults to 1.
zero_out_time (bool, optional): Always start time at 0 in the output data. Defaults to True.
initial_orientation (float[4]): Quaternion representing the starting orientation, Defaults to [1, 0.0001, 0.0001, 0.0001].
acc_data (numpy.ndarray): Nx4 array, where each row is [time, accX, accY, accZ]. TODO: Use this in orientation determination
"""
self.data = np.copy(input_data)
# scale input data
self.data[:, 0] *= time_scaling
self.data[:, 1:4] *= gyro_scaling
# Make sure input data is right handed. Final virtual camera rotation is left-handed
# while image rotation is right-handed.
self.data[:, 2] *= -1
# zero out timestamps
if zero_out_time:
self.data[:, 0] -= self.data[0, 0]
self.num_data_points = self.data.shape[0]
self.gyro_sample_rate = self.num_data_points / (self.data[-1, 0] -
self.data[0, 0])
# initial orientation quaternion
if type(initial_orientation) != type(None):
self.orientation = np.array(initial_orientation)
else:
self.orientation = np.array([1, 0.0001, 0.0001, 0.0001])
# Variables to save integration data
self.orientation_list = None
self.time_list = None
# IMU reference vectors
self.imuRefX = quat.vector(1, 0, 0)
self.imuRefY = quat.vector(0, 1, 0)
self.imuRefY = quat.vector(0, 0, 1)
self.already_integrated = False
def __init__(self,
input_data,
time_scaling=1,
gyro_scaling=1,
zero_out_time=True,
initial_orientation=None):
"""Initialize instance of gyroIntegrator for getting orientation from gyro data
Args:
input_data (numpy.ndarray): Nx4 array, where each row is [time, gyroX,gyroY,gyroZ]
time_scaling (int, optional): time * time_scaling should give time in second. Defaults to 1.
gyro_scaling (int, optional): gyro<xyz> * gyro_scaling should give angular velocity in rad/s. Defaults to 1.
zero_out_time (bool, optional): Always start time at 0 in the output data. Defaults to True.
initial_orientation (float[4]): Quaternion representing the starting orientation, Defaults to [1, 0.0001, 0.0001, 0.0001].
"""
self.data = np.copy(input_data)
# scale input data
self.data[:, 0] *= time_scaling
self.data[:, 1:4] *= gyro_scaling
# zero out timestamps
if zero_out_time:
self.data[:, 0] -= self.data[0, 0]
self.num_data_points = self.data.shape[0]
# initial orientation quaternion
if initial_orientation:
self.orientation = np.array(initial_orientation)
else:
self.orientation = np.array([1, 0.0001, 0.0001, 0.0001])
# Variables to save integration data
self.orientation_list = None
self.time_list = None
# IMU reference vectors
self.imuRefX = quart.vector(1, 0, 0)
self.imuRefY = quart.vector(0, 1, 0)
self.imuRefY = quart.vector(0, 0, 1)
self.already_integrated = False
def __init__(self,
gyro_data,
time_scaling=1,
gyro_scaling=1,
zero_out_time=True,
initial_orientation=None,
acc_data=None,
acc_scaling=1):
"""Initialize instance of gyroIntegrator for getting orientation from gyro data
Args:
gyro_data (numpy.ndarray): Nx4 array, where each row is [time, gyroX,gyroY,gyroZ]
time_scaling (int, optional): time * time_scaling should give time in second. Defaults to 1.
gyro_scaling (int, optional): gyro<xyz> * gyro_scaling should give angular velocity in rad/s. Defaults to 1.
zero_out_time (bool, optional): Always start time at 0 in the output data. Defaults to True.
initial_orientation (float[4]): Quaternion representing the starting orientation, Defaults to [1, 0.0001, 0.0001, 0.0001].
acc_data (numpy.ndarray): Nx4 array, where each row is [time, accX, accY, accZ]. TODO: Use this in orientation determination
acc_scaling (float): Scaling to give the acceleration in g
"""
# data is only the gyro
self.gyro = np.copy(gyro_data)
self.acc = None
self.last_used_acc = False
self.acc_cutoff = 1 # Hz, low cutoff
self.acc_available = False
if type(acc_data) != type(None):
# resample if they don't already match
if self.gyro.shape[0] == acc_data.shape[0]:
self.acc_available = True
self.acc = np.copy(acc_data)
self.gyro[:, 0] *= time_scaling
self.gyro[:, 1:4] *= acc_scaling
else:
print("Gyro and acceleration data don't line up")
self.acc_available = False
#if self.acc_available:
#print(self.acc.shape)
# Check for corrupted/out of order timestamps
time_order_check = self.gyro[:-1, 0] > self.gyro[1:, 0]
if np.any(time_order_check):
print("Truncated bad gyro data")
self.gyro = self.gyro[0:np.argmax(time_order_check) + 1, :]
if self.acc_available:
self.acc = self.acc[0:np.argmax(time_order_check) + 1, :]
# scale input data
self.gyro[:, 0] *= time_scaling
self.gyro[:, 1:4] *= gyro_scaling
# Make sure input data is right handed. Final virtual camera rotation is left-handed
# while image rotation is right-handed. Improve this later
#self.gyro[:,2] *= -1 # y axis
# zero out timestamps
if zero_out_time:
self.gyro[:, 0] -= self.gyro[0, 0]
if self.acc_available:
self.acc[:, 0] -= self.acc[0, 0]
self.num_data_points = self.gyro.shape[0]
self.gyro_sample_rate = self.num_data_points / (self.gyro[-1, 0] -
self.gyro[0, 0])
# initial orientation quaternion
if type(initial_orientation) != type(None):
self.initial_orientation = np.array(initial_orientation)
self.orientation = np.array(initial_orientation)
else:
self.initial_orientation = np.array([1, 0.0001, 0.0001, 0.0001])
self.orientation = np.array([1, 0.0001, 0.0001, 0.0001])
# Variables to save integration data
self.orientation_list = None
self.time_list = None
# IMU reference vectors
self.imuRefX = quat.vector(1, 0, 0)
self.imuRefY = quat.vector(0, 1, 0)
self.imuRefY = quat.vector(0, 0, 1)
# Gravity vector
# points upwards, since it's equivalent to an upwards acceleration at rest
self.grav_vec = np.array([0, 1, 0]) # Per convention it's upwards
self.already_integrated = False
self.smoothing_algo = None
self.interp_times = None
self.interp_orientations = None
