def stateToPX(self, x):
"""
y = stateToPX applies camera models to obtain pixel observations
INPUTS
x - 3*Np x 1 - 3D positions of points
OUTPUTS
y - 2 x Np x Nc - 2D camera observations of points
"""
# Extract 3D positions of points
p = x[0:3*self.Np,:].reshape((self.Np,3)).T
# Initialize pixel locations
y = np.kron( np.ones((2,self.Np,self.Nc)), np.nan)
for c,ca in enumerate(self.cams):
if isinstance(ca,dict):
R = ca['R']
t = ca['t']
A = ca['A']
d = ca['d']
else:
R = ca.R
t = ca.t
A = ca.A
d = ca.d
z = cam.zhang(p, R, t, A, d)
#y[...,c] = (np.dot(np.array([[0,-1],[-1,0]]),z)
# + np.kron(np.ones((1,z.shape[1])),A[[1,0],2:3]))
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 - 3*Np x 1 - 3D positions of points
OUTPUTS
y - 2 x Np x Nc - 2D camera observations of points
"""
# Extract 3D positions of points
p = x[0:3 * self.Np, :].reshape((self.Np, 3)).T
# Initialize pixel locations
y = np.kron(np.ones((2, self.Np, self.Nc)), np.nan)
for c, ca in enumerate(self.cams):
if isinstance(ca, dict):
R = ca['R']
t = ca['t']
A = ca['A']
d = ca['d']
else:
R = ca.R
t = ca.t
A = ca.A
d = ca.d
z = cam.zhang(p, R, t, A, d)
#y[...,c] = (np.dot(np.array([[0,-1],[-1,0]]),z)
# + np.kron(np.ones((1,z.shape[1])),A[[1,0],2:3]))
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 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
p = np.concatenate([ukf.stateToPX(xx.reshape((3,1))).reshape((2,ukf.Np,ukf.Nc,1)) for xx in x.T],axis=3)
# Add noise to pixel observations
sig = 20.
e = sig*np.random.randn(*p.shape)
pe = p+e
z = uk.mocapCam(ukf, pe)
plt.figure(1)
plt.clf()
col = ['b','g','r']
dy = dx*np.sqrt(N)
for s,c in zip([0,1,2],col):
plt.subplot(3,1,s+1)
plt.plot(x[s,:],c+'-',lw=2.)
plt.plot(z[s,:],c+'.',lw=2.)
plt.figure(2)
plt.clf()
col = ['b','g']
M = len(cams)
for m,ca in enumerate(cams):
qx = cam.zhang(x, ca['R'], ca['t'], ca['A'], ca['d'])
qz = cam.zhang(z, ca['R'], ca['t'], ca['A'], ca['d'])
for s,c in zip([0,1],col):
plt.subplot(M,2,m*M+s+1)
plt.plot(qx[s,:],c+'-',lw=2.)
plt.plot(qz[s,:],c+'.',lw=2.)
],
axis=3)
# Add noise to pixel observations
sig = 20.
e = sig * np.random.randn(*p.shape)
pe = p + e
z = uk.mocapCam(ukf, pe)
plt.figure(1)
plt.clf()
col = ['b', 'g', 'r']
dy = dx * np.sqrt(N)
for s, c in zip([0, 1, 2], col):
plt.subplot(3, 1, s + 1)
plt.plot(x[s, :], c + '-', lw=2.)
plt.plot(z[s, :], c + '.', lw=2.)
plt.figure(2)
plt.clf()
col = ['b', 'g']
M = len(cams)
for m, ca in enumerate(cams):
qx = cam.zhang(x, ca['R'], ca['t'], ca['A'], ca['d'])
qz = cam.zhang(z, ca['R'], ca['t'], ca['A'], ca['d'])
for s, c in zip([0, 1], col):
plt.subplot(M, 2, m * M + s + 1)
plt.plot(qx[s, :], c + '-', lw=2.)
plt.plot(qz[s, :], c + '.', lw=2.)
