import numpy as np import sys import scipy.integrate import math import argparse from sklearn.decomposition import PCA import scipy.signal as signal def project(v1, v2): """ Project vector v1 on v2 Return projected vector """ p = [np.dot(a, g) / np.dot(g,g) for a,g in zip(v1, v2)] p = np.array(p) p = [p*g for p,g in zip(p, v2)] p = np.array(p) return p def motion_axis(time, lin_accel, gravity, interval = 500): """ Returns the motion axis, which is the axis with the biggest variance lin_accel -- Linear acceleration gravity -- Gravity Lin_accel and gravity should have equal length """ p = project(lin_accel, gravity) #add time to vector p p = np.array([time, p[:,0], p[:,1], p[:,2]]).T start = 0 end = start + interval end_time = p[:,0][-1] #last timestep pca = PCA(n_components=1) result = [] while start < end_time: indices = np.where((p[:,0] >= start) & (p[:,0] < end)) Z = p[indices, 1:3][0] Z[:,0] = signal.medfilt(Z[:,0],31) Z[:,1] = signal.medfilt(Z[:,1],31) pca.fit(Z) x1 = pca.components_[0][0] y1 = pca.components_[0][1] result.append((end, x1, y1)) start += interval end += interval return np.array(result) def angle_between(v1, v2): l_a = np.linalg.norm(v1) l_b = np.linalg.norm(v2) cos_ab = np.dot(v1, v2 / (l_a * l_b)) angle = np.arccos(cos_ab) * 180/math.pi return min([180 - angle, angle]) def rotate_data_fhws(data, data_t, rotation, rotation_t): #Invert rotationmatrix np.linalg.inv(rotation) #Align rotation time according to data time tmp = [] for t in data_t: # Find indices of roation matrix that are earlier #than the current time of the sensor value ind = np.where(rotation_t <= t)[0] #Use the last index if len(ind) != 0: tmp.append(ind[-1]) else: tmp.append(0) #Only use the values of the rotation matrix that are aligned with the sensor data rotation = rotation[tmp] # Multiply data with rotation matrix rot_data = [] for i, row in enumerate(data): row = np.append(row, 1) rot_data.append(np.dot(rotation[i], row)) return np.array(rot_data) def rotate_data_lukas(data, rotation): #Invert rotationmatrix np.linalg.inv(rotation) rot_data = [] for i, row in enumerate(data): row = np.append(row, 1) rot_data.append(np.dot(rotation[i], row)) return np.array(rot_data) def read_data(fname): """ Read the data out of the file provided by FHWS sensor reader app """ time = np.loadtxt(fname, delimiter=";", usecols=[0], unpack=True) f = open(fname, 'r') lin_accel = [] gyros = [] rotations = [] gravity = [] start = time[0] time = [] gyro_tmp = [0, 0, 0] lin_accel_tmp = [0, 0, 0] gravity_tmp = [0, 0, 9.81] rotations_tmp = 16*[-1] s = 0 for line in f: line = line.split(";") t = int(line[0]) - start #Gyro-Data if line[1] == "3": gyro_tmp[0] = line[2] gyro_tmp[1] = line[3] gyro_tmp[2] = line[4] #Linear Acceleration-Data elif line[1] == "2": lin_accel_tmp[0] = line[2] lin_accel_tmp[1] = line[3] lin_accel_tmp[2] = line[4] #Gravity data elif line[1] == "1": gravity_tmp[0] = line[2] gravity_tmp[1] = line[3] gravity_tmp[2] = line[4] #Rotation-Data elif line[1] == "7": for i in range(0,16): rotations_tmp[i] = line[i+2] if s != t: gyros.append(gyro_tmp[:]) lin_accel.append(lin_accel_tmp[:]) gravity.append(gravity_tmp[:]) rotations.append(rotations_tmp[:]) time.append(t) s = t gyros = np.array(gyros, dtype=float) lin_accel = np.array(lin_accel, dtype=float) gravity = np.array(gravity, dtype=float) rotations = np.array(rotations, dtype=float) time = np.array(time, dtype = int) #HACK #In the first timestamps the rotation matrix is all zero, because #no measurements are available yet. #Avoid this by replacing these lines with the first measured #rotation matrix ind = np.where(rotations[:,0] == -1)[0] if len(ind) != 0: index = ind[-1] + 1 rotations[ind] = rotations[index] #Reshape matrix rotations = [row.reshape((4,4)) for row in rotations] rotations = np.array(rotations) return time, gyros, lin_accel, gravity, rotations def detect_turns(time, signal, interval): n_intervals = int(time[-1] / interval) + 1 result = [] for i in range(n_intervals): start = i * interval end = start + interval tmp = integrate(start, end, zip(time, signal)) * 180.0/math.pi result.append((end, tmp)) return np.array(result) def integrate(time_from, time_to, signal): """Integrate signal from time_from to time_to. Signal should be two dimensional. First dimension is the timestamp, second dimension is the signal value. dt is the interval between two recordings """ sum = 0 last_time = 0 #go through signal for value in signal: #check if time is in the given timespan if time_from <= value[0] < time_to: #multiply value with dt and add it to the sum = integral # sum += value[1] * dt sum += value[1] * ((value[0] - last_time)/1000.) last_time = value[0] #sum is the integral over rad/s return sum def write_to_file(fname, turns, motion): f = open(fname, 'w') for index, t in enumerate(turns): f.write(str(t[0]) + "," + str(t[1]) + "," + str(motion[index][1]) + "\n") f.close() def deg_to_rad(deg): return deg * math.pi / 180.0 def rad_to_deg(rad): return rad * 180.0 / math.pi def main(): parser = argparse.ArgumentParser() parser.add_argument("fname_sensor", help = "Gyroscope file") parser.add_argument("fname_output", help = "Output file, where timestamps and angle of heading will be saved") parser.add_argument("--time", help = "Time interval, over which gyroscope will be integrated (default=500ms)", type=int) parser.add_argument("--rad", help = "Output angles in rad (default in degree)", action = "store_true") parser.add_argument("--file_format", help = "Sensor data file format [fhws|lukas] (default: fhws)", type = str) parser.add_argument("--cosy", help = "Coordinate system of the gyroscope data [world|device] (default: device). If given in device, the data will automatically be rotated in world coordinates.", type = str) args = parser.parse_args() #Choose between file format of sensor data and coordinate system file_format = "fhws" cosy = "device" if args.file_format: file_format = args.file_format if args.cosy: cosy = args.cosy #My own data format if file_format == "lukas": delimiter = "," time_cols = [40] time = np.loadtxt(args.fname_sensor, delimiter=delimiter, usecols=time_cols, skiprows = 1, unpack=True) if cosy == "device": gyros_cols = [9, 10, 11] lin_accel_cols = [6, 7, 8] else: gyros_cols = [34, 35,36] lin_accel_cols = [37, 38, 39] grav_cols = [3, 4, 5] gyroX, gyroY, gyroZ = np.loadtxt(args.fname_sensor, delimiter=delimiter, usecols=gyros_cols, skiprows = 1, unpack=True) rotation = np.loadtxt(args.fname_sensor, delimiter = delimiter, usecols=range(18,34), skiprows=1, unpack=True) lin_accel_X, lin_accel_Y, lin_accel_Z = np.loadtxt(args.fname_sensor, delimiter=delimiter, usecols=lin_accel_cols, skiprows=1, unpack=True) gravity_X, gravity_Y, gravity_Z = np.loadtxt(args.fname_sensor, delimiter=delimiter, usecols=grav_cols, skiprows=1, unpack=True) rotation = rotation.T rotation = [row.reshape((4,4)) for row in rotation] # rotation = np.array(rotation).T print rotation gyro = np.array([gyroX, gyroY, gyroZ]).T lin_accel = np.array([lin_accel_X, lin_accel_Y, lin_accel_Z]).T gravity = np.array([gravity_X, gravity_Y, gravity_Z]).T if cosy == "device": world_gyro = rotate_data_lukas(gyro, rotation) world_lin_accel = rotate_data_lukas(lin_accel, rotation) else: world_gyro = gyro world_lin_accel = lin_accel #FHWS file format else: time, gyro, lin_accel, gravity, rotation = read_data(args.fname_sensor) if cosy == "device": world_gyro = rotate_data_lukas(gyro, rotation) world_lin_accel = rotate_data_lukas(lin_accel, rotation) else: print "Option 'fhws' in combination with 'world' not available" return gyroX = world_gyro[:,0] gyroY = world_gyro[:,1] gyroZ = world_gyro[:,2] lin_accel_X = world_lin_accel[:,0] lin_accel_Y = world_lin_accel[:,1] lin_accel_Z = world_lin_accel[:,2] #Parameters #--------- time_interval = 500 if args.time: time_interval = args.time turns = detect_turns(time, gyroZ, time_interval) motion = motion_axis(time, lin_accel, gravity, 500) angles = [] for index, axis in enumerate(motion): if index == 0: angle = 0 else: x_1 = motion[index-1][1] y_1 = motion[index-1][2] x_2 = axis[1] y_2 = axis[2] a = np.array([x_1, y_1]) b = np.array([x_2, y_2]) angle = angle_between(a,b) angles.append((axis[0], angle)) np.set_printoptions(suppress=True) turns = np.array(turns) angles = np.array(angles) if args.rad: turns[:,1] = deg_to_rad(turns[:,1]) print "Sum of all angles: ", np.sum(turns[:,1]) write_to_file(args.fname_output, turns, angles) if __name__ == "__main__": main()