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
