Exemplo n.º 1
0
    def make_cam_from_params(self, params):
        if 1:
            t = params[:3]
            rod = params[3:]
            rmat = rodrigues2matrix( rod )
            d = self.intrinsic_dict.copy()
            d['translation'] = t
            d['Q'] = rmat
            cam_model = CameraModel.from_dict(d)
            return cam_model

        C = params[:3]
        quat = params[3:]
        qmag = np.sqrt(np.sum(quat**2))
        quat = quat/qmag

        R,rquat=get_rotation_matrix_and_quaternion(quat)

        t = -np.dot(R, C)

        d = self.intrinsic_dict.copy()
        d['translation'] = t
        d['Q'] = R
        cam_model = CameraModel.from_dict(d)
        return cam_model
Exemplo n.º 2
0
    def make_cam_from_params(self, params):
        if 1:
            t = params[:3]
            rod = params[3:]
            rmat = rodrigues2matrix(rod)
            d = self.intrinsic_dict.copy()
            d['translation'] = t
            d['Q'] = rmat
            cam_model = CameraModel.from_dict(d)
            return cam_model

        C = params[:3]
        quat = params[3:]
        qmag = np.sqrt(np.sum(quat**2))
        quat = quat / qmag

        R, rquat = get_rotation_matrix_and_quaternion(quat)

        t = -np.dot(R, C)

        d = self.intrinsic_dict.copy()
        d['translation'] = t
        d['Q'] = R
        cam_model = CameraModel.from_dict(d)
        return cam_model
Exemplo n.º 3
0
    def generate_camera(self):
        (width,height)=(self.width,self.height)=(640,480)
        center = 1,2,3
        rot_axis = np.array((4,5,6.7))
        rot_axis = rot_axis / np.sum(rot_axis**2)
        rquat = tf.transformations.quaternion_about_axis(0.1, (rot_axis.tolist()))
        rmat,_ = get_rotation_matrix_and_quaternion(rquat)

        parts = make_M( 1234.56, width, height,
                        rmat, center)

        if self.use_distortion:
            dist = [-0.4, .2, 0, 0, 0]
        else:
            dist = [0, 0, 0, 0, 0]

        self.cam = CameraModel.load_camera_from_M(parts['M'],
                                                  width=width,height=height,
                                                  distortion_coefficients=dist)
Exemplo n.º 4
0
    def generate_images(self):
        """make checkerboard images in camera view"""
        max_theta = 100.0*D2R
        axis = (0,1,0)
        self.check_w = 8
        self.check_h = 6
        checkerboard_w = self.check_w+2
        checkerboard_h = self.check_h+2
        self.check_size = 0.024

        base_cc_x=(np.arange(checkerboard_w)-checkerboard_w/2.0)*self.check_size
        base_cc_y=(np.arange(checkerboard_h)-checkerboard_h/2.0)*self.check_size
        base_cc = []
        save_idx = []
        for i,y in enumerate(base_cc_y):
            for j,x in enumerate(base_cc_x):
                if (i>0) and (i<checkerboard_h-1):
                    if (j>0) and (j<checkerboard_w-1):
                        # save indices of actual checkerboard corners
                        save_idx.append(len(base_cc))
                base_cc.append( (x,y,0) )
        save_idx = np.array(save_idx)

        base_cc = np.array(base_cc).T
        self.db = []

        center_pix = (self.cam.width/2.0, self.cam.height/2.0)
        n_images = 20
        for i in range(n_images):
            dist = 0.9 + 0.1*(i%3)
            theta = i/float(n_images-1)*max_theta - max_theta*0.5

            rquat = tf.transformations.quaternion_about_axis(theta, axis)
            rmat,_ = get_rotation_matrix_and_quaternion(rquat)

            this_cc = np.dot(rmat,base_cc)

            first_pixel = np.array( center_pix, copy=True )
            atmp = i*np.pi/2.
            dir_offset = np.array((np.cos(atmp), np.sin(atmp)))
            offset = dir_offset*40.0
            first_pixel += offset
            first_pixel.shape = (1,2)
            first_3d = self.cam.project_pixel_to_3d_ray(first_pixel,
                                                        distorted=True,
                                                        distance=dist )
            check_3d = this_cc.T + first_3d

            check_pixels = self.cam.project_3d_to_pixel(check_3d,distorted=True)
            im = draw_checkerboard(check_pixels,checkerboard_w,checkerboard_h,
                                   self.cam.width,self.cam.height)
            imsave = np.empty( (self.cam.height, self.cam.width, 3),
                               dtype=np.uint8)
            for chan in range(3):
                imsave[:,:,chan] = (im*255).astype(np.uint8)

            wcs3d = check_3d[save_idx] # world coords
            ccs3d = np.dot( self.cam.get_rotation(), wcs3d.T ).T + self.cam.translation
            ccs2d = check_pixels[save_idx] # pixel coords
            if DRAW:
                import scipy.misc
                scipy.misc.imsave( 'im%03d.png'%i, imsave )
            self.db.append( {'wc':wcs3d, 'cc':ccs3d, 'pix':ccs2d, 'im':imsave })