def two_view_reconstruction(p1, p2, camera1, camera2, threshold):
"""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)
# Note on threshold:
# See opengv doc on thresholds here:
# http://laurentkneip.github.io/opengv/page_how_to_use.html
# Here we arbitrarily assume that the threshold is given for a camera of
# focal length 1. Also, arctan(threshold) \approx threshold since
# threshold is small
T = multiview.relative_pose_ransac(
b1, b2, b"STEWENIUS", 1 - np.cos(threshold), 1000, 0.999)
R = T[:, :3]
t = T[:, 3]
inliers = _two_view_reconstruction_inliers(b1, b2, R, t, threshold)
if inliers.sum() > 5:
T = multiview.relative_pose_optimize_nonlinear(b1[inliers],
b2[inliers], t, R)
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 robust_match_calibrated(p1, p2, camera1, camera2, matches, config):
"""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, b"STEWENIUS",
1 - np.cos(threshold), 1000, 0.999)
for relax in [4, 2, 1]:
inliers = _compute_inliers_bearings(b1, b2, T, 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],
b"STEWENIUS", T[:3, 3],
T[:3, :3], iterations)
inliers = _compute_inliers_bearings(b1, b2, T, threshold)
return matches[inliers]
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 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 robust_match_calibrated(p1, p2, camera1, camera2, matches, config):
"""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, b"STEWENIUS", 1 - np.cos(threshold), 1000, 0.999)
for relax in [4, 2, 1]:
inliers = _compute_inliers_bearings(b1, b2, T, relax * threshold)
if sum(inliers) < 8:
return np.array([])
T = pyopengv.relative_pose_optimize_nonlinear(
b1[inliers], b2[inliers], T[:3, 3], T[:3, :3])
inliers = _compute_inliers_bearings(b1, b2, T, threshold)
return matches[inliers]
def relative_pose_opengv(r_kp1, nn_r_kp2, intrinsics1, intrinsics2, px_thresh):
camera1 = intrinsics2camera(intrinsics1)
camera2 = intrinsics2camera(intrinsics2)
bearing_vectors1 = camera1.pixel_bearing_many(r_kp1)
bearing_vectors2 = camera2.pixel_bearing_many(nn_r_kp2)
# Convert pixel threshold to angular
avg_focal_length = (camera1.focal_x + camera1.focal_y + camera2.focal_x +
camera2.focal_y) / 4
angle_thresh = np.arctan2(px_thresh, avg_focal_length)
T = multiview.relative_pose_ransac(bearing_vectors1, bearing_vectors2,
b"STEWENIUS", 1 - np.cos(angle_thresh),
5000, 0.99999)
inliers = _compute_inliers_bearings(bearing_vectors1, bearing_vectors2, T,
angle_thresh)
T = multiview.relative_pose_optimize_nonlinear(bearing_vectors1[inliers],
bearing_vectors2[inliers],
T[:3, 3], T[:3, :3])
inliers = _compute_inliers_bearings(bearing_vectors1, bearing_vectors2, T,
angle_thresh)
return T, inliers
def robust_match_calibrated(p1, p2, camera1, camera2, matches, config):
"""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, "STEWENIUS",
1 - np.cos(threshold), 1000, 0.999)
inliers = _compute_inliers_bearings(b1, b2, T, threshold)
return matches[inliers]