Python cv2.Rodrigues方法代碼示例

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def pnp(points_3D, points_2D, cameraMatrix):
    try:
        distCoeffs = pnp.distCoeffs
    except:
        distCoeffs = np.zeros((8, 1), dtype='float32')

    assert points_2D.shape[0] == points_2D.shape[0], 'points 3D and points 2D must have same number of vertices'

    _, R_exp, t = cv2.solvePnP(points_3D,
                              # points_2D,
                              np.ascontiguousarray(points_2D[:,:2]).reshape((-1,1,2)),
                              cameraMatrix,
                              distCoeffs)
                              # , None, None, False, cv2.SOLVEPNP_UPNP)

    R, _ = cv2.Rodrigues(R_exp)
    return R, t 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def solve_head_pose(self, face_landmarks):
        indices = [17, 21, 22, 26, 36, 39, 42, 45, 31, 35]
        image_pts = np.zeros((len(indices), 2))
        for i in range(len(indices)):
            part = face_landmarks.part(indices[i])
            image_pts[i, 0] = part.x
            image_pts[i, 1] = part.y

        _, rotation_vec, translation_vec = cv2.solvePnP(self.face_model_points,
                                                        image_pts,
                                                        self.camera_matrix,
                                                        self.distortion_coeffs)
        projected_head_pose_box_points, _ = cv2.projectPoints(self.head_pose_box_points,
                                                              rotation_vec,
                                                              translation_vec,
                                                              self.camera_matrix,
                                                              self.distortion_coeffs)
        projected_head_pose_box_points = tuple(map(tuple, projected_head_pose_box_points.reshape(8, 2)))

        # Calculate euler angle
        rotation_mat, _ = cv2.Rodrigues(rotation_vec)
        pose_mat = cv2.hconcat((rotation_mat, translation_vec))
        _, _, _, _, _, _, euler_angles = cv2.decomposeProjectionMatrix(pose_mat)
        return projected_head_pose_box_points, euler_angles 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def fun(self, x, params):
        #skalowanie
        s = params[0]
        #rotacja
        r = params[1:4]
        #przesuniecie (translacja)
        t = params[4:6]
        w = params[6:]

        mean3DShape = x[0]
        blendshapes = x[1]

        #macierz rotacji z wektora rotacji, wzor Rodriguesa
        R = cv2.Rodrigues(r)[0]
        P = R[:2]
        shape3D = mean3DShape + np.sum(w[:, np.newaxis, np.newaxis] * blendshapes, axis=0)

        projected = s * np.dot(P, shape3D) + t[:, np.newaxis]

        return projected 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def rotmat2aa(rotmats):
    """
    Convert rotation matrices to angle-axis using opencv's Rodrigues formula.
    Args:
        rotmats: A np array of shape (..., 3, 3)

    Returns:
        A np array of shape (..., 3)
    """
    assert rotmats.shape[-1] == 3 and rotmats.shape[-2] == 3 and len(rotmats.shape) >= 3, 'invalid input dimension'
    orig_shape = rotmats.shape[:-2]
    rots = np.reshape(rotmats, [-1, 3, 3])
    aas = np.zeros([rots.shape[0], 3])
    for i in range(rots.shape[0]):
        aas[i] = np.squeeze(cv2.Rodrigues(rots[i])[0])
    return np.reshape(aas, orig_shape + (3,)) 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def rotmat2aa(rotmats):
    """
    Convert rotation matrices to angle-axis format.
    Args:
        oris: np array of shape (seq_length, n_joints*9).

    Returns: np array of shape (seq_length, n_joints*3)
    """
    seq_length = rotmats.shape[0]
    assert rotmats.shape[1] % 9 == 0
    n_joints = rotmats.shape[1] // 9
    ori = np.reshape(rotmats, [seq_length*n_joints, 3, 3])
    aas = np.zeros([seq_length*n_joints, 3])
    for i in range(ori.shape[0]):
        aas[i] = np.squeeze(cv2.Rodrigues(ori[i])[0])
    return np.reshape(aas, [seq_length, n_joints*3]) 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def getShape3D(mean3DShape, blendshapes, params):
    #skalowanie
    s = params[0]
    #rotacja
    r = params[1:4]
    #przesuniecie (translacja)
    t = params[4:6]
    w = params[6:]

    #macierz rotacji z wektora rotacji, wzor Rodriguesa
    R = cv2.Rodrigues(r)[0]
    shape3D = mean3DShape + np.sum(w[:, np.newaxis, np.newaxis] * blendshapes, axis=0)

    shape3D = s * np.dot(R, shape3D)
    shape3D[:2, :] = shape3D[:2, :] + t[:, np.newaxis]

    return shape3D 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def gen_RT_matrix(path):
    with open(path, 'r') as f:
        lines = f.readlines()
        lines = lines[0].split(' ')
        R_vec = np.array([float(lines[3]),float(lines[5]), float(lines[4])]).reshape(3, 1)
        T_vec = np.array([float(lines[0]),float(lines[1]), float(lines[2])]).reshape(3, 1)
        R_vec = np.array(R_vec).reshape(3,1)
        T_vec = np.array(T_vec).reshape(3,1)
        R_mat = cv2.Rodrigues(R_vec)[0].reshape(3,3)
        RT_mat = np.hstack((R_mat, T_vec)).reshape(3,4)
        RT_mat = np.vstack((RT_mat, [0,0,0,1])).reshape(4,4)
        return inv(RT_mat)


