In [ ]:
%pylab notebook
import numpy as np
import cv2
In [ ]:
import matplotlib.pyplot as plt
def norm(X):
return X/np.sqrt((X**2).sum(axis=0))
In [ ]:
!ls data/manuvers_raw/
In [ ]:
import pickle
def read_data(fname):
fd=open(fname,'rb')
data=[]
while 1:
try:
data.append(pickle.load(fd))
except EOFError:
break
return data
In [ ]:
if 0:
data_calib=read_data('data/manuvers_raw/movcalib2.pkl')
else:
import glob
data_calib=[]
for fname in glob.glob('data/manuvers_raw/*.pkl'):
data_calib += read_data(fname)
data_calib_acc=read_data('data/manuvers_raw/movcalib_acc.pkl')
#data_test=read_data('data/manuvers_raw/mov_test_5.pkl')
#data_test=read_data('data/manuvers_raw/mov_test_19c.pkl')
#data_test=read_data('data/manuvers_raw/mov_test_14.pkl')
#data_test=read_data('data/manuvers_raw/mov_test_17.pkl')
data_test=read_data('data/manuvers_raw/mov_test_18.pkl')
#data_test=read_data('data/manuvers_raw/mov_test_19.pkl')
#data_test=read_data('data/manuvers_raw/movcalib1_9090.pkl')
#data_test=read_data('data/manuvers_raw/movcalib_acc.pkl')
In [ ]:
data_test[600:603]
In [ ]:
mag_calib=np.array([a['mag'] for a in data_calib if 'mag' in a])
mag_test=np.array([a['mag'] for a in data_test if 'mag' in a])
acc_test=np.array([a['a/g'][:3] for a in data_test if 'a/g' in a])
acc_calib=np.array([a['a/g'] for a in data_calib_acc if 'a/g' in a])
mag_t=np.array([a['s_sync'] for a in data_test if 'mag' in a])
In [ ]:
plt.figure()
plt.subplot(3,1,1)
plt.title('calib samples mag')
plt.plot(mag_calib[:,0],'r')
plt.plot(mag_calib[:,1],'g')
plt.plot(mag_calib[:,2],'b')
plt.subplot(3,1,2)
plt.title('90 90 test samples mag')
plt.plot(mag_test[:,0],'r')
plt.plot(mag_test[:,1],'g')
plt.plot(mag_test[:,2],'b')
plt.subplot(3,1,3)
plt.title('acc calib samples')
acc_calib_filt=np.vstack([np.convolve(np.ones(10)/10.0,acc_calib[:,i]) for i in range(3)]).T
plt.plot(acc_calib_filt[:,0],'r')
plt.plot(acc_calib_filt[:,1],'g')
plt.plot(acc_calib_filt[:,2],'b')
In [ ]:
max_calib=mag_calib.max(axis=0)
#max_calib[2]=400
min_calib=mag_calib.min(axis=0)
mag_calib_scaled=mag_calib-min_calib
mag_calib_scaled=2.0*(mag_calib_scaled/(max_calib-min_calib)-0.5)
plt.figure()
plt.plot(mag_calib_scaled[:,1],mag_calib_scaled[:,2],'.g',alpha=0.9,markersize=1)
plt.plot(mag_calib_scaled[:,0],mag_calib_scaled[:,2],'.b',alpha=0.9,markersize=1)
plt.plot(mag_calib_scaled[:,0],mag_calib_scaled[:,1],'.r',alpha=0.9,markersize=1)
#plt.legend(['xy','yz','xz'])
#max_calib=np.array([776,570,410]) #[ 769. 579. 408.] [-617. -669. -619.]
#min_calib=np.array([-618,-670,-613])
plt.axis('equal')
plt.grid('on')
In [ ]:
print(max_calib,min_calib) #[ 769. 579. 408.] [-617. -669. -619.]
max_calib_acc=acc_calib_filt.max(axis=0)
min_calib_acc=acc_calib_filt.min(axis=0)
print(max_calib_acc,min_calib_acc)
mag_test_calib=mag_test-min_calib
mag_test_calib=2.0*(mag_test_calib/(max_calib-min_calib)-0.5)
acc_test_calib=acc_test-min_calib_acc
acc_test_calib=2.0*(acc_test_calib/(max_calib_acc-min_calib_acc)-0.5)
def smood(X,sz=10):
return np.convolve(X,ones(sz)/sz,'same')
for i in range(3):
acc_test_calib[:,i]=smood(acc_test_calib[:,i])
for i in range(3):
mag_test_calib[:,i]=smood(mag_test_calib[:,i])
plt.figure()
ax=plt.subplot(3,1,1)
plt.title('mag samples scaled')
plt.plot(mag_test_calib[:,0],'r')
plt.plot(mag_test_calib[:,1],'g')
plt.plot(mag_test_calib[:,2],'b')
plt.subplot(3,1,2,sharex=ax)
plt.title('acc samples scaled')
plt.plot(acc_test_calib[:,0],'r')
plt.plot(acc_test_calib[:,1],'g')
plt.plot(acc_test_calib[:,2],'b')
plt.subplot(3,1,3,sharex=ax)
plt.title('angle between vectors in deg')
mag_test_norm=norm(mag_test_calib.T)
acc_norm=norm(acc_test_calib.T)
plt.plot(np.degrees(np.arccos(np.sum(acc_norm*mag_test_norm,axis=0))))
#[ 769. 579. 408.] [-617. -669. -619.]
#(mag_test_norm**2).sum(axis=0)
In [ ]:
plt.figure()
plt.subplot(2,1,1)
plt.title('heading')
#ax,ay = norm(mag_test_calib)[:,[1,2]].T
ax,ay = mag_test_calib[:,[0,1]].T
plt.plot(np.unwrap(np.arctan2(ay,ax))/np.pi*180,'-+')
plt.subplot(2,1,2)
plt.title('magnetic field strength')
plt.plot((mag_test_calib**2).sum(axis=1),'-+')
In [ ]:
In [ ]:
#print(acc_test_norm[:,0])
accXmax_norm=norm(np.cross(acc_norm.T,mag_test_norm.T).T).T
accX_accXmax_norm=norm(np.cross(acc_norm.T,accXmax_norm).T).T
print(np.abs((acc_norm.T*accXmax_norm).sum(axis=1)).max()) #test perpendicular
print(np.abs((acc_norm.T*accX_accXmax_norm).sum(axis=1)).max()) #test perpendicular
print(np.abs((accXmax_norm*accX_accXmax_norm).sum(axis=1)).max()) #test perpendicular
In [ ]:
#testing chkpoints for 180 deg diffrence
rot_mat_array=np.hstack((acc_norm.T,accXmax_norm,accX_accXmax_norm)).T.reshape((3,3,-1))
r1=rot_mat_array.T[173]
r2=rot_mat_array.T[212]
rod=cv2.Rodrigues(np.dot(r1,r2.T))[0]
print('Rodrigues =\n',rod)
print('Rodrigues angle =\n',np.sqrt((rod**2).sum())/np.pi*180)
r1=rot_mat_array.T[450]
r2=rot_mat_array.T[500]
rod=cv2.Rodrigues(np.dot(r1,r2.T))[0]
print('Rodrigues =\n',rod)
print('Rodrigues angle =\n',np.sqrt((rod**2).sum())/np.pi*180)
In [ ]:
r1=rot_mat_array.T[210]
r2=rot_mat_array.T[250]
rod=cv2.Rodrigues(np.dot(r1,r2.T))[0]
print('Rodrigues =\n',rod)
print('Rodrigues angle =\n',np.sqrt((rod**2).sum())/np.pi*180)
In [ ]:
#import cv2
#axis_angle=np.hstack([cv2.Rodrigues(np.dot(rot_mat_array.T[130],R.T))[0] for R in rot_mat_array.T])
#angle_rot=np.sqrt((axis_angle.T**2).sum(axis=1))/np.pi*180
In [ ]:
#plt.figure()
#plot(angle_rot)
In [ ]:
import utils
In [ ]:
len(rot_mat_array.T)
In [ ]:
r0=rot_mat_array.T[210]
eu_angs=np.vstack([utils.rotationMatrixToEulerAngles(np.dot(rot_mat_array.T[i],r0.T)) for i in range(len(rot_mat_array.T))])
eu_angs=np.unwrap(eu_angs,axis=0)/np.pi*180
In [ ]:
plt.figure()
plt.plot(eu_angs[:,0])
plt.plot(eu_angs[:,1])
plt.plot(eu_angs[:,2])
plt.legend(['y','p','r'])
In [ ]:
In [ ]: