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)