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]))
