From 8794b57290878aa66e73ed5b33407dd91a395392 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 18 Mar 2024 08:34:00 -0400 Subject: [PATCH 01/18] add iw-shonan --- .../rotation/rotation_averaging_base.py | 39 +++++--- gtsfm/averaging/rotation/shonan.py | 92 ++++++++++++++----- gtsfm/multi_view_optimizer.py | 10 +- 3 files changed, 100 insertions(+), 41 deletions(-) diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index feb1b3597..bd26d6079 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -8,6 +8,7 @@ from typing import Dict, List, Optional, Tuple import dask +import numpy as np from dask.delayed import Delayed from gtsam import Pose3, Rot3 @@ -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], + 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. + 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 @@ -62,6 +65,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]], + v_corr_idxs: Dict[Tuple[int, int], np.ndarray], ) -> Tuple[List[Optional[Rot3]], GtsfmMetricsGroup]: """Runs rotation averaging and computes metrics. @@ -70,6 +74,7 @@ def _run_rotation_averaging_base( i2Ri1_dict: Relative rotations as dictionary (i1, i2): i2Ri1. i1Ti2_priors: Priors on relative poses as dictionary(i1, i2): PosePrior on i1Ti2. 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 @@ -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, v_corr_idxs) run_time = time.time() - start_time metrics = self.evaluate(wRis, wTi_gt) @@ -93,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] @@ -117,21 +122,27 @@ def create_computation_graph( i2Ri1_graph: Delayed, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], 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. - 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)( - 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, + v_corr_idxs=v_corr_idxs, ) - return wRis, metrics + return wRis, metrics \ No newline at end of file diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 9ddb6fb90..f19483171 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -10,6 +10,7 @@ Authors: Jing Wu, Ayush Baid, John Lambert """ + from typing import Dict, List, Optional, Set, Tuple import gtsam @@ -22,12 +23,15 @@ Rot3, ShonanAveraging3, ShonanAveragingParameters3, + Values, ) 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 +ROT3_DOF = 3 POSE3_DOF = 6 logger = logger_utils.get_logger() @@ -46,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 @@ -57,24 +62,40 @@ def __get_shonan_params(self) -> ShonanAveragingParameters3: shonan_params.setCertifyOptimality(True) 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] + # ) -> 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_2view_relative_rotations( - self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], old_to_new_idxs: Dict[int, int] - ) -> BetweenFactorPose3s: + self, + i2Ri1_dict: Dict[Tuple[int, int], Rot3], + num_correspondences_dict: Dict[Tuple[int, int], int], + ) -> gtsam.BinaryMeasurementsRot3: """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() - + measurements = gtsam.BinaryMeasurementsRot3() for (i1, i2), i2Ri1 in i2Ri1_dict.items(): - if i2Ri1 is not None: + if i2Ri1 is not None and num_correspondences_dict[(i1, i2)] > 0: # 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)) + noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, 1 / num_correspondences_dict[(i1, i2)]) + measurements.append(gtsam.BinaryMeasurementRot3(i2, i1, i2Ri1, noise_model)) - return between_factors + return measurements def _between_factors_from_pose_priors( self, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], old_to_new_idxs: Dict[int, int] @@ -99,7 +120,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, measurements: gtsam.BinaryMeasurementsRot3, initial: Optional[Values] ) -> List[Optional[Rot3]]: """Run the rotation averaging on a connected graph w/ N keys ordered consecutively [0,...,N-1]. @@ -120,12 +141,13 @@ def _run_with_consecutive_ordering( logger.info( "Running Shonan with %d constraints on %d nodes", - len(between_factors), + len(measurements), num_connected_nodes, ) - shonan = ShonanAveraging3(between_factors, self.__get_shonan_params()) + shonan = ShonanAveraging3(measurements, 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 +165,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) @@ -157,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], + 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. @@ -168,6 +191,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. + 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 @@ -181,19 +205,39 @@ def run_rotation_averaging( 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)} + old_to_new_idxs = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)} + + i2Ri1_dict_remapped = {(old_to_new_idxs[i1], old_to_new_idxs[i2]): i2Ri1 for (i1, i2), i2Ri1 in i2Ri1_dict.items()} + num_correspondences_dict: Dict[Tuple[int, int], int] = { + (old_to_new_idxs[i1], old_to_new_idxs[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( + len(nodes_with_edges), + i2Ri1_dict_remapped, + edge_weights={(i1, i2): -num_correspondences_dict.get((i1, i2), 0) for i1, i2 in i2Ri1_dict_remapped.keys()}, + ) + 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 + measurements: gtsam.BinaryMeasurementsRot3 = self.__between_factors_from_2view_relative_rotations( + i2Ri1_dict=i2Ri1_dict_remapped, num_correspondences_dict=num_correspondences_dict ) - between_factors.extend(self._between_factors_from_pose_priors(i1Ti2_priors, old_to_new_idxes)) + + # 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), measurements=measurements, initial=initial_values ) 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 + return wRi_list \ No newline at end of file diff --git a/gtsfm/multi_view_optimizer.py b/gtsfm/multi_view_optimizer.py index e6c557bac..daa9c4959 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, + 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) @@ -213,4 +217,4 @@ def filter_corr_by_idx(correspondences: Dict[Tuple[int, int], np.ndarray], idxs: Returns: Filtered correspondences. """ - return {k: v for k, v in correspondences.items() if k in idxs} + return {k: v for k, v in correspondences.items() if k in idxs} \ No newline at end of file From 3b4c8d91abb767770ef6b8321ab119f34ca1500b Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 18 Mar 2024 09:15:56 -0400 Subject: [PATCH 02/18] shonan cleanup --- gtsfm/averaging/rotation/shonan.py | 66 +++++++++++------------------- gtsfm/configs/unified.yaml | 2 + 2 files changed, 25 insertions(+), 43 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index f19483171..3b0800dd0 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -27,7 +27,6 @@ ) 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 @@ -42,18 +41,23 @@ 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, weight_by_inliers: bool = True + ) -> 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). + weight_by_inliers: Whether to weight paiiwse costs according to an uncertainty equal to the inverse number + of inlier correspondences per edge. """ super().__init__() self._two_view_rotation_sigma = two_view_rotation_sigma self._p_min = 5 self._p_max = 64 + self._weight_by_inliers = weight_by_inliers def __get_shonan_params(self) -> ShonanAveragingParameters3: lm_params = LevenbergMarquardtParams.CeresDefaults() @@ -62,25 +66,6 @@ def __get_shonan_params(self) -> ShonanAveragingParameters3: shonan_params.setCertifyOptimality(True) 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] - # ) -> 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_2view_relative_rotations( self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], @@ -88,12 +73,19 @@ def __between_factors_from_2view_relative_rotations( ) -> gtsam.BinaryMeasurementsRot3: """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? + + if not self._weight_by_inliers: + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma) + measurements = gtsam.BinaryMeasurementsRot3() for (i1, i2), i2Ri1 in i2Ri1_dict.items(): - if i2Ri1 is not None and num_correspondences_dict[(i1, i2)] > 0: + if i2Ri1 is None: + continue + if self._weight_by_inliers and num_correspondences_dict[(i1, i2)] > 0: # ignore translation during rotation averaging noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, 1 / num_correspondences_dict[(i1, i2)]) - measurements.append(gtsam.BinaryMeasurementRot3(i2, i1, i2Ri1, noise_model)) + + measurements.append(gtsam.BinaryMeasurementRot3(i2, i1, i2Ri1, noise_model)) return measurements @@ -120,7 +112,7 @@ def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float: return between_factors def _run_with_consecutive_ordering( - self, num_connected_nodes: int, measurements: gtsam.BinaryMeasurementsRot3, initial: Optional[Values] + self, num_connected_nodes: int, measurements: gtsam.BinaryMeasurementsRot3 ) -> List[Optional[Rot3]]: """Run the rotation averaging on a connected graph w/ N keys ordered consecutively [0,...,N-1]. @@ -131,7 +123,7 @@ def _run_with_consecutive_ordering( 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. + measurements: 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 @@ -146,8 +138,7 @@ def _run_with_consecutive_ordering( ) shonan = ShonanAveraging3(measurements, self.__get_shonan_params()) - if initial is None: - initial = shonan.initializeRandomly() + 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)) @@ -207,37 +198,26 @@ def run_rotation_averaging( nodes_with_edges = sorted(list(self._nodes_with_edges(i2Ri1_dict, i1Ti2_priors))) old_to_new_idxs = {old_idx: i for i, old_idx in enumerate(nodes_with_edges)} - i2Ri1_dict_remapped = {(old_to_new_idxs[i1], old_to_new_idxs[i2]): i2Ri1 for (i1, i2), i2Ri1 in i2Ri1_dict.items()} + i2Ri1_dict_remapped = { + (old_to_new_idxs[i1], old_to_new_idxs[i2]): i2Ri1 for (i1, i2), i2Ri1 in i2Ri1_dict.items() + } num_correspondences_dict: Dict[Tuple[int, int], int] = { (old_to_new_idxs[i1], old_to_new_idxs[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( - len(nodes_with_edges), - i2Ri1_dict_remapped, - edge_weights={(i1, i2): -num_correspondences_dict.get((i1, i2), 0) for i1, i2 in i2Ri1_dict_remapped.keys()}, - ) - initial_values = Values() - for i, wRi_initial_ in enumerate(wRi_initial_): - initial_values.insert(i, wRi_initial_) - measurements: gtsam.BinaryMeasurementsRot3 = self.__between_factors_from_2view_relative_rotations( i2Ri1_dict=i2Ri1_dict_remapped, num_correspondences_dict=num_correspondences_dict ) - # 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), measurements=measurements, initial=initial_values + num_connected_nodes=len(nodes_with_edges), measurements=measurements ) 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 \ No newline at end of file + return wRi_list diff --git a/gtsfm/configs/unified.yaml b/gtsfm/configs/unified.yaml index 8fef52586..f1937ea36 100644 --- a/gtsfm/configs/unified.yaml +++ b/gtsfm/configs/unified.yaml @@ -69,11 +69,13 @@ SceneOptimizer: rot_avg_module: _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging + weight_by_inliers: True trans_avg_module: _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM robust_measurement_noise: True projection_sampling_method: SAMPLE_INPUT_MEASUREMENTS + reject_outliers: True data_association_module: _target_: gtsfm.data_association.data_assoc.DataAssociation From 1d3908595275c49f28c3c12e89c8172c27302135 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 18 Mar 2024 09:39:50 -0400 Subject: [PATCH 03/18] flake8 fix --- gtsfm/averaging/rotation/rotation_averaging_base.py | 2 +- gtsfm/averaging/rotation/shonan.py | 2 -- gtsfm/multi_view_optimizer.py | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index bd26d6079..28d4c38ac 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -145,4 +145,4 @@ def create_computation_graph( v_corr_idxs=v_corr_idxs, ) - return wRis, metrics \ No newline at end of file + return wRis, metrics diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 3b0800dd0..6bc4b287a 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -19,11 +19,9 @@ BetweenFactorPose3, BetweenFactorPose3s, LevenbergMarquardtParams, - Pose3, Rot3, ShonanAveraging3, ShonanAveragingParameters3, - Values, ) import gtsfm.utils.logger as logger_utils diff --git a/gtsfm/multi_view_optimizer.py b/gtsfm/multi_view_optimizer.py index 82cc54e7f..c5d23e6d2 100644 --- a/gtsfm/multi_view_optimizer.py +++ b/gtsfm/multi_view_optimizer.py @@ -240,4 +240,4 @@ def filter_corr_by_idx(correspondences: Dict[Tuple[int, int], np.ndarray], idxs: Returns: Filtered correspondences. """ - return {k: v for k, v in correspondences.items() if k in idxs} \ No newline at end of file + return {k: v for k, v in correspondences.items() if k in idxs} From 9b73429c8ac4b2b1d990a5372ad88c3f52ffa542 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Thu, 21 Mar 2024 10:05:41 -0400 Subject: [PATCH 04/18] fall back to non-IW if minimum eigenvalue not computed, update tests for new API --- gtsfm/averaging/rotation/shonan.py | 32 ++++++++++++++++--------- tests/averaging/rotation/test_shonan.py | 31 +++++++++++++++++------- 2 files changed, 44 insertions(+), 19 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 6bc4b287a..12dc526f1 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -72,8 +72,8 @@ def __between_factors_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? - if not self._weight_by_inliers: - noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma) + # Default noise model if `self._weight_by_inliers` is False, or zero correspondences on edge. + noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma) measurements = gtsam.BinaryMeasurementsRot3() for (i1, i2), i2Ri1 in i2Ri1_dict.items(): @@ -204,16 +204,26 @@ def run_rotation_averaging( for (i1, i2) in v_corr_idxs.keys() if (i1, i2) in i2Ri1_dict } - measurements: gtsam.BinaryMeasurementsRot3 = self.__between_factors_from_2view_relative_rotations( - i2Ri1_dict=i2Ri1_dict_remapped, num_correspondences_dict=num_correspondences_dict - ) - - # 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), measurements=measurements - ) + def _create_factors_and_run() -> List[Rot3]: + measurements: gtsam.BinaryMeasurementsRot3 = self.__between_factors_from_2view_relative_rotations( + i2Ri1_dict=i2Ri1_dict_remapped, num_correspondences_dict=num_correspondences_dict + ) + + # 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), measurements=measurements + ) + return wRi_list_subset + + try: + wRi_list_subset = _create_factors_and_run() + except RuntimeError: + if self._weight_by_inliers is True: + # At times, Shonan's `SparseMinimumEigenValue` fails to compute minimum eigenvalue. + self._weight_by_inliers = False + wRi_list_subset = _create_factors_and_run() wRi_list = [None] * num_images for remapped_i, original_i in enumerate(nodes_with_edges): wRi_list[original_i] = wRi_list_subset[remapped_i] diff --git a/tests/averaging/rotation/test_shonan.py b/tests/averaging/rotation/test_shonan.py index b0aef22a4..587263018 100644 --- a/tests/averaging/rotation/test_shonan.py +++ b/tests/averaging/rotation/test_shonan.py @@ -38,7 +38,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] = {} - wRi_computed = self.obj.run_rotation_averaging(len(wRi_expected), i2Ri1_input, i1Ti2_priors) + v_corr_idxs = _create_dummy_correspondences(i2Ri1_input) + 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) ) @@ -103,8 +104,8 @@ def test_simple_with_prior(self): type=PosePriorType.SOFT_CONSTRAINT, ) } - - wRi_computed = self.obj.run_rotation_averaging(len(expected_wRi_list), i2Ri1_dict, i1Ti2_priors) + v_corr_idxs = _create_dummy_correspondences(i2Ri1_dict) + 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) ) @@ -121,13 +122,16 @@ def test_computation_graph(self): i2Ri1_graph = dask.delayed(i2Ri1_dict) - # use the GTSAM API directly (without dask) for rotation averaging + # 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) + v_corr_idxs = _create_dummy_correspondences(i2Ri1_dict) + expected_wRi_list = self.obj.run_rotation_averaging(num_poses, i2Ri1_dict, i1Ti2_priors, v_corr_idxs) - # use dask's computation graph + # 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, gt_wTi_list, v_corr_idxs=v_corr_idxs + ) with dask.config.set(scheduler="single-threaded"): wRi_list = dask.compute(rotations_graph)[0] @@ -168,12 +172,23 @@ def test_nonconsecutive_indices(self): } relative_pose_priors: Dict[Tuple[int, int], PosePrior] = {} - wRi_computed = self.obj.run_rotation_averaging(num_images, i2Ri1_input, relative_pose_priors) + v_corr_idxs = _create_dummy_correspondences(i2Ri1_input) + 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 _create_dummy_correspondences(i2Ri1_dict: Dict[Tuple[int, int], Rot3]) -> Dict[Tuple[int, int], np.ndarray]: + """Create dummy verified correspondences for each edge in view graph.""" + # Assume image has shape (img_h, img_w) = (1000,1000) + img_h = 1000 + v_corr_idxs_dict = { + (i1, i2): np.random.randint(low=0, high=img_h, size=(i1 + i2, 2)) for i1, i2 in i2Ri1_dict.keys() + } + return v_corr_idxs_dict + + if __name__ == "__main__": unittest.main() From 9b5ca435bab7f2079d208e0280052cdd5f218e8b Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Fri, 22 Mar 2024 22:25:11 -0400 Subject: [PATCH 05/18] add logging --- 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 12dc526f1..350cd6da0 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -220,7 +220,9 @@ def _create_factors_and_run() -> List[Rot3]: try: wRi_list_subset = _create_factors_and_run() except RuntimeError: + logger.exception("Shonan failed") if self._weight_by_inliers is True: + logger.info("Reattempting Shonan without inlier-weighted costs...") # At times, Shonan's `SparseMinimumEigenValue` fails to compute minimum eigenvalue. self._weight_by_inliers = False wRi_list_subset = _create_factors_and_run() From 735ad35c071c44c8c142b1a1e1465059822a5b0c Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 23 Mar 2024 03:40:06 -0400 Subject: [PATCH 06/18] fix typo --- 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 350cd6da0..df1f87320 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -48,7 +48,7 @@ def __init__( Args: two_view_rotation_sigma: Covariance to use (lower values -> more strictly adhere to input measurements). - weight_by_inliers: Whether to weight paiiwse costs according to an uncertainty equal to the inverse number + weight_by_inliers: Whether to weight pairwise costs according to an uncertainty equal to the inverse number of inlier correspondences per edge. """ super().__init__() From 37bbfd5751b294cdcd4f2c8965bd9f97902bd98e Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 23 Mar 2024 03:45:55 -0400 Subject: [PATCH 07/18] reorder args to put GT last --- gtsfm/averaging/rotation/rotation_averaging_base.py | 10 +++++----- tests/averaging/rotation/test_shonan.py | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsfm/averaging/rotation/rotation_averaging_base.py b/gtsfm/averaging/rotation/rotation_averaging_base.py index 28d4c38ac..a98912557 100644 --- a/gtsfm/averaging/rotation/rotation_averaging_base.py +++ b/gtsfm/averaging/rotation/rotation_averaging_base.py @@ -64,8 +64,8 @@ def _run_rotation_averaging_base( num_images: int, i2Ri1_dict: Dict[Tuple[int, int], Optional[Rot3]], i1Ti2_priors: Dict[Tuple[int, int], PosePrior], - wTi_gt: List[Optional[Pose3]], v_corr_idxs: Dict[Tuple[int, int], np.ndarray], + wTi_gt: List[Optional[Pose3]], ) -> Tuple[List[Optional[Rot3]], GtsfmMetricsGroup]: """Runs rotation averaging and computes metrics. @@ -73,8 +73,8 @@ def _run_rotation_averaging_base( 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. - wTi_gt: Ground truth global rotations to compare against. v_corr_idxs: Dict mapping image pair indices (i1, i2) to indices of verified correspondences. + wTi_gt: Ground truth global rotations to compare against. Returns: Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is @@ -121,8 +121,8 @@ def create_computation_graph( num_images: int, i2Ri1_graph: Delayed, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], - gt_wTi_list: List[Optional[Pose3]], v_corr_idxs: Dict[Tuple[int, int], np.ndarray], + gt_wTi_list: List[Optional[Pose3]], ) -> Tuple[Delayed, Delayed]: """Create the computation graph for performing rotation averaging. @@ -130,8 +130,8 @@ def create_computation_graph( 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. + gt_wTi_list: Ground truth poses, to be used for evaluation. Returns: Global rotations wrapped using dask.delayed. @@ -141,8 +141,8 @@ def create_computation_graph( num_images, i2Ri1_dict=i2Ri1_graph, i1Ti2_priors=i1Ti2_priors, - wTi_gt=gt_wTi_list, v_corr_idxs=v_corr_idxs, + wTi_gt=gt_wTi_list, ) return wRis, metrics diff --git a/tests/averaging/rotation/test_shonan.py b/tests/averaging/rotation/test_shonan.py index 587263018..20f40be64 100644 --- a/tests/averaging/rotation/test_shonan.py +++ b/tests/averaging/rotation/test_shonan.py @@ -130,7 +130,7 @@ def test_computation_graph(self): # 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, v_corr_idxs=v_corr_idxs + num_poses, i2Ri1_graph, i1Ti2_priors, v_corr_idxs=v_corr_idxs, gt_wTi_list=gt_wTi_list ) with dask.config.set(scheduler="single-threaded"): From 893d60f2b6820794d106850fdb3aee7968688999 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 23 Mar 2024 07:30:36 -0400 Subject: [PATCH 08/18] fix noise model --- 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 df1f87320..9556b043d 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -73,7 +73,7 @@ def __between_factors_from_2view_relative_rotations( # TODO: how to weight the noise model on relative rotations compared to priors? # Default noise model if `self._weight_by_inliers` is False, or zero correspondences on edge. - noise_model = gtsam.noiseModel.Isotropic.Sigma(POSE3_DOF, self._two_view_rotation_sigma) + noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, self._two_view_rotation_sigma) measurements = gtsam.BinaryMeasurementsRot3() for (i1, i2), i2Ri1 in i2Ri1_dict.items(): From 7c603c81f44583083a56e90cff1fbd56024d980a Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 23 Mar 2024 08:10:57 -0400 Subject: [PATCH 09/18] use pose priors and rename between_factor to measurement --- gtsfm/averaging/rotation/shonan.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 9556b043d..7781c8a08 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -64,7 +64,7 @@ def __get_shonan_params(self) -> ShonanAveragingParameters3: shonan_params.setCertifyOptimality(True) return shonan_params - def __between_factors_from_2view_relative_rotations( + def __measurements_from_2view_relative_rotations( self, i2Ri1_dict: Dict[Tuple[int, int], Rot3], num_correspondences_dict: Dict[Tuple[int, int], int], @@ -87,11 +87,11 @@ def __between_factors_from_2view_relative_rotations( return measurements - def _between_factors_from_pose_priors( + def _measurements_from_pose_priors( self, i1Ti2_priors: Dict[Tuple[int, int], PosePrior], old_to_new_idxs: Dict[int, int] - ) -> BetweenFactorPose3s: + ) -> gtsam.BinaryMeasurementsRot3: """Create between factors from the priors on relative poses.""" - between_factors = BetweenFactorPose3s() + measurements = gtsam.BinaryMeasurementsRot3() def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float: """Get the sigma to be used for the isotropic noise model. @@ -104,10 +104,10 @@ def get_isotropic_noise_model_sigma(covariance: np.ndarray) -> float: 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(i1_, i2_, i1Ti2_prior.value, noise_model)) + noise_model = gtsam.noiseModel.Isotropic.Sigma(ROT3_DOF, noise_model_sigma) + measurements.append(gtsam.BinaryMeasurementRot3(i1_, i2_, i1Ti2_prior.value.rotation(), noise_model)) - return between_factors + return measurements def _run_with_consecutive_ordering( self, num_connected_nodes: int, measurements: gtsam.BinaryMeasurementsRot3 @@ -206,11 +206,11 @@ def run_rotation_averaging( } def _create_factors_and_run() -> List[Rot3]: - measurements: gtsam.BinaryMeasurementsRot3 = self.__between_factors_from_2view_relative_rotations( + measurements: gtsam.BinaryMeasurementsRot3 = self.__measurements_from_2view_relative_rotations( i2Ri1_dict=i2Ri1_dict_remapped, num_correspondences_dict=num_correspondences_dict ) - # between_factors.extend(self._between_factors_from_pose_priors(i1Ti2_priors, old_to_new_idxes)) + measurements.extend(self._measurements_from_pose_priors(i1Ti2_priors, old_to_new_idxs)) wRi_list_subset = self._run_with_consecutive_ordering( num_connected_nodes=len(nodes_with_edges), measurements=measurements From 08d589e5422edf2f1b36a245120a993c001531e5 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 23 Mar 2024 09:58:48 -0400 Subject: [PATCH 10/18] flake8 fix --- gtsfm/averaging/rotation/shonan.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 7781c8a08..ff931dfe6 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -16,8 +16,6 @@ import gtsam import numpy as np from gtsam import ( - BetweenFactorPose3, - BetweenFactorPose3s, LevenbergMarquardtParams, Rot3, ShonanAveraging3, @@ -121,7 +119,7 @@ def _run_with_consecutive_ordering( Args: num_connected_nodes: Number of unique connected nodes (i.e. images) in the graph (<= the number of images in the dataset) - measurements: BetweenFactorPose3s created from relative rotations from 2-view estimator and the priors. + measurements: BinaryMeasurementsRot3 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 From 31040a2a538439cb2344ebde015624aab40e4938 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 23 Mar 2024 13:28:44 -0400 Subject: [PATCH 11/18] remove newlines --- gtsfm/averaging/rotation/shonan.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index ff931dfe6..1032664a6 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -207,9 +207,7 @@ def _create_factors_and_run() -> List[Rot3]: measurements: gtsam.BinaryMeasurementsRot3 = self.__measurements_from_2view_relative_rotations( i2Ri1_dict=i2Ri1_dict_remapped, num_correspondences_dict=num_correspondences_dict ) - measurements.extend(self._measurements_from_pose_priors(i1Ti2_priors, old_to_new_idxs)) - wRi_list_subset = self._run_with_consecutive_ordering( num_connected_nodes=len(nodes_with_edges), measurements=measurements ) From fd02691a6f4214d4ffce4969454ca5bdfe4a34cf Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 23 Mar 2024 16:13:04 -0400 Subject: [PATCH 12/18] support real correspondences for T&T --- ... run_scene_optimizer_tanks_and_temples.py} | 0 scripts/tanks_and_temples_benchmark.sh | 132 ++++++++++++++++++ 2 files changed, 132 insertions(+) rename gtsfm/runner/{run_scene_optimizer_synthetic_tanks_and_temples.py => run_scene_optimizer_tanks_and_temples.py} (100%) create mode 100644 scripts/tanks_and_temples_benchmark.sh diff --git a/gtsfm/runner/run_scene_optimizer_synthetic_tanks_and_temples.py b/gtsfm/runner/run_scene_optimizer_tanks_and_temples.py similarity index 100% rename from gtsfm/runner/run_scene_optimizer_synthetic_tanks_and_temples.py rename to gtsfm/runner/run_scene_optimizer_tanks_and_temples.py diff --git a/scripts/tanks_and_temples_benchmark.sh b/scripts/tanks_and_temples_benchmark.sh new file mode 100644 index 000000000..2829edb00 --- /dev/null +++ b/scripts/tanks_and_temples_benchmark.sh @@ -0,0 +1,132 @@ + + + +# Script to launch jobs over various datasets & front-ends. + +USER_ROOT=$1 +CLUSTER_CONFIG=$2 + +now=$(date +"%Y%m%d_%H%M%S") + + +datasets=( + # Tanks and Temples Dataset. + # barn-tanks-and-temples-410 + # truck-251 + # meetingroom-371 + courthouse-1106 + ignatius-263 + church-507 +) + +max_frame_lookahead_sizes=( + # 0 + # 5 + 10 + #15 +) + +num_matched_sizes=( + # 0 + 5 + # 10 + # 15 + # 20 + # 25 +) + +correspondence_generator_config_names=( + sift + lightglue + superglue + loftr + disk +) + +if [[ $CLUSTER_CONFIG ]] +then + CLUSTER_ARGS="--cluster_config $CLUSTER_CONFIG" +else + CLUSTER_ARGS="" +fi + + +for num_matched in ${num_matched_sizes[@]}; do + for max_frame_lookahead in ${max_frame_lookahead_sizes[@]}; do + for dataset in ${datasets[@]}; do + + + INTRINSICS_ARGS="--share_intrinsics" + + + if [[ $num_matched == 0 && $max_frame_lookahead == 0 ]] + then + # Matches must come from at least some retriever. + continue + fi + + for correspondence_generator_config_name in ${correspondence_generator_config_names[@]}; do + + if [[ $correspondence_generator_config_name == *"sift"* ]] + then + num_workers=1 + elif [[ $correspondence_generator_config_name == *"lightglue"* ]] + then + num_workers=1 + elif [[ $correspondence_generator_config_name == *"superglue"* ]] + then + num_workers=1 + elif [[ $correspondence_generator_config_name == *"disk"* ]] + then + num_workers=1 + elif [[ $correspondence_generator_config_name == *"loftr"* ]] + then + num_workers=1 + fi + + echo "Dataset: ${dataset}" + echo "Num matched: ${num_matched}" + echo "Max frame lookahead: ${max_frame_lookahead}" + echo "Correspondence Generator: ${correspondence_generator_config_name}" + echo "Num workers: ${num_workers}" + echo "Intrinsics: ${INTRINSICS_ARGS}" + + if [[ $dataset == *"truck-251"* ]] + then + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Truck + elif [[ $dataset == *"meetingroom-371"* ]] + then + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Meetingroom + elif [[ $dataset == *"courthouse-1106"* ]] + then + dataset_root=/usr/local/gtsfm-data/Tanks_and_Temples_Courthouse_1106 + elif [[ $dataset == *"ignatius-263"* ]] + then + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Ignatius + elif [[ $dataset == *"church-507"* ]] + then + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Church + fi + + OUTPUT_ROOT=${USER_ROOT}/${now}/${now}__${dataset}__results__num_matched${num_matched}__maxframelookahead${max_frame_lookahead}__760p__unified_${correspondence_generator_config_name} + mkdir -p $OUTPUT_ROOT + + python gtsfm/runner/run_scene_optimizer_1dsfm.py \ + --mvs_off \ + --config unified \ + --correspondence_generator_config_name $correspondence_generator_config_name \ + --dataset_root $dataset_root \ + --num_workers $num_workers \ + --num_matched $num_matched \ + --max_frame_lookahead $max_frame_lookahead \ + --worker_memory_limit "32GB" \ + --output_root $OUTPUT_ROOT \ + --max_resolution 760 \ + $INTRINSICS_ARGS $CLUSTER_ARGS \ + 2>&1 | tee $OUTPUT_ROOT/out.log + + done + done + done +done + From 318f30c718f7bab1a3c2bfff127a53acd13707c0 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 24 Mar 2024 08:03:45 -0400 Subject: [PATCH 13/18] update --- scripts/tanks_and_temples_benchmark.sh | 30 +++++++++++++------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/scripts/tanks_and_temples_benchmark.sh b/scripts/tanks_and_temples_benchmark.sh index 2829edb00..84a4b10c8 100644 --- a/scripts/tanks_and_temples_benchmark.sh +++ b/scripts/tanks_and_temples_benchmark.sh @@ -1,7 +1,5 @@ - - - -# Script to launch jobs over various datasets & front-ends. +# Script to launch jobs over various Tanks & Temples datasets & front-ends. +# https://www.tanksandtemples.org/download/ USER_ROOT=$1 CLUSTER_CONFIG=$2 @@ -10,13 +8,12 @@ now=$(date +"%Y%m%d_%H%M%S") datasets=( - # Tanks and Temples Dataset. # barn-tanks-and-temples-410 - # truck-251 + truck-251 # meetingroom-371 - courthouse-1106 - ignatius-263 - church-507 + # courthouse-1106 + # ignatius-263 + # church-507 ) max_frame_lookahead_sizes=( @@ -94,24 +91,27 @@ for num_matched in ${num_matched_sizes[@]}; do if [[ $dataset == *"truck-251"* ]] then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Truck - elif [[ $dataset == *"meetingroom-371"* ]] + elif [[ $dataset == *"barn-tanks-and-temples-410"* ]] then - dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Meetingroom + dataset_root="" + elif [[ $dataset == *"church-507"* ]] + then + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Church elif [[ $dataset == *"courthouse-1106"* ]] then - dataset_root=/usr/local/gtsfm-data/Tanks_and_Temples_Courthouse_1106 + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Courthouse elif [[ $dataset == *"ignatius-263"* ]] then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Ignatius - elif [[ $dataset == *"church-507"* ]] + elif [[ $dataset == *"meetingroom-371"* ]] then - dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Church + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Meetingroom fi OUTPUT_ROOT=${USER_ROOT}/${now}/${now}__${dataset}__results__num_matched${num_matched}__maxframelookahead${max_frame_lookahead}__760p__unified_${correspondence_generator_config_name} mkdir -p $OUTPUT_ROOT - python gtsfm/runner/run_scene_optimizer_1dsfm.py \ + python gtsfm/runner/run_scene_optimizer_tanks_and_temples.py \ --mvs_off \ --config unified \ --correspondence_generator_config_name $correspondence_generator_config_name \ From abaffc3a5daa652d732541e7b1b7ff5c274fc5f6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 24 Mar 2024 08:21:05 -0400 Subject: [PATCH 14/18] update --- gtsfm/loader/tanks_and_temples_loader.py | 216 +++++++++++------------ scripts/tanks_and_temples_benchmark.sh | 17 +- 2 files changed, 120 insertions(+), 113 deletions(-) mode change 100644 => 100755 scripts/tanks_and_temples_benchmark.sh diff --git a/gtsfm/loader/tanks_and_temples_loader.py b/gtsfm/loader/tanks_and_temples_loader.py index 9f36e57fa..8760c4faa 100644 --- a/gtsfm/loader/tanks_and_temples_loader.py +++ b/gtsfm/loader/tanks_and_temples_loader.py @@ -10,13 +10,13 @@ from typing import Dict, List, Optional import numpy as np -import open3d +# import open3d from gtsam import Cal3Bundler, Rot3, Pose3 import gtsfm.utils.geometry_comparisons as geom_comp_utils import gtsfm.utils.io as io_utils import gtsfm.utils.logger as logger_utils -import gtsfm.visualization.open3d_vis_utils as open3d_vis_utils +# import gtsfm.visualization.open3d_vis_utils as open3d_vis_utils from gtsfm.common.image import Image from gtsfm.loader.loader_base import LoaderBase @@ -176,112 +176,112 @@ def get_camera_pose(self, index: int) -> Optional[Pose3]: raise ValueError("Given GT rotation is not a member of SO(3) and GT metrics will be incorrect.") return wTi - def get_lidar_point_cloud(self, downsample_factor: int = 10) -> open3d.geometry.PointCloud: - """Returns ground-truth point cloud, captured using an industrial laser scanner. - - Move all LiDAR points to the COLMAP frame. - - Args: - downsample_factor: Downsampling factor on point cloud. - - Return: - Point cloud captured by laser scanner, in the COLMAP world frame. - """ - if not Path(self.lidar_ply_fpath).exists(): - raise ValueError("Cannot retrieve LiDAR scanned point cloud if `lidar_ply_fpath` not provided.") - pcd = open3d.io.read_point_cloud(self.lidar_ply_fpath) - points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pointcloud=pcd) - - if downsample_factor > 1: - points = points[::downsample_factor] - rgb = rgb[::downsample_factor] - - lidar_Sim3_colmap = _create_Sim3_from_tt_dataset_alignment_transform(self.lidar_Sim3_colmap) - colmap_Sim3_lidar = np.linalg.inv(lidar_Sim3_colmap) - # Transform LiDAR points to COLMAP coordinate frame. - points = transform_point_cloud_vectorized(points, colmap_Sim3_lidar) - return open3d_vis_utils.create_colored_point_cloud_open3d(point_cloud=points, rgb=rgb) - - def get_colmap_point_cloud(self, downsample_factor: int = 1) -> open3d.geometry.PointCloud: - """Returns COLMAP-reconstructed point cloud. - - Args: - downsample_factor: Downsampling factor on point cloud. - - Return: - Point cloud reconstructed by COLMAP, in the COLMAP world frame. - """ - if not Path(self.colmap_ply_fpath).exists(): - raise ValueError("Cannot retrieve COLMAP-reconstructed point cloud if `colmap_ply_fpath` not provided.") - pcd = open3d.io.read_point_cloud(self.colmap_ply_fpath) - points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pointcloud=pcd) - - if downsample_factor > 1: - points = points[::downsample_factor] - rgb = rgb[::downsample_factor] - return open3d_vis_utils.create_colored_point_cloud_open3d(point_cloud=points, rgb=rgb) - - def reconstruct_mesh( - self, - crop_by_polyhedron: bool = True, - reconstruction_algorithm: MeshReconstructionType = MeshReconstructionType.ALPHA_SHAPE, - ) -> open3d.geometry.TriangleMesh: - """Reconstructs mesh from LiDAR PLY file. - - Args: - crop_by_polyhedron: Whether to crop by a manually specified polyhedron, vs. simply - by range from global origin. - reconstruction_algorithm: Mesh reconstruction algorithm to use, given input point cloud. - - Returns: - Reconstructed mesh. - """ - # Get LiDAR point cloud, in camera coordinate frame. - pcd = self.get_lidar_point_cloud() - if crop_by_polyhedron: - pass - # pcd = crop_points_to_bounding_polyhedron(pcd, self.bounding_polyhedron_json_fpath) - - points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pcd) - if not crop_by_polyhedron: - max_radius = 4.0 - valid = np.linalg.norm(points, axis=1) < max_radius - points = points[valid] - rgb = rgb[valid] - pcd = open3d_vis_utils.create_colored_point_cloud_open3d(points, rgb) - pcd.estimate_normals() - - if reconstruction_algorithm == MeshReconstructionType.ALPHA_SHAPE: - alpha = 0.1 # 0.03 - print(f"alpha={alpha:.3f}") - mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd, alpha) - mesh.compute_vertex_normals() - - elif reconstruction_algorithm == MeshReconstructionType.BALL_PIVOTING: - radii = [0.005, 0.01, 0.02, 0.04] - mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting( - pcd, open3d.utility.DoubleVector(radii) - ) - - elif reconstruction_algorithm == MeshReconstructionType.POISSON_SURFACE: - mesh, densities = open3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd, depth=9) - return mesh - - -def crop_points_to_bounding_polyhedron(pcd: open3d.geometry.PointCloud, json_fpath: str) -> open3d.geometry.PointCloud: - """Crops a point cloud according to JSON-specified polyhedron crop bounds. - - Args: - pcd: Input point cloud. - json_fpath: Path to JSON file containing crop specification, including 'orthogonal_axis', - 'axis_min', 'axis_max', 'bounding_polygon'. - - Returns: - Cropped point cloud, according to `SelectionPolygonVolume`. - """ - vol = open3d.visualization.read_selection_polygon_volume(json_fpath) - cropped_pcd = vol.crop_point_cloud(pcd) - return cropped_pcd +# def get_lidar_point_cloud(self, downsample_factor: int = 10) -> open3d.geometry.PointCloud: +# """Returns ground-truth point cloud, captured using an industrial laser scanner. + +# Move all LiDAR points to the COLMAP frame. + +# Args: +# downsample_factor: Downsampling factor on point cloud. + +# Return: +# Point cloud captured by laser scanner, in the COLMAP world frame. +# """ +# if not Path(self.lidar_ply_fpath).exists(): +# raise ValueError("Cannot retrieve LiDAR scanned point cloud if `lidar_ply_fpath` not provided.") +# pcd = open3d.io.read_point_cloud(self.lidar_ply_fpath) +# points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pointcloud=pcd) + +# if downsample_factor > 1: +# points = points[::downsample_factor] +# rgb = rgb[::downsample_factor] + +# lidar_Sim3_colmap = _create_Sim3_from_tt_dataset_alignment_transform(self.lidar_Sim3_colmap) +# colmap_Sim3_lidar = np.linalg.inv(lidar_Sim3_colmap) +# # Transform LiDAR points to COLMAP coordinate frame. +# points = transform_point_cloud_vectorized(points, colmap_Sim3_lidar) +# return open3d_vis_utils.create_colored_point_cloud_open3d(point_cloud=points, rgb=rgb) + +# def get_colmap_point_cloud(self, downsample_factor: int = 1) -> open3d.geometry.PointCloud: +# """Returns COLMAP-reconstructed point cloud. + +# Args: +# downsample_factor: Downsampling factor on point cloud. + +# Return: +# Point cloud reconstructed by COLMAP, in the COLMAP world frame. +# """ +# if not Path(self.colmap_ply_fpath).exists(): +# raise ValueError("Cannot retrieve COLMAP-reconstructed point cloud if `colmap_ply_fpath` not provided.") +# pcd = open3d.io.read_point_cloud(self.colmap_ply_fpath) +# points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pointcloud=pcd) + +# if downsample_factor > 1: +# points = points[::downsample_factor] +# rgb = rgb[::downsample_factor] +# return open3d_vis_utils.create_colored_point_cloud_open3d(point_cloud=points, rgb=rgb) + +# def reconstruct_mesh( +# self, +# crop_by_polyhedron: bool = True, +# reconstruction_algorithm: MeshReconstructionType = MeshReconstructionType.ALPHA_SHAPE, +# ) -> open3d.geometry.TriangleMesh: +# """Reconstructs mesh from LiDAR PLY file. + +# Args: +# crop_by_polyhedron: Whether to crop by a manually specified polyhedron, vs. simply +# by range from global origin. +# reconstruction_algorithm: Mesh reconstruction algorithm to use, given input point cloud. + +# Returns: +# Reconstructed mesh. +# """ +# # Get LiDAR point cloud, in camera coordinate frame. +# pcd = self.get_lidar_point_cloud() +# if crop_by_polyhedron: +# pass +# # pcd = crop_points_to_bounding_polyhedron(pcd, self.bounding_polyhedron_json_fpath) + +# points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pcd) +# if not crop_by_polyhedron: +# max_radius = 4.0 +# valid = np.linalg.norm(points, axis=1) < max_radius +# points = points[valid] +# rgb = rgb[valid] +# pcd = open3d_vis_utils.create_colored_point_cloud_open3d(points, rgb) +# pcd.estimate_normals() + +# if reconstruction_algorithm == MeshReconstructionType.ALPHA_SHAPE: +# alpha = 0.1 # 0.03 +# print(f"alpha={alpha:.3f}") +# mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd, alpha) +# mesh.compute_vertex_normals() + +# elif reconstruction_algorithm == MeshReconstructionType.BALL_PIVOTING: +# radii = [0.005, 0.01, 0.02, 0.04] +# mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting( +# pcd, open3d.utility.DoubleVector(radii) +# ) + +# elif reconstruction_algorithm == MeshReconstructionType.POISSON_SURFACE: +# mesh, densities = open3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd, depth=9) +# return mesh + + +# def crop_points_to_bounding_polyhedron(pcd: open3d.geometry.PointCloud, json_fpath: str) -> open3d.geometry.PointCloud: +# """Crops a point cloud according to JSON-specified polyhedron crop bounds. + +# Args: +# pcd: Input point cloud. +# json_fpath: Path to JSON file containing crop specification, including 'orthogonal_axis', +# 'axis_min', 'axis_max', 'bounding_polygon'. + +# Returns: +# Cropped point cloud, according to `SelectionPolygonVolume`. +# """ +# vol = open3d.visualization.read_selection_polygon_volume(json_fpath) +# cropped_pcd = vol.crop_point_cloud(pcd) +# return cropped_pcd def _parse_redwood_data_log_file(log_fpath: str) -> Dict[int, Pose3]: diff --git a/scripts/tanks_and_temples_benchmark.sh b/scripts/tanks_and_temples_benchmark.sh old mode 100644 new mode 100755 index 84a4b10c8..1aa571b90 --- a/scripts/tanks_and_temples_benchmark.sh +++ b/scripts/tanks_and_temples_benchmark.sh @@ -34,10 +34,10 @@ num_matched_sizes=( correspondence_generator_config_names=( sift - lightglue - superglue - loftr - disk + # lightglue + # superglue + # loftr + # disk ) if [[ $CLUSTER_CONFIG ]] @@ -66,7 +66,7 @@ for num_matched in ${num_matched_sizes[@]}; do if [[ $correspondence_generator_config_name == *"sift"* ]] then - num_workers=1 + num_workers=10 elif [[ $correspondence_generator_config_name == *"lightglue"* ]] then num_workers=1 @@ -91,27 +91,34 @@ for num_matched in ${num_matched_sizes[@]}; do if [[ $dataset == *"truck-251"* ]] then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Truck + scene_name="Truck" elif [[ $dataset == *"barn-tanks-and-temples-410"* ]] then dataset_root="" + scene_name="Barn" elif [[ $dataset == *"church-507"* ]] then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Church + scene_name="Church" elif [[ $dataset == *"courthouse-1106"* ]] then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Courthouse + scene_name="Courthouse" elif [[ $dataset == *"ignatius-263"* ]] then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Ignatius + scene_name="Ignatius" elif [[ $dataset == *"meetingroom-371"* ]] then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Meetingroom + scene_name="Meetingroom" fi OUTPUT_ROOT=${USER_ROOT}/${now}/${now}__${dataset}__results__num_matched${num_matched}__maxframelookahead${max_frame_lookahead}__760p__unified_${correspondence_generator_config_name} mkdir -p $OUTPUT_ROOT python gtsfm/runner/run_scene_optimizer_tanks_and_temples.py \ + --scene_name $scene_name \ --mvs_off \ --config unified \ --correspondence_generator_config_name $correspondence_generator_config_name \ From fa88be89263a87126bbc5d54c0397ef9919c22b7 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 24 Mar 2024 09:05:08 -0400 Subject: [PATCH 15/18] update --- scripts/tanks_and_temples_benchmark.sh | 34 +++++++++++++++----------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/scripts/tanks_and_temples_benchmark.sh b/scripts/tanks_and_temples_benchmark.sh index 1aa571b90..009cdb585 100755 --- a/scripts/tanks_and_temples_benchmark.sh +++ b/scripts/tanks_and_temples_benchmark.sh @@ -9,11 +9,12 @@ now=$(date +"%Y%m%d_%H%M%S") datasets=( # barn-tanks-and-temples-410 + courthouse-1106 + ignatius-263 + church-507 + caterpillar-383 + meetingroom-371 truck-251 - # meetingroom-371 - # courthouse-1106 - # ignatius-263 - # church-507 ) max_frame_lookahead_sizes=( @@ -34,10 +35,10 @@ num_matched_sizes=( correspondence_generator_config_names=( sift - # lightglue - # superglue - # loftr - # disk + lightglue + superglue + loftr + disk ) if [[ $CLUSTER_CONFIG ]] @@ -66,7 +67,7 @@ for num_matched in ${num_matched_sizes[@]}; do if [[ $correspondence_generator_config_name == *"sift"* ]] then - num_workers=10 + num_workers=1 elif [[ $correspondence_generator_config_name == *"lightglue"* ]] then num_workers=1 @@ -88,14 +89,15 @@ for num_matched in ${num_matched_sizes[@]}; do echo "Num workers: ${num_workers}" echo "Intrinsics: ${INTRINSICS_ARGS}" - if [[ $dataset == *"truck-251"* ]] - then - dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Truck - scene_name="Truck" - elif [[ $dataset == *"barn-tanks-and-temples-410"* ]] + + if [[ $dataset == *"barn-tanks-and-temples-410"* ]] then dataset_root="" scene_name="Barn" + elif [[ $dataset == *"caterpillar-383"* ]] + then + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Caterpillar + scene_name="Caterpillar" elif [[ $dataset == *"church-507"* ]] then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Church @@ -112,6 +114,10 @@ for num_matched in ${num_matched_sizes[@]}; do then dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Meetingroom scene_name="Meetingroom" + elif [[ $dataset == *"truck-251"* ]] + then + dataset_root=/usr/local/gtsfm-data/TanksAndTemples/Truck + scene_name="Truck" fi OUTPUT_ROOT=${USER_ROOT}/${now}/${now}__${dataset}__results__num_matched${num_matched}__maxframelookahead${max_frame_lookahead}__760p__unified_${correspondence_generator_config_name} From f86032ac72ad76f42ee0132df92ff97736a54636 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Tue, 26 Mar 2024 00:29:35 -0400 Subject: [PATCH 16/18] propagate resolution --- gtsfm/runner/run_scene_optimizer_tanks_and_temples.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsfm/runner/run_scene_optimizer_tanks_and_temples.py b/gtsfm/runner/run_scene_optimizer_tanks_and_temples.py index bd647d4b3..05baa1bb2 100644 --- a/gtsfm/runner/run_scene_optimizer_tanks_and_temples.py +++ b/gtsfm/runner/run_scene_optimizer_tanks_and_temples.py @@ -13,9 +13,6 @@ logger = logger_utils.get_logger() -_TANKS_AND_TEMPLES_RESOLUTION_PX = 1080 - - # TODO(johnwlambert,travisdriver): Make this generic for any dataset with a GT mesh. class GtsfmRunnerSyntheticTanksAndTemplesLoader(GtsfmRunnerBase): tag = "GTSFM with LiDAR scans, COLMAP camera poses, and image names stored in Tanks and Temples format" @@ -24,7 +21,7 @@ def construct_argparser(self) -> argparse.ArgumentParser: parser = super().construct_argparser() parser.add_argument( - "--dataset_root", type=str, required=True, help="Path to zip file containing packaged data." + "--dataset_root", type=str, required=True, help="Path to dir, for unzipped file containing packaged data." ) parser.add_argument("--scene_name", type=str, required=True, help="Name of dataset scene.") parser.add_argument( @@ -52,7 +49,8 @@ def construct_loader(self) -> LoaderBase: ply_alignment_fpath=ply_alignment_fpath, bounding_polyhedron_json_fpath=bounding_polyhedron_json_fpath, colmap_ply_fpath=colmap_ply_fpath, - max_resolution=_TANKS_AND_TEMPLES_RESOLUTION_PX, + # NOTE: Native resolution for T&T is 1080 px. + max_resolution=self.parsed_args.max_resolution, max_num_images=self.parsed_args.max_num_images, ) return loader From aea1690950cd35429b07e6c2962958d4caa69e0b Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 27 Mar 2024 11:24:44 -0400 Subject: [PATCH 17/18] support barn --- scripts/tanks_and_temples_benchmark.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/tanks_and_temples_benchmark.sh b/scripts/tanks_and_temples_benchmark.sh index 009cdb585..d84281462 100755 --- a/scripts/tanks_and_temples_benchmark.sh +++ b/scripts/tanks_and_temples_benchmark.sh @@ -8,7 +8,7 @@ now=$(date +"%Y%m%d_%H%M%S") datasets=( - # barn-tanks-and-temples-410 + barn-tanks-and-temples-410 courthouse-1106 ignatius-263 church-507 @@ -92,7 +92,7 @@ for num_matched in ${num_matched_sizes[@]}; do if [[ $dataset == *"barn-tanks-and-temples-410"* ]] then - dataset_root="" + dataset_root="/usr/local/gtsfm-data/TanksAndTemples/Barn" scene_name="Barn" elif [[ $dataset == *"caterpillar-383"* ]] then From 7945761e7822e7644697d8a480cfd42b31174826 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 27 Mar 2024 11:26:55 -0400 Subject: [PATCH 18/18] open3d imports --- gtsfm/loader/tanks_and_temples_loader.py | 216 +++++++++++------------ 1 file changed, 108 insertions(+), 108 deletions(-) diff --git a/gtsfm/loader/tanks_and_temples_loader.py b/gtsfm/loader/tanks_and_temples_loader.py index 8760c4faa..9f36e57fa 100644 --- a/gtsfm/loader/tanks_and_temples_loader.py +++ b/gtsfm/loader/tanks_and_temples_loader.py @@ -10,13 +10,13 @@ from typing import Dict, List, Optional import numpy as np -# import open3d +import open3d from gtsam import Cal3Bundler, Rot3, Pose3 import gtsfm.utils.geometry_comparisons as geom_comp_utils import gtsfm.utils.io as io_utils import gtsfm.utils.logger as logger_utils -# import gtsfm.visualization.open3d_vis_utils as open3d_vis_utils +import gtsfm.visualization.open3d_vis_utils as open3d_vis_utils from gtsfm.common.image import Image from gtsfm.loader.loader_base import LoaderBase @@ -176,112 +176,112 @@ def get_camera_pose(self, index: int) -> Optional[Pose3]: raise ValueError("Given GT rotation is not a member of SO(3) and GT metrics will be incorrect.") return wTi -# def get_lidar_point_cloud(self, downsample_factor: int = 10) -> open3d.geometry.PointCloud: -# """Returns ground-truth point cloud, captured using an industrial laser scanner. - -# Move all LiDAR points to the COLMAP frame. - -# Args: -# downsample_factor: Downsampling factor on point cloud. - -# Return: -# Point cloud captured by laser scanner, in the COLMAP world frame. -# """ -# if not Path(self.lidar_ply_fpath).exists(): -# raise ValueError("Cannot retrieve LiDAR scanned point cloud if `lidar_ply_fpath` not provided.") -# pcd = open3d.io.read_point_cloud(self.lidar_ply_fpath) -# points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pointcloud=pcd) - -# if downsample_factor > 1: -# points = points[::downsample_factor] -# rgb = rgb[::downsample_factor] - -# lidar_Sim3_colmap = _create_Sim3_from_tt_dataset_alignment_transform(self.lidar_Sim3_colmap) -# colmap_Sim3_lidar = np.linalg.inv(lidar_Sim3_colmap) -# # Transform LiDAR points to COLMAP coordinate frame. -# points = transform_point_cloud_vectorized(points, colmap_Sim3_lidar) -# return open3d_vis_utils.create_colored_point_cloud_open3d(point_cloud=points, rgb=rgb) - -# def get_colmap_point_cloud(self, downsample_factor: int = 1) -> open3d.geometry.PointCloud: -# """Returns COLMAP-reconstructed point cloud. - -# Args: -# downsample_factor: Downsampling factor on point cloud. - -# Return: -# Point cloud reconstructed by COLMAP, in the COLMAP world frame. -# """ -# if not Path(self.colmap_ply_fpath).exists(): -# raise ValueError("Cannot retrieve COLMAP-reconstructed point cloud if `colmap_ply_fpath` not provided.") -# pcd = open3d.io.read_point_cloud(self.colmap_ply_fpath) -# points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pointcloud=pcd) - -# if downsample_factor > 1: -# points = points[::downsample_factor] -# rgb = rgb[::downsample_factor] -# return open3d_vis_utils.create_colored_point_cloud_open3d(point_cloud=points, rgb=rgb) - -# def reconstruct_mesh( -# self, -# crop_by_polyhedron: bool = True, -# reconstruction_algorithm: MeshReconstructionType = MeshReconstructionType.ALPHA_SHAPE, -# ) -> open3d.geometry.TriangleMesh: -# """Reconstructs mesh from LiDAR PLY file. - -# Args: -# crop_by_polyhedron: Whether to crop by a manually specified polyhedron, vs. simply -# by range from global origin. -# reconstruction_algorithm: Mesh reconstruction algorithm to use, given input point cloud. - -# Returns: -# Reconstructed mesh. -# """ -# # Get LiDAR point cloud, in camera coordinate frame. -# pcd = self.get_lidar_point_cloud() -# if crop_by_polyhedron: -# pass -# # pcd = crop_points_to_bounding_polyhedron(pcd, self.bounding_polyhedron_json_fpath) - -# points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pcd) -# if not crop_by_polyhedron: -# max_radius = 4.0 -# valid = np.linalg.norm(points, axis=1) < max_radius -# points = points[valid] -# rgb = rgb[valid] -# pcd = open3d_vis_utils.create_colored_point_cloud_open3d(points, rgb) -# pcd.estimate_normals() - -# if reconstruction_algorithm == MeshReconstructionType.ALPHA_SHAPE: -# alpha = 0.1 # 0.03 -# print(f"alpha={alpha:.3f}") -# mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd, alpha) -# mesh.compute_vertex_normals() - -# elif reconstruction_algorithm == MeshReconstructionType.BALL_PIVOTING: -# radii = [0.005, 0.01, 0.02, 0.04] -# mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting( -# pcd, open3d.utility.DoubleVector(radii) -# ) - -# elif reconstruction_algorithm == MeshReconstructionType.POISSON_SURFACE: -# mesh, densities = open3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd, depth=9) -# return mesh - - -# def crop_points_to_bounding_polyhedron(pcd: open3d.geometry.PointCloud, json_fpath: str) -> open3d.geometry.PointCloud: -# """Crops a point cloud according to JSON-specified polyhedron crop bounds. - -# Args: -# pcd: Input point cloud. -# json_fpath: Path to JSON file containing crop specification, including 'orthogonal_axis', -# 'axis_min', 'axis_max', 'bounding_polygon'. - -# Returns: -# Cropped point cloud, according to `SelectionPolygonVolume`. -# """ -# vol = open3d.visualization.read_selection_polygon_volume(json_fpath) -# cropped_pcd = vol.crop_point_cloud(pcd) -# return cropped_pcd + def get_lidar_point_cloud(self, downsample_factor: int = 10) -> open3d.geometry.PointCloud: + """Returns ground-truth point cloud, captured using an industrial laser scanner. + + Move all LiDAR points to the COLMAP frame. + + Args: + downsample_factor: Downsampling factor on point cloud. + + Return: + Point cloud captured by laser scanner, in the COLMAP world frame. + """ + if not Path(self.lidar_ply_fpath).exists(): + raise ValueError("Cannot retrieve LiDAR scanned point cloud if `lidar_ply_fpath` not provided.") + pcd = open3d.io.read_point_cloud(self.lidar_ply_fpath) + points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pointcloud=pcd) + + if downsample_factor > 1: + points = points[::downsample_factor] + rgb = rgb[::downsample_factor] + + lidar_Sim3_colmap = _create_Sim3_from_tt_dataset_alignment_transform(self.lidar_Sim3_colmap) + colmap_Sim3_lidar = np.linalg.inv(lidar_Sim3_colmap) + # Transform LiDAR points to COLMAP coordinate frame. + points = transform_point_cloud_vectorized(points, colmap_Sim3_lidar) + return open3d_vis_utils.create_colored_point_cloud_open3d(point_cloud=points, rgb=rgb) + + def get_colmap_point_cloud(self, downsample_factor: int = 1) -> open3d.geometry.PointCloud: + """Returns COLMAP-reconstructed point cloud. + + Args: + downsample_factor: Downsampling factor on point cloud. + + Return: + Point cloud reconstructed by COLMAP, in the COLMAP world frame. + """ + if not Path(self.colmap_ply_fpath).exists(): + raise ValueError("Cannot retrieve COLMAP-reconstructed point cloud if `colmap_ply_fpath` not provided.") + pcd = open3d.io.read_point_cloud(self.colmap_ply_fpath) + points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pointcloud=pcd) + + if downsample_factor > 1: + points = points[::downsample_factor] + rgb = rgb[::downsample_factor] + return open3d_vis_utils.create_colored_point_cloud_open3d(point_cloud=points, rgb=rgb) + + def reconstruct_mesh( + self, + crop_by_polyhedron: bool = True, + reconstruction_algorithm: MeshReconstructionType = MeshReconstructionType.ALPHA_SHAPE, + ) -> open3d.geometry.TriangleMesh: + """Reconstructs mesh from LiDAR PLY file. + + Args: + crop_by_polyhedron: Whether to crop by a manually specified polyhedron, vs. simply + by range from global origin. + reconstruction_algorithm: Mesh reconstruction algorithm to use, given input point cloud. + + Returns: + Reconstructed mesh. + """ + # Get LiDAR point cloud, in camera coordinate frame. + pcd = self.get_lidar_point_cloud() + if crop_by_polyhedron: + pass + # pcd = crop_points_to_bounding_polyhedron(pcd, self.bounding_polyhedron_json_fpath) + + points, rgb = open3d_vis_utils.convert_colored_open3d_point_cloud_to_numpy(pcd) + if not crop_by_polyhedron: + max_radius = 4.0 + valid = np.linalg.norm(points, axis=1) < max_radius + points = points[valid] + rgb = rgb[valid] + pcd = open3d_vis_utils.create_colored_point_cloud_open3d(points, rgb) + pcd.estimate_normals() + + if reconstruction_algorithm == MeshReconstructionType.ALPHA_SHAPE: + alpha = 0.1 # 0.03 + print(f"alpha={alpha:.3f}") + mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd, alpha) + mesh.compute_vertex_normals() + + elif reconstruction_algorithm == MeshReconstructionType.BALL_PIVOTING: + radii = [0.005, 0.01, 0.02, 0.04] + mesh = open3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting( + pcd, open3d.utility.DoubleVector(radii) + ) + + elif reconstruction_algorithm == MeshReconstructionType.POISSON_SURFACE: + mesh, densities = open3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd, depth=9) + return mesh + + +def crop_points_to_bounding_polyhedron(pcd: open3d.geometry.PointCloud, json_fpath: str) -> open3d.geometry.PointCloud: + """Crops a point cloud according to JSON-specified polyhedron crop bounds. + + Args: + pcd: Input point cloud. + json_fpath: Path to JSON file containing crop specification, including 'orthogonal_axis', + 'axis_min', 'axis_max', 'bounding_polygon'. + + Returns: + Cropped point cloud, according to `SelectionPolygonVolume`. + """ + vol = open3d.visualization.read_selection_polygon_volume(json_fpath) + cropped_pcd = vol.crop_point_cloud(pcd) + return cropped_pcd def _parse_redwood_data_log_file(log_fpath: str) -> Dict[int, Pose3]: