def compute_ba_pose_metrics( gt_wTi_list: List[Pose3], ba_output: GtsfmData, ) -> GtsfmMetricsGroup: """Compute pose errors w.r.t. GT for the bundle adjustment result. Note: inputs must be aligned beforehand to the ground truth. Args: gt_wTi_list: List of ground truth poses. ba_output: sparse multi-view result, as output of bundle adjustment. Returns: A group of metrics that describe errors associated with a bundle adjustment result (w.r.t. GT). """ wTi_aligned_list = ba_output.get_camera_poses() i2Ui1_dict_gt = get_twoview_translation_directions(gt_wTi_list) wRi_aligned_list, wti_aligned_list = get_rotations_translations_from_poses( wTi_aligned_list) gt_wRi_list, gt_wti_list = get_rotations_translations_from_poses( gt_wTi_list) metrics = [] metrics.append(compute_rotation_angle_metric(wRi_aligned_list, gt_wRi_list)) metrics.append( compute_translation_distance_metric(wti_aligned_list, gt_wti_list)) metrics.append( compute_translation_angle_metric(i2Ui1_dict_gt, wTi_aligned_list)) return GtsfmMetricsGroup(name="ba_pose_error_metrics", metrics=metrics)
def test_point_cloud_cameras_locked(self) -> None: """Tests the get_ortho_axis_alignment_transform() function with a GtsfmData object containing 11 point cloud points and 12 camera frustums from the door-12 dataset. Determines if the points and frustums are properly "locked" in with one another before and after the alignment transformation is applied. """ sample_data = GtsfmData(number_images=12) # Instantiate OlssonLoader to read camera poses from door12 dataset. wTi_list = self.loader._wTi_list # Add 12 camera frustums to sample_data. default_intrinsics = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0) for idx, pose in enumerate(wTi_list): camera = PinholeCameraCal3Bundler(pose, default_intrinsics) sample_data.add_camera(idx, camera) # fmt: off # These points are taken directly from the first 11 points generated by GTSFM on the door12 dataset (without # any separate alignment transformation being applied) points_3d = np.array( [[-1.4687794397729077, -1.4966178675020756, 14.583277665978546], [-1.6172612359102505, -1.0951470733744013, 14.579095414379562], [-3.2190882723771783, -4.463465966172758, 14.444076631000476], [-0.6754206497590093, -1.1132530165104157, 14.916222213341355], [-1.5514099044537981, -1.305810425894855, 14.584788688422206], [-1.551319353347404, -1.304881682597853, 14.58246449772602], [-1.9055918588057448, -1.192867982227922, 14.446379510423219], [-1.5936792439193013, -1.4398818807488012, 14.587749795933021], [-1.5937405395983737, -1.4401641027442411, 14.588167699143174], [-1.6599318889904735, -1.2273604755959784, 14.57861988411431], [2.1935589900444867, 1.6233406628428935, 12.610234497076608]]) # fmt: on # Add all point cloud points to sample_data for point_3d in points_3d: sample_data.add_track(SfmTrack(point_3d)) camera_translations = np.array( [pose.translation() for pose in sample_data.get_camera_poses()]) initial_relative_distances = scipy.spatial.distance.cdist( camera_translations, points_3d, metric="euclidean") # Apply alignment transformation to sample_data walignedTw = ellipsoid_utils.get_ortho_axis_alignment_transform( sample_data) walignedSw = Similarity3(R=walignedTw.rotation(), t=walignedTw.translation(), s=1.0) sample_data = sample_data.apply_Sim3(walignedSw) # Aggregate the final, transformed points num_tracks = sample_data.number_tracks() transformed_points_3d = [ np.array(sample_data.get_track(i).point3()) for i in range(num_tracks) ] transformed_points_3d = np.array(transformed_points_3d) transformed_camera_translations = np.array( [pose.translation() for pose in sample_data.get_camera_poses()]) final_relative_distances = scipy.spatial.distance.cdist( transformed_camera_translations, transformed_points_3d, metric="euclidean") npt.assert_almost_equal(final_relative_distances, initial_relative_distances, decimal=3)
def test_get_ortho_axis_alignment_transform(self) -> None: """Tests the get_ortho_axis_alignment_transform() function with a GtsfmData object containing 3 camera frustums and 6 points in the point cloud. All points lie on z=0 plane. All frustums lie on z=2 plane and look down on the z=0 plane. sample_data: output_data: y y | o | | | o | o | c c | c | ------------- x ==> --o--c-----c--o-- x o | c | | o o | | c = point at (xi,yi,0) with a camera frustum at (xi,yi,2) o = point at (xi,yi,0) """ sample_data = GtsfmData(number_images=3) default_intrinsics = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0) # Add 3 camera frustums to sample_data (looking down at z=0 plane) cam_translations = np.array([[-1, 1, 2], [1, 1, 2], [1, -1, 2]]) for i in range(len(cam_translations)): camera = PinholeCameraCal3Bundler( Pose3(Rot3(), cam_translations[i, :]), default_intrinsics) sample_data.add_camera(i, camera) # Add 6 tracks to sample_data # fmt: off points3d = np.array([ [1, 1, 0], [-1, 1, 0], [-2, 2, 0], [-1, -1, 0], [1, -1, 0], [2, -2, 0], [5, 5, 0] # represents an outlier in this set of points ]) # fmt: on for pt_3d in points3d: sample_data.add_track(SfmTrack(pt_3d)) # Apply alignment transformation to sample_data walignedTw = ellipsoid_utils.get_ortho_axis_alignment_transform( sample_data) walignedSw = Similarity3(R=walignedTw.rotation(), t=walignedTw.translation(), s=1.0) sample_data = sample_data.apply_Sim3(walignedSw) # Verify correct 3d points. computed_3d_points = np.array([ sample_data.get_track(i).point3() for i in range(sample_data.number_tracks()) ]) expected_3d_points = np.array([ [0, np.sqrt(2), 0], [-np.sqrt(2), 0, 0], [-2 * np.sqrt(2), 0, 0], [0, -np.sqrt(2), 0], [np.sqrt(2), 0, 0], [2 * np.sqrt(2), 0, 0], [0, 5 * np.sqrt(2), 0], ]) npt.assert_almost_equal(computed_3d_points, expected_3d_points, decimal=3) # Verify correct camera poses. expected_wTi_list = [ Pose3(walignedTw.rotation(), np.array([-np.sqrt(2), 0, 2])), Pose3(walignedTw.rotation(), np.array([0, np.sqrt(2), 2])), Pose3(walignedTw.rotation(), np.array([np.sqrt(2), 0, 2])), ] computed_wTi_list = sample_data.get_camera_poses() for wTi_computed, wTi_expected in zip(computed_wTi_list, expected_wTi_list): assert wTi_computed.equals(wTi_expected, tol=1e-9)
def test_align_via_Sim3_to_poses(self) -> None: """Ensure that alignment of a SFM result to ground truth camera poses works correctly. Consider a simple example, wih 3 estimated poses and 2 points. When fitting the Similarity(3), all correspondences should have no noise, and alignment should be exact. GT: =========================================== | . (pose 3) . X . . | . (pose 2). . (pose 0) . .(pose 1) . --X . . ----X . . --- X . . | | | Estimate: ===================================== | . (pose 3) | . | X . . | | . . (pose 0) | .(pose 1) . | X . . --- X . . | --------------------------- | | """ dummy_calibration = Cal3Bundler(fx=900, k1=0, k2=0, u0=100, v0=100) # fmt: off wTi_list_gt = [ Pose3(Rot3(), np.array([3, 0, 0])), # wTi0 Pose3(Rot3(), np.array([0, 0, 0])), # wTi1 Pose3(Rot3(), np.array([0, -3, 0])), # wTi2 Pose3(Rot3(), np.array([0, 3, 0])), # wTi3 ] # points_gt = [ # np.array([1, 1, 0]), # np.array([3, 3, 0]) # ] # pose graph is scaled by a factor of 2, and shifted also. wTi_list_est = [ Pose3(Rot3(), np.array([8, 2, 0])), # wTi0 Pose3(Rot3(), np.array([2, 2, 0])), # wTi1 None, # wTi2 Pose3(Rot3(), np.array([2, 8, 0])), # wTi3 ] points_est = [np.array([4, 4, 0]), np.array([8, 8, 0])] # fmt: on def add_dummy_measurements_to_track(track: SfmTrack) -> SfmTrack: """Add some dummy 2d measurements in three views in cameras 0,1,3.""" track.addMeasurement(0, np.array([100, 200])) track.addMeasurement(1, np.array([300, 400])) track.addMeasurement(3, np.array([500, 600])) return track sfm_result = GtsfmData(number_images=4) gt_gtsfm_data = GtsfmData(number_images=4) for gtsfm_data, wTi_list in zip([sfm_result, gt_gtsfm_data], [wTi_list_est, wTi_list_gt]): for i, wTi in enumerate(wTi_list): if wTi is None: continue gtsfm_data.add_camera( i, PinholeCameraCal3Bundler(wTi, dummy_calibration)) for pt in points_est: track = SfmTrack(pt) track = add_dummy_measurements_to_track(track) gtsfm_data.add_track(track) aligned_sfm_result = sfm_result.align_via_Sim3_to_poses( wTi_list_ref=gt_gtsfm_data.get_camera_poses()) # tracks and poses should match GT now, after applying estimated scale and shift. assert aligned_sfm_result == gt_gtsfm_data # 3d points from tracks should now match the GT. assert np.allclose( aligned_sfm_result.get_track(0).point3(), np.array([1.0, 1.0, 0.0])) assert np.allclose( aligned_sfm_result.get_track(1).point3(), np.array([3.0, 3.0, 0.0]))