Python cv2.SOLVEPNP_ITERATIVE Examples
The following are 9
code examples of cv2.SOLVEPNP_ITERATIVE().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
cv2
, or try the search function
.
.
Example #1
| Source File: face_model.py From pytorch_mpiigaze with MIT License | 6 votes |
|
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
Example #2
| Source File: BPnP.py From BPnP with MIT License | 6 votes |
|
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
Example #3
| Source File: BPnP.py From BPnP with MIT License | 6 votes |
|
def forward(ctx, pts2d, pts3d, K, ini_pose=None):
bs = pts2d.size(0)
n = pts2d.size(1)
device = pts2d.device
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))
pts3d_i_np = np.ascontiguousarray(pts3d[i].detach().cpu()).reshape((n,3))
if ini_pose is None:
_, rvec0, T0, _ = cv.solvePnPRansac(objectPoints=pts3d_i_np, imagePoints=pts2d_i_np, cameraMatrix=K_np, distCoeffs=None, flags=cv.SOLVEPNP_ITERATIVE, confidence=0.9999 ,reprojectionError=1)
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_i_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
Example #4
| Source File: process.py From edusense with BSD 3-Clause "New" or "Revised" License | 5 votes |
|
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
Example #5
| Source File: evaluation_utils.py From PVN3D with MIT License | 5 votes |
|
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)
Example #6
| Source File: video.py From cvcalib with Apache License 2.0 | 5 votes |
|
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
Example #7
| Source File: localization.py From hfnet with MIT License | 5 votes |
|
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
Example #8
| Source File: common.py From deepface with MIT License | 4 votes |
|
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
Example #9
| Source File: sketch_face.py From sketch_face with MIT License | 4 votes |
|
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
