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)