diff --git a/gtsfm/averaging/rotation/shonan.py b/gtsfm/averaging/rotation/shonan.py index 6e7c7158e..a6025b8f2 100644 --- a/gtsfm/averaging/rotation/shonan.py +++ b/gtsfm/averaging/rotation/shonan.py @@ -10,7 +10,6 @@ Authors: Jing Wu, Ayush Baid, John Lambert """ -import time from typing import Dict, List, Optional, Set, Tuple import gtsam diff --git a/gtsfm/averaging/translation/averaging_1dsfm.py b/gtsfm/averaging/translation/averaging_1dsfm.py index a7cec20a0..b2777bb9b 100644 --- a/gtsfm/averaging/translation/averaging_1dsfm.py +++ b/gtsfm/averaging/translation/averaging_1dsfm.py @@ -241,13 +241,13 @@ def compute_inliers( inlier_w_i2Ui1_dict = {} inlier_w_iUj_dict_tracks = {} inlier_cameras: Set[int] = set() - for i1, i2 in w_i2Ui1_dict: + for (i1, i2) in w_i2Ui1_dict: if (C(i2), C(i1)) in inliers: # there is a flip in indices from w_i2Ui1_dict to inliers. inlier_w_i2Ui1_dict[(i1, i2)] = w_i2Ui1_dict[(i1, i2)] inlier_cameras.add(i1) inlier_cameras.add(i2) - for j, i in w_iUj_dict_tracks: + for (j, i) in w_iUj_dict_tracks: # Same as above, `inliers` contains symbols that are flipped - C(i), L(j). # Only add an inlier camera-track measurements if the camera has other camera-camera inliers. if (C(i), L(j)) in inliers and i in inlier_cameras: diff --git a/gtsfm/bundle/bundle_adjustment.py b/gtsfm/bundle/bundle_adjustment.py index d7cd43401..57f0794be 100644 --- a/gtsfm/bundle/bundle_adjustment.py +++ b/gtsfm/bundle/bundle_adjustment.py @@ -241,9 +241,7 @@ def __construct_factor_graph( # Also add a prior on the position of the first landmark to fix the scale graph.push_back( gtsam.PriorFactorPoint3( - P(0), - initial_data.get_track(0).point3(), - gtsam.noiseModel.Isotropic.Sigma(POINT3_DOF, 0.1), + P(0), initial_data.get_track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(POINT3_DOF, 0.1) ) ) @@ -259,10 +257,7 @@ def __initial_values(self, initial_data: GtsfmData) -> Values: initial_values.insert(X(i), camera.pose()) if not self._shared_calib or loop_idx == 0: # add only one value if calibrations are shared - initial_values.insert( - K(self.__map_to_calibration_variable(i)), - camera.calibration(), - ) + initial_values.insert(K(self.__map_to_calibration_variable(i)), camera.calibration()) # add each SfmTrack for j in range(initial_data.number_tracks()): @@ -302,6 +297,7 @@ def run_ba_step( reproj_error_thresh: Optional[float], verbose: bool = True, ) -> Tuple[GtsfmData, GtsfmData, List[bool], float]: + """TODO""" cameras_to_model = self.__cameras_to_model(initial_data, absolute_pose_priors, relative_pose_priors) graph = self.__construct_factor_graph( cameras_to_model=cameras_to_model, @@ -377,12 +373,7 @@ def run_ba( if num_ba_steps > 1: logger.info( "[BA Step %d/%d] Error: %.2f, Number of tracks: %d" - % ( - step + 1, - num_ba_steps, - final_error, - filtered_result.number_tracks(), - ) + % (step + 1, num_ba_steps, final_error, filtered_result.number_tracks()) ) return optimized_data, filtered_result, valid_mask @@ -404,8 +395,7 @@ def evaluate( Metrics group containing metrics for both filtered and unfiltered BA results. """ ba_metrics = GtsfmMetricsGroup( - name=METRICS_GROUP, - metrics=metrics_utils.get_stats_for_sfmdata(unfiltered_data, suffix="_unfiltered"), + name=METRICS_GROUP, metrics=metrics_utils.get_stats_for_sfmdata(unfiltered_data, suffix="_unfiltered") ) poses_gt = [cam.pose() if cam is not None else None for cam in cameras_gt] @@ -447,7 +437,7 @@ def _run_ba_instrumented( cameras_gt: List[Optional[gtsfm_types.CAMERA_TYPE]], save_dir: Optional[str] = None, verbose: bool = True, - ): + ) -> Tuple[GtsfmData, GtsfmData, List[bool], GtsfmMetricsGroup]: """TODO""" logger.info( "Input: %d tracks on %d cameras", @@ -524,7 +514,7 @@ def create_computation_graph( absolute_pose_priors, relative_pose_priors, cameras_gt, - save_dir, + save_dir=save_dir, ) return filtered_sfm_data, metrics_graph