# read strandsXXXXX_YYYYY_AAAAA_mBB.convdata
# Dimension: 100*4*32*32  
# v[i,0:3,n,m] is the x,y,z position of the ith point on the [n,m]th strand.
# v[i,3,n,m] is a value related to the curvature of that point. 
# if v[:,:,n,m] all equals to 0, it means it is an empty strand.
# x: v[i,3,n,m][0] 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def compute_pose_error(kpts1, kpts2_3d_2, matches, vis1, vis2, T_2to1, K1,
                       reproj_thresh):
    valid = vis1[matches[:, 0]] & vis2[matches[:, 1]]
    matches = matches[valid]
    failure = (None, None)

    if len(matches) < 4:
        return failure

    kpts1 = kpts1[matches[:, 0]].astype(np.float32).reshape((-1, 1, 2))
    kpts2_3d_2 = kpts2_3d_2[matches[:, 1]].reshape((-1, 1, 3))
    success, R_vec, t, inliers = cv2.solvePnPRansac(
        kpts2_3d_2, kpts1, K1, np.zeros(4), flags=cv2.SOLVEPNP_P3P,
        iterationsCount=1000, reprojectionError=reproj_thresh)
    if not success:
        return failure

    R, _ = cv2.Rodrigues(R_vec)
    t = t[:, 0]

    error_t = np.linalg.norm(t - T_2to1[:3, 3])
    error_R = angle_error(R, T_2to1[:3, :3])
    return error_t, error_R 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def rotated(self,
                verts,
                deg,
                cam=None,
                axis='y',
                img=None,
                do_alpha=True,
                far=None,
                near=None,
                color_id=0,
                img_size=None):
        import math
        if axis == 'y':
            around = cv2.Rodrigues(np.array([0, math.radians(deg), 0]))[0]
        elif axis == 'x':
            around = cv2.Rodrigues(np.array([math.radians(deg), 0, 0]))[0]
        else:
            around = cv2.Rodrigues(np.array([0, 0, math.radians(deg)]))[0]
        center = verts.mean(axis=0)
        new_v = np.dot((verts - center), around) + center

        return self.__call__(
            new_v,
            cam,
            img=img,
            do_alpha=do_alpha,
            far=far,
            near=near,
            img_size=img_size,
            color_id=color_id) 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def global_rigid_transformation(pose, J, kintree_table, xp):
    results = {}
    pose = pose.reshape((-1,3))
    id_to_col = {kintree_table[1,i] : i for i in range(kintree_table.shape[1])}
    parent = {i : id_to_col[kintree_table[0,i]] for i in range(1, kintree_table.shape[1])}

    if xp == chumpy:
        from posemapper import Rodrigues
        rodrigues = lambda x : Rodrigues(x)
    else:
        import cv2
        rodrigues = lambda x : cv2.Rodrigues(x)[0]

    with_zeros = lambda x : xp.vstack((x, xp.array([[0.0, 0.0, 0.0, 1.0]])))
    results[0] = with_zeros(xp.hstack((rodrigues(pose[0,:]), J[0,:].reshape((3,1)))))        
        
    for i in range(1, kintree_table.shape[1]):
        results[i] = results[parent[i]].dot(with_zeros(xp.hstack((
            rodrigues(pose[i,:]),
            ((J[i,:] - J[parent[i],:]).reshape((3,1)))
            ))))

    pack = lambda x : xp.hstack([np.zeros((4, 3)), x.reshape((4,1))])
    
    results = [results[i] for i in sorted(results.keys())]
    results_global = results

    if True:
        results2 = [results[i] - (pack(
            results[i].dot(xp.concatenate( ( (J[i,:]), 0 ) )))
            ) for i in range(len(results))]
        results = results2
    result = xp.dstack(results)
    return result, results_global 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def compute_r(self):
        return cv2.Rodrigues(self.rt.r)[0] 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def compute_dr_wrt(self, wrt):
        if wrt is self.rt:
            return cv2.Rodrigues(self.rt.r)[1].T 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def lrotmin(p): 
    if isinstance(p, np.ndarray):
        p = p.ravel()[3:]
        return np.concatenate([(cv2.Rodrigues(np.array(pp))[0]-np.eye(3)).ravel() for pp in p.reshape((-1,3))]).ravel()        
    if p.ndim != 2 or p.shape[1] != 3:
        p = p.reshape((-1,3))
    p = p[1:]
    return ch.concatenate([(Rodrigues(pp)-ch.eye(3)).ravel() for pp in p]).ravel() 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def solve_pose_by_68_points(self, image_points):
        """
        Solve pose from all the 68 image points
        Return (rotation_vector, translation_vector) as pose.
        """

        if self.r_vec is None:
            (_, rotation_vector, translation_vector) = cv2.solvePnP(
                self.model_points_68, image_points, self.camera_matrix, self.dist_coefs)
            self.r_vec = rotation_vector
            self.t_vec = translation_vector

        (_, rotation_vector, translation_vector) = cv2.solvePnP(
            self.model_points_68,
            image_points,
            self.camera_matrix,
            self.dist_coefs,
            rvec=self.r_vec,
            tvec=self.t_vec,
            useExtrinsicGuess=True)

        R, _ = cv2.Rodrigues(rotation_vector)
        points_3d = R.dot(self.model_points_68.T) + translation_vector # 3x68
        reproject_image_points = self.camera_matrix.dot(points_3d).T # 68x2
        reproject_image_points /= reproject_image_points[:, 2:3]
        reproject_image_points = reproject_image_points[:, :2]
        reprojection_error = np.mean((image_points - reproject_image_points)**2)

        return reprojection_error, rotation_vector, translation_vector 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import Rodrigues [as 別名]
