Spaces:
Runtime error
Runtime error
File size: 6,862 Bytes
1bb1365 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 |
# Copyright (C) 2024-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).
#
# --------------------------------------------------------
# main pnp code
# --------------------------------------------------------
import cv2
import numpy as np
import quaternion
from dust3r.utils.geometry import opencv_to_colmap_intrinsics
from packaging import version
try:
import poselib # noqa
HAS_POSELIB = True
except Exception as e:
HAS_POSELIB = False
try:
import pycolmap # noqa
version_number = pycolmap.__version__
if version.parse(version_number) < version.parse("0.5.0"):
HAS_PYCOLMAP = False
else:
HAS_PYCOLMAP = True
except Exception as e:
HAS_PYCOLMAP = False
def run_pnp(
pts2D, pts3D, K, distortion=None, mode="cv2", reprojectionError=5, img_size=None
):
"""
use OPENCV model for distortion (4 values)
"""
assert mode in ["cv2", "poselib", "pycolmap"]
try:
if len(pts2D) > 4 and mode == "cv2":
confidence = 0.9999
iterationsCount = 10_000
if distortion is not None:
cv2_pts2ds = np.copy(pts2D)
cv2_pts2ds = cv2.undistortPoints(
cv2_pts2ds, K, np.array(distortion), R=None, P=K
)
pts2D = cv2_pts2ds.reshape((-1, 2))
success, r_pose, t_pose, _ = cv2.solvePnPRansac(
pts3D,
pts2D,
K,
None,
flags=cv2.SOLVEPNP_SQPNP,
iterationsCount=iterationsCount,
reprojectionError=reprojectionError,
confidence=confidence,
)
if not success:
return False, None
r_pose = cv2.Rodrigues(r_pose)[0] # world2cam == world2cam2
RT = np.r_[np.c_[r_pose, t_pose], [(0, 0, 0, 1)]] # world2cam2
return True, np.linalg.inv(RT) # cam2toworld
elif len(pts2D) > 4 and mode == "poselib":
assert HAS_POSELIB
confidence = 0.9999
iterationsCount = 10_000
# NOTE: `Camera` struct currently contains `width`/`height` fields,
# however these are not used anywhere in the code-base and are provided simply to be consistent with COLMAP.
# so we put garbage in there
colmap_intrinsics = opencv_to_colmap_intrinsics(K)
fx = colmap_intrinsics[0, 0]
fy = colmap_intrinsics[1, 1]
cx = colmap_intrinsics[0, 2]
cy = colmap_intrinsics[1, 2]
width = img_size[0] if img_size is not None else int(cx * 2)
height = img_size[1] if img_size is not None else int(cy * 2)
if distortion is None:
camera = {
"model": "PINHOLE",
"width": width,
"height": height,
"params": [fx, fy, cx, cy],
}
else:
camera = {
"model": "OPENCV",
"width": width,
"height": height,
"params": [fx, fy, cx, cy] + distortion,
}
pts2D = np.copy(pts2D)
pts2D[:, 0] += 0.5
pts2D[:, 1] += 0.5
pose, _ = poselib.estimate_absolute_pose(
pts2D,
pts3D,
camera,
{
"max_reproj_error": reprojectionError,
"max_iterations": iterationsCount,
"success_prob": confidence,
},
{},
)
if pose is None:
return False, None
RT = pose.Rt # (3x4)
RT = np.r_[RT, [(0, 0, 0, 1)]] # world2cam
return True, np.linalg.inv(RT) # cam2toworld
elif len(pts2D) > 4 and mode == "pycolmap":
assert HAS_PYCOLMAP
assert img_size is not None
pts2D = np.copy(pts2D)
pts2D[:, 0] += 0.5
pts2D[:, 1] += 0.5
colmap_intrinsics = opencv_to_colmap_intrinsics(K)
fx = colmap_intrinsics[0, 0]
fy = colmap_intrinsics[1, 1]
cx = colmap_intrinsics[0, 2]
cy = colmap_intrinsics[1, 2]
width = img_size[0]
height = img_size[1]
if distortion is None:
camera_dict = {
"model": "PINHOLE",
"width": width,
"height": height,
"params": [fx, fy, cx, cy],
}
else:
camera_dict = {
"model": "OPENCV",
"width": width,
"height": height,
"params": [fx, fy, cx, cy] + distortion,
}
pycolmap_camera = pycolmap.Camera(
model=camera_dict["model"],
width=camera_dict["width"],
height=camera_dict["height"],
params=camera_dict["params"],
)
pycolmap_estimation_options = dict(
ransac=dict(
max_error=reprojectionError,
min_inlier_ratio=0.01,
min_num_trials=1000,
max_num_trials=100000,
confidence=0.9999,
)
)
pycolmap_refinement_options = dict(
refine_focal_length=False, refine_extra_params=False
)
ret = pycolmap.absolute_pose_estimation(
pts2D,
pts3D,
pycolmap_camera,
estimation_options=pycolmap_estimation_options,
refinement_options=pycolmap_refinement_options,
)
if ret is None:
ret = {"success": False}
else:
ret["success"] = True
if callable(ret["cam_from_world"].matrix):
retmat = ret["cam_from_world"].matrix()
else:
retmat = ret["cam_from_world"].matrix
ret["qvec"] = quaternion.from_rotation_matrix(retmat[:3, :3])
ret["tvec"] = retmat[:3, 3]
if not (ret["success"] and ret["num_inliers"] > 0):
success = False
pose = None
else:
success = True
pr_world_to_querycam = np.r_[
ret["cam_from_world"].matrix(), [(0, 0, 0, 1)]
]
pose = np.linalg.inv(pr_world_to_querycam)
return success, pose
else:
return False, None
except Exception as e:
print(f"error during pnp: {e}")
return False, None
|