python类Rodrigues()的实例源码

def get_object_pose(self, marker, tag):
        # AR Tag Dimensions
        objPoints = np.zeros((4, 3), dtype=np.float64)
        objPoints[0,0] = -1.0*tag[1]/2.0
        objPoints[0,1] = tag[1]/2.0
        objPoints[0,2] = 0.0
        objPoints[1,0] = tag[1]/2.0
        objPoints[1,1] = tag[1]/2.0
        objPoints[1,2] = 0.0
        objPoints[2,0] = tag[1]/2.0
        objPoints[2,1] = -1*tag[1]/2.0
        objPoints[2,2] = 0.0
        objPoints[3,0] = -1*tag[1]/2.0
        objPoints[3,1] = -1*tag[1]/2.0
        objPoints[3,2] = 0.0

        # Get each corner of the tags
        imgPoints = np.zeros((4, 2), dtype=np.float64)
        for i in range(4):
            imgPoints[i, :] = marker.contours[i, 0, :]


        camPos = np.zeros((3, 1))
        camRot = np.zeros((3, 1))

        # SolvePnP
        retVal, rvec, tvec = cv2.solvePnP(objPoints, imgPoints, self.camMatrix, self.distCoeff)
        Rca, b = cv2.Rodrigues(rvec)
        Pca = tvec

        return [Pca, Rca]

def get_pupilpos(self,x_pixels=None,y_pixels=None,field=None,Coords='Display'):

        if x_pixels is not None or y_pixels is not None:
            if x_pixels is None or y_pixels is None:
                raise ValueError("X pixels and Y pixels must both be specified!")
            if np.shape(x_pixels) != np.shape(y_pixels):
                raise ValueError("X pixels array and Y pixels array must be the same size!")

            # Flatten everything and create output array        
            oldshape = np.shape(x_pixels)

            x_pixels = np.reshape(x_pixels,np.size(x_pixels),order='F')
            y_pixels = np.reshape(y_pixels,np.size(y_pixels),order='F')
            out = np.zeros(np.shape(x_pixels) + (3,))

            # Convert pixel coords if we need to
            if Coords.lower() == 'original':
                x_pixels,y_pixels = self.transform.original_to_display_coords(x_pixels,y_pixels)

            # Identify which sub-field each pixel is in
            pointfield = self.fieldmask[y_pixels.round().astype(int),x_pixels.round().astype(int)]
            if np.size(pointfield) == 1:
                pointfield = [pointfield]

            for i in range(np.size(x_pixels)):
                out[i,:] = self.get_pupilpos(field=pointfield[i])

            return np.reshape(out,oldshape + (3,),order='F')

        else:

            if field is None:
                if self.nfields == 1:
                    field = 0
                else:
                    raise Exception('This calibration has multiple sub-fields; you must specify a pixel location to get_pupilpos!')


            rotation_matrix = np.matrix(cv2.Rodrigues(self.fit_params[field].rvec)[0])
            CamPos = np.matrix(self.fit_params[field].tvec)
            CamPos = - (rotation_matrix.transpose() * CamPos)
            CamPos = np.array(CamPos)

            return np.array([CamPos[0][0],CamPos[1][0],CamPos[2][0]])


    # Get X and Y field of view of the camera (X and Y being horizontal and vertical of the detector)
    # Optional inputs: field - for cameras with split fields-of-view, the sub-field number to get the FOV of (int).
    #                  FullChipWithoutDistortion - ignores distortion and any split field-of-view, just returns the FOV
    #                                              for the full chip as if there was no distortion. Used in Calcam.Render() but probably not useful otherwise (bool).
    # Output: 2-element tuple with field of view in degrees: (horiz, vert) (tuple of floats)

def set_extrinsics(self,campos,upvec,camtar=None,view_dir=None,opt_axis = False):


        if camtar is not None:
            w = np.squeeze(np.array(camtar) - np.array(campos))
        elif view_dir is not None:
            w = view_dir
        else:
            raise ValueError('Either viewing target or view direction must be specified!')

        w = w / np.sqrt(np.sum(w**2))
        v = upvec / np.sqrt(np.sum(upvec**2))


        # opt_axis specifies whether the input campos or camtar are where we should
        # point the optical axis or the view centre. By defauly we assume the view centre.
        # In this case, we need another rotation matrix to rotate from the image centre
        # view direction (given) to the optical axis direction (stored). This applies for
        # intrinsics where the perspective centre is not at the the detector centre.
        if not opt_axis:
            R = np.zeros([3,3])
            # Optical axis direction in the camera coordinate system
            uz = np.array(self.normalise(self.image_display_shape[0]/2.,self.image_display_shape[1]/2.,0) + (1.,))
            uz = uz / np.sqrt(np.sum(uz**2))

            ux = np.array([1,0,0]) - uz[0]*uz
            ux = ux / np.sqrt(np.sum(ux**2))
            uy = np.cross(ux,uz)
            R[:,0] = ux
            R[:,1] = -uy
            R[:,2] = uz

            R = np.matrix(R).T

        else:
        # If we are pointing the optical axis, set this extra
        # rotation to be the identity.
            R = np.matrix([[1,0,0],[0,1,0],[0,0,1]])


        u = np.cross(w,v)

        Rmatrix = np.zeros([3,3])
        Rmatrix[:,0] = u
        Rmatrix[:,1] = -v
        Rmatrix[:,2] = w
        Rmatrix = np.matrix(Rmatrix)


        Rmatrix = Rmatrix * R
        campos = np.matrix(campos)
        if campos.shape[0] < campos.shape[1]:
            campos = campos.T

        self.fit_params[0].tvec = -Rmatrix.T * campos
        self.fit_params[0].rvec = -cv2.Rodrigues(Rmatrix)[0]