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') )
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)