def stateToPX(self, x):
"""
y = stateToPX applies camera models to obtain pixel observations
INPUTS
x - 6 x 1 - rigid body state
OUTPUTS
y - 2 x Nf x Nc - pixel observations in each camera
"""
# Transform geometry to world coordinate system
p = cam.rigid(self.g, geom.euler(x[0:3,0]), x[3:6,:])
# Initialize pixel locations
y = np.kron( np.ones((2,self.Nf,self.Nc)), np.nan)
# Loop through cameras
for c in range(self.Nc):
# Apply DLT
if hasattr(self,'dlt'):
y[...,c] = cam.dlt(p, self.dlt[...,c])
# Apply Zhang camera model
else:
if isinstance(self.cams[c],dict):
R = self.cams[c]['R']
t = self.cams[c]['t']
A = self.cams[c]['A']
d = self.cams[c]['d']
else:
R = self.cams[c].R
t = self.cams[c].t
A = self.cams[c].A
d = self.cams[c].d
z = cam.zhang(p, R, t, A, d)
y[...,c] = np.dot(np.array([[0,1],[1,0]]),z)
return y
def stateToPX(self, x):
"""
y = stateToPX applies camera models to obtain pixel observations
INPUTS
x - 6 x 1 - rigid body state
OUTPUTS
y - 2 x Nf x Nc - pixel observations in each camera
"""
# Transform geometry to world coordinate system
p = cam.rigid(self.g, geom.euler(x[0:3, 0]), x[3:6, :])
# Initialize pixel locations
y = np.kron(np.ones((2, self.Nf, self.Nc)), np.nan)
# Loop through cameras
for c in range(self.Nc):
# Apply DLT
if hasattr(self, 'dlt'):
y[..., c] = cam.dlt(p, self.dlt[..., c])
# Apply Zhang camera model
else:
if isinstance(self.cams[c], dict):
R = self.cams[c]['R']
t = self.cams[c]['t']
A = self.cams[c]['A']
d = self.cams[c]['d']
else:
R = self.cams[c].R
t = self.cams[c].t
A = self.cams[c].A
d = self.cams[c].d
z = cam.zhang(p, R, t, A, d)
y[..., c] = np.dot(np.array([[0, 1], [1, 0]]), z)
return y
def rigid(p,r,r2R=geom.euler):
"""
q = rigid apply rigid transformation to points
INPUTS
p - 3 x Nf - 3D points to transform
r - 6 x N - rigid body state
(optional)
r2R - function - transforms rotation vec (3x1) to rotation mat (3x3)
e.g. geom.euler or cva.rodrigues
OUTPUTS
q - 3 x Nf x N - transformed 3D points
"""
Nf = p.shape[1]
N = r.shape[1]
q = []
for n in range(N):
R = r2R(r[0:3,n:n+1])
t = r[3:6,n:n+1]
q.append(cam.rigid(p,R,t))
return np.dstack(q)
def rigid(p, r, r2R=geom.euler):
"""
q = rigid apply rigid transformation to points
INPUTS
p - 3 x Nf - 3D points to transform
r - 6 x N - rigid body state
(optional)
r2R - function - transforms rotation vec (3x1) to rotation mat (3x3)
e.g. geom.euler or cva.rodrigues
OUTPUTS
q - 3 x Nf x N - transformed 3D points
"""
Nf = p.shape[1]
N = r.shape[1]
q = []
for n in range(N):
R = r2R(r[0:3, n:n + 1])
t = r[3:6, n:n + 1]
q.append(cam.rigid(p, R, t))
return np.dstack(q)
def obs(self, x, **d):
"""
z = obs transforms geometry features to world coordinates
INPUTS
x - 6 x N - N rigid body state hypotheses
OUTPUTS
z - Nd*Nf x N - geometry feature locations
"""
# Initialize observation
z = np.kron( np.ones((self.Nd*self.Nf,x.shape[1])), np.nan);
# Loop through state hypotheses
for k in range(x.shape[1]):
# Transform geometry to world coordinate system
y = cam.rigid(self.g, geom.euler(x[0:3,k]), x[3:6,k:k+1])
z[:,k] = y.flatten(1)
if hasattr(self,'viz0'):
self.viz0 = self.viz0 + 1
if hasattr(self,'viz') and self.viz and not(self.viz0 % self.viz):
N = z.shape[1]
if 'mocap' in d.keys():
z0 = d['mocap']
if not('ld' in self.plt['2d'].keys()):
self.plt['2d']['ld'] = [[] for p in range(self.Nf)]
if not('ld' in self.plt['3d'].keys()):
self.plt['3d']['ld'] = [[] for p in range(self.Nf)]
xlim = []; ylim = []; zlim = []
ax2 = self.plt['2d']['ax']
ax3 = self.plt['3d']['ax']
for p in range(self.Nf):
dx = np.vstack((z[self.Nd*p, 1:(N+1)/2],
z[self.Nd*p, np.zeros((N-1)/2,dtype=int)],
z[self.Nd*p, (N+1)/2:]))
dy = np.vstack((z[self.Nd*p+1, 1:(N+1)/2],
z[self.Nd*p+1, np.zeros((N-1)/2,dtype=int)],
z[self.Nd*p+1, (N+1)/2:]))
dz = np.vstack((z[self.Nd*p+2, 1:(N+1)/2],
z[self.Nd*p+2, np.zeros((N-1)/2,dtype=int)],
z[self.Nd*p+2, (N+1)/2:]))
xlim.append([dx.min(),dx.max()])
ylim.append([dy.min(),dy.max()])
zlim.append([dz.min(),dz.max()])
if not self.plt['2d']['ld'][p]:
self.plt['2d']['ld'][p] = ax2.plot(dx,dy,'.-',lw=2,ms=10)
ax2.legend(self.labels,ncol=6)
ax2.axis('equal')
ax2.set_xlabel('$x$ (mm)');
ax2.set_ylabel('$y$ (mm)');
for k,l in enumerate(self.plt['2d']['ld'][p]):
l.set_xdata(dx[:,k]); l.set_ydata(dy[:,k])
if not self.plt['3d']['ld'][p]:
self.plt['3d']['ld'][p] = [ax3.plot(dx[:,k],dy[:,k],dz[:,k],'.-',lw=2,ms=10)[0] for k in range(dx.shape[1])]
#ax3.axis('equal')
ax3.set_xlabel('$x$ (mm)');
ax3.set_ylabel('$y$ (mm)');
ax3.set_zlabel('$z$ (mm)');
ax3.view_init(elev=20.,azim=-130.)
for k,l in enumerate(self.plt['3d']['ld'][p]):
l.set_xdata(dx[:,k]); l.set_ydata(dy[:,k])
l.set_3d_properties(dz[:,k])
#ax3.auto_scale_xyz(dx[:,k],dz[:,k],dz[:,k])
if not('ly0' in self.plt['2d'].keys()):
self.plt['2d']['ly0'], = ax2.plot(z0[0,:],z0[1,:],'rx',lw=2,ms=10)
self.plt['2d']['ly0'].set_xdata(z0[0,:]);
self.plt['2d']['ly0'].set_ydata(z0[1,:])
if not('lt' in self.plt['2d'].keys()):
bbox = dict(facecolor='white')
self.plt['2d']['lt'] = [ax2.text(z0[0,k],z0[1,k],'%d'%k,bbox=bbox) for k in range(z0.shape[1])]
for k in range(z0.shape[1]):
if not( np.isnan(z0[0,k]) ):
self.plt['2d']['lt'][k].set_x(z0[0,k]+10)
self.plt['2d']['lt'][k].set_y(z0[1,k]+10)
if not('ly0' in self.plt['3d'].keys()):
self.plt['3d']['ly0'], = ax3.plot(z0[0,:],z0[1,:],z0[2,:],'rx',lw=2,ms=10)
self.plt['3d']['ly0'].set_xdata(z0[0,:]);
self.plt['3d']['ly0'].set_ydata(z0[1,:])
self.plt['3d']['ly0'].set_3d_properties(z0[2,:])
xlim = np.array(xlim); xlim = np.array([xlim[:,0].min(),xlim[:,1].max()])
xlim = (xlim[0]-0.1*np.diff(xlim),xlim[1].max()+0.1*np.diff(xlim))
ylim = np.array(ylim); ylim = np.array([ylim[:,0].min(),ylim[:,1].max()])
ylim = (ylim[0]-0.1*np.diff(ylim),ylim[1].max()+0.2*np.diff(ylim))
zlim = np.array(zlim); zlim = np.array([zlim[:,0].min(),zlim[:,1].max()])
zlim = (zlim[0]-0.1*np.diff(zlim),zlim[1].max()+0.1*np.diff(zlim))
ax2.set_xlim(xlim); ax2.set_ylim(ylim)
ax3.set_xlim(xlim); ax3.set_ylim(ylim); ax3.set_zlim(zlim)
#ax2.relim()
#ax2.autoscale_view(True,True,True)
#ax3.relim()
ax2.set_title('%d'%self.viz0)
self.fig.canvas.draw()
return z
def obs(self, x, **d):
"""
z = obs transforms geometry features to world coordinates
INPUTS
x - 6 x N - N rigid body state hypotheses
OUTPUTS
z - Nd*Nf x N - geometry feature locations
"""
# Initialize observation
z = np.kron(np.ones((self.Nd * self.Nf, x.shape[1])), np.nan)
# Loop through state hypotheses
for k in range(x.shape[1]):
# Transform geometry to world coordinate system
y = cam.rigid(self.g, geom.euler(x[0:3, k]), x[3:6, k:k + 1])
z[:, k] = y.flatten(1)
if hasattr(self, 'viz0'):
self.viz0 = self.viz0 + 1
if hasattr(self, 'viz') and self.viz and not (self.viz0 % self.viz):
N = z.shape[1]
if 'mocap' in d.keys():
z0 = d['mocap']
if not ('ld' in self.plt['2d'].keys()):
self.plt['2d']['ld'] = [[] for p in range(self.Nf)]
if not ('ld' in self.plt['3d'].keys()):
self.plt['3d']['ld'] = [[] for p in range(self.Nf)]
xlim = []
ylim = []
zlim = []
ax2 = self.plt['2d']['ax']
ax3 = self.plt['3d']['ax']
for p in range(self.Nf):
dx = np.vstack(
(z[self.Nd * p, 1:(N + 1) / 2],
z[self.Nd * p,
np.zeros((N - 1) / 2, dtype=int)], z[self.Nd * p,
(N + 1) / 2:]))
dy = np.vstack(
(z[self.Nd * p + 1, 1:(N + 1) / 2],
z[self.Nd * p + 1,
np.zeros((N - 1) / 2, dtype=int)], z[self.Nd * p + 1,
(N + 1) / 2:]))
dz = np.vstack(
(z[self.Nd * p + 2, 1:(N + 1) / 2],
z[self.Nd * p + 2,
np.zeros((N - 1) / 2, dtype=int)], z[self.Nd * p + 2,
(N + 1) / 2:]))
xlim.append([dx.min(), dx.max()])
ylim.append([dy.min(), dy.max()])
zlim.append([dz.min(), dz.max()])
if not self.plt['2d']['ld'][p]:
self.plt['2d']['ld'][p] = ax2.plot(dx,
dy,
'.-',
lw=2,
ms=10)
ax2.legend(self.labels, ncol=6)
ax2.axis('equal')
ax2.set_xlabel('$x$ (mm)')
ax2.set_ylabel('$y$ (mm)')
for k, l in enumerate(self.plt['2d']['ld'][p]):
l.set_xdata(dx[:, k])
l.set_ydata(dy[:, k])
if not self.plt['3d']['ld'][p]:
self.plt['3d']['ld'][p] = [
ax3.plot(dx[:, k],
dy[:, k],
dz[:, k],
'.-',
lw=2,
ms=10)[0] for k in range(dx.shape[1])
]
#ax3.axis('equal')
ax3.set_xlabel('$x$ (mm)')
ax3.set_ylabel('$y$ (mm)')
ax3.set_zlabel('$z$ (mm)')
ax3.view_init(elev=20., azim=-130.)
for k, l in enumerate(self.plt['3d']['ld'][p]):
l.set_xdata(dx[:, k])
l.set_ydata(dy[:, k])
l.set_3d_properties(dz[:, k])
#ax3.auto_scale_xyz(dx[:,k],dz[:,k],dz[:,k])
if not ('ly0' in self.plt['2d'].keys()):
self.plt['2d']['ly0'], = ax2.plot(z0[0, :],
z0[1, :],
'rx',
lw=2,
ms=10)
self.plt['2d']['ly0'].set_xdata(z0[0, :])
self.plt['2d']['ly0'].set_ydata(z0[1, :])
if not ('lt' in self.plt['2d'].keys()):
bbox = dict(facecolor='white')
self.plt['2d']['lt'] = [
ax2.text(z0[0, k], z0[1, k], '%d' % k, bbox=bbox)
for k in range(z0.shape[1])
]
for k in range(z0.shape[1]):
if not (np.isnan(z0[0, k])):
self.plt['2d']['lt'][k].set_x(z0[0, k] + 10)
self.plt['2d']['lt'][k].set_y(z0[1, k] + 10)
if not ('ly0' in self.plt['3d'].keys()):
self.plt['3d']['ly0'], = ax3.plot(z0[0, :],
z0[1, :],
z0[2, :],
'rx',
lw=2,
ms=10)
self.plt['3d']['ly0'].set_xdata(z0[0, :])
self.plt['3d']['ly0'].set_ydata(z0[1, :])
self.plt['3d']['ly0'].set_3d_properties(z0[2, :])
xlim = np.array(xlim)
xlim = np.array([xlim[:, 0].min(), xlim[:, 1].max()])
xlim = (xlim[0] - 0.1 * np.diff(xlim),
xlim[1].max() + 0.1 * np.diff(xlim))
ylim = np.array(ylim)
ylim = np.array([ylim[:, 0].min(), ylim[:, 1].max()])
ylim = (ylim[0] - 0.1 * np.diff(ylim),
ylim[1].max() + 0.2 * np.diff(ylim))
zlim = np.array(zlim)
zlim = np.array([zlim[:, 0].min(), zlim[:, 1].max()])
zlim = (zlim[0] - 0.1 * np.diff(zlim),
zlim[1].max() + 0.1 * np.diff(zlim))
ax2.set_xlim(xlim)
ax2.set_ylim(ylim)
ax3.set_xlim(xlim)
ax3.set_ylim(ylim)
ax3.set_zlim(zlim)
#ax2.relim()
#ax2.autoscale_view(True,True,True)
#ax3.relim()
ax2.set_title('%d' % self.viz0)
self.fig.canvas.draw()
return z
