def two_view_reconstruction(
p1: np.ndarray,
p2: np.ndarray,
camera1: pygeometry.Camera,
camera2: pygeometry.Camera,
threshold: float,
iterations: int,
) -> Tuple[np.ndarray, np.ndarray, List[int]]:
"""Reconstruct two views using the 5-point method.
Args:
p1, p2: lists points in the images
camera1, camera2: Camera models
threshold: reprojection error threshold
Returns:
rotation, translation and inlier list
"""
b1 = camera1.pixel_bearing_many(p1)
b2 = camera2.pixel_bearing_many(p2)
T = multiview.relative_pose_ransac(b1, b2, threshold, 1000, 0.999)
R = T[:, :3]
t = T[:, 3]
inliers = _two_view_reconstruction_inliers(b1, b2, R, t, threshold)
if len(inliers) > 5:
T = multiview.relative_pose_optimize_nonlinear(b1[inliers],
b2[inliers], t, R,
iterations)
R = T[:, :3]
t = T[:, 3]
inliers = _two_view_reconstruction_inliers(b1, b2, R, t, threshold)
return cv2.Rodrigues(R.T)[0].ravel(), -R.T.dot(t), inliers
def two_view_reconstruction_rotation_only(
p1: np.ndarray,
p2: np.ndarray,
camera1: pygeometry.Camera,
camera2: pygeometry.Camera,
threshold: float,
) -> Tuple[np.ndarray, List[int]]:
"""Find rotation between two views from point correspondences.
Args:
p1, p2: lists points in the images
camera1, camera2: Camera models
threshold: reprojection error threshold
Returns:
rotation and inlier list
"""
b1 = camera1.pixel_bearing_many(p1)
b2 = camera2.pixel_bearing_many(p2)
R = multiview.relative_pose_ransac_rotation_only(b1, b2, threshold, 1000,
0.999)
inliers = _two_view_rotation_inliers(b1, b2, R, threshold)
return cv2.Rodrigues(R.T)[0].ravel(), inliers
def robust_match_calibrated(
p1: np.ndarray,
p2: np.ndarray,
camera1: pygeometry.Camera,
camera2: pygeometry.Camera,
matches: np.ndarray,
config: Dict[str, Any],
) -> np.ndarray:
"""Filter matches by estimating the Essential matrix via RANSAC."""
if len(matches) < 8:
return np.array([])
p1 = p1[matches[:, 0]][:, :2].copy()
p2 = p2[matches[:, 1]][:, :2].copy()
b1 = camera1.pixel_bearing_many(p1)
b2 = camera2.pixel_bearing_many(p2)
threshold = config["robust_matching_calib_threshold"]
T = multiview.relative_pose_ransac(b1, b2, threshold, 1000, 0.999)
for relax in [4, 2, 1]:
inliers = compute_inliers_bearings(b1, b2, T[:, :3], T[:, 3],
relax * threshold)
if np.sum(inliers) < 8:
return np.array([])
iterations = config["five_point_refine_match_iterations"]
T = multiview.relative_pose_optimize_nonlinear(b1[inliers],
b2[inliers], T[:3, 3],
T[:3, :3], iterations)
inliers = compute_inliers_bearings(b1, b2, T[:, :3], T[:, 3], threshold)
return matches[inliers]
def assert_cameras_equal(cam1: pygeometry.Camera,
cam2: pygeometry.Camera) -> None:
assert np.allclose(cam1.get_parameters_values(),
cam2.get_parameters_values())
assert cam1.projection_type == cam2.projection_type
assert cam1.width == cam2.width
assert cam1.height == cam2.height
assert cam1.id == cam2.id
def two_view_reconstruction_plane_based(
p1: np.ndarray,
p2: np.ndarray,
camera1: pygeometry.Camera,
camera2: pygeometry.Camera,
threshold: float,
) -> Tuple[Optional[np.ndarray], Optional[np.ndarray], List[int]]:
"""Reconstruct two views from point correspondences lying on a plane.
Args:
p1, p2: lists points in the images
camera1, camera2: Camera models
threshold: reprojection error threshold
Returns:
rotation, translation and inlier list
"""
b1 = camera1.pixel_bearing_many(p1)
b2 = camera2.pixel_bearing_many(p2)
x1 = multiview.euclidean(b1)
x2 = multiview.euclidean(b2)
H, inliers = cv2.findHomography(x1, x2, cv2.RANSAC, threshold)
motions = multiview.motion_from_plane_homography(H)
if len(motions) == 0:
return None, None, []
motion_inliers = []
for R, t, _, _ in motions:
inliers = _two_view_reconstruction_inliers(b1, b2, R.T, -R.T.dot(t),
threshold)
motion_inliers.append(inliers)
best = np.argmax(map(len, motion_inliers))
R, t, n, d = motions[best]
inliers = motion_inliers[best]
return cv2.Rodrigues(R)[0].ravel(), t, inliers
def _get_camera_from_bundle(ba: pybundle.BundleAdjuster,
camera: pygeometry.Camera):
"""Read camera parameters from a bundle adjustment problem."""
c = ba.get_camera(camera.id)
for k, v in c.get_parameters_map().items():
camera.set_parameter_value(k, v)
def _cameras_statistics(camera_model: pygeometry.Camera) -> Dict[str, Any]:
camera_stats = {}
for param_type, param_value in camera_model.get_parameters_map().items():
camera_stats[str(param_type).split(".")[1]] = param_value
return camera_stats