Пример #1
0
  def plane(self,fmts=['png'],out1=50.,out2=15.,**kwds):
    """
    Fit ground plane to data

    Inputs
      fmts - [str,...] - list of figure formats to export
      out1,out2 - float - magic numbers for outlier rejection

    Usage 
      >> d.shape # raw data, N samples of M markers with 3 coordinates (x,y,z)
      (N, M, 3)
      >> c = np.dot(d, R.T) - t # rectified data

    Effects
      - generates fi+'_cal.py' file containing dict of R,t,n
    """
    # unpack data
    t = self.t; d = self.d; g = self.g; hz=self.hz
    di,fi = os.path.split(self.fi)
    nn = np.logical_not( np.any( np.isnan(d[:,:,0]), axis=1) ).nonzero()[0]
    assert nn.size > 0
    N,M,_ = d.shape

    # swap axes in mocap hardware-dependent way
    if self.dev == 'opti':
      R0 = np.array([[0,0,1],[1,0,0],[0,1,0]])
    elif self.dev == 'vicon':
      R0 = np.identity(3)
    # R0 in SO(3)
    assert ( ( np.all(np.dot(R0,R0.T) == np.identity(3)) ) 
            and ( np.linalg.det(R0) == 1.0 ) ) 

    d = np.dot(d, R0.T)

    # collect non-nan data
    x = d[...,0]; y = d[...,1]; z = d[...,2]
    nn = np.logical_not(np.isnan(x.flatten())).nonzero()
    p = np.vstack((x.flatten()[nn],
                   y.flatten()[nn],
                   z.flatten()[nn])).T
    m = p.mean(axis=0)
    p -= m 
    # remove outliers
    p = p[np.abs(p[:,2]) < out1,:]
    # fit plane to data (n is normal vec)
    n = geom.plane(p)
    # rotate normal vertical
    R = geom.orient(n)
    p = np.dot(p,R.T)
    # save plane data
    s = util.Struct(R=np.dot(R,R0),t=np.dot(m,R.T),n=n)
    s.write( os.path.join(di,ddir,fi+'_cal.py') )
Пример #2
0
  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)