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_optitrack/

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_optitrack/test7.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 [ ]:
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])
alt_test=np.array([a['alt'] for a in data_test if 'alt' in a])

In [ ]:
data_test[0:6]

In [ ]:
#data_test[0:6]
figure()
subplot(1,2,1)
odata_alt=[]
for data in data_test:
    if 'o_sync' in data:
        odata_alt.append((data['o_sync'],*data['odata'].position))
odata_alt=np.array(odata_alt)
plot(mag_t,alt_test-alt_test[0],'-r')
plot(odata_alt[:,0],odata_alt[:,2],'-b')
plot(mag_t,acc_test[:,2]/2e4,'-g')
subplot(1,2,2)
plot(odata_alt[:,1],odata_alt[:,3],'-b')

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 [ ]: