Python cv2.SOLVEPNP_ITERATIVE属性代码示例

# 需要导入模块: import cv2 [as 别名]
# 或者: from cv2 import SOLVEPNP_ITERATIVE [as 别名]
def estimate_head_pose(self, face: Face, camera: Camera) -> None:
        """Estimate the head pose by fitting 3D template model."""
        # If the number of the template points is small, cv2.solvePnP
        # becomes unstable, so set the default value for rvec and tvec
        # and set useExtrinsicGuess to True.
        # The default values of rvec and tvec below mean that the
        # initial estimate of the head pose is not rotated and the
        # face is in front of the camera.
        rvec = np.zeros(3, dtype=np.float)
        tvec = np.array([0, 0, 1], dtype=np.float)
        _, rvec, tvec = cv2.solvePnP(self.LANDMARKS,
                                     face.landmarks,
                                     camera.camera_matrix,
                                     camera.dist_coefficients,
                                     rvec,
                                     tvec,
                                     useExtrinsicGuess=True,
                                     flags=cv2.SOLVEPNP_ITERATIVE)
        rot = Rotation.from_rotvec(rvec)
        face.head_pose_rot = rot
        face.head_position = tvec
        face.reye.head_pose_rot = rot
        face.leye.head_pose_rot = rot 

# 需要导入模块: import cv2 [as 别名]
# 或者: from cv2 import SOLVEPNP_ITERATIVE [as 别名]
def forward(ctx, pts2d, pts3d, K, ini_pose=None):
        bs = pts2d.size(0)
        n = pts2d.size(1)
        device = pts2d.device
        pts3d_np = np.array(pts3d.detach().cpu())
        K_np = np.array(K.detach().cpu())
        P_6d = torch.zeros(bs,6,device=device)

        for i in range(bs):
            pts2d_i_np = np.ascontiguousarray(pts2d[i].detach().cpu()).reshape((n,1,2))
            if ini_pose is None:
                _, rvec0, T0, _ = cv.solvePnPRansac(objectPoints=pts3d_np, imagePoints=pts2d_i_np, cameraMatrix=K_np, distCoeffs=None, flags=cv.SOLVEPNP_ITERATIVE, confidence=0.9999 ,reprojectionError=3)
            else:
                rvec0 = np.array(ini_pose[i, 0:3].cpu().view(3, 1))
                T0 = np.array(ini_pose[i, 3:6].cpu().view(3, 1))
            _, rvec, T = cv.solvePnP(objectPoints=pts3d_np, imagePoints=pts2d_i_np, cameraMatrix=K_np, distCoeffs=None, flags=cv.SOLVEPNP_ITERATIVE, useExtrinsicGuess=True, rvec=rvec0, tvec=T0)
            angle_axis = torch.tensor(rvec,device=device,dtype=torch.float).view(1, 3)
            T = torch.tensor(T,device=device,dtype=torch.float).view(1, 3)
            P_6d[i,:] = torch.cat((angle_axis,T),dim=-1)

        ctx.save_for_backward(pts2d,P_6d,pts3d,K)
        return P_6d 

# 需要导入模块: import cv2 [as 别名]
# 或者: from cv2 import SOLVEPNP_ITERATIVE [as 别名]
def get_3d_head_position(pose,size):
    image_points = np.array([
                            (pose[0][0], pose[0][1]),     # Nose tip
                            (pose[15][0], pose[15][1]),   # Right eye
                            (pose[14][0], pose[14][1]),   # Left eye
                            (pose[17][0], pose[17][1]),   # Right ear
                            (pose[16][0], pose[16][1]),   # Left ear
                        ], dtype="double")
    model_points = np.array([
                            (-48.0, 0.0, 21.0),       # Nose tip
                            (-5.0, -65.5, -20.0),     # Right eye 
                            (-5.0, 65.6, -20.0),      # Left eye
                            (-6.0, -77.5, -100.0),    # Right ear
                            (-6.0, 77.5, -100.0)      # Left ear 
                         ])

    c_x = size[1]/2
    c_y = size[0]/2
    f_x = c_x / np.tan(60/2 * np.pi / 180 )
    f_y = f_x

    camera_matrix = np.array(
                         [[f_x, 0, c_x],
                         [0, f_y, c_y],
                         [0, 0, 1]], dtype = "double"
                         )
    dist_coeffs = np.zeros((4,1))
    (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)
    tvec = (int(translation_vector[0]/100.0),int(translation_vector[1]/100.0),abs(int(translation_vector[2]/100.0))) 
    return tvec 

# 需要导入模块: import cv2 [as 别名]
# 或者: from cv2 import SOLVEPNP_ITERATIVE [as 别名]
def pnp(points_3d, points_2d, camera_matrix,method=cv2.SOLVEPNP_ITERATIVE):
    try:
        dist_coeffs = pnp.dist_coeffs
    except:
        dist_coeffs = np.zeros(shape=[8, 1], dtype='float64')

    assert points_3d.shape[0] == points_2d.shape[0], 'points 3D and points 2D must have same number of vertices'
    if method==cv2.SOLVEPNP_EPNP:
        points_3d=np.expand_dims(points_3d, 0)
        points_2d=np.expand_dims(points_2d, 0)

    points_2d = np.ascontiguousarray(points_2d.astype(np.float64))
    points_3d = np.ascontiguousarray(points_3d.astype(np.float64))
    camera_matrix = camera_matrix.astype(np.float64)
    # _, R_exp, t = cv2.solvePnP(points_3d,
    #                            points_2d,
    #                            camera_matrix,
    #                            dist_coeffs,
    #                            flags=method)
    #                           # , None, None, False, cv2.SOLVEPNP_UPNP)

    _, R_exp, t, _ = cv2.solvePnPRansac(points_3d,
                               points_2d,
                               camera_matrix,
                               dist_coeffs,
                               )

    R, _ = cv2.Rodrigues(R_exp)
    # trans_3d=np.matmul(points_3d,R.transpose())+t.transpose()
    # if np.max(trans_3d[:,2]<0):
    #     R=-R
    #     t=-t

    return np.concatenate([R, t], axis=-1) 

# 需要导入模块: import cv2 [as 别名]
# 或者: from cv2 import SOLVEPNP_ITERATIVE [as 别名]
def find_current_pose(self, object_points, intrinsics):
        """
        Find camera pose relative to object using current image point set,
        object_points are treated as world coordinates
        """
        success, rotation_vector, translation_vector = cv2.solvePnPRansac(object_points, self.current_image_points,
                                                                          intrinsics.intrinsic_mat,
                                                                          intrinsics.distortion_coeffs,
                                                                          flags=cv2.SOLVEPNP_ITERATIVE)[0:3]
        if success:
            self.poses.append(Pose(rotation=rotation_vector, translation_vector=translation_vector))
        else:
            self.poses.append(None)
        return success 

# 需要导入模块: import cv2 [as 别名]
# 或者: from cv2 import SOLVEPNP_ITERATIVE [as 别名]
def do_pnp(kpts, lms, query_info, config):
    kpts = kpts.astype(np.float32).reshape((-1, 1, 2))
    lms = lms.astype(np.float32).reshape((-1, 1, 3))

    success, R_vec, t, inliers = cv2.solvePnPRansac(
        lms, kpts, query_info.K, np.array([query_info.dist, 0, 0, 0]),
        iterationsCount=5000, reprojectionError=config['reproj_error'],
        flags=cv2.SOLVEPNP_P3P)

    if success:
        inliers = inliers[:, 0]
        num_inliers = len(inliers)
        inlier_ratio = len(inliers) / len(kpts)
        success &= num_inliers >= config['min_inliers']

        ret, R_vec, t = cv2.solvePnP(
                lms[inliers], kpts[inliers], query_info.K,
                np.array([query_info.dist, 0, 0, 0]), rvec=R_vec, tvec=t,
                useExtrinsicGuess=True, flags=cv2.SOLVEPNP_ITERATIVE)
        assert ret

        query_T_w = np.eye(4)
        query_T_w[:3, :3] = cv2.Rodrigues(R_vec)[0]
        query_T_w[:3, 3] = t[:, 0]
        w_T_query = np.linalg.inv(query_T_w)

        ret = LocResult(success, num_inliers, inlier_ratio, w_T_query)
    else:
        inliers = np.empty((0,), np.int32)
        ret = loc_failure

    return ret, inliers 

