def test_select_largest_connected_component(self, graph_largest_cc_mock): """Test pruning to largest connected component according to tracks. The function under test calles the graph utility, which has been mocked and we test the call against the mocked object. """ gtsfm_data = GtsfmData(5) cam = PinholeCameraCal3Bundler(Pose3(), Cal3Bundler()) # add the same camera at all indices for i in range(gtsfm_data.number_images()): gtsfm_data.add_camera(i, cam) # add two tracks to create two connected components track_1 = SfmTrack( np.random.randn(3)) # track with 2 cameras, which will be dropped track_1.add_measurement(idx=0, m=np.random.randn(2)) track_1.add_measurement(idx=3, m=np.random.randn(2)) track_2 = SfmTrack( np.random.randn(3)) # track with 3 cameras, which will be retained track_2.add_measurement(idx=1, m=np.random.randn(2)) track_2.add_measurement(idx=2, m=np.random.randn(2)) track_2.add_measurement(idx=4, m=np.random.randn(2)) gtsfm_data.add_track(track_1) gtsfm_data.add_track(track_2) largest_component_data = gtsfm_data.select_largest_connected_component( ) # check the graph util function called with the edges defined by tracks graph_largest_cc_mock.assert_called_once_with([(0, 3), (1, 2), (1, 4), (2, 4)]) # check the expected cameras coming just from track_2 expected_camera_indices = [1, 2, 4] computed_camera_indices = largest_component_data.get_valid_camera_indices( ) self.assertListEqual(computed_camera_indices, expected_camera_indices) # check that there is just one track expected_num_tracks = 1 computed_num_tracks = largest_component_data.number_tracks() self.assertEqual(computed_num_tracks, expected_num_tracks) # check the exact track computed_track = largest_component_data.get_track(0) self.assertTrue(computed_track.equals(track_2, EQUALITY_TOLERANCE))
def run( self, num_images: int, cameras: Dict[int, PinholeCameraCal3Bundler], corr_idxs_dict: Dict[Tuple[int, int], np.ndarray], keypoints_list: List[Keypoints], images: Optional[List[Image]] = None ) -> Tuple[GtsfmData, Dict[str, Any]]: """Perform the data association. Args: num_images: Number of images in the scene. cameras: dictionary, with image index -> camera mapping. corr_idxs_dict: dictionary, with key as image pair (i1,i2) and value as matching keypoint indices. keypoints_list: keypoints for each image. images: a list of all images in scene (optional and only for track patch visualization) viz_patch_sz: width and height of patches, if if dumping/visualizing a patch for each 2d track measurement Returns: Cameras and tracks as GtsfmData. """ # generate tracks for 3D points using pairwise correspondences tracks_2d = SfmTrack2d.generate_tracks_from_pairwise_matches( corr_idxs_dict, keypoints_list) if self.save_track_patches_viz and images is not None: io_utils.save_track_visualizations(tracks_2d, images, save_dir=os.path.join( "plots", "tracks_2d")) # metrics on tracks w/o triangulation check num_tracks_2d = len(tracks_2d) track_lengths = list(map(lambda x: x.number_measurements(), tracks_2d)) mean_2d_track_length = np.mean(track_lengths) logger.debug("[Data association] input number of tracks: %s", num_tracks_2d) logger.debug("[Data association] input avg. track length: %s", mean_2d_track_length) # initializer of 3D landmark for each track point3d_initializer = Point3dInitializer( cameras, self.mode, self.reproj_error_thresh, self.num_ransac_hypotheses, ) num_tracks_w_cheirality_exceptions = 0 per_accepted_track_avg_errors = [] per_rejected_track_avg_errors = [] # form GtsfmData object after triangulation triangulated_data = GtsfmData(num_images) # add all cameras for i, camera in cameras.items(): triangulated_data.add_camera(i, camera) # add valid tracks where triangulation is successful for track_2d in tracks_2d: # triangulate and filter based on reprojection error sfm_track, avg_track_reproj_error, is_cheirality_failure = point3d_initializer.triangulate( track_2d) if is_cheirality_failure: num_tracks_w_cheirality_exceptions += 1 if avg_track_reproj_error is not None: # need no more than 3 significant figures in json report avg_track_reproj_error = np.round(avg_track_reproj_error, 3) if sfm_track is not None and self.__validate_track(sfm_track): triangulated_data.add_track(sfm_track) per_accepted_track_avg_errors.append(avg_track_reproj_error) else: per_rejected_track_avg_errors.append(avg_track_reproj_error) track_cheirality_failure_ratio = num_tracks_w_cheirality_exceptions / len( tracks_2d) # pick only the largest connected component connected_data = triangulated_data.select_largest_connected_component() num_accepted_tracks = connected_data.number_tracks() accepted_tracks_ratio = num_accepted_tracks / len(tracks_2d) mean_3d_track_length, median_3d_track_length = connected_data.get_track_length_statistics( ) track_lengths_3d = connected_data.get_track_lengths() logger.debug("[Data association] output number of tracks: %s", num_accepted_tracks) logger.debug("[Data association] output avg. track length: %s", np.round(mean_3d_track_length, 2)) # dump the 3d point cloud before Bundle Adjustment for offline visualization points_3d = [ list(connected_data.get_track(j).point3()) for j in range(num_accepted_tracks) ] # bin edges are halfway between each integer track_lengths_histogram, _ = np.histogram(track_lengths_3d, bins=np.linspace( -0.5, 10.5, 12)) # min possible track len is 2, above 10 is improbable histogram_dict = { f"num_len_{i}_tracks": int(track_lengths_histogram[i]) for i in range(2, 11) } data_assoc_metrics = { "mean_2d_track_length": np.round(mean_2d_track_length, 3), "accepted_tracks_ratio": np.round(accepted_tracks_ratio, 3), "track_cheirality_failure_ratio": np.round(track_cheirality_failure_ratio, 3), "num_accepted_tracks": num_accepted_tracks, "3d_tracks_length": { "median": median_3d_track_length, "mean": mean_3d_track_length, "min": int(track_lengths_3d.min()) if track_lengths_3d.size > 0 else None, "max": int(track_lengths_3d.max()) if track_lengths_3d.size > 0 else None, "track_lengths_histogram": histogram_dict, }, "mean_accepted_track_avg_error": np.array(per_accepted_track_avg_errors).mean(), "per_rejected_track_avg_errors": per_rejected_track_avg_errors, "per_accepted_track_avg_errors": per_accepted_track_avg_errors, "points_3d": points_3d, } return connected_data, data_assoc_metrics
def run( self, num_images: int, cameras: Dict[int, gtsfm_types.CAMERA_TYPE], corr_idxs_dict: Dict[Tuple[int, int], np.ndarray], keypoints_list: List[Keypoints], cameras_gt: List[Optional[gtsfm_types.CALIBRATION_TYPE]], relative_pose_priors: Dict[Tuple[int, int], Optional[PosePrior]], images: Optional[List[Image]] = None, ) -> Tuple[GtsfmData, GtsfmMetricsGroup]: """Perform the data association. Args: num_images: Number of images in the scene. cameras: dictionary, with image index -> camera mapping. corr_idxs_dict: dictionary, with key as image pair (i1,i2) and value as matching keypoint indices. keypoints_list: keypoints for each image. cameras_gt: list of GT cameras, to be used for benchmarking the tracks. images: a list of all images in scene (optional and only for track patch visualization) Returns: A tuple of GtsfmData with cameras and tracks, and a GtsfmMetricsGroup with data association metrics """ # generate tracks for 3D points using pairwise correspondences tracks_estimator = DsfTracksEstimator() tracks_2d = tracks_estimator.run(corr_idxs_dict, keypoints_list) if self.save_track_patches_viz and images is not None: io_utils.save_track_visualizations(tracks_2d, images, save_dir=os.path.join("plots", "tracks_2d")) # track lengths w/o triangulation check track_lengths_2d = np.array(list(map(lambda x: int(x.number_measurements()), tracks_2d)), dtype=np.uint32) 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)) # Initialize 3D landmark for each track point3d_initializer = Point3dInitializer(cameras, self.triangulation_options) # form GtsfmData object after triangulation triangulated_data = GtsfmData(num_images) # add all cameras for i, camera in cameras.items(): triangulated_data.add_camera(i, camera) exit_codes_wrt_gt = track_utils.classify_tracks2d_with_gt_cameras(tracks=tracks_2d, cameras_gt=cameras_gt) # 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) exit_codes_wrt_computed.append(triangulation_exit_code) if triangulation_exit_code == TriangulationExitCode.CHEIRALITY_FAILURE: continue if sfm_track is not None and self.__validate_track(sfm_track): triangulated_data.add_track(sfm_track) per_accepted_track_avg_errors.append(avg_track_reproj_error) else: per_rejected_track_avg_errors.append(avg_track_reproj_error) # aggregate the exit codes to get the distribution w.r.t each triangulation exit # get the exit codes distribution w.r.t. the camera params computed by the upstream modules of GTSFM exit_codes_wrt_computed_distribution = Counter(exit_codes_wrt_computed) # compute the exit codes distribution w.r.t. a tuple of exit codes: the exit code when triangulated with the # ground truth cameras and the exit code when triangulated with the computed cameras. exit_codes_wrt_gt_and_computed_distribution = None if exit_codes_wrt_gt is not None: exit_codes_wrt_gt_and_computed_distribution = Counter(zip(exit_codes_wrt_gt, exit_codes_wrt_computed)) track_cheirality_failure_ratio = exit_codes_wrt_computed_distribution[ TriangulationExitCode.CHEIRALITY_FAILURE ] / len(tracks_2d) # pick only the largest connected component # TODO(Ayush): remove this for hilti as disconnected components not an issue? cam_edges_from_prior = [k for k, v in relative_pose_priors.items() if v is not None] connected_data = triangulated_data.select_largest_connected_component(extra_camera_edges=cam_edges_from_prior) num_accepted_tracks = connected_data.number_tracks() accepted_tracks_ratio = num_accepted_tracks / len(tracks_2d) mean_3d_track_length, median_3d_track_length = connected_data.get_track_length_statistics() track_lengths_3d = connected_data.get_track_lengths() logger.debug("[Data association] output number of tracks: %s", num_accepted_tracks) logger.debug("[Data association] output avg. track length: %.2f", mean_3d_track_length) data_assoc_metrics = GtsfmMetricsGroup( "data_association_metrics", [ GtsfmMetric( "2D_track_lengths", track_lengths_2d, store_full_data=False, plot_type=GtsfmMetric.PlotType.HISTOGRAM, ), GtsfmMetric("accepted_tracks_ratio", accepted_tracks_ratio), GtsfmMetric("track_cheirality_failure_ratio", track_cheirality_failure_ratio), GtsfmMetric("num_accepted_tracks", num_accepted_tracks), GtsfmMetric( "3d_tracks_length", track_lengths_3d, store_full_data=False, plot_type=GtsfmMetric.PlotType.HISTOGRAM, ), GtsfmMetric("accepted_track_avg_errors_px", per_accepted_track_avg_errors, store_full_data=False), GtsfmMetric( "rejected_track_avg_errors_px", np.array(per_rejected_track_avg_errors).astype(np.float32), store_full_data=False, ), GtsfmMetric(name="number_cameras", data=len(connected_data.get_valid_camera_indices())), ], ) if exit_codes_wrt_gt_and_computed_distribution is not None: for (gt_exit_code, computed_exit_code), count in exit_codes_wrt_gt_and_computed_distribution.items(): # Each track has 2 associated exit codes: the triangulation exit codes w.r.t ground truth cameras # and w.r.t cameras computed by upstream modules of GTSFM. We get the distribution of the number of # tracks for each pair of (triangulation exit code w.r.t GT cams, triangulation exit code w.r.t # computed cams) metric_name = "#tracks triangulated with GT cams: {}, computed cams: {}".format( gt_exit_code.name, computed_exit_code.name ) data_assoc_metrics.add_metric(GtsfmMetric(name=metric_name, data=count)) return connected_data, data_assoc_metrics