示例#1
0
文件: body.py 项目: abestick/uk
    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
示例#2
0
    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
示例#3
0
文件: body.py 项目: abestick/uk
    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
示例#4
0
    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