# 需要导入模块: import cv2 [as 别名]
# 或者: from cv2 import SOLVEPNP_ITERATIVE [as 别名]
def landmark_to_pose(landmark, image_shape):
    image_points = np.array([
        landmark[33],  # (359, 391),  # Nose tip
        landmark[8],  # (399, 561),  # Chin
        landmark[36],  # (337, 297),  # Left eye left corner
        landmark[45],  # (513, 301),  # Right eye right corne
        landmark[48],  # (345, 465),  # Left Mouth corner
        landmark[54],  # (453, 469)  # Right mouth corner
    ], dtype='double')

    model_points = np.array([
        (0.0, 0.0, 0.0),  # Nose tip
        (0.0, -330.0, -65.0),  # Chin
        (-225.0, 170.0, -135.0),  # Left eye left corner
        (225.0, 170.0, -135.0),  # Right eye right corne
        (-150.0, -150.0, -125.0),  # Left Mouth corner
        (150.0, -150.0, -125.0)  # Right mouth corner
    ])

    center = (image_shape[1] / 2, image_shape[0] / 2)
    focal_length = center[0] / np.tan(60 / 2 * np.pi / 180)
    camera_matrix = np.array(
        [[focal_length, 0, center[0]],
         [0, focal_length, center[1]],
         [0, 0, 1]], dtype="double"
    )

    dist_coeffs = np.zeros((4, 1))  # Assuming no lens distortion
    success, rotation_vector, translation_vector = cv2.solvePnP(model_points, image_points, camera_matrix,
                                                                dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)

    rotation, jacobian = cv2.Rodrigues(rotation_vector)
    translation = np.array(translation_vector).reshape(-1, 1).T[0]

    permutation_marker_to_ros = np.array((
        (0.0, 0.0, 1.0, 0.0),
        (1.0, 0.0, 0.0, 0.0),
        (0.0, 1.0, 0.0, 0.0),
        (0.0, 0.0, 0.0, 1.0)
    ), dtype=np.float64)
    permutation_camera_to_ros = np.array((
        (0.0, 0.0, 1.0, 0.0),
        (-1.0, 0.0, 0.0, 0.0),
        (0.0, -1.0, 0.0, 0.0),
        (0.0, 0.0, 0.0, 1.0)
    ), dtype=np.float64)

    relation_cv = np.concatenate((
        np.concatenate((rotation, translation.reshape(3, 1)), axis=1),
        np.array((0.0, 0.0, 0.0, 1.0)).reshape(1, 4)
    ), axis=0)
    relation = permutation_camera_to_ros.dot(relation_cv)
    relation = relation.dot(np.linalg.inv(permutation_marker_to_ros))

    if success:
        nose_end_point2D, jacobian = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vector,
                                                       translation_vector, camera_matrix, dist_coeffs)
        return rotationMatrixToEulerAngles(relation), nose_end_point2D
    else:
        return None, None 

# 需要导入模块: import cv2 [as 别名]
# 或者: from cv2 import SOLVEPNP_ITERATIVE [as 别名]
def compute_pose(face, canvas=None):
    """Take face landmarks and return a 2D vector representing the direction of pose.
    
    This code was taken adapted from:
    https://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/
    """

    # Extract key parts of the face from face landmarks. Read the post above for details.
    image_points = np.array([
#        face['nose_tip'][2],
        face['nose_bridge'][3],
        face['chin'][8],
        face['left_eye'][0],
        face['right_eye'][3],
        face['top_lip'][0],
        face['bottom_lip'][0]],
        dtype = 'double')
    image_points *= settings['scale_frame']

    # If a canvas was passed in, render the key points on the face.
    if canvas is not None:
        for point in image_points:
            cv2.circle(canvas, (int(point[0]), int(point[1])), 10, (255, 0, 0), -1)

    # 3D model points.
    model_points = np.array([
            (0.0, 0.0, 0.0),             # Nose tip
            (0.0, -330.0, -65.0),        # Chin
            (-225.0, 170.0, -135.0),     # Left eye left corner
            (225.0, 170.0, -135.0),      # Right eye right corne
            (-150.0, -150.0, -125.0),    # Left Mouth corner
            (150.0, -150.0, -125.0)      # Right mouth corner
            ])

    # Camera internals
    size = (settings['height'], settings['width'])
    focal_length = size[1]
    center = (size[1]/2, size[0]/2)
    camera_matrix = np.array(
        [[focal_length, 0, center[0]],
         [0, focal_length, center[1]],
         [0, 0, 1]], dtype = "double"
        )

    dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion
    (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)

    # Project a 3D point (0, 0, 1000.0) onto the image plane.
    # We use this to draw a line sticking out of the nose
    (nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vector, translation_vector, camera_matrix, dist_coeffs)
    p1 = ( int(image_points[0][0]), int(image_points[0][1]))
    p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))

    if canvas is not None:
        # Draw a line pointing in the direction of the pose
        cv2.line(canvas, p1, p2, (255,0,0), 2)

    pose = [p2[0] - p1[0], p2[1] - p1[1]]

    return pose