def align_estimated_gtsfm_data( ba_input: GtsfmData, ba_output: GtsfmData, gt_pose_graph: List[Optional[Pose3]] ) -> Tuple[GtsfmData, GtsfmData, List[Optional[Pose3]]]: """Creates modified GtsfmData objects that emulate ba_input and ba_output but with point cloud and camera frustums aligned to the x,y,z axes. Also transforms GT camera poses to be aligned to axes. Args: ba_input: GtsfmData input to bundle adjustment. ba_output: GtsfmData output from bundle adjustment. gt_pose_graph: list of GT camera poses. Returns: Updated ba_input GtsfmData object aligned to axes. Updated ba_output GtsfmData object aligned to axes. Updated gt_pose_graph with GT poses aligned to axes. """ walignedTw = ellipsoid_utils.get_ortho_axis_alignment_transform(ba_output) walignedSw = Similarity3(R=walignedTw.rotation(), t=walignedTw.translation(), s=1.0) ba_input = ba_input.apply_Sim3(walignedSw) ba_output = ba_output.apply_Sim3(walignedSw) gt_pose_graph = [walignedSw.transformFrom(wTi) if wTi is not None else None for wTi in gt_pose_graph] return ba_input, ba_output, gt_pose_graph
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_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)