def ukf(self,ord=2,N=np.inf,Ninit=20,viz=0,**kwds):
"""
Use UKF to track previously-loaded trajectory
Inputs:
(optional)
ord - int - order of state derivative to track
N - int - max number of samples to track
Ninit - int - # of init iterations for ukf
viz - int - # of samps to skip between vizualization; 0 to disable
plts - [str,...] - list of plots to generate
Outputs:
X - N x 6 - rigid body state estimate at each sample
Effects:
- assigns self.X
- saves X to fi+'_ukf.py' and fi+'_ukf.npz'
"""
# unpack data
t = self.t; d = self.d; g = self.g; fi = self.fi; hz=self.hz
nn = np.logical_not( np.any( np.isnan(d[:,:,0]), axis=1) ).nonzero()[0]
assert nn.size > 0
n = 0
if nn[0] > 0:
n = nn[0]
print self.trk+' not visible until sample #%d; trimming data' % n
t = t[n:]; d = d[n:,:,:]
di,fi = os.path.split(fi)
N0,_,_ = d.shape; N = min(N,N0)
# init ukf
from uk import uk, body, pts
X0 = np.hstack( ( np.zeros(2), 2*np.random.rand(), # pitch,roll,yaw
num.nanmean(d[:100,:,:],axis=0).mean(axis=0) ) ) # xyz
Qd = ( np.hstack( (np.array([1,1,1])*2e-3, np.array([1,1,1])*5e+0) ) )
for o in range(ord-1):
X0 = np.hstack( ( X0, np.zeros(6) ) )
Qd = np.hstack( ( Qd, Qd[-6:]*1e-1) )
b = body.Mocap( X0, g.T, viz=viz, Qd=Qd );
b.Ninit = Ninit;
self.b = b
print 'running ukf on %d samps' % N; ti = time()
t = t[:N]
j = dict(pitch=0,roll=1,yaw=2,x=3,y=4,z=5)
X = uk.mocap( b, np.swapaxes(d[:N,:,:],1,2) ).T
N,M = X.shape
X = np.vstack(( np.nan*np.zeros((n,M)), X ))
X[:,0:3] *= rad2deg
u = dict(pitch='deg',roll='deg',yaw='deg',x='mm',y='mm',z='mm')
self.j = j; self.u = u; self.X = X
print '%0.1f sec' % (time() - ti)
s = util.Struct(X0=X0,Qd=Qd,ord=ord,b=b,j=j,u=u)
dir = os.path.join(di,ddir)
if not os.path.exists( dir ):
os.mkdir( dir )
s.write( os.path.join(dir,fi+'_ukf.py') )
np.savez(os.path.join(dir,fi+'_ukf.npz'),X=X,hz=hz)
return X
def geom(self,**kwds):
"""
Fit rigid body geometry
Effects:
- assigns self.g
- saves g to fi+'_geom.npz'
"""
# unpack data
di,fi = os.path.split(self.fi)
d = self.d
N,M,D = d.shape
# samples where all features appear
nn = np.logical_not( np.any(np.isnan(d[:,:,0]),axis=1) ).nonzero()[0]
#assert nn.size > 0
# fit geometry to pairwise distance data
pd0 = []; ij0 = []
for i,j in zip(*[list(a) for a in np.triu(np.ones((M,M)),1).nonzero()]):
ij0.append([i,j])
pd0.append(np.sqrt(np.sum((d[:,i,:] - d[:,j,:])**2,axis=1)))
pd0 = np.array(pd0).T;
d0 = num.nanmean(pd0,axis=0); ij0 = np.array(ij0)
self.pd0 = pd0; self.d0 = d0
g0 = d[nn[0],:,:]
# TODO: fix geometry fitting
if 1:
g = g0.copy()
else:
print 'fitting geom'; ti = time()
g,info,flag = geom.fit( g0, ij0, d0 )
print '%0.1f sec' % (time() - ti)
pd = []; pd0 = []
for i,j in zip(*[list(a) for a in np.triu(np.ones((M,M)),1).nonzero()]):
pd.append( np.sqrt( np.sum((g[i,:] - g[j,:])**2) ) )
pd0.append( np.sqrt( np.sum((g0[i,:] - g0[j,:])**2) ) )
pd = np.array(pd).T;
pd0 = np.array(pd0).T;
# center and rotate geom flat
m = np.mean(g,axis=0)
g = g - m
n = geom.plane(g)
R = geom.orient(n)
g = np.dot(g,R.T)
self.g = g
# save data
dir = os.path.join(di,ddir)
if not os.path.exists( dir ):
os.mkdir( dir )
np.savez(os.path.join(dir,fi+'_geom.npz'),g=g,pd0=pd0,d0=d0)
