diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index 21b67d907..21a316c7c 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -12,18 +12,18 @@ jobs: matrix: config_dataset_info: [ - # config dataset lookahead img-extension source loader max-res share-intrinsics - [sift_front_end, door-12, 12, JPG, test_data, olsson-loader, 1296, true], - [deep_front_end, door-12, 12, JPG, test_data, olsson-loader, 1296, true], - [sift_front_end, skydio-8, 8, jpg, gdrive , colmap-loader, 760, true], - [deep_front_end, skydio-8, 8, jpg, gdrive, colmap-loader, 760, true], - [sift_front_end, skydio-32, 32, jpg, gdrive, colmap-loader, 760, true], - [deep_front_end, skydio-32, 32, jpg, gdrive, colmap-loader, 760, true], - [deep_front_end, skydio-501, 15, jpg, wget, colmap-loader, 760, true], - [sift_front_end, palace-fine-arts-281, 25, jpg, wget, olsson-loader, 320, true], - [deep_front_end, notre-dame-20, 20, jpg, gdrive, colmap-loader, 760, false], - [sift_front_end, 2011205_rc3, 65, png, wget, astronet, 1024, true], - [deep_front_end, 2011205_rc3, 20, png, wget, astronet, 1024, true], + # config dataset lookahead img-extension source loader max-res share-intrinsics + [sift_front_end, door-12, 12, JPG, test_data, olsson-loader, 1296, true], + [deep_front_end, door-12, 12, JPG, test_data, olsson-loader, 1296, true], + [sift_front_end, skydio-8, 8, jpg, gdrive , colmap-loader, 760, true], + [deep_front_end, skydio-8, 8, jpg, gdrive, colmap-loader, 760, true], + [sift_front_end, skydio-32, 32, jpg, gdrive, colmap-loader, 760, true], + [deep_front_end, skydio-32, 32, jpg, gdrive, colmap-loader, 760, true], + [deep_front_end, skydio-501, 15, jpg, wget, colmap-loader, 760, true], + [sift_front_end, palace-fine-arts-281, 25, jpg, wget, olsson-loader, 320, true], + [deep_front_end, notre-dame-20, 20, jpg, gdrive, colmap-loader, 760, false], + [sift_front_end, 2011205_rc3, 65, png, wget, astronet, 1024, true], + [deep_front_end_astronet, 2011205_rc3, 20, png, wget, astronet, 1024, true], ] defaults: run: diff --git a/gtsfm/configs/deep_front_end.yaml b/gtsfm/configs/deep_front_end.yaml index 5c270e86a..f7410ca7b 100644 --- a/gtsfm/configs/deep_front_end.yaml +++ b/gtsfm/configs/deep_front_end.yaml @@ -51,12 +51,14 @@ SceneOptimizer: data_association_module: _target_: gtsfm.data_association.data_assoc.DataAssociation - reproj_error_thresh: 100 min_track_len: 2 - mode: - _target_: gtsfm.data_association.data_assoc.TriangulationParam - value: 0 # 0 corresponds to NO_RANSAC - num_ransac_hypotheses: 20 + triangulation_options: + _target_: gtsfm.data_association.point3d_initializer.TriangulationOptions + reproj_error_threshold: 10 + mode: + _target_: gtsfm.data_association.point3d_initializer.TriangulationSamplingMode + value: RANSAC_SAMPLE_UNIFORM + max_num_hypotheses: 100 save_track_patches_viz: False bundle_adjustment_module: diff --git a/gtsfm/configs/deep_front_end_astronet.yaml b/gtsfm/configs/deep_front_end_astronet.yaml new file mode 100644 index 000000000..e71804142 --- /dev/null +++ b/gtsfm/configs/deep_front_end_astronet.yaml @@ -0,0 +1,72 @@ +# Relaxes triangulation RANSAC threshold to 100 (10x threshold of deep_front_end.yaml) + +SceneOptimizer: + _target_: gtsfm.scene_optimizer.SceneOptimizer + save_gtsfm_data: True + save_two_view_correspondences_viz: False + save_3d_viz: True + pose_angular_error_thresh: 5 # degrees + + feature_extractor: + _target_: gtsfm.feature_extractor.FeatureExtractor + detector_descriptor: + _target_: gtsfm.frontend.cacher.detector_descriptor_cacher.DetectorDescriptorCacher + detector_descriptor_obj: + _target_: gtsfm.frontend.detector_descriptor.superpoint.SuperPointDetectorDescriptor + max_keypoints: 5000 + + two_view_estimator: + _target_: gtsfm.two_view_estimator.TwoViewEstimator + bundle_adjust_2view: True + eval_threshold_px: 4 # in px + bundle_adjust_2view_maxiters: 100 + + matcher: + _target_: gtsfm.frontend.cacher.matcher_cacher.MatcherCacher + matcher_obj: + _target_: gtsfm.frontend.matcher.superglue_matcher.SuperGlueMatcher + use_outdoor_model: True + + verifier: + _target_: gtsfm.frontend.verifier.ransac.Ransac + use_intrinsics_in_verification: True + estimation_threshold_px: 4 # for H/E/F estimators + + inlier_support_processor: + _target_: gtsfm.two_view_estimator.InlierSupportProcessor + min_num_inliers_est_model: 15 + min_inlier_ratio_est_model: 0.1 + + multiview_optimizer: + _target_: gtsfm.multi_view_optimizer.MultiViewOptimizer + view_graph_estimator: + _target_: gtsfm.view_graph_estimator.cycle_consistent_rotation_estimator.CycleConsistentRotationViewGraphEstimator + edge_error_aggregation_criterion: MEDIAN_EDGE_ERROR + + rot_avg_module: + _target_: gtsfm.averaging.rotation.shonan.ShonanRotationAveraging + + trans_avg_module: + _target_: gtsfm.averaging.translation.averaging_1dsfm.TranslationAveraging1DSFM + robust_measurement_noise: True + + data_association_module: + _target_: gtsfm.data_association.data_assoc.DataAssociation + min_track_len: 2 + triangulation_options: + _target_: gtsfm.data_association.point3d_initializer.TriangulationOptions + reproj_error_threshold: 100 + mode: + _target_: gtsfm.data_association.point3d_initializer.TriangulationSamplingMode + value: RANSAC_SAMPLE_UNIFORM + max_num_hypotheses: 100 + save_track_patches_viz: False + + bundle_adjustment_module: + _target_: gtsfm.bundle.bundle_adjustment.BundleAdjustmentOptimizer + output_reproj_error_thresh: 3 # for post-optimization filtering + robust_measurement_noise: True + shared_calib: False + + dense_multiview_optimizer: + _target_: gtsfm.densify.mvs_patchmatchnet.MVSPatchmatchNet diff --git a/gtsfm/configs/scene_optimizer_unit_test_config.yaml b/gtsfm/configs/scene_optimizer_unit_test_config.yaml index 0c97663a0..ed7e00dd4 100644 --- a/gtsfm/configs/scene_optimizer_unit_test_config.yaml +++ b/gtsfm/configs/scene_optimizer_unit_test_config.yaml @@ -10,7 +10,6 @@ SceneOptimizer: detector_descriptor: _target_: gtsfm.frontend.detector_descriptor.sift.SIFTDetectorDescriptor - two_view_estimator: _target_: gtsfm.two_view_estimator.TwoViewEstimator bundle_adjust_2view: True @@ -44,12 +43,14 @@ SceneOptimizer: data_association_module: _target_: gtsfm.data_association.data_assoc.DataAssociation - reproj_error_thresh: 100 min_track_len: 2 - mode: - _target_: gtsfm.data_association.data_assoc.TriangulationParam - value: 0 # 0 corresponds to NO_RANSAC - num_ransac_hypotheses: 20 + triangulation_options: + _target_: gtsfm.data_association.point3d_initializer.TriangulationOptions + reproj_error_threshold: 10 + mode: + _target_: gtsfm.data_association.point3d_initializer.TriangulationSamplingMode + value: NO_RANSAC + max_num_hypotheses: 20 save_track_patches_viz: False bundle_adjustment_module: diff --git a/gtsfm/configs/sift_front_end.yaml b/gtsfm/configs/sift_front_end.yaml index 59b461e53..a61bdb90a 100644 --- a/gtsfm/configs/sift_front_end.yaml +++ b/gtsfm/configs/sift_front_end.yaml @@ -50,12 +50,14 @@ SceneOptimizer: data_association_module: _target_: gtsfm.data_association.data_assoc.DataAssociation - reproj_error_thresh: 100 min_track_len: 2 - mode: - _target_: gtsfm.data_association.data_assoc.TriangulationParam - value: 0 # 0 corresponds to NO_RANSAC - num_ransac_hypotheses: 20 + triangulation_options: + _target_: gtsfm.data_association.point3d_initializer.TriangulationOptions + reproj_error_threshold: 100 + mode: + _target_: gtsfm.data_association.point3d_initializer.TriangulationSamplingMode + value: RANSAC_SAMPLE_UNIFORM + max_num_hypotheses: 100 save_track_patches_viz: False bundle_adjustment_module: diff --git a/gtsfm/data_association/data_assoc.py b/gtsfm/data_association/data_assoc.py index d7044a12e..eea12f078 100644 --- a/gtsfm/data_association/data_assoc.py +++ b/gtsfm/data_association/data_assoc.py @@ -25,7 +25,7 @@ from gtsfm.common.sfm_track import SfmTrack2d from gtsfm.data_association.point3d_initializer import ( Point3dInitializer, - TriangulationParam, + TriangulationOptions, TriangulationExitCode, ) from gtsfm.common.image import Image @@ -40,17 +40,14 @@ class DataAssociation(NamedTuple): """Class to form feature tracks; for each track, call LandmarkInitializer. Args: - reproj_error_thresh: the maximum reprojection error allowed. min_track_len: min length required for valid feature track / min nb of supporting views required for a landmark to be valid. - mode: triangulation mode, which dictates whether or not to use robust estimation. - num_ransac_hypotheses (optional): number of hypothesis for RANSAC-based triangulation. + triangulation_options: options for triangulating points. + save_track_patches_viz: whether to save a mosaic of individual patches associated with each track. """ - reproj_error_thresh: float min_track_len: int - mode: TriangulationParam - num_ransac_hypotheses: Optional[int] = None + triangulation_options: TriangulationOptions save_track_patches_viz: Optional[bool] = False def __validate_track(self, sfm_track: Optional[SfmTrack]) -> bool: @@ -91,16 +88,8 @@ def run( logger.debug("[Data association] input number of tracks: %s", len(tracks_2d)) logger.debug("[Data association] input avg. track length: %s", np.mean(track_lengths_2d)) - # initializer of 3D landmark for each track - point3d_initializer = Point3dInitializer( - cameras, - self.mode, - self.reproj_error_thresh, - self.num_ransac_hypotheses, - ) - - per_accepted_track_avg_errors = [] - per_rejected_track_avg_errors = [] + # Initialize 3D landmark for each track + point3d_initializer = Point3dInitializer(cameras, self.triangulation_options) # form GtsfmData object after triangulation triangulated_data = GtsfmData(num_images) @@ -113,8 +102,10 @@ def run( if cameras_gt is not None: exit_codes_wrt_gt = track_utils.classify_tracks2d_with_gt_cameras(tracks=tracks_2d, cameras_gt=cameras_gt) - exit_codes_wrt_computed: List[TriangulationExitCode] = [] # add valid tracks where triangulation is successful + exit_codes_wrt_computed: List[TriangulationExitCode] = [] + per_accepted_track_avg_errors = [] + per_rejected_track_avg_errors = [] for track_2d in tracks_2d: # triangulate and filter based on reprojection error sfm_track, avg_track_reproj_error, triangulation_exit_code = point3d_initializer.triangulate(track_2d) diff --git a/gtsfm/data_association/point3d_initializer.py b/gtsfm/data_association/point3d_initializer.py index 9b2a0908f..d11069b43 100644 --- a/gtsfm/data_association/point3d_initializer.py +++ b/gtsfm/data_association/point3d_initializer.py @@ -7,6 +7,7 @@ Authors: Sushmita Warrier, Xiaolong Wu, John Lambert, Travis Driver """ +import sys import itertools from enum import Enum from typing import Dict, List, NamedTuple, Optional, Tuple @@ -46,13 +47,70 @@ class TriangulationExitCode(Enum): EXCEEDS_REPROJ_THRESH = 4 # estimated 3d point exceeds reprojection threshold -class TriangulationParam(Enum): - """Triangulation modes.""" +class TriangulationSamplingMode(str, Enum): + """Triangulation modes. - NO_RANSAC = 0 # do not use filtering - RANSAC_SAMPLE_UNIFORM = 1 # sample a pair of cameras uniformly at random - RANSAC_SAMPLE_BIASED_BASELINE = 2 # sample pair of cameras based on largest estimated baseline - RANSAC_TOPK_BASELINES = 3 # deterministically choose hypotheses with largest estimate baseline + NO_RANSAC: do not use filtering. + RANSAC_SAMPLE_UNIFORM: sample a pair of cameras uniformly at random. + RANSAC_SAMPLE_BIASED_BASELINE: sample pair of cameras based by largest estimated baseline. + RANSAC_TOPK_BASELINES: deterministically choose hypotheses with largest estimate baseline. + """ + + NO_RANSAC = "NO_RANSAC" + RANSAC_SAMPLE_UNIFORM = "RANSAC_SAMPLE_UNIFORM" + RANSAC_SAMPLE_BIASED_BASELINE = "RANSAC_SAMPLE_BIASED_BASELINE" + RANSAC_TOPK_BASELINES = "RANSAC_TOPK_BASELINES" + + +class TriangulationOptions(NamedTuple): + """Options for triangulation solver. + + Based upon COLMAP's RANSAC class: + Reference: https://github.com/colmap/colmap/blob/dev/src/optim/ransac.h + + See the following slides for a derivation of the #req'd samples: http://www.cse.psu.edu/~rtc12/CSE486/lecture15.pdf + + Args: + reproj_error_threshold: the maximum reprojection error allowed. + mode: triangulation mode, which dictates whether or not to use robust estimation. + min_inlier_ratio: a priori assumed minimum probability that a point is an inlier. + confidence: desired confidence that at least one hypothesis is outlier free. + dyn_num_hypotheses_multiplier: multiplication factor for dynamically computed hyptheses based on confidence. + min_num_hypotheses: minimum number of hypotheses. + max_num_hypotheses: maximum number of hypotheses. + """ + + reproj_error_threshold: float + mode: TriangulationSamplingMode + + # RANSAC parameters + min_inlier_ratio: float = 0.1 + confidence: float = 0.9999 + dyn_num_hypotheses_multiplier: float = 3.0 + min_num_hypotheses: int = 0 + max_num_hypotheses: int = sys.maxsize + + def __check_ransac_params(self) -> None: + """Check that input value are valid""" + assert self.reproj_error_threshold > 0 + assert 0 < self.min_inlier_ratio < 1 + assert 0 < self.confidence < 1 + assert self.dyn_num_hypotheses_multiplier > 0 + assert 0 <= self.min_num_hypotheses < self.max_num_hypotheses + + def num_ransac_hypotheses(self) -> int: + """Compute maximum number of hypotheses. + + The RANSAC module defaults to 2749 iterations, computed as: + np.log(1-0.9999) / np.log( 1 - 0.1 **2) * 3 = 2749.3 + """ + self.__check_ransac_params() + dyn_num_hypotheses = int( + (np.log(1 - self.confidence) / np.log(1 - self.min_inlier_ratio ** NUM_SAMPLES_PER_RANSAC_HYPOTHESIS)) + * self.dyn_num_hypotheses_multiplier + ) + num_hypotheses = max(min(self.max_num_hypotheses, dyn_num_hypotheses), self.min_num_hypotheses) + return num_hypotheses class Point3dInitializer(NamedTuple): @@ -70,9 +128,7 @@ class Point3dInitializer(NamedTuple): """ track_camera_dict: Dict[int, PinholeCameraCal3Bundler] - mode: TriangulationParam - reproj_error_thresh: float - num_ransac_hypotheses: Optional[int] = None + options: TriangulationOptions def execute_ransac_variant(self, track_2d: SfmTrack2d) -> np.ndarray: """Execute RANSAC algorithm to find best subset 2d measurements for a 3d point. @@ -85,14 +141,12 @@ def execute_ransac_variant(self, track_2d: SfmTrack2d) -> np.ndarray: best_inliers: boolean array of length N. Indices of measurements are set to true if they correspond to the best RANSAC hypothesis """ - if self.num_ransac_hypotheses is None: - raise ValueError("RANSAC triangulation requested but number of hypothesis is None.") # Generate all possible matches measurement_pairs = generate_measurement_pairs(track_2d) # limit the number of samples to the number of available pairs - num_hypotheses = min(self.num_ransac_hypotheses, len(measurement_pairs)) + num_hypotheses = min(self.options.num_ransac_hypotheses(), len(measurement_pairs)) # Sampling samples = self.sample_ransac_hypotheses(track_2d, measurement_pairs, num_hypotheses) @@ -101,7 +155,6 @@ def execute_ransac_variant(self, track_2d: SfmTrack2d) -> np.ndarray: best_num_votes = 0 best_error = MAX_TRACK_REPROJ_ERROR best_inliers = np.zeros(len(track_2d.measurements), dtype=bool) - for sample_idxs in samples: k1, k2 = measurement_pairs[sample_idxs] @@ -142,7 +195,7 @@ def execute_ransac_variant(self, track_2d: SfmTrack2d) -> np.ndarray: # The best solution should correspond to the one with most inliers # If the inlier number are the same, check the average error of inliers - is_inlier = errors < self.reproj_error_thresh + is_inlier = errors < self.options.reproj_error_threshold # tally the number of votes inlier_errors = errors[is_inlier] @@ -174,13 +227,13 @@ def triangulate(self, track_2d: SfmTrack2d) -> Tuple[Optional[SfmTrack], Optiona to a cheirality exception upon triangulation """ # Check if we will run RANSAC, or not. - if self.mode in [ - TriangulationParam.RANSAC_SAMPLE_UNIFORM, - TriangulationParam.RANSAC_SAMPLE_BIASED_BASELINE, - TriangulationParam.RANSAC_TOPK_BASELINES, + if self.options.mode in [ + TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM, + TriangulationSamplingMode.RANSAC_SAMPLE_BIASED_BASELINE, + TriangulationSamplingMode.RANSAC_TOPK_BASELINES, ]: best_inliers = self.execute_ransac_variant(track_2d) - elif self.mode == TriangulationParam.NO_RANSAC: + elif self.options.mode == TriangulationSamplingMode.NO_RANSAC: best_inliers = np.ones(len(track_2d.measurements), dtype=bool) # all marked as inliers # Verify we have at least 2 inliers. @@ -213,7 +266,7 @@ def triangulate(self, track_2d: SfmTrack2d) -> Tuple[Optional[SfmTrack], Optiona ) # Check that all measurements are within reprojection error threshold. - if not np.all(reproj_errors.flatten() < self.reproj_error_thresh): + if not np.all(reproj_errors.flatten() < self.options.reproj_error_threshold): return None, avg_track_reproj_error, TriangulationExitCode.EXCEEDS_REPROJ_THRESH # Create a gtsam.SfmTrack with the triangulated 3d point and associated 2d measurements. @@ -241,9 +294,9 @@ def sample_ransac_hypotheses( # Initialize scores as uniform distribution scores = np.ones(len(measurement_pairs), dtype=float) - if self.mode in [ - TriangulationParam.RANSAC_SAMPLE_BIASED_BASELINE, - TriangulationParam.RANSAC_TOPK_BASELINES, + if self.options.mode in [ + TriangulationSamplingMode.RANSAC_SAMPLE_BIASED_BASELINE, + TriangulationSamplingMode.RANSAC_TOPK_BASELINES, ]: for k, (k1, k2) in enumerate(measurement_pairs): i1, _ = track.measurements[k1] @@ -259,9 +312,9 @@ def sample_ransac_hypotheses( if sum(scores) <= 0.0: raise Exception("Sum of scores cannot be zero (or smaller than zero)! It must a bug somewhere") - if self.mode in [ - TriangulationParam.RANSAC_SAMPLE_UNIFORM, - TriangulationParam.RANSAC_SAMPLE_BIASED_BASELINE, + if self.options.mode in [ + TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM, + TriangulationSamplingMode.RANSAC_SAMPLE_BIASED_BASELINE, ]: sample_indices = np.random.choice( len(scores), @@ -270,7 +323,7 @@ def sample_ransac_hypotheses( p=scores / scores.sum(), ) - if self.mode == TriangulationParam.RANSAC_TOPK_BASELINES: + if self.options.mode == TriangulationSamplingMode.RANSAC_TOPK_BASELINES: sample_indices = np.argsort(scores)[-num_hypotheses:] return sample_indices.tolist() diff --git a/gtsfm/utils/tracks.py b/gtsfm/utils/tracks.py index 80f76dd2b..630653a6f 100644 --- a/gtsfm/utils/tracks.py +++ b/gtsfm/utils/tracks.py @@ -9,7 +9,8 @@ from gtsfm.common.sfm_track import SfmMeasurement, SfmTrack2d from gtsfm.data_association.point3d_initializer import ( Point3dInitializer, - TriangulationParam, + TriangulationSamplingMode, + TriangulationOptions, TriangulationExitCode, ) @@ -31,7 +32,10 @@ def classify_tracks2d_with_gt_cameras( # do a simple triangulation with the GT cameras cameras_dict: Dict[int, Cal3Bundler] = {i: cam for i, cam in enumerate(cameras_gt)} point3d_initializer = Point3dInitializer( - track_camera_dict=cameras_dict, mode=TriangulationParam.NO_RANSAC, reproj_error_thresh=reproj_error_thresh_px + track_camera_dict=cameras_dict, + options=TriangulationOptions( + reproj_error_threshold=reproj_error_thresh_px, mode=TriangulationSamplingMode.NO_RANSAC + ), ) exit_codes: List[TriangulationExitCode] = [] diff --git a/tests/data_association/test_data_association.py b/tests/data_association/test_data_association.py index da7f02abf..1490e383a 100644 --- a/tests/data_association/test_data_association.py +++ b/tests/data_association/test_data_association.py @@ -15,7 +15,8 @@ from gtsam.utils.test_case import GtsamTestCase from gtsfm.common.keypoints import Keypoints -from gtsfm.data_association.data_assoc import DataAssociation, TriangulationParam +from gtsfm.data_association.data_assoc import DataAssociation +from gtsfm.data_association.point3d_initializer import TriangulationOptions, TriangulationSamplingMode def get_pose3_vector(num_poses: int) -> Pose3Vector: @@ -95,25 +96,25 @@ def setUp(self): def test_ransac_sample_biased_baseline_sharedCal_2poses(self): """ """ - mode = TriangulationParam.RANSAC_SAMPLE_BIASED_BASELINE + mode = TriangulationSamplingMode.RANSAC_SAMPLE_BIASED_BASELINE self.verify_triangulation_sharedCal_2poses(mode) def test_ransac_topk_baselines_sharedCal_2poses(self): """ """ - mode = TriangulationParam.RANSAC_TOPK_BASELINES + mode = TriangulationSamplingMode.RANSAC_TOPK_BASELINES self.verify_triangulation_sharedCal_2poses(mode) def test_ransac_sample_uniform_sharedCal_2poses(self): """ """ - mode = TriangulationParam.RANSAC_SAMPLE_UNIFORM + mode = TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM self.verify_triangulation_sharedCal_2poses(mode) def test_no_ransac_sharedCal_2poses(self): """ """ - mode = TriangulationParam.NO_RANSAC + mode = TriangulationSamplingMode.NO_RANSAC self.verify_triangulation_sharedCal_2poses(mode) - def verify_triangulation_sharedCal_2poses(self, triangulation_mode: TriangulationParam): + def verify_triangulation_sharedCal_2poses(self, triangulation_mode: TriangulationSamplingMode): """Tests that the triangulation is accurate for shared calibration with a specified triangulation mode. Checks whether the triangulated landmark map formed from 2 measurements is valid, if min track length = 3 @@ -132,12 +133,10 @@ def verify_triangulation_sharedCal_2poses(self, triangulation_mode: Triangulatio # since there is only one measurement in each image, both assigned feature index 0 matches_dict = {(0, 1): np.array([[0, 0]])} - da = DataAssociation( - reproj_error_thresh=5, # 5 px - min_track_len=3, # at least 3 measurements required - mode=triangulation_mode, - num_ransac_hypotheses=20, + triangulation_options = TriangulationOptions( + reproj_error_threshold=5, mode=triangulation_mode, min_num_hypotheses=20 ) + da = DataAssociation(min_track_len=3, triangulation_options=triangulation_options) triangulated_landmark_map, _ = da.run(len(cameras), cameras, matches_dict, keypoints_list) # assert that we cannot obtain even 1 length-3 track if we have only 2 camera poses # result should be empty, since nb_measurements < min track length @@ -167,11 +166,9 @@ def test_triangulation_individualCal_without_ransac(self): # since there is only one measurement in each image, both assigned feature index 0 matches_dict = {(0, 1): np.array([[0, 0]])} - da = DataAssociation( - reproj_error_thresh=5, # 5 px - min_track_len=2, # at least 2 measurements required - mode=TriangulationParam.NO_RANSAC, - ) + triangulation_options = TriangulationOptions(reproj_error_threshold=5, mode=TriangulationSamplingMode.NO_RANSAC) + da = DataAssociation(min_track_len=2, triangulation_options=triangulation_options) + sfm_data, _ = da.run(len(cameras), cameras, matches_dict, keypoints_list) estimated_landmark = sfm_data.get_track(0).point3() self.gtsamAssertEquals(estimated_landmark, self.expected_landmark, 1e-2) @@ -181,25 +178,25 @@ def test_triangulation_individualCal_without_ransac(self): def test_ransac_sample_biased_baseline_sharedCal_3poses(self): """ """ - mode = TriangulationParam.RANSAC_SAMPLE_BIASED_BASELINE + mode = TriangulationSamplingMode.RANSAC_SAMPLE_BIASED_BASELINE self.verify_triangulation_sharedCal_3poses(mode) def test_ransac_topk_baselines_sharedCal_3poses(self): """ """ - mode = TriangulationParam.RANSAC_TOPK_BASELINES + mode = TriangulationSamplingMode.RANSAC_TOPK_BASELINES self.verify_triangulation_sharedCal_3poses(mode) def test_ransac_sample_uniform_sharedCal_3poses(self): """ """ - mode = TriangulationParam.RANSAC_SAMPLE_UNIFORM + mode = TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM self.verify_triangulation_sharedCal_3poses(mode) def test_no_ransac_sharedCal_3poses(self): """ """ - mode = TriangulationParam.NO_RANSAC + mode = TriangulationSamplingMode.NO_RANSAC self.verify_triangulation_sharedCal_3poses(mode) - def verify_triangulation_sharedCal_3poses(self, triangulation_mode: TriangulationParam): + def verify_triangulation_sharedCal_3poses(self, triangulation_mode: TriangulationSamplingMode): """Tests that the triangulation is accurate for shared calibration with a specified triangulation mode. Checks whether the sfm data formed from 3 measurements is valid. The noise vectors represent the amount of @@ -216,12 +213,10 @@ def verify_triangulation_sharedCal_3poses(self, triangulation_mode: Triangulatio # since there is only one measurement in each image, both assigned feature index 0 matches_dict = {(0, 1): np.array([[0, 0]]), (1, 2): np.array([[0, 0]])} - da = DataAssociation( - reproj_error_thresh=5, # 5 px - min_track_len=3, # at least 3 measurements required - mode=triangulation_mode, - num_ransac_hypotheses=20, + triangulation_options = TriangulationOptions( + reproj_error_threshold=5, mode=triangulation_mode, min_num_hypotheses=20 ) + da = DataAssociation(min_track_len=3, triangulation_options=triangulation_options) sfm_data, _ = da.run(len(cameras), cameras, matches_dict, keypoints_list) estimated_landmark = sfm_data.get_track(0).point3() @@ -237,12 +232,10 @@ def verify_triangulation_sharedCal_3poses(self, triangulation_mode: Triangulatio def test_data_association_with_missing_camera(self): """Tests the data association with input tracks which use a camera index for which the camera doesn't exist.""" - da = DataAssociation( - reproj_error_thresh=5, # 5 px - min_track_len=3, # at least 3 measurements required - mode=TriangulationParam.NO_RANSAC, - num_ransac_hypotheses=20, + triangulation_options = TriangulationOptions( + reproj_error_threshold=5, mode=TriangulationSamplingMode.NO_RANSAC, min_num_hypotheses=20 ) + da = DataAssociation(min_track_len=3, triangulation_options=triangulation_options) # add cameras 0 and 2 cameras = { @@ -278,12 +271,10 @@ def test_create_computation_graph(self): matches_dict = {(0, 1): np.array([[0, 0]]), (1, 2): np.array([[0, 0]])} # Run without computation graph - da = DataAssociation( - reproj_error_thresh=5, # 5 px - min_track_len=3, # at least 3 measurements required - mode=TriangulationParam.RANSAC_TOPK_BASELINES, - num_ransac_hypotheses=20, + triangulation_options = TriangulationOptions( + reproj_error_threshold=5, mode=TriangulationSamplingMode.RANSAC_TOPK_BASELINES, min_num_hypotheses=20 ) + da = DataAssociation(min_track_len=3, triangulation_options=triangulation_options) expected_sfm_data, expected_metrics = da.run(len(cameras), cameras, matches_dict, keypoints_list) # Run with computation graph diff --git a/tests/data_association/test_point3d_initializer.py b/tests/data_association/test_point3d_initializer.py index d19b69063..d7b5f14b6 100644 --- a/tests/data_association/test_point3d_initializer.py +++ b/tests/data_association/test_point3d_initializer.py @@ -14,7 +14,11 @@ from gtsam import Cal3_S2, Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3 from gtsam.examples import SFMdata from gtsfm.common.sfm_track import SfmMeasurement, SfmTrack2d -from gtsfm.data_association.point3d_initializer import Point3dInitializer, TriangulationParam +from gtsfm.data_association.point3d_initializer import ( + Point3dInitializer, + TriangulationSamplingMode, + TriangulationOptions, +) from gtsfm.loader.olsson_loader import OlssonLoader # path for data used in this test @@ -60,6 +64,34 @@ def get_track_with_duplicate_measurements() -> List[SfmMeasurement]: return new_measurements +class TestTriangulationOptions(unittest.TestCase): + """Unit tests for TriangulationOptions""" + + def test_options_ransac(self) -> None: + """Asserts values of default RANSAC options.""" + triangulation_options = TriangulationOptions( + reproj_error_threshold=5, mode=TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM + ) + assert triangulation_options.num_ransac_hypotheses() == 2749 + + def test_options_ransac_min_hypotheses(self) -> None: + """Assert that number of hypotheses is overwritten if less than minimum.""" + triangulation_options = TriangulationOptions( + reproj_error_threshold=5, mode=TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM, min_num_hypotheses=10000 + ) + assert triangulation_options.num_ransac_hypotheses() == 10000 + + def test_options_ransac_max_hypotheses(self) -> None: + """Assert that number of hypotheses is overwritten if greater than maximum.""" + triangulation_options = TriangulationOptions( + reproj_error_threshold=5, + mode=TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM, + min_inlier_ratio=1e-4, + max_num_hypotheses=1000, + ) + assert triangulation_options.num_ransac_hypotheses() == 1000 + + class TestPoint3dInitializer(unittest.TestCase): """Unit tests for Point3dInitializer.""" @@ -67,11 +99,14 @@ def setUp(self): super().setUp() self.simple_triangulation_initializer = Point3dInitializer( - CAMERAS, TriangulationParam.NO_RANSAC, reproj_error_thresh=5 + CAMERAS, TriangulationOptions(reproj_error_threshold=5, mode=TriangulationSamplingMode.NO_RANSAC) ) self.ransac_uniform_sampling_initializer = Point3dInitializer( - CAMERAS, TriangulationParam.RANSAC_SAMPLE_UNIFORM, reproj_error_thresh=5, num_ransac_hypotheses=100 + CAMERAS, + TriangulationOptions( + reproj_error_threshold=5, mode=TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM, min_num_hypotheses=100 + ), ) def __runWithCorrectMeasurements(self, obj: Point3dInitializer) -> bool: @@ -120,9 +155,7 @@ def __runWithCheiralityException(self, obj: Point3dInitializer) -> bool: for i, cam in cameras.items() } - obj_with_flipped_cameras = Point3dInitializer( - flipped_cameras, obj.mode, obj.reproj_error_thresh, obj.num_ransac_hypotheses - ) + obj_with_flipped_cameras = Point3dInitializer(flipped_cameras, obj.options) sfm_track, _, _ = obj_with_flipped_cameras.triangulate(SfmTrack2d(MEASUREMENTS)) @@ -190,7 +223,9 @@ def testSimpleTriangulationOnDoorDataset(self): for i in range(len(loader)) } - initializer = Point3dInitializer(camera_dict, TriangulationParam.NO_RANSAC, reproj_error_thresh=1e5) + initializer = Point3dInitializer( + camera_dict, TriangulationOptions(mode=TriangulationSamplingMode.NO_RANSAC, reproj_error_threshold=1e5) + ) # tracks which have expected failures # (both tracks have incorrect measurements) @@ -253,7 +288,9 @@ def setUp(self): # cannot succeed later if only 2 views are provided and one of them is from camera 0. cameras = {1: PinholeCameraCal3Bundler(wTi1, calibration), 2: PinholeCameraCal3Bundler(wTi2, calibration)} - self.triangulator = Point3dInitializer(cameras, TriangulationParam.NO_RANSAC, reproj_error_thresh=5) + self.triangulator = Point3dInitializer( + cameras, TriangulationOptions(mode=TriangulationSamplingMode.NO_RANSAC, reproj_error_threshold=5) + ) def test_extract_measurements_unestimated_camera(self) -> None: """Ensure triangulation args are None for length-2 tracks where one or more measurements come from