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