def evaluate(
self, unfiltered_data: GtsfmData, filtered_data: GtsfmData,
cameras_gt: List[Optional[gtsfm_types.CAMERA_TYPE]]
) -> GtsfmMetricsGroup:
"""
Args:
unfiltered_data: optimized BA result, before filtering landmarks by reprojection error.
filtered_data: optimized BA result, after filtering landmarks and cameras.
cameras_gt: cameras with GT intrinsics and GT extrinsics.
Returns:
Metrics group containing metrics for both filtered and unfiltered BA results.
"""
ba_metrics = GtsfmMetricsGroup(
name=METRICS_GROUP,
metrics=metrics_utils.get_stats_for_sfmdata(unfiltered_data,
suffix="_unfiltered"))
poses_gt = [
cam.pose() if cam is not None else None for cam in cameras_gt
]
valid_poses_gt_count = len(poses_gt) - poses_gt.count(None)
if valid_poses_gt_count == 0:
return ba_metrics
# align the sparse multi-view estimate after BA to the ground truth pose graph.
aligned_filtered_data = filtered_data.align_via_Sim3_to_poses(
wTi_list_ref=poses_gt)
ba_pose_error_metrics = metrics_utils.compute_ba_pose_metrics(
gt_wTi_list=poses_gt, ba_output=aligned_filtered_data)
ba_metrics.extend(metrics_group=ba_pose_error_metrics)
output_tracks_exit_codes = track_utils.classify_tracks3d_with_gt_cameras(
tracks=aligned_filtered_data.get_tracks(), cameras_gt=cameras_gt)
output_tracks_exit_codes_distribution = Counter(
output_tracks_exit_codes)
for exit_code, count in output_tracks_exit_codes_distribution.items():
metric_name = "Filtered tracks triangulated with GT cams: {}".format(
exit_code.name)
ba_metrics.add_metric(GtsfmMetric(name=metric_name, data=count))
ba_metrics.add_metrics(
metrics_utils.get_stats_for_sfmdata(aligned_filtered_data,
suffix="_filtered"))
# ba_metrics.save_to_json(os.path.join(METRICS_PATH, "bundle_adjustment_metrics.json"))
logger.info("[Result] Mean track length %.3f",
np.mean(aligned_filtered_data.get_track_lengths()))
logger.info("[Result] Median track length %.3f",
np.median(aligned_filtered_data.get_track_lengths()))
aligned_filtered_data.log_scene_reprojection_error_stats()
return ba_metrics
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]))
