Add relative rotation metric on Rotation Averaging module

gtsfm/averaging/rotation/rotation_averaging_base.py

@@ -80,23 +80,29 @@ def _run_rotation_averaging_base(
         wRis = self.run_rotation_averaging(num_images, i2Ri1_dict, i1Ti2_priors)
         run_time = time.time() - start_time
 
-        metrics = self.evaluate(wRis, wTi_gt)
+        metrics = self.evaluate(wRis, wTi_gt, i2Ri1_dict)
         metrics.add_metric(GtsfmMetric("total_duration_sec", run_time))
 
         return wRis, metrics
 
-    def evaluate(self, wRi_computed: List[Optional[Rot3]], wTi_gt: List[Optional[Pose3]]) -> GtsfmMetricsGroup:
+    def evaluate(
+        self,
+        wRi_computed: List[Optional[Rot3]],
+        wTi_gt: List[Optional[Pose3]],
+        i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]],
+    ) -> GtsfmMetricsGroup:
         """Evaluates the global rotations computed by the rotation averaging implementation.
 
         Args:
             wRi_computed: List of global rotations computed.
             wTi_gt: Ground truth global rotations to compare against.
-
-        Raises:
-            ValueError: If the length of the computed and GT list differ.
+            i2Ri1_dict: Relative rotations as dictionary (i1, i2): i2Ri1.
 
         Returns:
             Metrics on global rotations.
+
+        Raises:
+            ValueError: If the length of the computed and GT list differ.
         """
         wRi_gt = [wTi.rotation() if wTi is not None else None for wTi in wTi_gt]
 
@@ -108,6 +114,12 @@ def evaluate(self, wRi_computed: List[Optional[Rot3]], wTi_gt: List[Optional[Pos
         metrics = []
         metrics.append(GtsfmMetric(name="num_rotations_computed", data=len([x for x in wRi_computed if x is not None])))
         metrics.append(metric_utils.compute_rotation_angle_metric(wRi_aligned, wRi_gt))
+        metrics.append(
+            metric_utils.compute_relative_rotation_angle_metric(
+                i2Ri1_dict=i2Ri1_dict,
+                wRi_list=wRi_computed,
+            )
+        )
         return GtsfmMetricsGroup(name="rotation_averaging_metrics", metrics=metrics)
 
     def create_computation_graph(

gtsfm/utils/geometry_comparisons.py

@@ -19,11 +19,11 @@ def align_rotations(aRi_list: List[Optional[Rot3]], bRi_list: List[Optional[Rot3
     """Aligns the list of rotations to the reference list by using Karcher mean.
 
     Args:
-        aRi_list: reference rotations in frame "a" which are the targets for alignment
-        bRi_list: input rotations which need to be aligned to frame "a"
+        aRi_list: Reference rotations in frame "a" which are the targets for alignment
+        bRi_list: Input rotations which need to be aligned to frame "a"
 
     Returns:
-        aRi_list_: transformed input rotations previously "bRi_list" but now which
+        aRi_list_: Transformed input rotations previously "bRi_list" but now which
             have the same origin as reference (now living in "a" frame)
     """
     aRb_list = [
@@ -263,17 +263,41 @@ def compare_global_poses(
     return rotations_equal and translations_equal
 
 
+def compute_rotation_angle_measurement_consistency(
+    i2Ri1: Optional[Unit3], wRi2: Optional[Pose3], wRi1: Optional[Pose3]
+) -> Optional[float]:
+    """Compute angle between a [ TODO ] between 2 poses.
+
+    Given a relative rotation measurement from i2 to i1, and the estimated global rotations of
+    i1 and i2, returns the angular difference between the relative vs. synthetic measurements.
+
+    Args:
+        i2Ri1: Relative rotation measurement.
+        wRi2: Global rotation of camera i2.
+        wRi1: Global rotation of camera i1.
+
+    Returns:
+        Angle between two-view measurement and synthetic relative rotation in degrees.
+    """
+    if i2Ri1 is None or wRi2 is None or wRi1 is None:
+        return None
+
+    # Synthetic measurement.
+    i2Ri1_synthetic = wRi2.between(wRi1)
+    return compute_relative_rotation_angle(i2Ri1, i2Ri1_synthetic)
+
+
 def compute_relative_rotation_angle(R_1: Optional[Rot3], R_2: Optional[Rot3]) -> Optional[float]:
     """Compute the angle between two rotations.
 
     Note: the angle is the norm of the angle-axis representation.
 
     Args:
-        R_1: the first rotation.
-        R_2: the second rotation.
+        R_1: The first rotation.
+        R_2: The second rotation.
 
     Returns:
-        the angle between two rotations, in degrees
+        The angle between two rotations, in degrees.
     """
 
     if R_1 is None or R_2 is None:
@@ -292,16 +316,16 @@ def compute_relative_unit_translation_angle(U_1: Optional[Unit3], U_2: Optional[
     """Compute the angle between two unit-translations.
 
     Args:
-        U_1: the first unit-translation.
-        U_2: the second unit-translation.
+        U_1: The first unit-translation.
+        U_2: The second unit-translation.
 
     Returns:
-        the angle between the two unit-vectors, in degrees
+        The angle between the two unit-vectors, in degrees.
     """
     if U_1 is None or U_2 is None:
         return None
 
-    # TODO: expose Unit3's dot function and use it directly
+    # TODO: expose Unit3's dot function and use it directly.
     dot_product = np.dot(U_1.point3(), U_2.point3())
     dot_product = np.clip(dot_product, -1, 1)
     angle_rad = np.arccos(dot_product)

gtsfm/utils/metrics.py

@@ -212,7 +212,7 @@ def compute_keypoint_intersections(
 
 
 def compute_rotation_angle_metric(wRi_list: List[Optional[Rot3]], gt_wRi_list: List[Optional[Pose3]]) -> GtsfmMetric:
-    """Computes statistics for the angle between estimated and GT rotations.
+    """Computes statistics for the angle between estimated and GT global rotations.
 
     Assumes that the estimated and GT rotations have been aligned and do not
     have a gauge freedom.
@@ -233,6 +233,27 @@ def compute_rotation_angle_metric(wRi_list: List[Optional[Rot3]], gt_wRi_list: L
     return GtsfmMetric("rotation_angle_error_deg", errors)
 
 
+def compute_relative_rotation_angle_metric(
+    i2Ri1_dict: Dict[Tuple[int, int], Optional[Unit3]], wRi_list: List[Optional[Pose3]]
+) -> GtsfmMetric:
+    """Computes consistency statistics between estimated global rotations and relative rotation measurements.
+
+    Args:
+        i2Ri1_dict: List of relative rotation measurements.
+        wRi_list: List of estimated camera global rotations.
+
+    Returns:
+        A GtsfmMetric for the relative rotation angle errors, in degrees.
+    """
+    angles: List[Optional[float]] = []
+    for i1, i2 in i2Ri1_dict:
+        i2Ri1 = i2Ri1_dict[(i1, i2)]
+        angles.append(
+            comp_utils.compute_rotation_angle_measurement_consistency(i2Ri1, wRi2=wRi_list[i2], wRi1=wRi_list[i1])
+        )
+    return GtsfmMetric("relative_rotation_angle_consistency_error_deg", np.array(angles, dtype=np.float32))
+
+
 def compute_translation_distance_metric(
     wti_list: List[Optional[Point3]], gt_wti_list: List[Optional[Point3]]
 ) -> GtsfmMetric: