def plot_sfm_data_3d(sfm_data: GtsfmData, ax: Axes, max_plot_radius: float = 50) -> None: """Plot the camera poses and landmarks in 3D matplotlib plot. Args: sfm_data: SfmData object with camera and tracks. ax: axis to plot on. max_plot_radius: maximum distance threshold away from any camera for which a point will be plotted """ camera_poses = [ sfm_data.get_camera(i).pose() for i in sfm_data.get_valid_camera_indices() ] plot_poses_3d(camera_poses, ax) num_tracks = sfm_data.number_tracks() # Restrict 3d points to some radius of camera poses points_3d = np.array( [list(sfm_data.get_track(j).point3()) for j in range(num_tracks)]) nearby_points_3d = comp_utils.get_points_within_radius_of_cameras( camera_poses, points_3d, max_plot_radius) # plot 3D points for landmark in nearby_points_3d: ax.plot(landmark[0], landmark[1], landmark[2], "g.", markersize=1)
def test_get_points_within_radius_of_cameras_no_poses(): """Catch degenerate input.""" wTi_list = [] points_3d = np.array([[-15, 0, 0], [0, 15, 0], [-5, 0, 0], [15, 0, 0], [25, 0, 0]]) radius = 10.0 nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras( wTi_list, points_3d, radius) assert nearby_points_3d is None, "At least one camera pose must be provided"
def test_get_points_within_radius_of_cameras_negative_radius(): """Catch degenerate input.""" wTi0 = Pose3(Rot3(), np.zeros(3)) wTi1 = Pose3(Rot3(), np.array([10.0, 0, 0])) wTi_list = [wTi0, wTi1] points_3d = np.array([[-15, 0, 0], [0, 15, 0], [-5, 0, 0], [15, 0, 0], [25, 0, 0]]) radius = -5 nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras( wTi_list, points_3d, radius) assert nearby_points_3d is None, "Non-positive radius is not allowed"
def test_get_points_within_radius_of_cameras_no_points(): """Catch degenerate input.""" wTi0 = Pose3(Rot3(), np.zeros(3)) wTi1 = Pose3(Rot3(), np.array([10.0, 0, 0])) wTi_list = [wTi0, wTi1] points_3d = np.zeros((0, 3)) radius = 10.0 nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras( wTi_list, points_3d, radius) assert nearby_points_3d is None, "At least one 3d point must be provided"
def test_get_points_within_radius_of_cameras(): """Verify that points that fall outside of 10 meter radius of two camera poses. Cameras are placed at (0,0,0) and (10,0,0). """ wTi0 = Pose3(Rot3(), np.zeros(3)) wTi1 = Pose3(Rot3(), np.array([10.0, 0, 0])) wTi_list = [wTi0, wTi1] points_3d = np.array([[-15, 0, 0], [0, 15, 0], [-5, 0, 0], [15, 0, 0], [25, 0, 0]]) radius = 10.0 nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras( wTi_list, points_3d, radius) expected_nearby_points_3d = np.array([[-5, 0, 0], [15, 0, 0]]) np.testing.assert_allclose(nearby_points_3d, expected_nearby_points_3d)