Ejemplo n.º 1
0
    def test_bundle_adjust(self):
        """Tests the bundle adjustment for relative pose on a simulated scene."""
        two_view_estimator = TwoViewEstimator(matcher=None,
                                              verifier=None,
                                              inlier_support_processor=None,
                                              bundle_adjust_2view=True,
                                              eval_threshold_px=4)

        # Extract example poses.
        i1Ri2 = EXAMPLE_DATA.get_camera(1).pose().rotation()
        i1ti2 = EXAMPLE_DATA.get_camera(1).pose().translation()
        i2Ti1 = Pose3(i1Ri2, i1ti2).inverse()
        i2Ei1 = EssentialMatrix(i2Ti1.rotation(), Unit3(i2Ti1.translation()))

        # Perform bundle adjustment.
        i2Ri1_optimized, i2Ui1_optimized, corr_idxs = two_view_estimator.bundle_adjust(
            keypoints_i1=self.keypoints_i1,
            keypoints_i2=self.keypoints_i2,
            verified_corr_idxs=self.corr_idxs,
            camera_intrinsics_i1=Cal3Bundler(),
            camera_intrinsics_i2=Cal3Bundler(),
            i2Ri1_initial=i2Ei1.rotation(),
            i2Ui1_initial=i2Ei1.direction(),
            i2Ti1_prior=None,
        )

        # Assert
        rotation_angular_error = comp_utils.compute_relative_rotation_angle(
            i2Ri1_optimized, i2Ei1.rotation())
        translation_angular_error = comp_utils.compute_relative_unit_translation_angle(
            i2Ui1_optimized, i2Ei1.direction())
        self.assertLessEqual(rotation_angular_error, 1)
        self.assertLessEqual(translation_angular_error, 1)
        np.testing.assert_allclose(corr_idxs, self.corr_idxs)
Ejemplo n.º 2
0
def compute_epipolar_distances(normalized_coords_i1: np.ndarray,
                               normalized_coords_i2: np.ndarray,
                               i2Ei1: EssentialMatrix) -> Optional[np.ndarray]:
    """Compute symmetric point-line epipolar distances between normalized coordinates of correspondences.

    Args:
        normalized_coords_i1: normalized coordinates in image i1, of shape Nx2.
        normalized_coords_i2: corr. normalized coordinates in image i2, of shape Nx2.
        i2Ei1: essential matrix between two images

    Returns:
        Symmetric epipolar distances for each row of the input, of shape N.
    """
    if (normalized_coords_i1 is None or normalized_coords_i1.size == 0
            or normalized_coords_i2 is None or normalized_coords_i2.size == 0):
        return None

    # construct the essential matrix in the opposite directin
    i2Ti1 = Pose3(i2Ei1.rotation(), i2Ei1.direction().point3())
    i1Ti2 = i2Ti1.inverse()
    i1Ei2 = EssentialMatrix(i1Ti2.rotation(), Unit3(i1Ti2.translation()))

    # get lines in i2 and i1
    epipolar_lines_i2 = feature_utils.convert_to_epipolar_lines(
        normalized_coords_i1, i2Ei1)
    epipolar_lines_i1 = feature_utils.convert_to_epipolar_lines(
        normalized_coords_i2, i1Ei2)

    # compute two distances and average them
    return 0.5 * (feature_utils.compute_point_line_distances(
        normalized_coords_i1, epipolar_lines_i1) +
                  feature_utils.compute_point_line_distances(
                      normalized_coords_i2, epipolar_lines_i2))