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)