From 8ad34537e970283576f58cc5709f615526438f70 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Mon, 18 Dec 2023 22:09:05 -0800 Subject: [PATCH 01/15] Add GNC rotation averaging --- gtsfm/averaging/rotation/gnc.py | 220 ++++++++++++++++++ gtsfm/configs/deep_front_end.yaml | 8 +- gtsfm/configs/deep_front_end_astrovision.yaml | 2 +- gtsfm/configs/sift_front_end.yaml | 2 +- gtsfm/configs/sift_front_end_astrovision.yaml | 2 +- gtsfm/multi_view_optimizer.py | 4 +- 6 files changed, 229 insertions(+), 9 deletions(-) create mode 100644 gtsfm/averaging/rotation/gnc.py diff --git a/gtsfm/averaging/rotation/gnc.py b/gtsfm/averaging/rotation/gnc.py new file mode 100644 index 000000000..3db8865bb --- /dev/null +++ b/gtsfm/averaging/rotation/gnc.py @@ -0,0 +1,220 @@ +"""Shonan Rotation Averaging. + +The algorithm was proposed in "Shonan Rotation Averaging:Global Optimality by +Surfing SO(p)^n" and is implemented by wrapping up over implementation provided +by GTSAM. + +References: +- https://arxiv.org/abs/2008.02737 +- https://gtsam.org/ + +Authors: Jing Wu, Ayush Baid, John Lambert +""" +from typing import Dict, List, Optional, Set, Tuple + +import gtsam +import numpy as np +from gtsam import ( + BetweenFactorPose3, + BetweenFactorPose3s, + Pose3, + Rot3, + ShonanAveraging3, + GncLMOptimizer, + GncLMParams +) + +import gtsfm.utils.logger as logger_utils +from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase +from gtsfm.common.pose_prior import PosePrior + +ROT3_DOF = 3 +POSE3_DOF = 6 + +logger = logger_utils.get_logger() + +_DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1.0 + + +class GncRotationAveraging(RotationAveragingBase): + """Performs Shonan rotation averaging.""" + + def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_SIGMA) -> None: + """Initializes module. + + Note: `p_min` and `p_max` describe the minimum and maximum relaxation rank. + + Args: + two_view_rotation_sigma: Covariance to use (lower values -> more strictly adhere to input measurements). + """ + self._two_view_rotation_sigma = two_view_rotation_sigma + + def __get_gnc_params(self) -> GncLMParams: + params = GncLMParams() + return params + + def __get_shonan_params(self) -> gtsam.ShonanAveragingParameters3: + lm_params = gtsam.LevenbergMarquardtParams.CeresDefaults() + shonan_params = gtsam.ShonanAveragingParameters3(lm_params) + shonan_params.setUseHuber(False) + shonan_params.setCertifyOptimality(True) + return shonan_params + + def __graph_from_2view_relative_rotations( + self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int] + ) -> BetweenFactorPose3s: + """Create between factors from relative rotations computed by the 2-view estimator.""" + # TODO: how to weight the noise model on relative rotations compared to priors? + noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, self._two_view_rotation_sigma) + between_factors = gtsam.NonlinearFactorGraph() + # graph.addPriorRot3(gtsam.symbol("R", 0), gtsam.Rot3(np.eye(3)), sigma_R0) + + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + if i2Ri1 is not None: + i2_ = old_to_new_idxs[i2] + i1_ = old_to_new_idxs[i1] + between_factors.add(gtsam.BetweenFactorRot3(i2_, i1_, i2Ri1, noise_model)) + + return between_factors + + def __between_factors_from_2view_relative_rotations( + self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int] + ) -> BetweenFactorPose3s: + """Create between factors from relative rotations computed by the 2-view estimator.""" + # TODO: how to weight the noise model on relative rotations compared to priors? + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma) + + between_factors = BetweenFactorPose3s() + + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + if i2Ri1 is not None: + # ignore translation during rotation averaging + i2Ti1 = Pose3(i2Ri1, np.zeros(3)) + i2_ = old_to_new_idxs[i2] + i1_ = old_to_new_idxs[i1] + between_factors.append(BetweenFactorPose3(i2_, i1_, i2Ti1, noise_model)) + + return between_factors + + + def _between_factors_from_pose_priors( + self, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], old_to_new_idxs: Dict[int, int] + ) -> BetweenFactorPose3s: + """Create between factors from the priors on relative poses.""" + between_factors = BetweenFactorPose3s() + + def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float: + """Get the sigma to be used for the isotropic noise model. + We compute the average of the diagonal entries of the covariance matrix. + """ + avg_cov = np.average(np.diag(covariance), axis=None) + return np.sqrt(avg_cov) + + for (i1, i2), i1Ti2_prior in i1Ti2_priors.items(): + i1_ = old_to_new_idxs[i1] + i2_ = old_to_new_idxs[i2] + noise_model_sigma = get_isotropic_noise_model_sigma(i1Ti2_prior.covariance) + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, noise_model_sigma) + between_factors.append(BetweenFactorPose3(i2_, i1_, i1Ti2_prior.value, noise_model)) + + return between_factors + + def _run_with_consecutive_ordering( + self, num_connected_nodes: int, graph: gtsam.NonlinearFactorGraph, between_factors: BetweenFactorPose3s + ) -> List[Optional[Rot3]]: + """Run the rotation averaging on a connected graph w/ N keys ordered consecutively [0,...,N-1]. + + Note: GTSAM requires the N input nodes to be connected and ordered from [0 ... N-1]. + Modifying GTSAM would require a major philosophical overhaul, so we perform the re-ordering + here in a sort of "wrapper". See https://github.com/borglab/gtsam/issues/784 for more details. + + Args: + num_connected_nodes: Number of unique connected nodes (i.e. images) in the graph + (<= the number of images in the dataset) + between_factors: BetweenFactorPose3s created from relative rotations from 2-view estimator and the priors. + + Returns: + Global rotations for each **CONNECTED** camera pose, i.e. wRi, as a list. The number of entries in + the list is `num_connected_nodes`. The list may contain `None` where the global rotation could + not be computed (either underconstrained system or ill-constrained system). + """ + + logger.info("Running GNC rotation averaging...") + shonan = ShonanAveraging3(between_factors, self.__get_shonan_params()) + initial = shonan.initializeRandomly() + + optimizer = GncLMOptimizer(graph, initial, self.__get_gnc_params()) + result = optimizer.optimize() + + wRi_list_consecutive = [None] * num_connected_nodes + for i in range(num_connected_nodes): + if result.exists(i): + wRi_list_consecutive[i] = result.atRot3(i) + logger.info(wRi_list_consecutive) + + return wRi_list_consecutive + + def _nodes_with_edges( + self, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], relative_pose_priors: Dict[Tuple[int, int], PosePrior] + ) -> Set[int]: + """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(): + unique_nodes_with_edges.add(i1) + unique_nodes_with_edges.add(i2) + for (i1, i2) in relative_pose_priors.keys(): + unique_nodes_with_edges.add(i1) + unique_nodes_with_edges.add(i2) + + return unique_nodes_with_edges + + def run_rotation_averaging( + self, + num_images: int, + i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], + i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + ) -> List[Optional[Rot3]]: + """Run the rotation averaging on a connected graph with arbitrary keys, where each key is a image/pose index. + + Note: functions as a wrapper that re-orders keys to prepare a graph w/ N keys ordered [0,...,N-1]. + All input nodes must belong to a single connected component, in order to obtain an absolute pose for each + camera in a single, global coordinate frame. + + 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. + i1Ti2_priors: Priors on relative poses. + + Returns: + Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is + `num_images`. The list may contain `None` where the global rotation could not be computed (either + underconstrained system or ill-constrained system), or where the camera pose had no valid observation + in the input to run_rotation_averaging(). + """ + if len(i2Ri1_dict) == 0: + logger.warning("Shonan cannot proceed: No cycle-consistent triplets found after filtering.") + wRi_list = [None] * num_images + return wRi_list + + 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)} + + between_factors = self.__between_factors_from_2view_relative_rotations( + i2Ri1_dict, old_to_new_idxes + ) + + graph: gtsam.NonlinearFactorGraph = self.__graph_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), graph=graph, between_factors=between_factors + ) + + wRi_list = [None] * num_images + for remapped_i, original_i in enumerate(nodes_with_edges): + wRi_list[original_i] = wRi_list_subset[remapped_i] + + return wRi_list diff --git a/gtsfm/configs/deep_front_end.yaml b/gtsfm/configs/deep_front_end.yaml index 861692d46..cb866b20f 100644 --- a/gtsfm/configs/deep_front_end.yaml +++ b/gtsfm/configs/deep_front_end.yaml @@ -67,7 +67,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging + _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM @@ -95,6 +95,6 @@ SceneOptimizer: calibration_prior_noise_sigma: 1e-5 measurement_noise_sigma: 1.0 - # comment out to not run - dense_multiview_optimizer: - _target_: gtsfm.densify.mvs_patchmatchnet.MVSPatchmatchNet + # # comment out to not run + # dense_multiview_optimizer: + # _target_: gtsfm.densify.mvs_patchmatchnet.MVSPatchmatchNet diff --git a/gtsfm/configs/deep_front_end_astrovision.yaml b/gtsfm/configs/deep_front_end_astrovision.yaml index 7051c3a0a..8f971bfc0 100644 --- a/gtsfm/configs/deep_front_end_astrovision.yaml +++ b/gtsfm/configs/deep_front_end_astrovision.yaml @@ -66,7 +66,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging + _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM diff --git a/gtsfm/configs/sift_front_end.yaml b/gtsfm/configs/sift_front_end.yaml index 16ce76994..cfdd7f101 100644 --- a/gtsfm/configs/sift_front_end.yaml +++ b/gtsfm/configs/sift_front_end.yaml @@ -68,7 +68,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging + _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM diff --git a/gtsfm/configs/sift_front_end_astrovision.yaml b/gtsfm/configs/sift_front_end_astrovision.yaml index 0c800b6a6..3e9124881 100644 --- a/gtsfm/configs/sift_front_end_astrovision.yaml +++ b/gtsfm/configs/sift_front_end_astrovision.yaml @@ -64,7 +64,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging + _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM diff --git a/gtsfm/multi_view_optimizer.py b/gtsfm/multi_view_optimizer.py index 03b14a1a9..d11b33d6a 100644 --- a/gtsfm/multi_view_optimizer.py +++ b/gtsfm/multi_view_optimizer.py @@ -22,7 +22,7 @@ from gtsfm.data_association.data_assoc import DataAssociation from gtsfm.evaluation.metrics import GtsfmMetricsGroup from gtsfm.view_graph_estimator.view_graph_estimator_base import ViewGraphEstimatorBase -from gtsfm.data_association.cpp_dsf_tracks_estimator import CppDsfTracksEstimator +from gtsfm.data_association.dsf_tracks_estimator import DsfTracksEstimator from gtsfm.common.two_view_estimation_report import TwoViewEstimationReport @@ -194,5 +194,5 @@ def init_cameras( def get_2d_tracks( corr_idxs_dict: Dict[Tuple[int, int], np.ndarray], keypoints_list: List[Keypoints] ) -> List[SfmTrack2d]: - tracks_estimator = CppDsfTracksEstimator() + tracks_estimator = DsfTracksEstimator() return tracks_estimator.run(corr_idxs_dict, keypoints_list) From 97d930fb57e45bd06f7768003fa4e0c3340b5cd8 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Tue, 19 Dec 2023 08:55:49 -0800 Subject: [PATCH 02/15] Update unified config --- gtsfm/configs/unified.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsfm/configs/unified.yaml b/gtsfm/configs/unified.yaml index 8fef52586..764ca8f14 100644 --- a/gtsfm/configs/unified.yaml +++ b/gtsfm/configs/unified.yaml @@ -68,7 +68,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging + _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM From f3014bd6dd190cc25a1d4cb672b66d3c3ad498d0 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Tue, 19 Dec 2023 15:26:08 -0800 Subject: [PATCH 03/15] Add combined Shonan and GNC --- gtsfm/averaging/rotation/gnc.py | 2 +- gtsfm/averaging/rotation/shonan_gnc.py | 234 +++++++++++++++++++++++++ gtsfm/configs/deep_front_end.yaml | 2 +- 3 files changed, 236 insertions(+), 2 deletions(-) create mode 100644 gtsfm/averaging/rotation/shonan_gnc.py diff --git a/gtsfm/averaging/rotation/gnc.py b/gtsfm/averaging/rotation/gnc.py index 3db8865bb..30187a950 100644 --- a/gtsfm/averaging/rotation/gnc.py +++ b/gtsfm/averaging/rotation/gnc.py @@ -33,7 +33,7 @@ logger = logger_utils.get_logger() -_DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1.0 +_DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1e-1 class GncRotationAveraging(RotationAveragingBase): diff --git a/gtsfm/averaging/rotation/shonan_gnc.py b/gtsfm/averaging/rotation/shonan_gnc.py new file mode 100644 index 000000000..b50273c57 --- /dev/null +++ b/gtsfm/averaging/rotation/shonan_gnc.py @@ -0,0 +1,234 @@ +"""Shonan Rotation Averaging. + +The algorithm was proposed in "Shonan Rotation Averaging:Global Optimality by +Surfing SO(p)^n" and is implemented by wrapping up over implementation provided +by GTSAM. + +References: +- https://arxiv.org/abs/2008.02737 +- https://gtsam.org/ + +Authors: Jing Wu, Ayush Baid, John Lambert +""" +from typing import Dict, List, Optional, Set, Tuple + +import gtsam +import numpy as np +from gtsam import ( + BetweenFactorPose3, + BetweenFactorPose3s, + LevenbergMarquardtParams, + Pose3, + Rot3, + ShonanAveraging3, + ShonanAveragingParameters3, + GncLMOptimizer, + GncLMParams +) + +import gtsfm.utils.logger as logger_utils +from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase +from gtsfm.common.pose_prior import PosePrior + +ROT3_DOF = 3 +POSE3_DOF = 6 + +logger = logger_utils.get_logger() + +_DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1.0 + + +class CombinedShonanGncRotationAveraging(RotationAveragingBase): + """Performs Shonan rotation averaging.""" + + def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_SIGMA) -> None: + """Initializes module. + + Note: `p_min` and `p_max` describe the minimum and maximum relaxation rank. + + Args: + two_view_rotation_sigma: Covariance to use (lower values -> more strictly adhere to input measurements). + """ + self._two_view_rotation_sigma = two_view_rotation_sigma + self._p_min = 5 + self._p_max = 30 + + def __get_gnc_params(self) -> GncLMParams: + params = GncLMParams() + return params + + def __get_shonan_params(self) -> gtsam.ShonanAveragingParameters3: + lm_params = LevenbergMarquardtParams.CeresDefaults() + shonan_params = ShonanAveragingParameters3(lm_params) + shonan_params.setUseHuber(False) + shonan_params.setCertifyOptimality(True) + return shonan_params + + def __graph_from_2view_relative_rotations( + self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int] + ) -> BetweenFactorPose3s: + """Create between factors from relative rotations computed by the 2-view estimator.""" + # TODO: how to weight the noise model on relative rotations compared to priors? + noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, self._two_view_rotation_sigma) + between_factors = gtsam.NonlinearFactorGraph() + # graph.addPriorRot3(gtsam.symbol("R", 0), gtsam.Rot3(np.eye(3)), sigma_R0) + + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + if i2Ri1 is not None: + i2_ = old_to_new_idxs[i2] + i1_ = old_to_new_idxs[i1] + between_factors.add(gtsam.BetweenFactorRot3(i2_, i1_, i2Ri1, noise_model)) + + return between_factors + + def __between_factors_from_2view_relative_rotations( + self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int] + ) -> BetweenFactorPose3s: + """Create between factors from relative rotations computed by the 2-view estimator.""" + # TODO: how to weight the noise model on relative rotations compared to priors? + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma) + + between_factors = BetweenFactorPose3s() + + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + if i2Ri1 is not None: + # ignore translation during rotation averaging + i2Ti1 = Pose3(i2Ri1, np.zeros(3)) + i2_ = old_to_new_idxs[i2] + i1_ = old_to_new_idxs[i1] + between_factors.append(BetweenFactorPose3(i2_, i1_, i2Ti1, noise_model)) + + return between_factors + + + def _between_factors_from_pose_priors( + self, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], old_to_new_idxs: Dict[int, int] + ) -> BetweenFactorPose3s: + """Create between factors from the priors on relative poses.""" + between_factors = BetweenFactorPose3s() + + def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float: + """Get the sigma to be used for the isotropic noise model. + We compute the average of the diagonal entries of the covariance matrix. + """ + avg_cov = np.average(np.diag(covariance), axis=None) + return np.sqrt(avg_cov) + + for (i1, i2), i1Ti2_prior in i1Ti2_priors.items(): + i1_ = old_to_new_idxs[i1] + i2_ = old_to_new_idxs[i2] + noise_model_sigma = get_isotropic_noise_model_sigma(i1Ti2_prior.covariance) + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, noise_model_sigma) + between_factors.append(BetweenFactorPose3(i2_, i1_, i1Ti2_prior.value, noise_model)) + + return between_factors + + def _run_with_consecutive_ordering( + self, num_connected_nodes: int, graph: gtsam.NonlinearFactorGraph, between_factors: BetweenFactorPose3s + ) -> List[Optional[Rot3]]: + """Run the rotation averaging on a connected graph w/ N keys ordered consecutively [0,...,N-1]. + + Note: GTSAM requires the N input nodes to be connected and ordered from [0 ... N-1]. + Modifying GTSAM would require a major philosophical overhaul, so we perform the re-ordering + here in a sort of "wrapper". See https://github.com/borglab/gtsam/issues/784 for more details. + + Args: + num_connected_nodes: Number of unique connected nodes (i.e. images) in the graph + (<= the number of images in the dataset) + between_factors: BetweenFactorPose3s created from relative rotations from 2-view estimator and the priors. + + Returns: + Global rotations for each **CONNECTED** camera pose, i.e. wRi, as a list. The number of entries in + the list is `num_connected_nodes`. The list may contain `None` where the global rotation could + not be computed (either underconstrained system or ill-constrained system). + """ + # Run Shonan. + logger.info( + "Running Shonan with %d constraints on %d nodes", + len(between_factors), + num_connected_nodes, + ) + shonan = ShonanAveraging3(between_factors, self.__get_shonan_params()) + + 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)) + + # Run GNC. + logger.info("Running GNC rotation averaging...") + optimizer = GncLMOptimizer(graph, result, self.__get_gnc_params()) + result = optimizer.optimize() + + wRi_list_consecutive = [None] * num_connected_nodes + for i in range(num_connected_nodes): + if result.exists(i): + wRi_list_consecutive[i] = result.atRot3(i) + logger.info(wRi_list_consecutive) + + return wRi_list_consecutive + + def _nodes_with_edges( + self, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], relative_pose_priors: Dict[Tuple[int, int], PosePrior] + ) -> Set[int]: + """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(): + unique_nodes_with_edges.add(i1) + unique_nodes_with_edges.add(i2) + for (i1, i2) in relative_pose_priors.keys(): + unique_nodes_with_edges.add(i1) + unique_nodes_with_edges.add(i2) + + return unique_nodes_with_edges + + def run_rotation_averaging( + self, + num_images: int, + i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], + i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + ) -> List[Optional[Rot3]]: + """Run the rotation averaging on a connected graph with arbitrary keys, where each key is a image/pose index. + + Note: functions as a wrapper that re-orders keys to prepare a graph w/ N keys ordered [0,...,N-1]. + All input nodes must belong to a single connected component, in order to obtain an absolute pose for each + camera in a single, global coordinate frame. + + 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. + i1Ti2_priors: Priors on relative poses. + + Returns: + Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is + `num_images`. The list may contain `None` where the global rotation could not be computed (either + underconstrained system or ill-constrained system), or where the camera pose had no valid observation + in the input to run_rotation_averaging(). + """ + if len(i2Ri1_dict) == 0: + logger.warning("Shonan cannot proceed: No cycle-consistent triplets found after filtering.") + wRi_list = [None] * num_images + return wRi_list + + 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)} + + between_factors = self.__between_factors_from_2view_relative_rotations( + i2Ri1_dict, old_to_new_idxes + ) + + graph: gtsam.NonlinearFactorGraph = self.__graph_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), graph=graph, between_factors=between_factors + ) + + wRi_list = [None] * num_images + for remapped_i, original_i in enumerate(nodes_with_edges): + wRi_list[original_i] = wRi_list_subset[remapped_i] + + return wRi_list diff --git a/gtsfm/configs/deep_front_end.yaml b/gtsfm/configs/deep_front_end.yaml index cb866b20f..e7cd2e0b1 100644 --- a/gtsfm/configs/deep_front_end.yaml +++ b/gtsfm/configs/deep_front_end.yaml @@ -67,7 +67,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging + _target_: gtsfm.averaging.rotation.shonan_gnc.CombinedShonanGncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM From 08d11956132516064a2a32851591a1e704781e49 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 08:44:27 -0800 Subject: [PATCH 04/15] Switch to combined Shonan and GNC --- gtsfm/configs/unified.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsfm/configs/unified.yaml b/gtsfm/configs/unified.yaml index 764ca8f14..4486d7f18 100644 --- a/gtsfm/configs/unified.yaml +++ b/gtsfm/configs/unified.yaml @@ -68,7 +68,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging + _target_: gtsfm.averaging.rotation.shonan_gnc.CombinedShonanGncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM From f313c861d8b208700f5be9fe149df6fa4d601204 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 09:55:45 -0800 Subject: [PATCH 05/15] Increase num_matched to 10 --- gtsfm/averaging/rotation/gnc.py | 62 +++++++++++++++++-- .../rotation/rotation_averaging_base.py | 8 ++- gtsfm/configs/deep_front_end.yaml | 2 +- gtsfm/configs/unified.yaml | 2 +- gtsfm/multi_view_optimizer.py | 2 +- 5 files changed, 67 insertions(+), 9 deletions(-) diff --git a/gtsfm/averaging/rotation/gnc.py b/gtsfm/averaging/rotation/gnc.py index 30187a950..67a599cc6 100644 --- a/gtsfm/averaging/rotation/gnc.py +++ b/gtsfm/averaging/rotation/gnc.py @@ -14,6 +14,7 @@ import gtsam import numpy as np +import scipy from gtsam import ( BetweenFactorPose3, BetweenFactorPose3s, @@ -120,7 +121,11 @@ def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float: return between_factors def _run_with_consecutive_ordering( - self, num_connected_nodes: int, graph: gtsam.NonlinearFactorGraph, between_factors: BetweenFactorPose3s + self, + num_connected_nodes: int, + graph: gtsam.NonlinearFactorGraph, + between_factors: BetweenFactorPose3s, + initial: gtsam.Values, ) -> List[Optional[Rot3]]: """Run the rotation averaging on a connected graph w/ N keys ordered consecutively [0,...,N-1]. @@ -140,8 +145,8 @@ def _run_with_consecutive_ordering( """ logger.info("Running GNC rotation averaging...") - shonan = ShonanAveraging3(between_factors, self.__get_shonan_params()) - initial = shonan.initializeRandomly() + #shonan = ShonanAveraging3(between_factors, self.__get_shonan_params()) + #initial = shonan.initializeRandomly() optimizer = GncLMOptimizer(graph, initial, self.__get_gnc_params()) result = optimizer.optimize() @@ -174,6 +179,7 @@ def run_rotation_averaging( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + 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. @@ -204,13 +210,15 @@ def run_rotation_averaging( i2Ri1_dict, old_to_new_idxes ) + initial = initialize_mst(num_images, i2Ri1_dict, corr_idxs) + graph: gtsam.NonlinearFactorGraph = self.__graph_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), graph=graph, between_factors=between_factors + len(nodes_with_edges), graph, between_factors, initial ) wRi_list = [None] * num_images @@ -218,3 +226,49 @@ def run_rotation_averaging( wRi_list[original_i] = wRi_list_subset[remapped_i] 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], + ) -> 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(-len(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. + i_mst, j_mst = Tcsr.nonzero() + logger.info(i_mst) + logger.info(j_mst) + iR0_dict = {0: np.eye(3)} + for (i, j) in zip(i_mst, j_mst): + 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: + assert False + + # Add to Values object. + initial = gtsam.Values() + for i, iR0 in iR0_dict.items(): + initial.insert(i, Rot3(iR0)) + + return initial + + + diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index 7fec94a9a..09f862122 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -7,6 +7,7 @@ from typing import Dict, List, Optional, Tuple import dask +import numpy as np from dask.delayed import Delayed from gtsam import Pose3, Rot3 @@ -41,6 +42,7 @@ def run_rotation_averaging( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> List[Optional[Rot3]]: """Run the rotation averaging. @@ -61,6 +63,7 @@ def _run_rotation_averaging_base( i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], wTi_gt: List[Optional[Pose3]], + corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> Tuple[List[Optional[Rot3]], GtsfmMetricsGroup]: """Runs rotation averaging and computes metrics. @@ -77,7 +80,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, corr_idxs) run_time = time.time() - start_time metrics = self.evaluate(wRis, wTi_gt) @@ -116,6 +119,7 @@ def create_computation_graph( i2Ri1_graph: Delayed, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], gt_wTi_list: List[Optional[Pose3]], + corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> Tuple[Delayed, Delayed]: """Create the computation graph for performing rotation averaging. @@ -130,7 +134,7 @@ 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_graph, i1Ti2_priors, gt_wTi_list, corr_idxs ) return wRis, metrics diff --git a/gtsfm/configs/deep_front_end.yaml b/gtsfm/configs/deep_front_end.yaml index e7cd2e0b1..cb866b20f 100644 --- a/gtsfm/configs/deep_front_end.yaml +++ b/gtsfm/configs/deep_front_end.yaml @@ -67,7 +67,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan_gnc.CombinedShonanGncRotationAveraging + _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM diff --git a/gtsfm/configs/unified.yaml b/gtsfm/configs/unified.yaml index 4486d7f18..a2fcb8353 100644 --- a/gtsfm/configs/unified.yaml +++ b/gtsfm/configs/unified.yaml @@ -15,7 +15,7 @@ SceneOptimizer: _target_: gtsfm.frontend.global_descriptor.netvlad_global_descriptor.NetVLADGlobalDescriptor retriever: _target_: gtsfm.retriever.joint_netvlad_sequential_retriever.JointNetVLADSequentialRetriever - num_matched: 5 + num_matched: 10 min_score: 0.3 max_frame_lookahead: 15 diff --git a/gtsfm/multi_view_optimizer.py b/gtsfm/multi_view_optimizer.py index d11b33d6a..775f2d444 100644 --- a/gtsfm/multi_view_optimizer.py +++ b/gtsfm/multi_view_optimizer.py @@ -124,7 +124,7 @@ 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, relative_pose_priors, gt_wTi_list, viewgraph_v_corr_idxs_graph ) tracks2d_graph = dask.delayed(get_2d_tracks)(viewgraph_v_corr_idxs_graph, keypoints_list) From 20dd5efe8505ad588dfcdd334561a57e08970250 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 10:07:22 -0800 Subject: [PATCH 06/15] Initialize GNC with MST --- gtsfm/averaging/rotation/gnc.py | 4 +--- gtsfm/configs/unified.yaml | 4 ++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsfm/averaging/rotation/gnc.py b/gtsfm/averaging/rotation/gnc.py index 67a599cc6..6842be9dd 100644 --- a/gtsfm/averaging/rotation/gnc.py +++ b/gtsfm/averaging/rotation/gnc.py @@ -34,7 +34,7 @@ logger = logger_utils.get_logger() -_DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1e-1 +_DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1.0 class GncRotationAveraging(RotationAveragingBase): @@ -248,8 +248,6 @@ def initialize_mst( # Build global rotations from MST. i_mst, j_mst = Tcsr.nonzero() - logger.info(i_mst) - logger.info(j_mst) iR0_dict = {0: np.eye(3)} for (i, j) in zip(i_mst, j_mst): if i in iR0_dict: diff --git a/gtsfm/configs/unified.yaml b/gtsfm/configs/unified.yaml index a2fcb8353..764ca8f14 100644 --- a/gtsfm/configs/unified.yaml +++ b/gtsfm/configs/unified.yaml @@ -15,7 +15,7 @@ SceneOptimizer: _target_: gtsfm.frontend.global_descriptor.netvlad_global_descriptor.NetVLADGlobalDescriptor retriever: _target_: gtsfm.retriever.joint_netvlad_sequential_retriever.JointNetVLADSequentialRetriever - num_matched: 10 + num_matched: 5 min_score: 0.3 max_frame_lookahead: 15 @@ -68,7 +68,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan_gnc.CombinedShonanGncRotationAveraging + _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM From 0316797645aaef9677a97cf19c0ff258d2ca6820 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 12:05:09 -0800 Subject: [PATCH 07/15] Debugging MST --- gtsfm/averaging/rotation/gnc.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/gtsfm/averaging/rotation/gnc.py b/gtsfm/averaging/rotation/gnc.py index 6842be9dd..a3da910d7 100644 --- a/gtsfm/averaging/rotation/gnc.py +++ b/gtsfm/averaging/rotation/gnc.py @@ -210,7 +210,7 @@ def run_rotation_averaging( i2Ri1_dict, old_to_new_idxes ) - initial = initialize_mst(num_images, i2Ri1_dict, corr_idxs) + initial = initialize_mst(num_images, i2Ri1_dict, corr_idxs, old_to_new_idxes) graph: gtsam.NonlinearFactorGraph = self.__graph_from_2view_relative_rotations( i2Ri1_dict, old_to_new_idxes @@ -232,6 +232,7 @@ 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. @@ -248,8 +249,14 @@ def initialize_mst( # Build global rotations from MST. i_mst, j_mst = Tcsr.nonzero() - iR0_dict = {0: np.eye(3)} - for (i, j) in zip(i_mst, j_mst): + 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] @@ -259,12 +266,16 @@ def initialize_mst( 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(i, Rot3(iR0)) + initial.insert(old_to_new_idxs[i], Rot3(iR0)) return initial From 4dc91c32395c0043c35864f244fcd9de0c528763 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 12:22:55 -0800 Subject: [PATCH 08/15] Remove MST max iterations --- gtsfm/averaging/rotation/gnc.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsfm/averaging/rotation/gnc.py b/gtsfm/averaging/rotation/gnc.py index a3da910d7..30a018afc 100644 --- a/gtsfm/averaging/rotation/gnc.py +++ b/gtsfm/averaging/rotation/gnc.py @@ -253,7 +253,7 @@ def initialize_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 + # max_iters = num_images * 10 iter = 0 while len(edges_mst) > 0: i, j = edges_mst.pop(0) @@ -268,9 +268,9 @@ def initialize_mst( else: edges_mst.append((i, j)) iter += 1 - if iter >= max_iters: - logger.info("Reached max MST iters.") - assert False + # if iter >= max_iters: + # logger.info("Reached max MST iters.") + # assert False # Add to Values object. initial = gtsam.Values() From 1cff18c16653d07ae80ecdc3bd488426b19259f9 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 13:15:35 -0800 Subject: [PATCH 09/15] Use Shonan weighted by # inliers --- gtsfm/averaging/rotation/gnc.py | 3 ++- gtsfm/averaging/rotation/shonan.py | 11 +++++++---- gtsfm/configs/unified.yaml | 2 +- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/gtsfm/averaging/rotation/gnc.py b/gtsfm/averaging/rotation/gnc.py index 30a018afc..5fac3dcef 100644 --- a/gtsfm/averaging/rotation/gnc.py +++ b/gtsfm/averaging/rotation/gnc.py @@ -242,7 +242,8 @@ def initialize_mst( continue row.append(i1) col.append(i2) - data.append(-len(corr_idxs[(i1, 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)) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index a6025b8f2..6f9ac2d0d 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -58,17 +58,19 @@ def __get_shonan_params(self) -> ShonanAveragingParameters3: return shonan_params def __between_factors_from_2view_relative_rotations( - self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int] + self, + i2Ri1_dict: Dict[Tuple[int, int], Rot3], + corr_idxs: Dict[Tuple[int, int], np.ndarray], + old_to_new_idxs: Dict[int, int], ) -> BetweenFactorPose3s: """Create between factors from relative rotations computed by the 2-view estimator.""" # TODO: how to weight the noise model on relative rotations compared to priors? - noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma) between_factors = BetweenFactorPose3s() - for (i1, i2), i2Ri1 in i2Ri1_dict.items(): if i2Ri1 is not None: # ignore translation during rotation averaging + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, 1 / corr_idxs[(i1, i2)].shape[0]) i2Ti1 = Pose3(i2Ri1, np.zeros(3)) i2_ = old_to_new_idxs[i2] i1_ = old_to_new_idxs[i1] @@ -157,6 +159,7 @@ def run_rotation_averaging( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + 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. @@ -184,7 +187,7 @@ def run_rotation_averaging( old_to_new_idxes = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)} between_factors: BetweenFactorPose3s = self.__between_factors_from_2view_relative_rotations( - i2Ri1_dict, old_to_new_idxes + i2Ri1_dict, corr_idxs, old_to_new_idxes ) between_factors.extend(self._between_factors_from_pose_priors(i1Ti2_priors, old_to_new_idxes)) diff --git a/gtsfm/configs/unified.yaml b/gtsfm/configs/unified.yaml index 764ca8f14..8fef52586 100644 --- a/gtsfm/configs/unified.yaml +++ b/gtsfm/configs/unified.yaml @@ -68,7 +68,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.gnc.GncRotationAveraging + _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM From 5fb4c37c0682160678a67cdd2cf6d6b14c7e38b9 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 13:31:14 -0800 Subject: [PATCH 10/15] Increase max p to 64 --- gtsfm/averaging/rotation/gnc.py | 1 + gtsfm/averaging/rotation/shonan.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsfm/averaging/rotation/gnc.py b/gtsfm/averaging/rotation/gnc.py index 5fac3dcef..adbfa8d75 100644 --- a/gtsfm/averaging/rotation/gnc.py +++ b/gtsfm/averaging/rotation/gnc.py @@ -249,6 +249,7 @@ def initialize_mst( 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) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 6f9ac2d0d..cdca7391f 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -48,7 +48,7 @@ def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_S """ self._two_view_rotation_sigma = two_view_rotation_sigma self._p_min = 5 - self._p_max = 30 + self._p_max = 64 def __get_shonan_params(self) -> ShonanAveragingParameters3: lm_params = LevenbergMarquardtParams.CeresDefaults() From 93aa0b2e1a3c73b17f63236f1b4852e4e708a0dd Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 14:52:22 -0800 Subject: [PATCH 11/15] Use Huber loss --- 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 cdca7391f..9a52a8f9d 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -53,7 +53,7 @@ def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_S def __get_shonan_params(self) -> ShonanAveragingParameters3: lm_params = LevenbergMarquardtParams.CeresDefaults() shonan_params = ShonanAveragingParameters3(lm_params) - shonan_params.setUseHuber(False) + shonan_params.setUseHuber(True) shonan_params.setCertifyOptimality(True) return shonan_params From 883258e33a5ad768ecece1539bd8216903c32410 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 15:30:28 -0800 Subject: [PATCH 12/15] Use Huber --- gtsfm/averaging/rotation/shonan.py | 5 ++--- gtsfm/averaging/rotation/shonan_gnc.py | 13 +++++++------ gtsfm/configs/unified.yaml | 2 +- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 9a52a8f9d..1f6fbb4bf 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -48,13 +48,13 @@ def __init__(self, two_view_rotation_sigma: float = _DEFAULT_TWO_VIEW_ROTATION_S """ self._two_view_rotation_sigma = two_view_rotation_sigma self._p_min = 5 - self._p_max = 64 + self._p_max = 5 def __get_shonan_params(self) -> ShonanAveragingParameters3: lm_params = LevenbergMarquardtParams.CeresDefaults() shonan_params = ShonanAveragingParameters3(lm_params) shonan_params.setUseHuber(True) - shonan_params.setCertifyOptimality(True) + shonan_params.setCertifyOptimality(False) return shonan_params def __between_factors_from_2view_relative_rotations( @@ -65,7 +65,6 @@ def __between_factors_from_2view_relative_rotations( ) -> BetweenFactorPose3s: """Create between factors from relative rotations computed by the 2-view estimator.""" # TODO: how to weight the noise model on relative rotations compared to priors? - between_factors = BetweenFactorPose3s() for (i1, i2), i2Ri1 in i2Ri1_dict.items(): if i2Ri1 is not None: diff --git a/gtsfm/averaging/rotation/shonan_gnc.py b/gtsfm/averaging/rotation/shonan_gnc.py index b50273c57..23e1de726 100644 --- a/gtsfm/averaging/rotation/shonan_gnc.py +++ b/gtsfm/averaging/rotation/shonan_gnc.py @@ -82,17 +82,18 @@ def __graph_from_2view_relative_rotations( return between_factors def __between_factors_from_2view_relative_rotations( - self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int] + self, + i2Ri1_dict: Dict[Tuple[int, int], Rot3], + corr_idxs: Dict[Tuple[int, int], np.ndarray], + old_to_new_idxs: Dict[int, int], ) -> BetweenFactorPose3s: """Create between factors from relative rotations computed by the 2-view estimator.""" # TODO: how to weight the noise model on relative rotations compared to priors? - noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma) - between_factors = BetweenFactorPose3s() - for (i1, i2), i2Ri1 in i2Ri1_dict.items(): if i2Ri1 is not None: # ignore translation during rotation averaging + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, 1 / corr_idxs[(i1, i2)].shape[0]) i2Ti1 = Pose3(i2Ri1, np.zeros(3)) i2_ = old_to_new_idxs[i2] i1_ = old_to_new_idxs[i1] @@ -100,7 +101,6 @@ def __between_factors_from_2view_relative_rotations( return between_factors - def _between_factors_from_pose_priors( self, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], old_to_new_idxs: Dict[int, int] ) -> BetweenFactorPose3s: @@ -188,6 +188,7 @@ def run_rotation_averaging( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], + 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. @@ -215,7 +216,7 @@ def run_rotation_averaging( old_to_new_idxes = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)} between_factors = self.__between_factors_from_2view_relative_rotations( - i2Ri1_dict, old_to_new_idxes + i2Ri1_dict, corr_idxs, old_to_new_idxes ) graph: gtsam.NonlinearFactorGraph = self.__graph_from_2view_relative_rotations( diff --git a/gtsfm/configs/unified.yaml b/gtsfm/configs/unified.yaml index 8fef52586..4486d7f18 100644 --- a/gtsfm/configs/unified.yaml +++ b/gtsfm/configs/unified.yaml @@ -68,7 +68,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging + _target_: gtsfm.averaging.rotation.shonan_gnc.CombinedShonanGncRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM From 301be6b4f7867b9d7078dc4f5db1cff55ed9a6de Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 16:00:32 -0800 Subject: [PATCH 13/15] Weight GNC with matches --- gtsfm/averaging/rotation/shonan_gnc.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan_gnc.py b/gtsfm/averaging/rotation/shonan_gnc.py index 23e1de726..c87349cc1 100644 --- a/gtsfm/averaging/rotation/shonan_gnc.py +++ b/gtsfm/averaging/rotation/shonan_gnc.py @@ -35,7 +35,7 @@ logger = logger_utils.get_logger() -_DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1.0 +_DEFAULT_TWO_VIEW_ROTATION_SIGMA = 1e-1 class CombinedShonanGncRotationAveraging(RotationAveragingBase): @@ -65,16 +65,18 @@ def __get_shonan_params(self) -> gtsam.ShonanAveragingParameters3: return shonan_params def __graph_from_2view_relative_rotations( - self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int] + self, + i2Ri1_dict: Dict[Tuple[int, int], Rot3], + corr_idxs: Dict[Tuple[int, int], np.ndarray], + old_to_new_idxs: Dict[int, int], ) -> BetweenFactorPose3s: """Create between factors from relative rotations computed by the 2-view estimator.""" # TODO: how to weight the noise model on relative rotations compared to priors? - noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, self._two_view_rotation_sigma) between_factors = gtsam.NonlinearFactorGraph() # graph.addPriorRot3(gtsam.symbol("R", 0), gtsam.Rot3(np.eye(3)), sigma_R0) - for (i1, i2), i2Ri1 in i2Ri1_dict.items(): if i2Ri1 is not None: + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, 1 / corr_idxs[(i1, i2)].shape[0]) i2_ = old_to_new_idxs[i2] i1_ = old_to_new_idxs[i1] between_factors.add(gtsam.BetweenFactorRot3(i2_, i1_, i2Ri1, noise_model)) From 8842c5d6121147e479cbddf36da6b35e80c57e59 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 16:06:37 -0800 Subject: [PATCH 14/15] Dummy push to make CI run --- gtsfm/averaging/rotation/shonan_gnc.py | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsfm/averaging/rotation/shonan_gnc.py b/gtsfm/averaging/rotation/shonan_gnc.py index c87349cc1..4b173dc2e 100644 --- a/gtsfm/averaging/rotation/shonan_gnc.py +++ b/gtsfm/averaging/rotation/shonan_gnc.py @@ -73,6 +73,7 @@ def __graph_from_2view_relative_rotations( """Create between factors from relative rotations computed by the 2-view estimator.""" # TODO: how to weight the noise model on relative rotations compared to priors? between_factors = gtsam.NonlinearFactorGraph() + # graph.addPriorRot3(gtsam.symbol("R", 0), gtsam.Rot3(np.eye(3)), sigma_R0) for (i1, i2), i2Ri1 in i2Ri1_dict.items(): if i2Ri1 is not None: From 17876b8ebdfea5306e6b2936cc5c6e6fa9a2aa97 Mon Sep 17 00:00:00 2001 From: travisdriver Date: Wed, 20 Dec 2023 16:24:21 -0800 Subject: [PATCH 15/15] IW-Shonan w/ Huber --- gtsfm/averaging/rotation/shonan_gnc.py | 6 +++--- gtsfm/configs/unified.yaml | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan_gnc.py b/gtsfm/averaging/rotation/shonan_gnc.py index 4b173dc2e..2069aacb8 100644 --- a/gtsfm/averaging/rotation/shonan_gnc.py +++ b/gtsfm/averaging/rotation/shonan_gnc.py @@ -77,7 +77,7 @@ def __graph_from_2view_relative_rotations( # graph.addPriorRot3(gtsam.symbol("R", 0), gtsam.Rot3(np.eye(3)), sigma_R0) for (i1, i2), i2Ri1 in i2Ri1_dict.items(): if i2Ri1 is not None: - noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, 1 / corr_idxs[(i1, i2)].shape[0]) + noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, 1 / corr_idxs[(i1, i2)].shape[0]) i2_ = old_to_new_idxs[i2] i1_ = old_to_new_idxs[i1] between_factors.add(gtsam.BetweenFactorRot3(i2_, i1_, i2Ri1, noise_model)) @@ -96,7 +96,7 @@ def __between_factors_from_2view_relative_rotations( for (i1, i2), i2Ri1 in i2Ri1_dict.items(): if i2Ri1 is not None: # ignore translation during rotation averaging - noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, 1 / corr_idxs[(i1, i2)].shape[0]) + noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, 1 / corr_idxs[(i1, i2)].shape[0]) i2Ti1 = Pose3(i2Ri1, np.zeros(3)) i2_ = old_to_new_idxs[i2] i1_ = old_to_new_idxs[i1] @@ -223,7 +223,7 @@ def run_rotation_averaging( ) graph: gtsam.NonlinearFactorGraph = self.__graph_from_2view_relative_rotations( - i2Ri1_dict, old_to_new_idxes + i2Ri1_dict, corr_idxs, old_to_new_idxes ) # between_factors.extend(self._between_factors_from_pose_priors(i1Ti2_priors, old_to_new_idxes)) diff --git a/gtsfm/configs/unified.yaml b/gtsfm/configs/unified.yaml index 4486d7f18..8fef52586 100644 --- a/gtsfm/configs/unified.yaml +++ b/gtsfm/configs/unified.yaml @@ -68,7 +68,7 @@ SceneOptimizer: edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR rot_avg_module: - _target_: gtsfm.averaging.rotation.shonan_gnc.CombinedShonanGncRotationAveraging + _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM