From f9361696488a1a10ba7651af6772db0a77825a4d Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 31 Jan 2024 22:52:09 -0800 Subject: [PATCH 01/27] Initialize shonan using minimum spanning tree --- gtsfm/averaging/rotation/shonan.py | 51 +++++++++++++++++++++++++++--- 1 file changed, 46 insertions(+), 5 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 9ddb6fb90..2b27d45d9 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -13,6 +13,7 @@ from typing import Dict, List, Optional, Set, Tuple import gtsam +import networkx as nx import numpy as np from gtsam import ( BetweenFactorPose3, @@ -22,6 +23,7 @@ Rot3, ShonanAveraging3, ShonanAveragingParameters3, + Values, ) import gtsfm.utils.logger as logger_utils @@ -35,6 +37,36 @@ _DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1.0 +def random_rotation() -> Rot3: + """Sample a random rotation by generating a sample from the 4d unit sphere.""" + q = np.random.randn(4) * 0.03 + # make unit-length quaternion + q /= np.linalg.norm(q) + qw, qx, qy, qz = q + R = Rot3(qw, qx, qy, qz) + return R + + +def initialize_global_rotations_using_mst(num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3]) -> List[Rot3]: + # Create a graph from the relative rotations dictionary + graph = nx.Graph() + for i1, i2 in i2Ri1_dict.keys(): + # TODO: use inlier count as weight + graph.add_edge(i1, i2, weight=1) + + # Compute the Minimum Spanning Tree (MST) + mst = nx.minimum_spanning_tree(graph) + + wRis = [random_rotation() for _ in range(num_images)] + for i1, i2 in sorted(mst.edges): + if (i1, i2) in i2Ri1_dict: + wRis[i2] = wRis[i1] * i2Ri1_dict[(i1, i2)].inverse() + else: + wRis[i2] = wRis[i1] * i2Ri1_dict[(i2, i1)] + + return wRis + + class ShonanRotationAveraging(RotationAveragingBase): """Performs Shonan rotation averaging.""" @@ -99,7 +131,7 @@ def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float: return between_factors def _run_with_consecutive_ordering( - self, num_connected_nodes: int, between_factors: BetweenFactorPose3s + self, num_connected_nodes: int, between_factors: BetweenFactorPose3s, initial=Optional[Values] ) -> List[Optional[Rot3]]: """Run the rotation averaging on a connected graph w/ N keys ordered consecutively [0,...,N-1]. @@ -125,7 +157,8 @@ def _run_with_consecutive_ordering( ) shonan = ShonanAveraging3(between_factors, self.__get_shonan_params()) - initial = shonan.initializeRandomly() + if initial is None: + initial = shonan.initializeRandomly() logger.info("Initial cost: %.5f", shonan.cost(initial)) result, _ = shonan.run(initial, self._p_min, self._p_max) logger.info("Final cost: %.5f", shonan.cost(result)) @@ -143,10 +176,10 @@ def _nodes_with_edges( """Gets the nodes with edges which are to be modelled as between factors.""" unique_nodes_with_edges = set() - for (i1, i2) in i2Ri1_dict.keys(): + for i1, i2 in i2Ri1_dict.keys(): unique_nodes_with_edges.add(i1) unique_nodes_with_edges.add(i2) - for (i1, i2) in relative_pose_priors.keys(): + for i1, i2 in relative_pose_priors.keys(): unique_nodes_with_edges.add(i1) unique_nodes_with_edges.add(i2) @@ -183,13 +216,21 @@ def run_rotation_averaging( nodes_with_edges = sorted(list(self._nodes_with_edges(i2Ri1_dict, i1Ti2_priors))) old_to_new_idxes = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)} + i2Ri1_dict_ = { + (old_to_new_idxes[edge[0]], old_to_new_idxes[edge[1]]): i2Ri1 for edge, i2Ri1 in i2Ri1_dict.items() + } + wRi_initial_ = initialize_global_rotations_using_mst(len(nodes_with_edges), i2Ri1_dict_) + initial_values = Values() + for i, wRi_initial_ in enumerate(wRi_initial_): + initial_values.insert(i, wRi_initial_) + between_factors: BetweenFactorPose3s = self.__between_factors_from_2view_relative_rotations( i2Ri1_dict, old_to_new_idxes ) between_factors.extend(self._between_factors_from_pose_priors(i1Ti2_priors, old_to_new_idxes)) wRi_list_subset = self._run_with_consecutive_ordering( - num_connected_nodes=len(nodes_with_edges), between_factors=between_factors + num_connected_nodes=len(nodes_with_edges), between_factors=between_factors, initial=initial_values ) wRi_list = [None] * num_images From 1dea8a6bf0b0635cdd6c1c3dc64b15464182db57 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 3 Feb 2024 02:30:05 -0800 Subject: [PATCH 02/27] Move out MST initialization into utility function --- gtsfm/averaging/rotation/shonan.py | 33 ++------------------------ gtsfm/utils/rotation.py | 38 ++++++++++++++++++++++++++++++ tests/utils/test_rotation_utils.py | 28 ++++++++++++++++++++++ 3 files changed, 68 insertions(+), 31 deletions(-) create mode 100644 gtsfm/utils/rotation.py create mode 100644 tests/utils/test_rotation_utils.py diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 2b27d45d9..106826499 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -27,6 +27,7 @@ ) import gtsfm.utils.logger as logger_utils +import gtsfm.utils.rotation as rotation_util from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase from gtsfm.common.pose_prior import PosePrior @@ -37,36 +38,6 @@ _DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1.0 -def random_rotation() -> Rot3: - """Sample a random rotation by generating a sample from the 4d unit sphere.""" - q = np.random.randn(4) * 0.03 - # make unit-length quaternion - q /= np.linalg.norm(q) - qw, qx, qy, qz = q - R = Rot3(qw, qx, qy, qz) - return R - - -def initialize_global_rotations_using_mst(num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3]) -> List[Rot3]: - # Create a graph from the relative rotations dictionary - graph = nx.Graph() - for i1, i2 in i2Ri1_dict.keys(): - # TODO: use inlier count as weight - graph.add_edge(i1, i2, weight=1) - - # Compute the Minimum Spanning Tree (MST) - mst = nx.minimum_spanning_tree(graph) - - wRis = [random_rotation() for _ in range(num_images)] - for i1, i2 in sorted(mst.edges): - if (i1, i2) in i2Ri1_dict: - wRis[i2] = wRis[i1] * i2Ri1_dict[(i1, i2)].inverse() - else: - wRis[i2] = wRis[i1] * i2Ri1_dict[(i2, i1)] - - return wRis - - class ShonanRotationAveraging(RotationAveragingBase): """Performs Shonan rotation averaging.""" @@ -219,7 +190,7 @@ def run_rotation_averaging( i2Ri1_dict_ = { (old_to_new_idxes[edge[0]], old_to_new_idxes[edge[1]]): i2Ri1 for edge, i2Ri1 in i2Ri1_dict.items() } - wRi_initial_ = initialize_global_rotations_using_mst(len(nodes_with_edges), i2Ri1_dict_) + wRi_initial_ = rotation_util.initialize_global_rotations_using_mst(len(nodes_with_edges), i2Ri1_dict_) initial_values = Values() for i, wRi_initial_ in enumerate(wRi_initial_): initial_values.insert(i, wRi_initial_) diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py new file mode 100644 index 000000000..c4ec0d45f --- /dev/null +++ b/gtsfm/utils/rotation.py @@ -0,0 +1,38 @@ +"""Utility functions for rotations. + +Authors: Ayush Baid +""" +from typing import Dict, List, Tuple + +import networkx as nx +import numpy as np +from gtsam import Rot3 + + +def random_rotation() -> Rot3: + """Sample a random rotation by generating a sample from the 4d unit sphere.""" + q = np.random.randn(4) * 0.03 + # make unit-length quaternion + q /= np.linalg.norm(q) + qw, qx, qy, qz = q + return Rot3(qw, qx, qy, qz) + + +def initialize_global_rotations_using_mst(num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3]) -> List[Rot3]: + # Create a graph from the relative rotations dictionary + graph = nx.Graph() + for i1, i2 in i2Ri1_dict.keys(): + # TODO: use inlier count as weight + graph.add_edge(i1, i2, weight=1) + + # Compute the Minimum Spanning Tree (MST) + mst = nx.minimum_spanning_tree(graph) + + wRis = [random_rotation() for _ in range(num_images)] + for i1, i2 in sorted(mst.edges): + if (i1, i2) in i2Ri1_dict: + wRis[i2] = wRis[i1] * i2Ri1_dict[(i1, i2)].inverse() + else: + wRis[i2] = wRis[i1] * i2Ri1_dict[(i2, i1)] + + return wRis diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py new file mode 100644 index 000000000..cdeaf8e30 --- /dev/null +++ b/tests/utils/test_rotation_utils.py @@ -0,0 +1,28 @@ +"""Unit tests for rotation utils. + +Authors: Ayush Baid +""" +import unittest + +import gtsfm.utils.geometry_comparisons as geometry_comparisons +import gtsfm.utils.rotation as rotation_util +import tests.data.sample_poses as sample_poses + +ROTATION_ANGLE_ERROR_THRESHOLD_DEG = 2 + + +class TestRotationUtil(unittest.TestCase): + def test_mst_initialization(self): + """Test for 4 poses in a circle, with a pose connected all others.""" + i2Ri1_dict, wRi_expected = sample_poses.convert_data_for_rotation_averaging( + sample_poses.CIRCLE_ALL_EDGES_GLOBAL_POSES, sample_poses.CIRCLE_ALL_EDGES_RELATIVE_POSES + ) + + wRi_computed = rotation_util.initialize_global_rotations_using_mst(len(wRi_expected), i2Ri1_dict) + self.assertTrue( + geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) + ) + + +if __name__ == "__main__": + unittest.main() From 1a1de1f71f90c6d7d6cf85c19b3eaf071c3464f6 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 19 Feb 2024 20:53:14 -0800 Subject: [PATCH 03/27] Add typinh and docstring --- gtsfm/averaging/rotation/shonan.py | 2 +- gtsfm/utils/rotation.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 106826499..1b448b949 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -102,7 +102,7 @@ def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float: return between_factors def _run_with_consecutive_ordering( - self, num_connected_nodes: int, between_factors: BetweenFactorPose3s, initial=Optional[Values] + self, num_connected_nodes: int, between_factors: BetweenFactorPose3s, initial: Optional[Values] ) -> List[Optional[Rot3]]: """Run the rotation averaging on a connected graph w/ N keys ordered consecutively [0,...,N-1]. diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index c4ec0d45f..9869e0273 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -19,6 +19,7 @@ def random_rotation() -> Rot3: def initialize_global_rotations_using_mst(num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3]) -> List[Rot3]: + num_images: Number of images in the scene. # Create a graph from the relative rotations dictionary graph = nx.Graph() for i1, i2 in i2Ri1_dict.keys(): From d5302838ef767ac7f75dfd2fbbb86e34e5c2010d Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 26 Feb 2024 22:27:28 -0800 Subject: [PATCH 04/27] Use number of inliers as edge weights for MST --- .../rotation/rotation_averaging_base.py | 21 ++++++--- gtsfm/averaging/rotation/shonan.py | 19 ++++++-- gtsfm/multi_view_optimizer.py | 8 +++- gtsfm/utils/rotation.py | 10 +++-- tests/averaging/rotation/test_shonan.py | 43 ++++++++++++++++--- tests/utils/test_rotation_utils.py | 8 +++- 6 files changed, 89 insertions(+), 20 deletions(-) diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index feb1b3597..373c44361 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -14,6 +14,7 @@ import gtsfm.utils.alignment as alignment_utils import gtsfm.utils.metrics as metric_utils from gtsfm.common.pose_prior import PosePrior +from gtsfm.common.two_view_estimation_report import TwoViewEstimationReport from gtsfm.evaluation.metrics import GtsfmMetric, GtsfmMetricsGroup from gtsfm.ui.gtsfm_process import GTSFMProcess, UiMetadata @@ -42,13 +43,15 @@ def run_rotation_averaging( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + two_view_estimation_reports: Dict[Tuple[int, int], TwoViewEstimationReport], ) -> List[Optional[Rot3]]: """Run the rotation averaging. Args: num_images: number of poses. - i2Ri1_dict: relative rotations as dictionary (i1, i2): i2Ri1. + i2Ri1_dict: dictionary of two view rotation information (containing i2Ri1), keyed by (i1, i2). i1Ti2_priors: priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. + two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. Returns: Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is @@ -61,14 +64,16 @@ def _run_rotation_averaging_base( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + two_view_estimation_reports: Dict[Tuple[int, int], TwoViewEstimationReport], wTi_gt: List[Optional[Pose3]], ) -> Tuple[List[Optional[Rot3]], GtsfmMetricsGroup]: """Runs rotation averaging and computes metrics. Args: num_images: Number of poses. - i2Ri1_dict: Relative rotations as dictionary (i1, i2): i2Ri1. + i2Ri1_dict: dictionary of two view rotation information (containing i2Ri1), keyed by (i1, i2). i1Ti2_priors: Priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. + two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. wTi_gt: Ground truth global rotations to compare against. Returns: @@ -78,7 +83,7 @@ def _run_rotation_averaging_base( Metrics on global rotations. """ start_time = time.time() - wRis = self.run_rotation_averaging(num_images, i2Ri1_dict, i1Ti2_priors) + wRis = self.run_rotation_averaging(num_images, i2Ri1_dict, i1Ti2_priors, two_view_estimation_reports) run_time = time.time() - start_time metrics = self.evaluate(wRis, wTi_gt) @@ -116,14 +121,16 @@ def create_computation_graph( num_images: int, i2Ri1_graph: Delayed, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + two_view_estimation_reports: Dict[Tuple[int, int], TwoViewEstimationReport], gt_wTi_list: List[Optional[Pose3]], ) -> Tuple[Delayed, Delayed]: """Create the computation graph for performing rotation averaging. Args: num_images: number of poses. - i2Ri1_graph: dictionary of relative rotations as a delayed task. + i2Ri1_graph: dictionary of relative rotation info as a delayed task. i1Ti2_priors: priors on relative poses as (i1, i2): PosePrior on i1Ti2. + two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. gt_wTi_list: ground truth poses, to be used for evaluation. Returns: @@ -131,7 +138,11 @@ def create_computation_graph( """ wRis, metrics = dask.delayed(self._run_rotation_averaging_base, nout=2)( - num_images, i2Ri1_dict=i2Ri1_graph, i1Ti2_priors=i1Ti2_priors, wTi_gt=gt_wTi_list + num_images, + i2Ri1_dict=i2Ri1_graph, + i1Ti2_priors=i1Ti2_priors, + wTi_gt=gt_wTi_list, + two_view_estimation_reports=two_view_estimation_reports, ) return wRis, metrics diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 1b448b949..3fbb56d15 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -10,10 +10,10 @@ Authors: Jing Wu, Ayush Baid, John Lambert """ + from typing import Dict, List, Optional, Set, Tuple import gtsam -import networkx as nx import numpy as np from gtsam import ( BetweenFactorPose3, @@ -30,6 +30,7 @@ import gtsfm.utils.rotation as rotation_util from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase from gtsfm.common.pose_prior import PosePrior +from gtsfm.common.two_view_estimation_report import TwoViewEstimationReport POSE3_DOF = 6 @@ -49,6 +50,7 @@ def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_S Args: two_view_rotation_sigma: Covariance to use (lower values -> more strictly adhere to input measurements). """ + super().__init__() self._two_view_rotation_sigma = two_view_rotation_sigma self._p_min = 5 self._p_max = 64 @@ -161,6 +163,7 @@ def run_rotation_averaging( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + two_view_estimation_reports: Dict[Tuple[int, int], TwoViewEstimationReport], ) -> List[Optional[Rot3]]: """Run the rotation averaging on a connected graph with arbitrary keys, where each key is a image/pose index. @@ -170,8 +173,9 @@ def run_rotation_averaging( Args: num_images: Number of images. Since we have one pose per image, it is also the number of poses. - i2Ri1_dict: Relative rotations for each image pair-edge as dictionary (i1, i2): i2Ri1. + i2Ri1_dict: Relative rotations for each image pair-edge as dictionaryy (i1, i2): i2Ri1. i1Ti2_priors: Priors on relative poses. + two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. Returns: Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is @@ -190,7 +194,16 @@ def run_rotation_averaging( i2Ri1_dict_ = { (old_to_new_idxes[edge[0]], old_to_new_idxes[edge[1]]): i2Ri1 for edge, i2Ri1 in i2Ri1_dict.items() } - wRi_initial_ = rotation_util.initialize_global_rotations_using_mst(len(nodes_with_edges), i2Ri1_dict_) + num_correspondences_dict: Dict[Tuple[int, int], int] = { + (old_to_new_idxes[edge[0]], old_to_new_idxes[edge[1]]): int(report.num_inliers_est_model) + for edge, report in two_view_estimation_reports.items() + if edge in i2Ri1_dict + } + wRi_initial_ = rotation_util.initialize_global_rotations_using_mst( + len(nodes_with_edges), + i2Ri1_dict_, + edge_weights={(i1, i2): min(num_correspondences_dict.get((i1, i2), 0), 1) for i1, i2 in i2Ri1_dict.keys()}, + ) initial_values = Values() for i, wRi_initial_ in enumerate(wRi_initial_): initial_values.insert(i, wRi_initial_) diff --git a/gtsfm/multi_view_optimizer.py b/gtsfm/multi_view_optimizer.py index e6c557bac..c76ebb699 100644 --- a/gtsfm/multi_view_optimizer.py +++ b/gtsfm/multi_view_optimizer.py @@ -63,7 +63,7 @@ def create_computation_graph( all_intrinsics: List[Optional[gtsfm_types.CALIBRATION_TYPE]], absolute_pose_priors: List[Optional[PosePrior]], relative_pose_priors: Dict[Tuple[int, int], PosePrior], - two_view_reports_dict: Optional[Dict[Tuple[int, int], TwoViewEstimationReport]], + two_view_reports_dict: Dict[Tuple[int, int], TwoViewEstimationReport], cameras_gt: List[Optional[gtsfm_types.CAMERA_TYPE]], gt_wTi_list: List[Optional[Pose3]], output_root: Optional[Path] = None, @@ -126,7 +126,11 @@ def create_computation_graph( viewgraph_i2Ri1_graph, viewgraph_i2Ui1_graph, relative_pose_priors ) delayed_wRi, rot_avg_metrics = self.rot_avg_module.create_computation_graph( - num_images, pruned_i2Ri1_graph, i1Ti2_priors=relative_pose_priors, gt_wTi_list=gt_wTi_list + num_images, + pruned_i2Ri1_graph, + i1Ti2_priors=relative_pose_priors, + two_view_estimation_reports=viewgraph_two_view_reports_graph, + gt_wTi_list=gt_wTi_list, ) tracks2d_graph = dask.delayed(get_2d_tracks)(viewgraph_v_corr_idxs_graph, keypoints_list) diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index 9869e0273..42251142d 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -2,6 +2,7 @@ Authors: Ayush Baid """ + from typing import Dict, List, Tuple import networkx as nx @@ -18,13 +19,14 @@ def random_rotation() -> Rot3: return Rot3(qw, qx, qy, qz) -def initialize_global_rotations_using_mst(num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3]) -> List[Rot3]: - num_images: Number of images in the scene. +def initialize_global_rotations_using_mst( + num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3], edge_weights: Dict[Tuple[int, int], int] +) -> List[Rot3]: + """Initialize rotations using minimum spanning tree (weighted by number of correspondences)""" # Create a graph from the relative rotations dictionary graph = nx.Graph() for i1, i2 in i2Ri1_dict.keys(): - # TODO: use inlier count as weight - graph.add_edge(i1, i2, weight=1) + graph.add_edge(i1, i2, weight=edge_weights.get((i1, i2), 0)) # Compute the Minimum Spanning Tree (MST) mst = nx.minimum_spanning_tree(graph) diff --git a/tests/averaging/rotation/test_shonan.py b/tests/averaging/rotation/test_shonan.py index b0aef22a4..ee73c149b 100644 --- a/tests/averaging/rotation/test_shonan.py +++ b/tests/averaging/rotation/test_shonan.py @@ -2,9 +2,11 @@ Authors: Ayush Baid, John Lambert """ + import pickle import unittest from typing import Dict, List, Tuple +import random import dask import numpy as np @@ -15,6 +17,7 @@ from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase from gtsfm.averaging.rotation.shonan import ShonanRotationAveraging from gtsfm.common.pose_prior import PosePrior, PosePriorType +from gtsfm.common.two_view_estimation_report import TwoViewEstimationReport ROTATION_ANGLE_ERROR_THRESHOLD_DEG = 2 @@ -38,7 +41,13 @@ def __execute_test(self, i2Ri1_input: Dict[Tuple[int, int], Rot3], wRi_expected: wRi_expected: expected global rotations. """ i1Ti2_priors: Dict[Tuple[int, int], PosePrior] = {} - wRi_computed = self.obj.run_rotation_averaging(len(wRi_expected), i2Ri1_input, i1Ti2_priors) + two_view_estimation_reports = { + (i1, i2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=random.randint(0, 100)) + for i1, i2 in i2Ri1_input.keys() + } + wRi_computed = self.obj.run_rotation_averaging( + len(wRi_expected), i2Ri1_input, i1Ti2_priors, two_view_estimation_reports + ) self.assertTrue( geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) ) @@ -104,7 +113,14 @@ def test_simple_with_prior(self): ) } - wRi_computed = self.obj.run_rotation_averaging(len(expected_wRi_list), i2Ri1_dict, i1Ti2_priors) + two_view_estimation_reports = { + (1, 0): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=1), + (0, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=1), + } + + wRi_computed = self.obj.run_rotation_averaging( + len(expected_wRi_list), i2Ri1_dict, i1Ti2_priors, two_view_estimation_reports + ) self.assertTrue( geometry_comparisons.compare_rotations(wRi_computed, expected_wRi_list, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) ) @@ -118,16 +134,24 @@ def test_computation_graph(self): (0, 1): Rot3.RzRyRx(0, np.deg2rad(30), 0), (1, 2): Rot3.RzRyRx(0, 0, np.deg2rad(20)), } + two_view_estimation_reports = { + (0, 1): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=200), + (1, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=500), + } i2Ri1_graph = dask.delayed(i2Ri1_dict) # use the GTSAM API directly (without dask) for rotation averaging i1Ti2_priors: Dict[Tuple[int, int], PosePrior] = {} - expected_wRi_list = self.obj.run_rotation_averaging(num_poses, i2Ri1_dict, i1Ti2_priors) + expected_wRi_list = self.obj.run_rotation_averaging( + num_poses, i2Ri1_dict, i1Ti2_priors, two_view_estimation_reports + ) # use dask's computation graph gt_wTi_list = [None] * len(expected_wRi_list) - rotations_graph, _ = self.obj.create_computation_graph(num_poses, i2Ri1_graph, i1Ti2_priors, gt_wTi_list) + rotations_graph, _ = self.obj.create_computation_graph( + num_poses, i2Ri1_graph, i1Ti2_priors, two_view_estimation_reports, gt_wTi_list + ) with dask.config.set(scheduler="single-threaded"): wRi_list = dask.compute(rotations_graph)[0] @@ -167,8 +191,17 @@ def test_nonconsecutive_indices(self): (1, 3): wTi3.between(wTi1).rotation(), } + # Keys do not overlap with i2Ri1_dict + two_view_estimation_reports = { + (1, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=200), + (1, 3): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=500), + (0, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=0), + } + relative_pose_priors: Dict[Tuple[int, int], PosePrior] = {} - wRi_computed = self.obj.run_rotation_averaging(num_images, i2Ri1_input, relative_pose_priors) + wRi_computed = self.obj.run_rotation_averaging( + num_images, i2Ri1_input, relative_pose_priors, two_view_estimation_reports + ) wRi_expected = [None, wTi1.rotation(), wTi2.rotation(), wTi3.rotation()] self.assertTrue( geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, angular_error_threshold_degrees=0.1) diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py index cdeaf8e30..773ae22e8 100644 --- a/tests/utils/test_rotation_utils.py +++ b/tests/utils/test_rotation_utils.py @@ -2,7 +2,9 @@ Authors: Ayush Baid """ + import unittest +import random import gtsfm.utils.geometry_comparisons as geometry_comparisons import gtsfm.utils.rotation as rotation_util @@ -18,7 +20,11 @@ def test_mst_initialization(self): sample_poses.CIRCLE_ALL_EDGES_GLOBAL_POSES, sample_poses.CIRCLE_ALL_EDGES_RELATIVE_POSES ) - wRi_computed = rotation_util.initialize_global_rotations_using_mst(len(wRi_expected), i2Ri1_dict) + wRi_computed = rotation_util.initialize_global_rotations_using_mst( + len(wRi_expected), + i2Ri1_dict, + edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, + ) self.assertTrue( geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) ) From 91c4cc45a7220e4e7cac0bf46f8e5bf4268bfc4c Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 28 Feb 2024 22:35:05 -0800 Subject: [PATCH 05/27] Remove unused import --- tests/utils/test_rotation_utils.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py index 773ae22e8..626a9c00d 100644 --- a/tests/utils/test_rotation_utils.py +++ b/tests/utils/test_rotation_utils.py @@ -4,7 +4,6 @@ """ import unittest -import random import gtsfm.utils.geometry_comparisons as geometry_comparisons import gtsfm.utils.rotation as rotation_util From 63eeccd622e93438e7ea834ed9a6e41062913401 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 28 Feb 2024 23:01:31 -0800 Subject: [PATCH 06/27] Fix bugs and add docstring --- .../rotation/rotation_averaging_base.py | 6 +++--- gtsfm/averaging/rotation/shonan.py | 5 +++-- gtsfm/utils/rotation.py | 17 ++++++++++++++--- 3 files changed, 20 insertions(+), 8 deletions(-) diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index 373c44361..2d6816c1b 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -49,7 +49,7 @@ def run_rotation_averaging( Args: num_images: number of poses. - i2Ri1_dict: dictionary of two view rotation information (containing i2Ri1), keyed by (i1, i2). + i2Ri1_dict: relative rotations as dictionary (i1, i2): i2Ri1. i1Ti2_priors: priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. @@ -71,7 +71,7 @@ def _run_rotation_averaging_base( Args: num_images: Number of poses. - i2Ri1_dict: dictionary of two view rotation information (containing i2Ri1), keyed by (i1, i2). + i2Ri1_dict: relative rotations as dictionary (i1, i2): i2Ri1. i1Ti2_priors: Priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. wTi_gt: Ground truth global rotations to compare against. @@ -128,7 +128,7 @@ def create_computation_graph( Args: num_images: number of poses. - i2Ri1_graph: dictionary of relative rotation info as a delayed task. + i2Ri1_graph: dictionary of relative rotations as a delayed task. i1Ti2_priors: priors on relative poses as (i1, i2): PosePrior on i1Ti2. two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. gt_wTi_list: ground truth poses, to be used for evaluation. diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 3fbb56d15..7fcbd3f77 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -173,7 +173,7 @@ def run_rotation_averaging( Args: num_images: Number of images. Since we have one pose per image, it is also the number of poses. - i2Ri1_dict: Relative rotations for each image pair-edge as dictionaryy (i1, i2): i2Ri1. + i2Ri1_dict: Relative rotations for each image pair-edge as dictionary (i1, i2): i2Ri1. i1Ti2_priors: Priors on relative poses. two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. @@ -199,10 +199,11 @@ def run_rotation_averaging( for edge, report in two_view_estimation_reports.items() if edge in i2Ri1_dict } + # Use negative of the number of correspondences as the edge weight. wRi_initial_ = rotation_util.initialize_global_rotations_using_mst( len(nodes_with_edges), i2Ri1_dict_, - edge_weights={(i1, i2): min(num_correspondences_dict.get((i1, i2), 0), 1) for i1, i2 in i2Ri1_dict.keys()}, + edge_weights={(i1, i2): -num_correspondences_dict.get((i1, i2), 0) for i1, i2 in i2Ri1_dict_.keys()}, ) initial_values = Values() for i, wRi_initial_ in enumerate(wRi_initial_): diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index 42251142d..924ae5da3 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -22,16 +22,27 @@ def random_rotation() -> Rot3: def initialize_global_rotations_using_mst( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3], edge_weights: Dict[Tuple[int, int], int] ) -> List[Rot3]: - """Initialize rotations using minimum spanning tree (weighted by number of correspondences)""" + """Initialize rotations using minimum spanning tree (weighted by number of correspondences) + + Args: + num_images: number of images in the scene. + i2Ri1_dict: dictionary of relative rotations (i1, i2): i2Ri1 + edge_weights: weight of the edges (i1, i2). All edges in i2Ri1 must have an edge weight + + Returns: + Global rotations wRi initialized using an MST. Randomly initialized if we have a forest. + """ # Create a graph from the relative rotations dictionary graph = nx.Graph() for i1, i2 in i2Ri1_dict.keys(): - graph.add_edge(i1, i2, weight=edge_weights.get((i1, i2), 0)) + graph.add_edge(i1, i2, weight=edge_weights[(i1, i2)]) + + assert nx.is_connected(graph), "Relative rotation graph is not connected" # Compute the Minimum Spanning Tree (MST) mst = nx.minimum_spanning_tree(graph) - wRis = [random_rotation() for _ in range(num_images)] + wRis = [Rot3() for _ in range(num_images)] for i1, i2 in sorted(mst.edges): if (i1, i2) in i2Ri1_dict: wRis[i2] = wRis[i1] * i2Ri1_dict[(i1, i2)].inverse() From f5f61d563a63176d163becda3d4fd1492f91317a Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 11 Mar 2024 06:27:16 -0400 Subject: [PATCH 07/27] cleanup docstrings --- gtsfm/utils/rotation.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index 924ae5da3..74506d92b 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -13,7 +13,7 @@ def random_rotation() -> Rot3: """Sample a random rotation by generating a sample from the 4d unit sphere.""" q = np.random.randn(4) * 0.03 - # make unit-length quaternion + # Make unit-length quaternion. q /= np.linalg.norm(q) qw, qx, qy, qz = q return Rot3(qw, qx, qy, qz) @@ -22,17 +22,17 @@ def random_rotation() -> Rot3: def initialize_global_rotations_using_mst( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3], edge_weights: Dict[Tuple[int, int], int] ) -> List[Rot3]: - """Initialize rotations using minimum spanning tree (weighted by number of correspondences) + """Initializes rotations using minimum spanning tree (weighted by number of correspondences). Args: - num_images: number of images in the scene. - i2Ri1_dict: dictionary of relative rotations (i1, i2): i2Ri1 - edge_weights: weight of the edges (i1, i2). All edges in i2Ri1 must have an edge weight + num_images: Number of images in the scene. + i2Ri1_dict: Dictionary of relative rotations (i1, i2): i2Ri1. + edge_weights: Weight of the edges (i1, i2). All edges in i2Ri1 must have an edge weight. Returns: Global rotations wRi initialized using an MST. Randomly initialized if we have a forest. """ - # Create a graph from the relative rotations dictionary + # Create a graph from the relative rotations dictionary. graph = nx.Graph() for i1, i2 in i2Ri1_dict.keys(): graph.add_edge(i1, i2, weight=edge_weights[(i1, i2)]) @@ -44,9 +44,11 @@ def initialize_global_rotations_using_mst( wRis = [Rot3() for _ in range(num_images)] for i1, i2 in sorted(mst.edges): + # NOTE: i1, i2 may not be in sorted order here. May need to reverse ordering. if (i1, i2) in i2Ri1_dict: - wRis[i2] = wRis[i1] * i2Ri1_dict[(i1, i2)].inverse() + i1Ri2 = i2Ri1_dict[(i1, i2)].inverse() else: - wRis[i2] = wRis[i1] * i2Ri1_dict[(i2, i1)] + i1Ri2 = i2Ri1_dict[(i2, i1)] + wRis[i2] = wRis[i1] * i1Ri2 return wRis From 97701426fad5b66bbf29557d77a4e56bb08dd859 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 11 Mar 2024 07:29:23 -0400 Subject: [PATCH 08/27] add more tests --- gtsfm/utils/rotation.py | 68 ++++++++++++++--- tests/utils/test_rotation_utils.py | 117 ++++++++++++++++++++++++++++- 2 files changed, 173 insertions(+), 12 deletions(-) diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index 74506d92b..e6c889e03 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -6,19 +6,9 @@ from typing import Dict, List, Tuple import networkx as nx -import numpy as np from gtsam import Rot3 -def random_rotation() -> Rot3: - """Sample a random rotation by generating a sample from the 4d unit sphere.""" - q = np.random.randn(4) * 0.03 - # Make unit-length quaternion. - q /= np.linalg.norm(q) - qw, qx, qy, qz = q - return Rot3(qw, qx, qy, qz) - - def initialize_global_rotations_using_mst( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3], edge_weights: Dict[Tuple[int, int], int] ) -> List[Rot3]: @@ -37,7 +27,8 @@ def initialize_global_rotations_using_mst( for i1, i2 in i2Ri1_dict.keys(): graph.add_edge(i1, i2, weight=edge_weights[(i1, i2)]) - assert nx.is_connected(graph), "Relative rotation graph is not connected" + if not nx.is_connected(graph): + raise ValueError("Relative rotation graph is not connected") # Compute the Minimum Spanning Tree (MST) mst = nx.minimum_spanning_tree(graph) @@ -52,3 +43,58 @@ def initialize_global_rotations_using_mst( wRis[i2] = wRis[i1] * i1Ri2 return wRis + + + +def initialize_mst( + num_images: int, + i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], + corr_idxs: Dict[Tuple[int, int], np.ndarray], + old_to_new_idxs: Dict[int, int], + ) -> gtsam.Values: + """Initialize global rotations using the minimum spanning tree (MST).""" + # Compute MST. + row, col, data = [], [], [] + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + if i2Ri1 is None: + continue + row.append(i1) + col.append(i2) + data.append(-corr_idxs[(i1, i2)].shape[0]) + logger.info(corr_idxs[(i1, i2)]) + corr_adjacency = scipy.sparse.coo_array((data, (row, col)), shape=(num_images, num_images)) + Tcsr = scipy.sparse.csgraph.minimum_spanning_tree(corr_adjacency) + logger.info(Tcsr.toarray().astype(int)) + + # Build global rotations from MST. + # TODO (travisdriver): This is simple but very inefficient. Use something else. + i_mst, j_mst = Tcsr.nonzero() + logger.info(i_mst) + logger.info(j_mst) + edges_mst = [(i, j) for (i, j) in zip(i_mst, j_mst)] + iR0_dict = {i_mst[0]: np.eye(3)} # pick the left index of the first edge as the seed + # max_iters = num_images * 10 + iter = 0 + while len(edges_mst) > 0: + i, j = edges_mst.pop(0) + if i in iR0_dict: + jRi = i2Ri1_dict[(i, j)].matrix() + iR0 = iR0_dict[i] + iR0_dict[j] = jRi @ iR0 + elif j in iR0_dict: + iRj = i2Ri1_dict[(i, j)].matrix().T + jR0 = iR0_dict[j] + iR0_dict[i] = iRj @ jR0 + else: + edges_mst.append((i, j)) + iter += 1 + # if iter >= max_iters: + # logger.info("Reached max MST iters.") + # assert False + + # Add to Values object. + initial = gtsam.Values() + for i, iR0 in iR0_dict.items(): + initial.insert(old_to_new_idxs[i], Rot3(iR0)) + + return initial \ No newline at end of file diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py index 626a9c00d..1a10d351c 100644 --- a/tests/utils/test_rotation_utils.py +++ b/tests/utils/test_rotation_utils.py @@ -2,8 +2,11 @@ Authors: Ayush Baid """ - import unittest +from typing import Dict, List, Tuple + +import numpy as np +from gtsam import Rot3 import gtsfm.utils.geometry_comparisons as geometry_comparisons import gtsfm.utils.rotation as rotation_util @@ -12,6 +15,89 @@ ROTATION_ANGLE_ERROR_THRESHOLD_DEG = 2 +RELATIVE_ROTATION_DICT = Dict[Tuple[int, int], Rot3] + + +def _get_ordered_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[float]]: + """Return data for a scenario with 5 camera poses, with ordering that follows their connectivity. + + Accordingly, we specify i1 < i2 for all edges (i1,i2). + + Graph topology: + + | 2 | 3 + o-- ... o-- + . . + . . + | | | + o-- ... --o --o + 0 1 4 + + Returns: + Tuple of mapping from image index pair to relative rotations, and expected global rotation angles. + """ + # Expected angles. + wRi_list_euler_deg_expected = [0,90,0,0,90] + + # Ground truth 3d rotations for 5 ordered poses (0,1,2,3,4) + wRi_list_gt = [Rot3.RzRyRx(np.deg2rad(Rz_deg), 0, 0) for Rz_deg in wRi_list_euler_deg_expected] + + edges = [(0, 1), (1, 2), (2, 3), (3, 4)] + i2Ri1_dict = _create_synthetic_relative_pose_measurements(wRi_list_gt, edges=edges) + + + return i2Ri1_dict, wRi_list_euler_deg_expected + + +def _get_mixed_order_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[float]]: + """Return data for a scenario with 5 camera poses, with ordering that does NOT follow their connectivity. + + Below, we do NOT specify i1 < i2 for all edges (i1,i2). + + Graph topology: + + | 3 | 0 + o-- ... o-- + . . + . . + | | | + o-- ... --o --o + 4 1 2 + + """ + # Expected angles. + wRi_list_euler_deg_expected = [0,90,90,0,0] + + # Ground truth 2d rotations for 5 ordered poses (0,1,2,3,4) + wRi_list_gt = [Rot3.RzRyRx(np.deg2rad(Rz_deg), 0, 0) for Rz_deg in wRi_list_euler_deg_expected] + + edges = [(1, 4), (1, 3), (0, 3), (0, 2)] + i2Ri1_dict = _create_synthetic_relative_pose_measurements(wRi_list_gt=wRi_list_gt, edges=edges) + + return i2Ri1_dict, wRi_list_euler_deg_expected + + +def _create_synthetic_relative_pose_measurements( + wRi_list_gt: List[Rot3], edges: List[Tuple[int, int]] +) -> Dict[Tuple[int, int], Rot3]: + """Generate synthetic relative rotation measurements, from ground truth global rotations. + + Args: + wRi_list_gt: List of (3,3) rotation matrices. + edges: Edges as pairs of image indices. + + Returns: + i2Ri1_dict: Relative rotation measurements. + """ + i2Ri1_dict = {} + for i1, i2 in edges: + wRi2 = wRi_list_gt[i2] + wRi1 = wRi_list_gt[i1] + i2Ri1_dict[(i1, i2)] = wRi2.inverse() * wRi1 + + return i2Ri1_dict + + class TestRotationUtil(unittest.TestCase): def test_mst_initialization(self): """Test for 4 poses in a circle, with a pose connected all others.""" @@ -28,6 +114,35 @@ def test_mst_initialization(self): geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) ) + def test_greedily_construct_st_ordered_chain(self) -> None: + """Ensures that we can greedily construct a Spanning Tree for an ordered chain.""" + + i2Ri1_dict, wRi_list_euler_deg_expected = _get_ordered_chain_pose_data() + + num_images = 5 + wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( + num_images, + i2Ri1_dict, + edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, + ) + + wRi_list_euler_deg_est = [np.rad2deg(wRi.roll()) for wRi in wRi_list_computed] + assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) + + def test_greedily_construct_st_mixed_order_chain(self) -> None: + """Ensures that we can greedily construct a Spanning Tree for an unordered chain.""" + i2Ri1_dict, wRi_list_euler_deg_expected = _get_mixed_order_chain_pose_data() + + num_images = 5 + wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( + num_images, + i2Ri1_dict, + edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, + ) + + wRi_list_euler_deg_est = [np.rad2deg(wRi.roll()) for wRi in wRi_list_computed] + assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) + if __name__ == "__main__": unittest.main() From c3a658925ee3cb33f121721c3c1c18fdb74935da Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 11 Mar 2024 08:24:54 -0400 Subject: [PATCH 09/27] add different impl --- gtsfm/utils/rotation.py | 156 +++++++++++++++++------------ tests/utils/test_rotation_utils.py | 75 +++++++++----- 2 files changed, 144 insertions(+), 87 deletions(-) diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index e6c889e03..c4092c956 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -33,68 +33,98 @@ def initialize_global_rotations_using_mst( # Compute the Minimum Spanning Tree (MST) mst = nx.minimum_spanning_tree(graph) - wRis = [Rot3() for _ in range(num_images)] - for i1, i2 in sorted(mst.edges): - # NOTE: i1, i2 may not be in sorted order here. May need to reverse ordering. - if (i1, i2) in i2Ri1_dict: - i1Ri2 = i2Ri1_dict[(i1, i2)].inverse() - else: - i1Ri2 = i2Ri1_dict[(i2, i1)] - wRis[i2] = wRis[i1] * i1Ri2 - - return wRis - - - -def initialize_mst( - num_images: int, - i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], - corr_idxs: Dict[Tuple[int, int], np.ndarray], - old_to_new_idxs: Dict[int, int], - ) -> gtsam.Values: - """Initialize global rotations using the minimum spanning tree (MST).""" - # Compute MST. - row, col, data = [], [], [] - for (i1, i2), i2Ri1 in i2Ri1_dict.items(): - if i2Ri1 is None: - continue - row.append(i1) - col.append(i2) - data.append(-corr_idxs[(i1, i2)].shape[0]) - logger.info(corr_idxs[(i1, i2)]) - corr_adjacency = scipy.sparse.coo_array((data, (row, col)), shape=(num_images, num_images)) - Tcsr = scipy.sparse.csgraph.minimum_spanning_tree(corr_adjacency) - logger.info(Tcsr.toarray().astype(int)) - - # Build global rotations from MST. - # TODO (travisdriver): This is simple but very inefficient. Use something else. - i_mst, j_mst = Tcsr.nonzero() - logger.info(i_mst) - logger.info(j_mst) - edges_mst = [(i, j) for (i, j) in zip(i_mst, j_mst)] - iR0_dict = {i_mst[0]: np.eye(3)} # pick the left index of the first edge as the seed - # max_iters = num_images * 10 - iter = 0 - while len(edges_mst) > 0: - i, j = edges_mst.pop(0) - if i in iR0_dict: - jRi = i2Ri1_dict[(i, j)].matrix() - iR0 = iR0_dict[i] - iR0_dict[j] = jRi @ iR0 - elif j in iR0_dict: - iRj = i2Ri1_dict[(i, j)].matrix().T - jR0 = iR0_dict[j] - iR0_dict[i] = iRj @ jR0 - else: - edges_mst.append((i, j)) - iter += 1 - # if iter >= max_iters: - # logger.info("Reached max MST iters.") - # assert False + # MST graph. + G = nx.Graph() + G.add_edges_from(mst.edges) + + wRi_list = [None] * num_images + # Choose origin node. + origin_node = list(G.nodes)[0] + wRi_list[origin_node] = Rot3() + + # Ignore 0th node, as we already set its global pose as the origin + for dst_node in list(G.nodes)[1:]: + + # Determine the path to this node from the origin. ordered from [origin_node,...,dst_node] + path = nx.shortest_path(G, source=origin_node, target=dst_node) + + wRi = Rot3() + for (i1, i2) in zip(path[:-1], path[1:]): + + # NOTE: i1, i2 may not be in sorted order here. May need to reverse ordering. + if i1 < i2: + i1Ri2 = i2Ri1_dict[(i1, i2)].inverse() + else: + i1Ri2 = i2Ri1_dict[(i2, i1)] + + # wRi = wR0 * 0R1 + wRi = wRi * i1Ri2 + + wRi_list[dst_node] = wRi + + return wRi_list + + + +# def initialize_mst( +# num_images: int, +# i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], +# corr_idxs: Dict[Tuple[int, int], np.ndarray], +# old_to_new_idxs: Dict[int, int], +# ) -> gtsam.Values: +# """Initialize global rotations using the minimum spanning tree (MST). + +# Args: +# num_images: Number of images in the scene. +# i2Ri1_dict: Dictionary of relative rotations (i1, i2): i2Ri1. +# corr_idxs: +# old_to_new_idxs: + +# Returns: +# Initialization of global rotations for Values. +# """ +# # Compute MST. +# row, col, data = [], [], [] +# for (i1, i2), i2Ri1 in i2Ri1_dict.items(): +# if i2Ri1 is None: +# continue +# row.append(i1) +# col.append(i2) +# data.append(-corr_idxs[(i1, i2)].shape[0]) +# logger.info(corr_idxs[(i1, i2)]) +# corr_adjacency = scipy.sparse.coo_array((data, (row, col)), shape=(num_images, num_images)) +# Tcsr = scipy.sparse.csgraph.minimum_spanning_tree(corr_adjacency) +# logger.info(Tcsr.toarray().astype(int)) + +# # Build global rotations from MST. +# # TODO (travisdriver): This is simple but very inefficient. Use something else. +# i_mst, j_mst = Tcsr.nonzero() +# logger.info(i_mst) +# logger.info(j_mst) +# edges_mst = [(i, j) for (i, j) in zip(i_mst, j_mst)] +# iR0_dict = {i_mst[0]: np.eye(3)} # pick the left index of the first edge as the seed +# # max_iters = num_images * 10 +# iter = 0 +# while len(edges_mst) > 0: +# i, j = edges_mst.pop(0) +# if i in iR0_dict: +# jRi = i2Ri1_dict[(i, j)].matrix() +# iR0 = iR0_dict[i] +# iR0_dict[j] = jRi @ iR0 +# elif j in iR0_dict: +# iRj = i2Ri1_dict[(i, j)].matrix().T +# jR0 = iR0_dict[j] +# iR0_dict[i] = iRj @ jR0 +# else: +# edges_mst.append((i, j)) +# iter += 1 +# # if iter >= max_iters: +# # logger.info("Reached max MST iters.") +# # assert False - # Add to Values object. - initial = gtsam.Values() - for i, iR0 in iR0_dict.items(): - initial.insert(old_to_new_idxs[i], Rot3(iR0)) +# # Add to Values object. +# initial = gtsam.Values() +# for i, iR0 in iR0_dict.items(): +# initial.insert(old_to_new_idxs[i], Rot3(iR0)) - return initial \ No newline at end of file +# return initial \ No newline at end of file diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py index 1a10d351c..3cc685891 100644 --- a/tests/utils/test_rotation_utils.py +++ b/tests/utils/test_rotation_utils.py @@ -37,7 +37,7 @@ def _get_ordered_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[float]] Tuple of mapping from image index pair to relative rotations, and expected global rotation angles. """ # Expected angles. - wRi_list_euler_deg_expected = [0,90,0,0,90] + wRi_list_euler_deg_expected = np.array([0,90,0,0,90]) # Ground truth 3d rotations for 5 ordered poses (0,1,2,3,4) wRi_list_gt = [Rot3.RzRyRx(np.deg2rad(Rz_deg), 0, 0) for Rz_deg in wRi_list_euler_deg_expected] @@ -66,7 +66,7 @@ def _get_mixed_order_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[flo """ # Expected angles. - wRi_list_euler_deg_expected = [0,90,90,0,0] + wRi_list_euler_deg_expected = np.array([0,90,90,0,0]) # Ground truth 2d rotations for 5 ordered poses (0,1,2,3,4) wRi_list_gt = [Rot3.RzRyRx(np.deg2rad(Rz_deg), 0, 0) for Rz_deg in wRi_list_euler_deg_expected] @@ -98,6 +98,32 @@ def _create_synthetic_relative_pose_measurements( return i2Ri1_dict +# def _wrap_angles(angles: np.ndarray, period: float = 180) -> np.ndarray: +# """Map angles (in radians) from domain [-∞, ∞] to [0, π). This function is +# the inverse of `np.unwrap`. + +# Args: + +# Returns: +# Angles (in radians) mapped to the interval [0, π). +# """ + +# # Map angles to [0, ∞]. +# angles = np.abs(angles) + +# # Calculate floor division and remainder simultaneously. +# divs, mods = np.divmod(angles, period) + +# # Select angles which exceed specified period. +# angle_complement_mask = np.nonzero(divs) + +# # Take set complement of `mods` w.r.t. the set [0, π]. +# # `mods` must be nonzero, thus the image is the interval [0, π). +# angles[angle_complement_mask] = period - mods[angle_complement_mask] +# return angles + + + class TestRotationUtil(unittest.TestCase): def test_mst_initialization(self): """Test for 4 poses in a circle, with a pose connected all others.""" @@ -114,34 +140,35 @@ def test_mst_initialization(self): geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) ) - def test_greedily_construct_st_ordered_chain(self) -> None: - """Ensures that we can greedily construct a Spanning Tree for an ordered chain.""" + # def test_greedily_construct_st_ordered_chain(self) -> None: + # """Ensures that we can greedily construct a Spanning Tree for an ordered chain.""" - i2Ri1_dict, wRi_list_euler_deg_expected = _get_ordered_chain_pose_data() + # i2Ri1_dict, wRi_list_euler_deg_expected = _get_ordered_chain_pose_data() - num_images = 5 - wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( - num_images, - i2Ri1_dict, - edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, - ) + # num_images = 5 + # wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( + # num_images, + # i2Ri1_dict, + # edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, + # ) - wRi_list_euler_deg_est = [np.rad2deg(wRi.roll()) for wRi in wRi_list_computed] - assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) + # wRi_list_euler_deg_est = [np.rad2deg(wRi.roll()) for wRi in wRi_list_computed] + # assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) - def test_greedily_construct_st_mixed_order_chain(self) -> None: - """Ensures that we can greedily construct a Spanning Tree for an unordered chain.""" - i2Ri1_dict, wRi_list_euler_deg_expected = _get_mixed_order_chain_pose_data() + # def test_greedily_construct_st_mixed_order_chain(self) -> None: + # """Ensures that we can greedily construct a Spanning Tree for an unordered chain.""" + # i2Ri1_dict, wRi_list_euler_deg_expected = _get_mixed_order_chain_pose_data() - num_images = 5 - wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( - num_images, - i2Ri1_dict, - edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, - ) + # num_images = 5 + # wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( + # num_images, + # i2Ri1_dict, + # edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, + # ) - wRi_list_euler_deg_est = [np.rad2deg(wRi.roll()) for wRi in wRi_list_computed] - assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) + # wRi_list_euler_deg_est = np.array([np.rad2deg(wRi.roll()) for wRi in wRi_list_computed]) + # import pdb; pdb.set_trace() + # assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) if __name__ == "__main__": From 8e0bafe5f5c86a01e3d5008721e20f100f19dc30 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 11 Mar 2024 16:22:17 -0400 Subject: [PATCH 10/27] clean up notation --- gtsfm/utils/geometry_comparisons.py | 3 ++- gtsfm/utils/rotation.py | 11 +++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsfm/utils/geometry_comparisons.py b/gtsfm/utils/geometry_comparisons.py index bdc72cea2..1dddccb03 100644 --- a/gtsfm/utils/geometry_comparisons.py +++ b/gtsfm/utils/geometry_comparisons.py @@ -30,7 +30,8 @@ def compare_rotations( Args: aTi_list: 1st list of rotations. bTi_list: 2nd list of rotations. - angular_error_threshold_degrees: the threshold for angular error between two rotations. + angular_error_threshold_degrees: Threshold for angular error between two rotations. + Returns: Result of the comparison. """ diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index c4092c956..ed6786e9d 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -48,19 +48,18 @@ def initialize_global_rotations_using_mst( # Determine the path to this node from the origin. ordered from [origin_node,...,dst_node] path = nx.shortest_path(G, source=origin_node, target=dst_node) - wRi = Rot3() + # Chain relative rotations w.r.t. origin node. Initialize as identity Rot3 w.r.t origin node `i1`. + wRi1 = Rot3() for (i1, i2) in zip(path[:-1], path[1:]): - # NOTE: i1, i2 may not be in sorted order here. May need to reverse ordering. if i1 < i2: i1Ri2 = i2Ri1_dict[(i1, i2)].inverse() else: i1Ri2 = i2Ri1_dict[(i2, i1)] + # Path order is (origin -> ... -> i1 -> i2 -> ... -> dst_node). Set `i2` to be new `i1`. + wRi1 = wRi1 * i1Ri2 - # wRi = wR0 * 0R1 - wRi = wRi * i1Ri2 - - wRi_list[dst_node] = wRi + wRi_list[dst_node] = wRi1 return wRi_list From ebfff908a5137bae440fb80110ecf1aa5b39aae8 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 11 Mar 2024 16:22:32 -0400 Subject: [PATCH 11/27] clean up test --- tests/utils/test_rotation_utils.py | 85 ++++++++++++++---------------- 1 file changed, 40 insertions(+), 45 deletions(-) diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py index 3cc685891..e140fcd36 100644 --- a/tests/utils/test_rotation_utils.py +++ b/tests/utils/test_rotation_utils.py @@ -87,7 +87,7 @@ def _create_synthetic_relative_pose_measurements( edges: Edges as pairs of image indices. Returns: - i2Ri1_dict: Relative rotation measurements. + Relative rotation measurements. """ i2Ri1_dict = {} for i1, i2 in edges: @@ -98,30 +98,21 @@ def _create_synthetic_relative_pose_measurements( return i2Ri1_dict -# def _wrap_angles(angles: np.ndarray, period: float = 180) -> np.ndarray: -# """Map angles (in radians) from domain [-∞, ∞] to [0, π). This function is -# the inverse of `np.unwrap`. +def _wrap_angles(angles: np.ndarray) -> np.ndarray: + """Map angle (in degrees) from domain [-\infty, \infty] to [0, 360). -# Args: - -# Returns: -# Angles (in radians) mapped to the interval [0, π). -# """ - -# # Map angles to [0, ∞]. -# angles = np.abs(angles) - -# # Calculate floor division and remainder simultaneously. -# divs, mods = np.divmod(angles, period) - -# # Select angles which exceed specified period. -# angle_complement_mask = np.nonzero(divs) + Args: + angles: Array of shape (N,) representing angles (in degrees) in any interval. -# # Take set complement of `mods` w.r.t. the set [0, π]. -# # `mods` must be nonzero, thus the image is the interval [0, π). -# angles[angle_complement_mask] = period - mods[angle_complement_mask] -# return angles + Returns: + Array of shape (N,) representing the angles (in degrees) mapped to the interval [0, 360]. + """ + # Reduce the angle + angles = angles % 360 + # Force it to be the positive remainder, so that 0 <= angle < 360 + angles = (angles + 360) % 360 + return angles class TestRotationUtil(unittest.TestCase): @@ -140,35 +131,39 @@ def test_mst_initialization(self): geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) ) - # def test_greedily_construct_st_ordered_chain(self) -> None: - # """Ensures that we can greedily construct a Spanning Tree for an ordered chain.""" + def test_greedily_construct_st_ordered_chain(self) -> None: + """Ensures that we can greedily construct a Spanning Tree for an ordered chain.""" - # i2Ri1_dict, wRi_list_euler_deg_expected = _get_ordered_chain_pose_data() + i2Ri1_dict, wRi_list_euler_deg_expected = _get_ordered_chain_pose_data() - # num_images = 5 - # wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( - # num_images, - # i2Ri1_dict, - # edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, - # ) + num_images = 5 + wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( + num_images, + i2Ri1_dict, + edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, + ) + + wRi_list_euler_deg_est = [np.rad2deg(wRi.roll()) for wRi in wRi_list_computed] + assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) + + def test_greedily_construct_st_mixed_order_chain(self) -> None: + """Ensures that we can greedily construct a Spanning Tree for an unordered chain.""" + i2Ri1_dict, wRi_list_euler_deg_expected = _get_mixed_order_chain_pose_data() - # wRi_list_euler_deg_est = [np.rad2deg(wRi.roll()) for wRi in wRi_list_computed] - # assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) + num_images = 5 + wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( + num_images, + i2Ri1_dict, + edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, + ) - # def test_greedily_construct_st_mixed_order_chain(self) -> None: - # """Ensures that we can greedily construct a Spanning Tree for an unordered chain.""" - # i2Ri1_dict, wRi_list_euler_deg_expected = _get_mixed_order_chain_pose_data() + wRi_list_euler_deg_est = np.array([np.rad2deg(wRi.roll()) for wRi in wRi_list_computed]) - # num_images = 5 - # wRi_list_computed = rotation_util.initialize_global_rotations_using_mst( - # num_images, - # i2Ri1_dict, - # edge_weights={(i1, i2): (i1 + i2) * 100 for i1, i2 in i2Ri1_dict.keys()}, - # ) + # Make sure both lists of angles start at 0 deg. + wRi_list_euler_deg_est -= wRi_list_euler_deg_est[0] + wRi_list_euler_deg_expected -= wRi_list_euler_deg_expected[0] - # wRi_list_euler_deg_est = np.array([np.rad2deg(wRi.roll()) for wRi in wRi_list_computed]) - # import pdb; pdb.set_trace() - # assert np.allclose(wRi_list_euler_deg_est, wRi_list_euler_deg_expected) + assert np.allclose(_wrap_angles(wRi_list_euler_deg_est), _wrap_angles(wRi_list_euler_deg_expected)) if __name__ == "__main__": From e6d200a5ecf1625ddd85de61d2c5d1df71eeab94 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 11 Mar 2024 16:23:24 -0400 Subject: [PATCH 12/27] python black fixes --- gtsfm/utils/rotation.py | 18 ++++++++---------- tests/utils/test_rotation_utils.py | 11 +++++------ 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index ed6786e9d..c816c1093 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -44,13 +44,12 @@ def initialize_global_rotations_using_mst( # Ignore 0th node, as we already set its global pose as the origin for dst_node in list(G.nodes)[1:]: - # Determine the path to this node from the origin. ordered from [origin_node,...,dst_node] path = nx.shortest_path(G, source=origin_node, target=dst_node) # Chain relative rotations w.r.t. origin node. Initialize as identity Rot3 w.r.t origin node `i1`. wRi1 = Rot3() - for (i1, i2) in zip(path[:-1], path[1:]): + for i1, i2 in zip(path[:-1], path[1:]): # NOTE: i1, i2 may not be in sorted order here. May need to reverse ordering. if i1 < i2: i1Ri2 = i2Ri1_dict[(i1, i2)].inverse() @@ -64,10 +63,9 @@ def initialize_global_rotations_using_mst( return wRi_list - # def initialize_mst( -# num_images: int, -# i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], +# num_images: int, +# i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], # corr_idxs: Dict[Tuple[int, int], np.ndarray], # old_to_new_idxs: Dict[int, int], # ) -> gtsam.Values: @@ -76,8 +74,8 @@ def initialize_global_rotations_using_mst( # Args: # num_images: Number of images in the scene. # i2Ri1_dict: Dictionary of relative rotations (i1, i2): i2Ri1. -# corr_idxs: -# old_to_new_idxs: +# corr_idxs: +# old_to_new_idxs: # Returns: # Initialization of global rotations for Values. @@ -120,10 +118,10 @@ def initialize_global_rotations_using_mst( # # if iter >= max_iters: # # logger.info("Reached max MST iters.") # # assert False - + # # Add to Values object. # initial = gtsam.Values() # for i, iR0 in iR0_dict.items(): # initial.insert(old_to_new_idxs[i], Rot3(iR0)) - -# return initial \ No newline at end of file + +# return initial diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py index e140fcd36..d96e911f4 100644 --- a/tests/utils/test_rotation_utils.py +++ b/tests/utils/test_rotation_utils.py @@ -37,7 +37,7 @@ def _get_ordered_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[float]] Tuple of mapping from image index pair to relative rotations, and expected global rotation angles. """ # Expected angles. - wRi_list_euler_deg_expected = np.array([0,90,0,0,90]) + wRi_list_euler_deg_expected = np.array([0, 90, 0, 0, 90]) # Ground truth 3d rotations for 5 ordered poses (0,1,2,3,4) wRi_list_gt = [Rot3.RzRyRx(np.deg2rad(Rz_deg), 0, 0) for Rz_deg in wRi_list_euler_deg_expected] @@ -45,7 +45,6 @@ def _get_ordered_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[float]] edges = [(0, 1), (1, 2), (2, 3), (3, 4)] i2Ri1_dict = _create_synthetic_relative_pose_measurements(wRi_list_gt, edges=edges) - return i2Ri1_dict, wRi_list_euler_deg_expected @@ -66,7 +65,7 @@ def _get_mixed_order_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[flo """ # Expected angles. - wRi_list_euler_deg_expected = np.array([0,90,90,0,0]) + wRi_list_euler_deg_expected = np.array([0, 90, 90, 0, 0]) # Ground truth 2d rotations for 5 ordered poses (0,1,2,3,4) wRi_list_gt = [Rot3.RzRyRx(np.deg2rad(Rz_deg), 0, 0) for Rz_deg in wRi_list_euler_deg_expected] @@ -107,10 +106,10 @@ def _wrap_angles(angles: np.ndarray) -> np.ndarray: Returns: Array of shape (N,) representing the angles (in degrees) mapped to the interval [0, 360]. """ - # Reduce the angle - angles = angles % 360 + # Reduce the angle + angles = angles % 360 - # Force it to be the positive remainder, so that 0 <= angle < 360 + # Force it to be the positive remainder, so that 0 <= angle < 360 angles = (angles + 360) % 360 return angles From 83c354b0489872d470fdcd728fa647d6945ed8dc Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 11 Mar 2024 19:04:14 -0400 Subject: [PATCH 13/27] fix flake8 --- tests/utils/test_rotation_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py index d96e911f4..614888efd 100644 --- a/tests/utils/test_rotation_utils.py +++ b/tests/utils/test_rotation_utils.py @@ -98,7 +98,7 @@ def _create_synthetic_relative_pose_measurements( def _wrap_angles(angles: np.ndarray) -> np.ndarray: - """Map angle (in degrees) from domain [-\infty, \infty] to [0, 360). + r"""Map angle (in degrees) from domain [-∞, ∞] to [0, 360). Args: angles: Array of shape (N,) representing angles (in degrees) in any interval. From c655512d158c0b88d785d81f4e5e91858630e931 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Tue, 12 Mar 2024 00:29:44 -0400 Subject: [PATCH 14/27] use i1 < i2 convention for pair indices --- tests/averaging/rotation/test_shonan.py | 80 +++++++++++++------------ tests/data/sample_poses.py | 6 +- 2 files changed, 45 insertions(+), 41 deletions(-) diff --git a/tests/averaging/rotation/test_shonan.py b/tests/averaging/rotation/test_shonan.py index ee73c149b..bdfe9cc20 100644 --- a/tests/averaging/rotation/test_shonan.py +++ b/tests/averaging/rotation/test_shonan.py @@ -37,8 +37,8 @@ def __execute_test(self, i2Ri1_input: Dict[Tuple[int, int], Rot3], wRi_expected: """Helper function to run the averagaing and assert w/ expected. Args: - i2Ri1_input: relative rotations, which are input to the algorithm. - wRi_expected: expected global rotations. + i2Ri1_input: Relative rotations, which are input to the algorithm. + wRi_expected: Expected global rotations. """ i1Ti2_priors: Dict[Tuple[int, int], PosePrior] = {} two_view_estimation_reports = { @@ -80,50 +80,54 @@ def test_panorama(self): ) self.__execute_test(i2Ri1_dict, wRi_expected) - def test_simple(self): + def test_simple_three_nodes_two_measurements(self): """Test a simple case with three relative rotations.""" + i0Ri1 = Rot3.RzRyRx(0, np.deg2rad(30), 0) + i1Ri2 = Rot3.RzRyRx(0, 0, np.deg2rad(20)) + i0Ri2 = i0Ri1.compose(i1Ri2) + i2Ri1_dict = { - (1, 0): Rot3.RzRyRx(0, np.deg2rad(30), 0), - (2, 1): Rot3.RzRyRx(0, 0, np.deg2rad(20)), + (0, 1): i0Ri1.inverse(), + (1, 2): i1Ri2.inverse() } expected_wRi_list = [ - Rot3.RzRyRx(0, 0, 0), - Rot3.RzRyRx(0, np.deg2rad(30), 0), - i2Ri1_dict[(1, 0)].compose(i2Ri1_dict[(2, 1)]), + Rot3(), + i0Ri1, + i0Ri2 ] self.__execute_test(i2Ri1_dict, expected_wRi_list) - def test_simple_with_prior(self): - """Test a simple case with 1 measurement and a single pose prior.""" - expected_wRi_list = [Rot3.RzRyRx(0, 0, 0), Rot3.RzRyRx(0, np.deg2rad(30), 0), Rot3.RzRyRx(np.deg2rad(30), 0, 0)] - - i2Ri1_dict = { - (1, 0): Rot3.RzRyRx(0, np.deg2rad(30), 0), - } - - expected_0R2 = expected_wRi_list[0].between(expected_wRi_list[2]) - i1Ti2_priors = { - (0, 2): PosePrior( - value=Pose3(expected_0R2, np.zeros((3,))), - covariance=np.eye(6) * 1e-5, - type=PosePriorType.SOFT_CONSTRAINT, - ) - } - - two_view_estimation_reports = { - (1, 0): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=1), - (0, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=1), - } - - wRi_computed = self.obj.run_rotation_averaging( - len(expected_wRi_list), i2Ri1_dict, i1Ti2_priors, two_view_estimation_reports - ) - self.assertTrue( - geometry_comparisons.compare_rotations(wRi_computed, expected_wRi_list, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) - ) + # def test_simple_with_prior(self): + # """Test a simple case with 1 measurement and a single pose prior.""" + # expected_wRi_list = [Rot3.RzRyRx(0, 0, 0), Rot3.RzRyRx(0, np.deg2rad(30), 0), Rot3.RzRyRx(np.deg2rad(30), 0, 0)] + + # i2Ri1_dict = { + # (0, 1): expected_wRi_list[1].between(expected_wRi_list[0]) + # } + + # expected_0R2 = expected_wRi_list[0].between(expected_wRi_list[2]) + # i1Ti2_priors = { + # (0, 2): PosePrior( + # value=Pose3(expected_0R2, np.zeros((3,))), + # covariance=np.eye(6) * 1e-5, + # type=PosePriorType.SOFT_CONSTRAINT, + # ) + # } + + # two_view_estimation_reports = { + # (0, 1): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=1), + # (0, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=1), + # } + + # wRi_computed = self.obj.run_rotation_averaging( + # len(expected_wRi_list), i2Ri1_dict, i1Ti2_priors, two_view_estimation_reports + # ) + # self.assertTrue( + # geometry_comparisons.compare_rotations(wRi_computed, expected_wRi_list, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) + # ) def test_computation_graph(self): """Test the dask computation graph execution using a valid collection of relative poses.""" @@ -177,7 +181,7 @@ def test_nonconsecutive_indices(self): """ num_images = 4 - # assume pose 0 is orphaned in the visibility graph + # Assume pose 0 is orphaned in the visibility graph # Let wTi0's (R,t) be parameterized as identity Rot3(), and t = [1,1,0] wTi1 = Pose3(Rot3(), np.array([3, 1, 0])) wTi2 = Pose3(Rot3(), np.array([3, 3, 0])) @@ -191,7 +195,7 @@ def test_nonconsecutive_indices(self): (1, 3): wTi3.between(wTi1).rotation(), } - # Keys do not overlap with i2Ri1_dict + # Keys do not overlap with i2Ri1_dict. two_view_estimation_reports = { (1, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=200), (1, 3): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=500), diff --git a/tests/data/sample_poses.py b/tests/data/sample_poses.py index 0f484a0e3..1aacd7e5f 100644 --- a/tests/data/sample_poses.py +++ b/tests/data/sample_poses.py @@ -21,8 +21,8 @@ def generate_relative_from_global( """Generate relative poses from global poses. Args: - wTi_list: global poses. - pair_indices: pairs (i1, i2) to construct relative poses for. + wTi_list: Global poses. + pair_indices: Pairs (i1, i2) to construct relative poses for. Returns: Dictionary (i1, i2) -> i2Ti1 for all requested pairs. @@ -37,7 +37,7 @@ def generate_relative_from_global( CIRCLE_TWO_EDGES_GLOBAL_POSES = SFMdata.createPoses(Cal3_S2(fx=1, fy=1, s=0, u0=0, v0=0))[::2] CIRCLE_TWO_EDGES_RELATIVE_POSES = generate_relative_from_global( - CIRCLE_TWO_EDGES_GLOBAL_POSES, [(1, 0), (2, 1), (3, 2), (0, 3)] + CIRCLE_TWO_EDGES_GLOBAL_POSES, [(0, 1), (1, 2), (2, 3), (0, 3)] ) """4 poses in the circle of radius 5m, all looking at the center of the circle. From fbdf99a24d34f27af820e47f1eb9d3d4e0fe9909 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 13 Mar 2024 03:21:35 -0400 Subject: [PATCH 15/27] use v_corr_idxs instead of two_view_estimation_reports to get inlier counts --- .../rotation/rotation_averaging_base.py | 40 +++++++++---------- gtsfm/averaging/rotation/shonan.py | 11 +++-- gtsfm/multi_view_optimizer.py | 2 +- 3 files changed, 26 insertions(+), 27 deletions(-) diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index 2d6816c1b..faa41b4fd 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -43,15 +43,15 @@ def run_rotation_averaging( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], - two_view_estimation_reports: Dict[Tuple[int, int], TwoViewEstimationReport], + v_corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> List[Optional[Rot3]]: """Run the rotation averaging. Args: - num_images: number of poses. - i2Ri1_dict: relative rotations as dictionary (i1, i2): i2Ri1. - i1Ti2_priors: priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. - two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. + num_images: Number of poses. + i2Ri1_dict: Relative rotations as dictionary (i1, i2): i2Ri1. + i1Ti2_priors: Priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. + v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences. Returns: Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is @@ -64,17 +64,17 @@ def _run_rotation_averaging_base( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], - two_view_estimation_reports: Dict[Tuple[int, int], TwoViewEstimationReport], wTi_gt: List[Optional[Pose3]], + v_corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> Tuple[List[Optional[Rot3]], GtsfmMetricsGroup]: """Runs rotation averaging and computes metrics. Args: num_images: Number of poses. - i2Ri1_dict: relative rotations as dictionary (i1, i2): i2Ri1. + i2Ri1_dict: Relative rotations as dictionary (i1, i2): i2Ri1. i1Ti2_priors: Priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. - two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. wTi_gt: Ground truth global rotations to compare against. + v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences. Returns: Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is @@ -83,7 +83,7 @@ def _run_rotation_averaging_base( Metrics on global rotations. """ start_time = time.time() - wRis = self.run_rotation_averaging(num_images, i2Ri1_dict, i1Ti2_priors, two_view_estimation_reports) + wRis = self.run_rotation_averaging(num_images, i2Ri1_dict, i1Ti2_priors, v_corr_idxs) run_time = time.time() - start_time metrics = self.evaluate(wRis, wTi_gt) @@ -98,11 +98,11 @@ def evaluate(self, wRi_computed: List[Optional[Rot3]], wTi_gt: List[Optional[Pos 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. - 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] @@ -121,20 +121,20 @@ def create_computation_graph( num_images: int, i2Ri1_graph: Delayed, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], - two_view_estimation_reports: Dict[Tuple[int, int], TwoViewEstimationReport], gt_wTi_list: List[Optional[Pose3]], + v_corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> Tuple[Delayed, Delayed]: """Create the computation graph for performing rotation averaging. Args: - num_images: number of poses. - i2Ri1_graph: dictionary of relative rotations as a delayed task. - i1Ti2_priors: priors on relative poses as (i1, i2): PosePrior on i1Ti2. - two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. - gt_wTi_list: ground truth poses, to be used for evaluation. + num_images: Number of poses. + i2Ri1_graph: Dictionary of relative rotations as a delayed task. + i1Ti2_priors: Priors on relative poses as (i1, i2): PosePrior on i1Ti2. + gt_wTi_list: Ground truth poses, to be used for evaluation. + v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences. Returns: - global rotations wrapped using dask.delayed. + Global rotations wrapped using dask.delayed. """ wRis, metrics = dask.delayed(self._run_rotation_averaging_base, nout=2)( @@ -142,7 +142,7 @@ def create_computation_graph( i2Ri1_dict=i2Ri1_graph, i1Ti2_priors=i1Ti2_priors, wTi_gt=gt_wTi_list, - two_view_estimation_reports=two_view_estimation_reports, + v_corr_idxs=v_corr_idxs, ) return wRis, metrics diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 7fcbd3f77..f0f37f497 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -163,7 +163,7 @@ def run_rotation_averaging( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], - two_view_estimation_reports: Dict[Tuple[int, int], TwoViewEstimationReport], + v_corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> List[Optional[Rot3]]: """Run the rotation averaging on a connected graph with arbitrary keys, where each key is a image/pose index. @@ -175,7 +175,7 @@ def run_rotation_averaging( num_images: Number of images. Since we have one pose per image, it is also the number of poses. i2Ri1_dict: Relative rotations for each image pair-edge as dictionary (i1, i2): i2Ri1. i1Ti2_priors: Priors on relative poses. - two_view_estimation_reports: information related to 2-view pose estimation and correspondence verification. + v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences. Returns: Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is @@ -192,12 +192,11 @@ def run_rotation_averaging( old_to_new_idxes = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)} i2Ri1_dict_ = { - (old_to_new_idxes[edge[0]], old_to_new_idxes[edge[1]]): i2Ri1 for edge, i2Ri1 in i2Ri1_dict.items() + (old_to_new_idxes[i1], old_to_new_idxes[i2]): i2Ri1 for (i1,i2), i2Ri1 in i2Ri1_dict.items() } num_correspondences_dict: Dict[Tuple[int, int], int] = { - (old_to_new_idxes[edge[0]], old_to_new_idxes[edge[1]]): int(report.num_inliers_est_model) - for edge, report in two_view_estimation_reports.items() - if edge in i2Ri1_dict + (old_to_new_idxes[i1], old_to_new_idxes[i2]): len(v_corr_idxs[(i1, i2)]) + for (i1,i2) in v_corr_idxs.keys() if (i1,i2) in i2Ri1_dict } # Use negative of the number of correspondences as the edge weight. wRi_initial_ = rotation_util.initialize_global_rotations_using_mst( diff --git a/gtsfm/multi_view_optimizer.py b/gtsfm/multi_view_optimizer.py index c76ebb699..c55e0633b 100644 --- a/gtsfm/multi_view_optimizer.py +++ b/gtsfm/multi_view_optimizer.py @@ -129,8 +129,8 @@ def create_computation_graph( num_images, pruned_i2Ri1_graph, i1Ti2_priors=relative_pose_priors, - two_view_estimation_reports=viewgraph_two_view_reports_graph, gt_wTi_list=gt_wTi_list, + v_corr_idxs=viewgraph_v_corr_idxs_graph, ) tracks2d_graph = dask.delayed(get_2d_tracks)(viewgraph_v_corr_idxs_graph, keypoints_list) From 3a1ec06e2bc0de288e0e39ff003149a0ed2b8340 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 13 Mar 2024 03:22:04 -0400 Subject: [PATCH 16/27] python black reformat --- gtsfm/averaging/rotation/shonan.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index f0f37f497..bc3fafe5a 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -191,12 +191,11 @@ def run_rotation_averaging( nodes_with_edges = sorted(list(self._nodes_with_edges(i2Ri1_dict, i1Ti2_priors))) old_to_new_idxes = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)} - i2Ri1_dict_ = { - (old_to_new_idxes[i1], old_to_new_idxes[i2]): i2Ri1 for (i1,i2), i2Ri1 in i2Ri1_dict.items() - } + i2Ri1_dict_ = {(old_to_new_idxes[i1], old_to_new_idxes[i2]): i2Ri1 for (i1, i2), i2Ri1 in i2Ri1_dict.items()} num_correspondences_dict: Dict[Tuple[int, int], int] = { (old_to_new_idxes[i1], old_to_new_idxes[i2]): len(v_corr_idxs[(i1, i2)]) - for (i1,i2) in v_corr_idxs.keys() if (i1,i2) in i2Ri1_dict + for (i1, i2) in v_corr_idxs.keys() + if (i1, i2) in i2Ri1_dict } # Use negative of the number of correspondences as the edge weight. wRi_initial_ = rotation_util.initialize_global_rotations_using_mst( From 429177ab6a26c970e03bf3b62f9a49a584845620 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 13 Mar 2024 04:16:00 -0400 Subject: [PATCH 17/27] fix import error --- gtsfm/averaging/rotation/rotation_averaging_base.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index faa41b4fd..28d4c38ac 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -8,13 +8,13 @@ from typing import Dict, List, Optional, Tuple import dask +import numpy as np from dask.delayed import Delayed from gtsam import Pose3, Rot3 import gtsfm.utils.alignment as alignment_utils import gtsfm.utils.metrics as metric_utils from gtsfm.common.pose_prior import PosePrior -from gtsfm.common.two_view_estimation_report import TwoViewEstimationReport from gtsfm.evaluation.metrics import GtsfmMetric, GtsfmMetricsGroup from gtsfm.ui.gtsfm_process import GTSFMProcess, UiMetadata From beef33b6d6b4f494cdd3df73014cf2c933ccc0c5 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 13 May 2024 22:53:41 -0700 Subject: [PATCH 18/27] Control MST init with flag plus fixes --- gtsfm/averaging/rotation/shonan.py | 24 +++--- gtsfm/utils/rotation.py | 66 +-------------- tests/averaging/rotation/test_shonan.py | 103 ++++++++++-------------- tests/utils/test_rotation_utils.py | 7 +- 4 files changed, 63 insertions(+), 137 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index bc3fafe5a..3d5902ad1 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -30,7 +30,6 @@ import gtsfm.utils.rotation as rotation_util from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase from gtsfm.common.pose_prior import PosePrior -from gtsfm.common.two_view_estimation_report import TwoViewEstimationReport POSE3_DOF = 6 @@ -42,7 +41,9 @@ class ShonanRotationAveraging(RotationAveragingBase): """Performs Shonan rotation averaging.""" - def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_SIGMA) -> None: + def __init__( + self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_SIGMA, use_mst_init: bool = False + ) -> None: """Initializes module. Note: `p_min` and `p_max` describe the minimum and maximum relaxation rank. @@ -52,6 +53,7 @@ def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_S """ super().__init__() self._two_view_rotation_sigma = two_view_rotation_sigma + self._use_mst_init = use_mst_init self._p_min = 5 self._p_max = 64 @@ -198,14 +200,16 @@ def run_rotation_averaging( if (i1, i2) in i2Ri1_dict } # Use negative of the number of correspondences as the edge weight. - wRi_initial_ = rotation_util.initialize_global_rotations_using_mst( - len(nodes_with_edges), - i2Ri1_dict_, - edge_weights={(i1, i2): -num_correspondences_dict.get((i1, i2), 0) for i1, i2 in i2Ri1_dict_.keys()}, - ) - initial_values = Values() - for i, wRi_initial_ in enumerate(wRi_initial_): - initial_values.insert(i, wRi_initial_) + initial_values: Optional[Values] = None + if self._use_mst_init: + wRi_initial_ = rotation_util.initialize_global_rotations_using_mst( + len(nodes_with_edges), + i2Ri1_dict_, + edge_weights={(i1, i2): -num_correspondences_dict.get((i1, i2), 0) for i1, i2 in i2Ri1_dict_.keys()}, + ) + initial_values = Values() + for i, wRi in enumerate(wRi_initial_): + initial_values.insert(i, wRi) between_factors: BetweenFactorPose3s = self.__between_factors_from_2view_relative_rotations( i2Ri1_dict, old_to_new_idxes diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index c816c1093..ef32fb19b 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -37,7 +37,7 @@ def initialize_global_rotations_using_mst( G = nx.Graph() G.add_edges_from(mst.edges) - wRi_list = [None] * num_images + wRi_list: List[Rot3] = [Rot3()] * num_images # Choose origin node. origin_node = list(G.nodes)[0] wRi_list[origin_node] = Rot3() @@ -61,67 +61,3 @@ def initialize_global_rotations_using_mst( wRi_list[dst_node] = wRi1 return wRi_list - - -# def initialize_mst( -# num_images: int, -# i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], -# corr_idxs: Dict[Tuple[int, int], np.ndarray], -# old_to_new_idxs: Dict[int, int], -# ) -> gtsam.Values: -# """Initialize global rotations using the minimum spanning tree (MST). - -# Args: -# num_images: Number of images in the scene. -# i2Ri1_dict: Dictionary of relative rotations (i1, i2): i2Ri1. -# corr_idxs: -# old_to_new_idxs: - -# Returns: -# Initialization of global rotations for Values. -# """ -# # Compute MST. -# row, col, data = [], [], [] -# for (i1, i2), i2Ri1 in i2Ri1_dict.items(): -# if i2Ri1 is None: -# continue -# row.append(i1) -# col.append(i2) -# data.append(-corr_idxs[(i1, i2)].shape[0]) -# logger.info(corr_idxs[(i1, i2)]) -# corr_adjacency = scipy.sparse.coo_array((data, (row, col)), shape=(num_images, num_images)) -# Tcsr = scipy.sparse.csgraph.minimum_spanning_tree(corr_adjacency) -# logger.info(Tcsr.toarray().astype(int)) - -# # Build global rotations from MST. -# # TODO (travisdriver): This is simple but very inefficient. Use something else. -# i_mst, j_mst = Tcsr.nonzero() -# logger.info(i_mst) -# logger.info(j_mst) -# edges_mst = [(i, j) for (i, j) in zip(i_mst, j_mst)] -# iR0_dict = {i_mst[0]: np.eye(3)} # pick the left index of the first edge as the seed -# # max_iters = num_images * 10 -# iter = 0 -# while len(edges_mst) > 0: -# i, j = edges_mst.pop(0) -# if i in iR0_dict: -# jRi = i2Ri1_dict[(i, j)].matrix() -# iR0 = iR0_dict[i] -# iR0_dict[j] = jRi @ iR0 -# elif j in iR0_dict: -# iRj = i2Ri1_dict[(i, j)].matrix().T -# jR0 = iR0_dict[j] -# iR0_dict[i] = iRj @ jR0 -# else: -# edges_mst.append((i, j)) -# iter += 1 -# # if iter >= max_iters: -# # logger.info("Reached max MST iters.") -# # assert False - -# # Add to Values object. -# initial = gtsam.Values() -# for i, iR0 in iR0_dict.items(): -# initial.insert(old_to_new_idxs[i], Rot3(iR0)) - -# return initial diff --git a/tests/averaging/rotation/test_shonan.py b/tests/averaging/rotation/test_shonan.py index bdfe9cc20..3cba58b3a 100644 --- a/tests/averaging/rotation/test_shonan.py +++ b/tests/averaging/rotation/test_shonan.py @@ -17,7 +17,6 @@ from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase from gtsfm.averaging.rotation.shonan import ShonanRotationAveraging from gtsfm.common.pose_prior import PosePrior, PosePriorType -from gtsfm.common.two_view_estimation_report import TwoViewEstimationReport ROTATION_ANGLE_ERROR_THRESHOLD_DEG = 2 @@ -41,13 +40,8 @@ def __execute_test(self, i2Ri1_input: Dict[Tuple[int, int], Rot3], wRi_expected: wRi_expected: Expected global rotations. """ i1Ti2_priors: Dict[Tuple[int, int], PosePrior] = {} - two_view_estimation_reports = { - (i1, i2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=random.randint(0, 100)) - for i1, i2 in i2Ri1_input.keys() - } - wRi_computed = self.obj.run_rotation_averaging( - len(wRi_expected), i2Ri1_input, i1Ti2_priors, two_view_estimation_reports - ) + v_corr_idxs = {(i1, i2): _generate_corr_idxs(random.randint(0, 100)) for i1, i2 in i2Ri1_input.keys()} + wRi_computed = self.obj.run_rotation_averaging(len(wRi_expected), i2Ri1_input, i1Ti2_priors, v_corr_idxs) self.assertTrue( geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) ) @@ -87,47 +81,36 @@ def test_simple_three_nodes_two_measurements(self): i1Ri2 = Rot3.RzRyRx(0, 0, np.deg2rad(20)) i0Ri2 = i0Ri1.compose(i1Ri2) - i2Ri1_dict = { - (0, 1): i0Ri1.inverse(), - (1, 2): i1Ri2.inverse() - } + i2Ri1_dict = {(0, 1): i0Ri1.inverse(), (1, 2): i1Ri2.inverse()} - expected_wRi_list = [ - Rot3(), - i0Ri1, - i0Ri2 - ] + expected_wRi_list = [Rot3(), i0Ri1, i0Ri2] self.__execute_test(i2Ri1_dict, expected_wRi_list) - # def test_simple_with_prior(self): - # """Test a simple case with 1 measurement and a single pose prior.""" - # expected_wRi_list = [Rot3.RzRyRx(0, 0, 0), Rot3.RzRyRx(0, np.deg2rad(30), 0), Rot3.RzRyRx(np.deg2rad(30), 0, 0)] - - # i2Ri1_dict = { - # (0, 1): expected_wRi_list[1].between(expected_wRi_list[0]) - # } - - # expected_0R2 = expected_wRi_list[0].between(expected_wRi_list[2]) - # i1Ti2_priors = { - # (0, 2): PosePrior( - # value=Pose3(expected_0R2, np.zeros((3,))), - # covariance=np.eye(6) * 1e-5, - # type=PosePriorType.SOFT_CONSTRAINT, - # ) - # } - - # two_view_estimation_reports = { - # (0, 1): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=1), - # (0, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=1), - # } - - # wRi_computed = self.obj.run_rotation_averaging( - # len(expected_wRi_list), i2Ri1_dict, i1Ti2_priors, two_view_estimation_reports - # ) - # self.assertTrue( - # geometry_comparisons.compare_rotations(wRi_computed, expected_wRi_list, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) - # ) + def test_simple_with_prior(self): + """Test a simple case with 1 measurement and a single pose prior.""" + expected_wRi_list = [Rot3.RzRyRx(0, 0, 0), Rot3.RzRyRx(0, np.deg2rad(30), 0), Rot3.RzRyRx(np.deg2rad(30), 0, 0)] + + i2Ri1_dict = {(0, 1): expected_wRi_list[1].between(expected_wRi_list[0])} + + expected_0R2 = expected_wRi_list[0].between(expected_wRi_list[2]) + i1Ti2_priors = { + (0, 2): PosePrior( + value=Pose3(expected_0R2, np.zeros((3,))), + covariance=np.eye(6) * 1e-5, + type=PosePriorType.SOFT_CONSTRAINT, + ) + } + + v_corr_idxs = { + (0, 1): _generate_corr_idxs(1), + (0, 2): _generate_corr_idxs(1), + } + + wRi_computed = self.obj.run_rotation_averaging(len(expected_wRi_list), i2Ri1_dict, i1Ti2_priors, v_corr_idxs) + self.assertTrue( + geometry_comparisons.compare_rotations(wRi_computed, expected_wRi_list, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) + ) def test_computation_graph(self): """Test the dask computation graph execution using a valid collection of relative poses.""" @@ -138,23 +121,21 @@ def test_computation_graph(self): (0, 1): Rot3.RzRyRx(0, np.deg2rad(30), 0), (1, 2): Rot3.RzRyRx(0, 0, np.deg2rad(20)), } - two_view_estimation_reports = { - (0, 1): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=200), - (1, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=500), + v_corr_idxs = { + (0, 1): _generate_corr_idxs(200), + (1, 2): _generate_corr_idxs(500), } i2Ri1_graph = dask.delayed(i2Ri1_dict) # use the GTSAM API directly (without dask) for rotation averaging i1Ti2_priors: Dict[Tuple[int, int], PosePrior] = {} - expected_wRi_list = self.obj.run_rotation_averaging( - num_poses, i2Ri1_dict, i1Ti2_priors, two_view_estimation_reports - ) + expected_wRi_list = self.obj.run_rotation_averaging(num_poses, i2Ri1_dict, i1Ti2_priors, v_corr_idxs) # use dask's computation graph gt_wTi_list = [None] * len(expected_wRi_list) rotations_graph, _ = self.obj.create_computation_graph( - num_poses, i2Ri1_graph, i1Ti2_priors, two_view_estimation_reports, gt_wTi_list + num_poses, i2Ri1_graph, i1Ti2_priors, gt_wTi_list, v_corr_idxs ) with dask.config.set(scheduler="single-threaded"): @@ -196,21 +177,25 @@ def test_nonconsecutive_indices(self): } # Keys do not overlap with i2Ri1_dict. - two_view_estimation_reports = { - (1, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=200), - (1, 3): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=500), - (0, 2): TwoViewEstimationReport(v_corr_idxs=np.array([]), num_inliers_est_model=0), + v_corr_idxs = { + (1, 2): _generate_corr_idxs(200), + (1, 3): _generate_corr_idxs(500), + (0, 2): _generate_corr_idxs(0), } relative_pose_priors: Dict[Tuple[int, int], PosePrior] = {} - wRi_computed = self.obj.run_rotation_averaging( - num_images, i2Ri1_input, relative_pose_priors, two_view_estimation_reports - ) + wRi_computed = self.obj.run_rotation_averaging(num_images, i2Ri1_input, relative_pose_priors, v_corr_idxs) wRi_expected = [None, wTi1.rotation(), wTi2.rotation(), wTi3.rotation()] self.assertTrue( geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, angular_error_threshold_degrees=0.1) ) + def _test_initialization(self, ) + + +def _generate_corr_idxs(num_corrs: int) -> np.ndarray: + return np.random.randint(low=0, high=10000, size=(num_corrs, 2)) + if __name__ == "__main__": unittest.main() diff --git a/tests/utils/test_rotation_utils.py b/tests/utils/test_rotation_utils.py index 614888efd..f7086e6ef 100644 --- a/tests/utils/test_rotation_utils.py +++ b/tests/utils/test_rotation_utils.py @@ -2,6 +2,7 @@ Authors: Ayush Baid """ + import unittest from typing import Dict, List, Tuple @@ -18,7 +19,7 @@ RELATIVE_ROTATION_DICT = Dict[Tuple[int, int], Rot3] -def _get_ordered_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[float]]: +def _get_ordered_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, np.ndarray]: """Return data for a scenario with 5 camera poses, with ordering that follows their connectivity. Accordingly, we specify i1 < i2 for all edges (i1,i2). @@ -48,7 +49,7 @@ def _get_ordered_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[float]] return i2Ri1_dict, wRi_list_euler_deg_expected -def _get_mixed_order_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, List[float]]: +def _get_mixed_order_chain_pose_data() -> Tuple[RELATIVE_ROTATION_DICT, np.ndarray]: """Return data for a scenario with 5 camera poses, with ordering that does NOT follow their connectivity. Below, we do NOT specify i1 < i2 for all edges (i1,i2). @@ -116,7 +117,7 @@ def _wrap_angles(angles: np.ndarray) -> np.ndarray: class TestRotationUtil(unittest.TestCase): def test_mst_initialization(self): - """Test for 4 poses in a circle, with a pose connected all others.""" + """Test for 4 poses in a circle, with a pose connected to all others.""" i2Ri1_dict, wRi_expected = sample_poses.convert_data_for_rotation_averaging( sample_poses.CIRCLE_ALL_EDGES_GLOBAL_POSES, sample_poses.CIRCLE_ALL_EDGES_RELATIVE_POSES ) From c01f254f66f805b4eb577aaf095ddf6d1fc7969e Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 13 May 2024 23:13:40 -0700 Subject: [PATCH 19/27] Log initialization technique --- gtsfm/averaging/rotation/shonan.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 3d5902ad1..74df5c94f 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -133,6 +133,7 @@ def _run_with_consecutive_ordering( shonan = ShonanAveraging3(between_factors, self.__get_shonan_params()) if initial is None: + logger.info("Using random initialization for Shonan") initial = shonan.initializeRandomly() logger.info("Initial cost: %.5f", shonan.cost(initial)) result, _ = shonan.run(initial, self._p_min, self._p_max) @@ -202,6 +203,7 @@ def run_rotation_averaging( # Use negative of the number of correspondences as the edge weight. initial_values: Optional[Values] = None if self._use_mst_init: + logger.info("Using MST initialization for Shonan") wRi_initial_ = rotation_util.initialize_global_rotations_using_mst( len(nodes_with_edges), i2Ri1_dict_, From 4d6c3a5d40270452e0e41ea5ab2e934d557e454e Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 13 May 2024 23:14:13 -0700 Subject: [PATCH 20/27] Add unit test comparing initializations --- gtsfm/utils/rotation.py | 11 +++++++ tests/averaging/rotation/test_shonan.py | 40 ++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index ef32fb19b..b3ba2a529 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -6,9 +6,20 @@ from typing import Dict, List, Tuple import networkx as nx +import numpy as np from gtsam import Rot3 +def random_rotation() -> Rot3: + """Sample a random rotation by generating a sample from the 4d unit sphere.""" + q = np.random.randn(4) + # make unit-length quaternion + q /= np.linalg.norm(q) + qw, qx, qy, qz = q + R = Rot3(qw, qx, qy, qz) + return R + + def initialize_global_rotations_using_mst( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Rot3], edge_weights: Dict[Tuple[int, int], int] ) -> List[Rot3]: diff --git a/tests/averaging/rotation/test_shonan.py b/tests/averaging/rotation/test_shonan.py index 3cba58b3a..a72f94e19 100644 --- a/tests/averaging/rotation/test_shonan.py +++ b/tests/averaging/rotation/test_shonan.py @@ -4,15 +4,16 @@ """ import pickle +import random import unittest from typing import Dict, List, Tuple -import random import dask import numpy as np from gtsam import Pose3, Rot3 import gtsfm.utils.geometry_comparisons as geometry_comparisons +import gtsfm.utils.rotation as rotation_util import tests.data.sample_poses as sample_poses from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase from gtsfm.averaging.rotation.shonan import ShonanRotationAveraging @@ -27,10 +28,10 @@ class TestShonanRotationAveraging(unittest.TestCase): All unit test functions defined in TestRotationAveragingBase are run automatically. """ - def setUp(self): + def setUp(self) -> None: super().setUp() - self.obj: RotationAveragingBase = ShonanRotationAveraging() + self.obj = ShonanRotationAveraging() def __execute_test(self, i2Ri1_input: Dict[Tuple[int, int], Rot3], wRi_expected: List[Rot3]) -> None: """Helper function to run the averagaing and assert w/ expected. @@ -190,7 +191,38 @@ def test_nonconsecutive_indices(self): geometry_comparisons.compare_rotations(wRi_computed, wRi_expected, angular_error_threshold_degrees=0.1) ) - def _test_initialization(self, ) + def test_initialization(self) -> None: + """Test that the result of Shonan is not dependent on the initialization.""" + i2Ri1_dict_noisefree, wRi_expected = sample_poses.convert_data_for_rotation_averaging( + sample_poses.CIRCLE_ALL_EDGES_GLOBAL_POSES, sample_poses.CIRCLE_ALL_EDGES_RELATIVE_POSES + ) + v_corr_idxs = {pair: _generate_corr_idxs(random.randint(1, 10)) for pair in i2Ri1_dict_noisefree.keys()} + + # Add noise to the relative rotations + i2Ri1_dict_noisy = { + pair: i2Ri1 * rotation_util.random_rotation() for pair, i2Ri1 in i2Ri1_dict_noisefree.items() + } + + wRi_computed_with_random_init = self.obj.run_rotation_averaging( + num_images=len(wRi_expected), + i2Ri1_dict=i2Ri1_dict_noisy, + i1Ti2_priors={}, + v_corr_idxs=v_corr_idxs, + ) + + shonan_mst_init = ShonanRotationAveraging(use_mst_init=True) + wRi_computed_with_mst_init = shonan_mst_init.run_rotation_averaging( + num_images=len(wRi_expected), + i2Ri1_dict=i2Ri1_dict_noisy, + i1Ti2_priors={}, + v_corr_idxs=v_corr_idxs, + ) + + self.assertTrue( + geometry_comparisons.compare_rotations( + wRi_computed_with_random_init, wRi_computed_with_mst_init, angular_error_threshold_degrees=0.1 + ) + ) def _generate_corr_idxs(num_corrs: int) -> np.ndarray: From 42b3e70efcd12c5493cf47a004d8800043ea8570 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 13 May 2024 23:18:07 -0700 Subject: [PATCH 21/27] Log shonan optimality --- gtsfm/averaging/rotation/shonan.py | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 74df5c94f..821e58842 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -138,6 +138,7 @@ def _run_with_consecutive_ordering( logger.info("Initial cost: %.5f", shonan.cost(initial)) result, _ = shonan.run(initial, self._p_min, self._p_max) logger.info("Final cost: %.5f", shonan.cost(result)) + logger.info("Shonan result optimal: %s", shonan.checkOptimality(result)) wRi_list_consecutive = [None] * num_connected_nodes for i in range(num_connected_nodes): From 8848d7628fa9f07ecca79b3c9c6c38d25867c283 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 13 May 2024 23:42:28 -0700 Subject: [PATCH 22/27] Remove optimality logging --- gtsfm/averaging/rotation/shonan.py | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 821e58842..74df5c94f 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -138,7 +138,6 @@ def _run_with_consecutive_ordering( logger.info("Initial cost: %.5f", shonan.cost(initial)) result, _ = shonan.run(initial, self._p_min, self._p_max) logger.info("Final cost: %.5f", shonan.cost(result)) - logger.info("Shonan result optimal: %s", shonan.checkOptimality(result)) wRi_list_consecutive = [None] * num_connected_nodes for i in range(num_connected_nodes): From 980b94d731ebecdc9803162a8bcdd2fddcaaae86 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 13 May 2024 23:43:22 -0700 Subject: [PATCH 23/27] Remove unused import --- tests/averaging/rotation/test_shonan.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/averaging/rotation/test_shonan.py b/tests/averaging/rotation/test_shonan.py index a72f94e19..77bc0cde3 100644 --- a/tests/averaging/rotation/test_shonan.py +++ b/tests/averaging/rotation/test_shonan.py @@ -15,7 +15,6 @@ import gtsfm.utils.geometry_comparisons as geometry_comparisons import gtsfm.utils.rotation as rotation_util import tests.data.sample_poses as sample_poses -from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase from gtsfm.averaging.rotation.shonan import ShonanRotationAveraging from gtsfm.common.pose_prior import PosePrior, PosePriorType From f55f2b3a39b7a62bbcfc962aa5f3aa6cf8e36ccc Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 20 May 2024 23:26:20 -0700 Subject: [PATCH 24/27] Merge master --- gtsfm/averaging/rotation/shonan.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 74df5c94f..e9f0428d5 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -54,7 +54,7 @@ def __init__( super().__init__() self._two_view_rotation_sigma = two_view_rotation_sigma self._use_mst_init = use_mst_init - self._p_min = 5 + self._p_min = 3 self._p_max = 64 def __get_shonan_params(self) -> ShonanAveragingParameters3: From 337c6db9c464247ad7a2848f945ede284bcdd3a3 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 25 May 2024 19:18:00 -0700 Subject: [PATCH 25/27] Add unit test for initialization on larger scene --- gtsfm/utils/geometry_comparisons.py | 3 +- gtsfm/utils/rotation.py | 8 +++-- tests/averaging/rotation/test_shonan.py | 45 +++++++++++++++++++++++++ 3 files changed, 52 insertions(+), 4 deletions(-) diff --git a/gtsfm/utils/geometry_comparisons.py b/gtsfm/utils/geometry_comparisons.py index 1dddccb03..87289ed0c 100644 --- a/gtsfm/utils/geometry_comparisons.py +++ b/gtsfm/utils/geometry_comparisons.py @@ -31,7 +31,7 @@ def compare_rotations( aTi_list: 1st list of rotations. bTi_list: 2nd list of rotations. angular_error_threshold_degrees: Threshold for angular error between two rotations. - + Returns: Result of the comparison. """ @@ -56,6 +56,7 @@ def compare_rotations( relative_rotations_angles = np.array( [compute_relative_rotation_angle(aRi, aRi_) for (aRi, aRi_) in zip(aRi_list, aRi_list_)], dtype=np.float32 ) + print(relative_rotations_angles) return np.all(relative_rotations_angles < angular_error_threshold_degrees) diff --git a/gtsfm/utils/rotation.py b/gtsfm/utils/rotation.py index b3ba2a529..cde9b51b9 100644 --- a/gtsfm/utils/rotation.py +++ b/gtsfm/utils/rotation.py @@ -10,14 +10,16 @@ from gtsam import Rot3 -def random_rotation() -> Rot3: +def random_rotation(angle_scale_factor: float = 0.1) -> Rot3: """Sample a random rotation by generating a sample from the 4d unit sphere.""" - q = np.random.randn(4) + q = np.random.rand(4) # make unit-length quaternion q /= np.linalg.norm(q) qw, qx, qy, qz = q R = Rot3(qw, qx, qy, qz) - return R + axis, angle = R.axisAngle() + angle = angle * angle_scale_factor + return Rot3.AxisAngle(axis.point3(), angle) def initialize_global_rotations_using_mst( diff --git a/tests/averaging/rotation/test_shonan.py b/tests/averaging/rotation/test_shonan.py index 77bc0cde3..c5db923a1 100644 --- a/tests/averaging/rotation/test_shonan.py +++ b/tests/averaging/rotation/test_shonan.py @@ -7,18 +7,22 @@ import random import unittest from typing import Dict, List, Tuple +from pathlib import Path import dask import numpy as np from gtsam import Pose3, Rot3 import gtsfm.utils.geometry_comparisons as geometry_comparisons +import gtsfm.utils.io as io_utils import gtsfm.utils.rotation as rotation_util import tests.data.sample_poses as sample_poses from gtsfm.averaging.rotation.shonan import ShonanRotationAveraging from gtsfm.common.pose_prior import PosePrior, PosePriorType ROTATION_ANGLE_ERROR_THRESHOLD_DEG = 2 +TEST_DATA_ROOT = Path(__file__).resolve().parent.parent.parent / "data" +LARGE_PROBLEM_BAL_FILE = TEST_DATA_ROOT / "problem-394-100368-pre.txt" class TestShonanRotationAveraging(unittest.TestCase): @@ -223,6 +227,47 @@ def test_initialization(self) -> None: ) ) + def test_initialization_big(self): + """Test that the result of Shonan is not dependent on the initialization on a bigger dataset.""" + gt_data = io_utils.read_bal(str(LARGE_PROBLEM_BAL_FILE)) + poses = gt_data.get_camera_poses()[:15] + pairs: List[Tuple[int, int]] = [] + for i in range(len(poses)): + for j in range(i + 1, min(i + 5, len(poses))): + pairs.append((i, j)) + + i2Ri1_dict_noisefree, _ = sample_poses.convert_data_for_rotation_averaging( + poses, sample_poses.generate_relative_from_global(poses, pairs) + ) + v_corr_idxs = {pair: _generate_corr_idxs(random.randint(1, 10)) for pair in i2Ri1_dict_noisefree.keys()} + + # Add noise to the relative rotations + i2Ri1_dict_noisy = { + pair: i2Ri1 * rotation_util.random_rotation(angle_scale_factor=0.5) + for pair, i2Ri1 in i2Ri1_dict_noisefree.items() + } + + wRi_computed_with_random_init = self.obj.run_rotation_averaging( + num_images=len(poses), + i2Ri1_dict=i2Ri1_dict_noisy, + i1Ti2_priors={}, + v_corr_idxs=v_corr_idxs, + ) + + shonan_mst_init = ShonanRotationAveraging(use_mst_init=True) + wRi_computed_with_mst_init = shonan_mst_init.run_rotation_averaging( + num_images=len(poses), + i2Ri1_dict=i2Ri1_dict_noisy, + i1Ti2_priors={}, + v_corr_idxs=v_corr_idxs, + ) + + self.assertTrue( + geometry_comparisons.compare_rotations( + wRi_computed_with_random_init, wRi_computed_with_mst_init, angular_error_threshold_degrees=0.1 + ) + ) + def _generate_corr_idxs(num_corrs: int) -> np.ndarray: return np.random.randint(low=0, high=10000, size=(num_corrs, 2)) From dde24e89ecb97dbb64f76c08725698b29ab64138 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sun, 12 May 2024 22:21:55 -0700 Subject: [PATCH 26/27] env v2 --- environment_v2_linux_cpuonly.yml | 56 ++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 environment_v2_linux_cpuonly.yml diff --git a/environment_v2_linux_cpuonly.yml b/environment_v2_linux_cpuonly.yml new file mode 100644 index 000000000..2e1e18020 --- /dev/null +++ b/environment_v2_linux_cpuonly.yml @@ -0,0 +1,56 @@ +name: gtsfm-v2 +channels: + # for priority order, we prefer pytorch as the highest priority as it supplies + # latest stable packages for numerous deep learning based methods. conda-forge + # supplies higher versions of packages like opencv compared to the defaults + # channel. + - pytorch + - conda-forge +dependencies: + # python essentials + - python + - pip + # formatting and dev environment + - black + - coverage + - mypy + - pylint + - pytest + - flake8 + - isort + # dask and related + - dask # same as dask[complete] pip distribution + - asyncssh + - python-graphviz + # core functionality and APIs + - matplotlib + - networkx + - numpy + - nodejs + - pandas + - pillow + - scikit-learn + - seaborn + - scipy + - hydra-core + - gtsam + # 3rd party algorithms for different modules + - cpuonly # replacement of cudatoolkit for cpu only machines + - pytorch + - torchvision + - kornia + - pycolmap + - opencv + # io + - h5py + - plotly + - tabulate + - simplejson + - open3d + - colour + - pydot + - trimesh + # testing + - parameterized + # - pip: + # - pydegensac From 35b3a7be9a1d2c53ce8781b10de2da437c4beba6 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 25 May 2024 19:53:20 -0700 Subject: [PATCH 27/27] Remove duplicate args plus update process meta --- gtsfm/averaging/rotation/rotation_averaging_base.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index 96ff8a5be..51d982a2b 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -26,12 +26,13 @@ class RotationAveragingBase(GTSFMProcess): rotations. """ + @staticmethod def get_ui_metadata() -> UiMetadata: """Returns data needed to display node and edge info for this process in the process graph.""" return UiMetadata( display_name="Rotation Averaging", - input_products=("View-Graph Relative Rotations", "Relative Pose Priors"), + input_products=("View-Graph Relative Rotations", "Relative Pose Priors", "Verified Correspondences"), output_products=("Global Rotations",), parent_plate="Sparse Reconstruction", ) @@ -66,7 +67,6 @@ def _run_rotation_averaging_base( i1Ti2_priors: Dict[Tuple[int, int], PosePrior], v_corr_idxs: Dict[Tuple[int, int], np.ndarray], wTi_gt: List[Optional[Pose3]], - v_corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> Tuple[List[Optional[Rot3]], GtsfmMetricsGroup]: """Runs rotation averaging and computes metrics. @@ -76,7 +76,6 @@ def _run_rotation_averaging_base( i1Ti2_priors: Priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences. wTi_gt: Ground truth global rotations to compare against. - v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences. Returns: Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is