def correctData(fileToCorrect, OS, rot): offset = np.array([-4,-1, 0]) rawdat, header = BiteplateUtils.loadTsv(fileToCorrect) numSensors = int(np.floor(rawdat.shape[1]/9)) swapDat= rawdat.copy() for j in range(1,numSensors-1): #Position values swapped swapDat[:,5+9*j] = rawdat[:,6+9*j] swapDat[:,6+9*j] = rawdat[:,5+9*j] swapDat[:,7+9*j] = -rawdat[:,7+9*j] #Quaternion values swapped swapDat[:,9+9*j] = rawdat[:,10+9*j] swapDat[:,10+9*j] = rawdat[:,9+9*j] swapDat[:,12+9*j] = -rawdat[:,11+9*j] BPCorrect = swapDat for j in range(1,numSensors-1): BPCorrect[:,5+9*j:8+9*j] = q.qvqc(rot,(swapDat[:,5+9*j:8+9*j]-OS))-offset BPCorrect[:,8+9*j:12+9*j] = q.correctQuat(swapDat[:,8+9*j:12*9+j], rot) BPCorrect[:,3+9*j] = j return BPCorrect, header
def correctData(fileToCorrect, OS, rot, resamp, freq): offset = np.array([-4,-1, 0]) rawdat, header = BiteplateUtils.loadTsv(fileToCorrect) numSensors = int(np.floor((rawdat.shape[1]-3)/9)) swapDat = rawdat.copy() BPCorrect = swapDat for j in range(1,numSensors): BPCorrect[:,5+9*j:8+9*j] = q.qvqc(rot,(swapDat[:,5+9*j:8+9*j]-OS))-offset BPCorrect[:,8+9*j:12+9*j] = q.correctQuat(swapDat[:,8+9*j:12+9*j], rot) BPCorrect[:,3+9*j] = j if resamp: BPCorrect = BiteplateUtils.resample(BPCorrect, freq) return BPCorrect, header
def getRotation(bpfile,osCol, msCol): rawdat, header = BiteplateUtils.loadTsv(bpfile) os = rawdat[:,(osCol):(osCol+3)] ms = rawdat[:,(msCol):(msCol+3)] #We don't rearrange the columns anymore. #os = np.column_stack((os[:,1],os[:,0],-os[:,2])) #ms = np.column_stack((ms[:,1],ms[:,0],-ms[:,2])) MS = np.nanmean(ms,axis=0) OS = np.nanmean(os,axis=0) REF = np.array([0,0,0]) ref_t = REF-OS ms_t = MS-OS z = np.cross(ms_t,ref_t) z = z/np.linalg.norm(z) y = np.cross(z,ms_t) y = y/np.linalg.norm(z) l1 = np.dot(y,ref_t)/np.linalg.norm(y) l2 = np.linalg.norm(ms_t) ref_tnew = np.array([0, abs(l1), 0]) ms_tnew = np.array([-1*abs(l2), 0, 0]) ref_ty = y*abs(l1)/np.linalg.norm(y) x1 = ms_t x2 = ms_tnew y1 = ref_ty y2 = ref_tnew norm1 = np.cross(x1,x2) norm1 = norm1/np.linalg.norm(norm1) norm2 = np.cross(y1,y2) norm2 = norm2/np.linalg.norm(norm2) f1 = np.cross(norm1,(x1+x2)/2) f1 = f1/np.linalg.norm(f1) f2 = np.cross(norm2, (y1+y2)/2) f2 = f2/np.linalg.norm(f2) axis = np.cross(f1,f2) axis = axis/np.linalg.norm(axis) r = np.linalg.norm(x1)*np.sin(np.arccos((np.dot(x1,axis))/(np.linalg.norm(x1)*np.linalg.norm(axis)))) theta = np.arcsin((np.sqrt((x1[0]-x2[0])**2+(x1[1]-x2[1])**2+(x1[2]-x2[2])**2)/(2*r))) if(theta>(np.pi/2)): theta = np.pi-theta q0 = np.cos(theta); qx = np.sin(theta)*axis[0] qy = np.sin(theta)*axis[1] qz = np.sin(theta)*axis[2] qout = np.array([q0,qx,qy,qz]) #Check your work! ms_new2 = q.qcvq(qout,(MS-OS)) if((ms_new2[0,1]<0.000001) and (ms_new2[0,2]<0.000001)): qout = q.qconj(qout) return OS, qout