def two_view_reconstruction_plane_based(p1, p2, camera1, camera2, threshold):
"""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)
motion_inliers = []
for R, t, n, d 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 test_motion_from_plane_homography():
R = tf.random_rotation_matrix()[:3, :3]
t = normalized(2 * np.random.rand(3) - 1)
n = normalized(2 * np.random.rand(3) - 1)
d = 2 * np.random.rand() - 1
scale = 2 * np.random.rand() - 1
H = scale * (d * R - np.outer(t, n))
motions = multiview.motion_from_plane_homography(H)
goodness = []
for Re, te, ne, de in motions:
scalee = np.linalg.norm(te)
good_R = np.allclose(R, Re)
good_t = np.allclose(normalized(te), t)
sign_n = np.sign(np.dot(ne, n))
good_n = np.allclose(sign_n * ne, n)
good_d = np.allclose(sign_n * de / scalee, d)
goodness.append(good_R and good_t and good_n and good_d)
assert any(goodness)
def test_motion_from_plane_homography():
R = tf.random_rotation_matrix()[:3, :3]
t = normalized(2 * np.random.rand(3) - 1)
n = normalized(2 * np.random.rand(3) - 1)
d = 2 * np.random.rand() - 1
scale = 2 * np.random.rand() - 1
H = scale * (d * R - np.outer(t, n))
motions = multiview.motion_from_plane_homography(H)
goodness = []
for Re, te, ne, de in motions:
scalee = np.linalg.norm(te)
good_R = np.allclose(R, Re)
good_t = np.allclose(normalized(te), t)
sign_n = np.sign(np.dot(ne, n))
good_n = np.allclose(sign_n * ne, n)
good_d = np.allclose(sign_n * de / scalee, d)
goodness.append(good_R and good_t and good_n and good_d)
assert any(goodness)