def pnp_ransac(self,rgb_aug_test,img_prob_ori,non_zero,v1,v2,u1,u2):
            rgb_aug_crop =rgb_aug_test[v1:v2,u1:u2]
            xyz = np.copy(rgb_aug_crop)
            xyz  =xyz/255
            xyz = xyz*2-1
            xyz[:,:,0]=xyz[:,:,0]*self.obj_scale[0]+self.obj_ct[0]
            xyz[:,:,1]=xyz[:,:,1]*self.obj_scale[1]+self.obj_ct[1]
            xyz[:,:,2]=xyz[:,:,2]*self.obj_scale[2]+self.obj_ct[2]
            confidence_mask = img_prob_ori< self.th_i
            valid_mask = np.logical_and(non_zero ,confidence_mask)

            vu_list_s= np.where(valid_mask==1)
            n_pts_s=len(vu_list_s[0])
            img_pts_s = np.zeros((n_pts_s,2))
            obj_pts_s=xyz[vu_list_s[0],vu_list_s[1]]
            img_pts_s[:]=np.stack( (vu_list_s[1],vu_list_s[0]),axis=1) #u,v order
            img_pts_s[:,0]=img_pts_s[:,0]+u1
            img_pts_s[:,1]=img_pts_s[:,1]+v1
            img_pts_s = np.ascontiguousarray(img_pts_s[:,:2]).reshape((n_pts_s,1,2))
            if(n_pts_s <6):
                return np.eye(3),np.array([0,0,0]),valid_mask,-1
            ret, rvec, tvec,inliers = cv2.solvePnPRansac(obj_pts_s, img_pts_s, self.camK,None,\
                                      flags=cv2.SOLVEPNP_EPNP,reprojectionError=3,iterationsCount=100)
            if(inliers is None):
                return np.eye(3),np.array([0,0,0]),-1,-1
            else:
                rot_pred = np.eye(3)
                tra_pred = tvec[:,0]
                cv2.Rodrigues(rvec, rot_pred)                
                return rot_pred,tra_pred,valid_mask,len(inliers)