def setUp(self):
super().setUp()
self.simple_triangulation_initializer = Point3dInitializer(
CAMERAS, TriangulationParam.NO_RANSAC, reproj_error_thresh=5)
self.ransac_uniform_sampling_initializer = Point3dInitializer(
CAMERAS,
TriangulationParam.RANSAC_SAMPLE_UNIFORM,
reproj_error_thresh=5,
num_ransac_hypotheses=100,
)
def classify_tracks2d_with_gt_cameras(
tracks: List[SfmTrack2d], cameras_gt: List[PinholeCameraCal3Bundler], reproj_error_thresh_px: float = 3
) -> List[TriangulationExitCode]:
"""Classifies the 2D tracks w.r.t ground truth cameras by performing triangulation and collecting exit codes.
Args:
tracks: list of 2d tracks.
cameras_gt: cameras with GT params.
reproj_error_thresh_px (optional): Reprojection error threshold (in pixels) for a track to be considered an
all-inlier one. Defaults to 3.
Returns:
The triangulation exit code for each input track, as list of the same length as of tracks.
"""
# do a simple triangulation with the GT cameras
cameras_dict: Dict[int, PinholeCameraCal3Bundler] = {i: cam for i, cam in enumerate(cameras_gt)}
point3d_initializer = Point3dInitializer(
track_camera_dict=cameras_dict,
options=TriangulationOptions(
reproj_error_threshold=reproj_error_thresh_px, mode=TriangulationSamplingMode.NO_RANSAC
),
)
exit_codes: List[TriangulationExitCode] = []
for track in tracks:
_, _, triangulation_exit_code = point3d_initializer.triangulate(track_2d=track)
exit_codes.append(triangulation_exit_code)
return exit_codes
def __runWithCheiralityException(self, obj: Point3dInitializer) -> bool:
"""Run the initialization in a a-cheiral setup, and check that the result is a None track."""
cameras = obj.track_camera_dict
# flip the cameras first
yaw = np.pi
camera_flip_pose = Pose3(Rot3.RzRyRx(yaw, 0, 0), np.zeros((3, 1)))
flipped_cameras = {
i: PinholeCameraCal3Bundler(cam.pose().compose(camera_flip_pose),
cam.calibration())
for i, cam in cameras.items()
}
obj_with_flipped_cameras = Point3dInitializer(
flipped_cameras,
obj.mode,
obj.reproj_error_thresh,
obj.num_ransac_hypotheses,
)
sfm_track = obj_with_flipped_cameras.triangulate(
SfmTrack2d(MEASUREMENTS))
return sfm_track is None
def __runWithSingleOutlier(self, obj: Point3dInitializer) -> bool:
"""Run the initialization for a track with all inlier measurements except one, and checks for correctness of
the estimated point."""
sfm_track = obj.triangulate(SfmTrack2d(get_track_with_one_outlier()))
point3d = sfm_track.point3()
return np.array_equal(point3d, LANDMARK_POINT)
def __runWithTwoMeasurements(self, obj: Point3dInitializer) -> bool:
"""Run the initialization with a track with all correct measurements, and checks for correctness of the
recovered 3D point."""
sfm_track = obj.triangulate(SfmTrack2d(MEASUREMENTS[:2]))
point3d = sfm_track.point3()
return np.allclose(point3d, LANDMARK_POINT)
def __runWithDuplicateMeasurements(self, obj: Point3dInitializer) -> bool:
"""Run the initialization for a track with all inlier measurements except one, and checks for correctness of
the estimated point."""
sfm_track = obj.triangulate(
SfmTrack2d(get_track_with_duplicate_measurements()))
point3d = sfm_track.point3()
return np.allclose(point3d, LANDMARK_POINT, atol=1, rtol=1e-1)
def testSimpleTriangulationOnDoorDataset(self):
"""Test the tracks of the door dataset using simple triangulation initialization. Using computed tracks with
ground truth camera params.
Expecting failures on 2 tracks which have incorrect matches."""
with open(DOOR_TRACKS_PATH, "rb") as handle:
tracks = pickle.load(handle)
loader = FolderLoader(DOOR_DATASET_PATH, image_extension="JPG")
camera_dict = {
i: PinholeCameraCal3Bundler(loader.get_camera_pose(i),
loader.get_camera_intrinsics(i))
for i in range(len(loader))
}
initializer = Point3dInitializer(camera_dict,
TriangulationParam.NO_RANSAC,
reproj_error_thresh=1e5)
# tracks which have expected failures
# (both tracks have incorrect measurements)
expected_failures = [
SfmTrack2d(measurements=[
SfmMeasurement(i=1,
uv=np.array([1252.22729492, 1487.29431152])),
SfmMeasurement(i=2,
uv=np.array([1170.96679688, 1407.35876465])),
SfmMeasurement(i=4, uv=np.array([263.32104492, 1489.76965332
])),
]),
SfmTrack2d(measurements=[
SfmMeasurement(i=6, uv=np.array([1142.34545898, 735.92169189
])),
SfmMeasurement(i=7, uv=np.array([1179.84155273, 763.04095459
])),
SfmMeasurement(i=9, uv=np.array([216.54107666, 774.74017334])),
]),
]
for track_2d in tracks:
triangulated_track = initializer.triangulate(track_2d)
if triangulated_track is None:
# assert we have failures which are already expected
self.assertIn(track_2d, expected_failures)
def run(
self,
num_images: int,
cameras: Dict[int, PinholeCameraCal3Bundler],
corr_idxs_dict: Dict[Tuple[int, int], np.ndarray],
keypoints_list: List[Keypoints],
images: Optional[List[Image]] = None
) -> Tuple[GtsfmData, Dict[str, Any]]:
"""Perform the data association.
Args:
num_images: Number of images in the scene.
cameras: dictionary, with image index -> camera mapping.
corr_idxs_dict: dictionary, with key as image pair (i1,i2) and value as matching keypoint indices.
keypoints_list: keypoints for each image.
images: a list of all images in scene (optional and only for track patch visualization)
viz_patch_sz: width and height of patches, if if dumping/visualizing a patch for each 2d track measurement
Returns:
Cameras and tracks as GtsfmData.
"""
# generate tracks for 3D points using pairwise correspondences
tracks_2d = SfmTrack2d.generate_tracks_from_pairwise_matches(
corr_idxs_dict, keypoints_list)
if self.save_track_patches_viz and images is not None:
io_utils.save_track_visualizations(tracks_2d,
images,
save_dir=os.path.join(
"plots", "tracks_2d"))
# metrics on tracks w/o triangulation check
num_tracks_2d = len(tracks_2d)
track_lengths = list(map(lambda x: x.number_measurements(), tracks_2d))
mean_2d_track_length = np.mean(track_lengths)
logger.debug("[Data association] input number of tracks: %s",
num_tracks_2d)
logger.debug("[Data association] input avg. track length: %s",
mean_2d_track_length)
# initializer of 3D landmark for each track
point3d_initializer = Point3dInitializer(
cameras,
self.mode,
self.reproj_error_thresh,
self.num_ransac_hypotheses,
)
num_tracks_w_cheirality_exceptions = 0
per_accepted_track_avg_errors = []
per_rejected_track_avg_errors = []
# form GtsfmData object after triangulation
triangulated_data = GtsfmData(num_images)
# add all cameras
for i, camera in cameras.items():
triangulated_data.add_camera(i, camera)
# add valid tracks where triangulation is successful
for track_2d in tracks_2d:
# triangulate and filter based on reprojection error
sfm_track, avg_track_reproj_error, is_cheirality_failure = point3d_initializer.triangulate(
track_2d)
if is_cheirality_failure:
num_tracks_w_cheirality_exceptions += 1
if avg_track_reproj_error is not None:
# need no more than 3 significant figures in json report
avg_track_reproj_error = np.round(avg_track_reproj_error, 3)
if sfm_track is not None and self.__validate_track(sfm_track):
triangulated_data.add_track(sfm_track)
per_accepted_track_avg_errors.append(avg_track_reproj_error)
else:
per_rejected_track_avg_errors.append(avg_track_reproj_error)
track_cheirality_failure_ratio = num_tracks_w_cheirality_exceptions / len(
tracks_2d)
# pick only the largest connected component
connected_data = triangulated_data.select_largest_connected_component()
num_accepted_tracks = connected_data.number_tracks()
accepted_tracks_ratio = num_accepted_tracks / len(tracks_2d)
mean_3d_track_length, median_3d_track_length = connected_data.get_track_length_statistics(
)
track_lengths_3d = connected_data.get_track_lengths()
logger.debug("[Data association] output number of tracks: %s",
num_accepted_tracks)
logger.debug("[Data association] output avg. track length: %s",
np.round(mean_3d_track_length, 2))
# dump the 3d point cloud before Bundle Adjustment for offline visualization
points_3d = [
list(connected_data.get_track(j).point3())
for j in range(num_accepted_tracks)
]
# bin edges are halfway between each integer
track_lengths_histogram, _ = np.histogram(track_lengths_3d,
bins=np.linspace(
-0.5, 10.5, 12))
# min possible track len is 2, above 10 is improbable
histogram_dict = {
f"num_len_{i}_tracks": int(track_lengths_histogram[i])
for i in range(2, 11)
}
data_assoc_metrics = {
"mean_2d_track_length":
np.round(mean_2d_track_length, 3),
"accepted_tracks_ratio":
np.round(accepted_tracks_ratio, 3),
"track_cheirality_failure_ratio":
np.round(track_cheirality_failure_ratio, 3),
"num_accepted_tracks":
num_accepted_tracks,
"3d_tracks_length": {
"median":
median_3d_track_length,
"mean":
mean_3d_track_length,
"min":
int(track_lengths_3d.min())
if track_lengths_3d.size > 0 else None,
"max":
int(track_lengths_3d.max())
if track_lengths_3d.size > 0 else None,
"track_lengths_histogram":
histogram_dict,
},
"mean_accepted_track_avg_error":
np.array(per_accepted_track_avg_errors).mean(),
"per_rejected_track_avg_errors":
per_rejected_track_avg_errors,
"per_accepted_track_avg_errors":
per_accepted_track_avg_errors,
"points_3d":
points_3d,
}
return connected_data, data_assoc_metrics
def __runWithOneMeasurement(self, obj: Point3dInitializer) -> bool:
"""Run the initialization with a track with all correct measurements, and checks for a None track as a result."""
sfm_track = obj.triangulate(SfmTrack2d(MEASUREMENTS[:1]))
return sfm_track is None
def run(
self,
cameras: Dict[int, PinholeCameraCal3Bundler],
corr_idxs_dict: Dict[Tuple[int, int], np.ndarray],
keypoints_list: List[Keypoints],
) -> Tuple[SfmData, Dict[str, Any]]:
"""Perform the data association.
Args:
cameras: dictionary, with image index -> camera mapping.
corr_idxs_dict: dictionary, with key as image pair (i1,i2) and value as matching keypoint indices.
keypoints_list: keypoints for each image.
Returns:
cameras and tracks as SfmData.
"""
# generate tracks for 3D points using pairwise correspondences
tracks_2d = SfmTrack2d.generate_tracks_from_pairwise_matches(
corr_idxs_dict, keypoints_list)
# metrics on tracks w/o triangulation check
num_tracks_2d = len(tracks_2d)
track_lengths = list(map(lambda x: x.number_measurements(), tracks_2d))
mean_2d_track_length = np.mean(track_lengths)
logger.debug("[Data association] input number of tracks: %s",
num_tracks_2d)
logger.debug("[Data association] input avg. track length: %s",
mean_2d_track_length)
# initializer of 3D landmark for each track
point3d_initializer = Point3dInitializer(
cameras,
self.mode,
self.reproj_error_thresh,
self.num_ransac_hypotheses,
)
num_tracks_w_cheirality_exceptions = 0
per_accepted_track_avg_errors = []
per_rejected_track_avg_errors = []
# form SFMdata object after triangulation
triangulated_data = SfmData()
for track_2d in tracks_2d:
# triangulate and filter based on reprojection error
sfm_track, avg_track_reproj_error, is_cheirality_failure = point3d_initializer.triangulate(
track_2d)
if is_cheirality_failure:
num_tracks_w_cheirality_exceptions += 1
if sfm_track is not None and self.__validate_track(sfm_track):
triangulated_data.add_track(sfm_track)
per_accepted_track_avg_errors.append(avg_track_reproj_error)
else:
per_rejected_track_avg_errors.append(avg_track_reproj_error)
num_accepted_tracks = triangulated_data.number_tracks()
accepted_tracks_ratio = num_accepted_tracks / len(tracks_2d)
track_cheirality_failure_ratio = num_tracks_w_cheirality_exceptions / len(
tracks_2d)
# TODO: improve dropped camera handling
num_cameras = len(cameras.keys())
expected_camera_indices = np.arange(num_cameras)
# add cameras to landmark_map
for i, cam in cameras.items():
if i != expected_camera_indices[i]:
raise RuntimeError("Some cameras must have been dropped ")
triangulated_data.add_camera(cam)
mean_3d_track_length, median_3d_track_length, track_lengths_3d = SfmResult(
triangulated_data, None).get_track_length_statistics()
logger.debug("[Data association] output number of tracks: %s",
num_accepted_tracks)
logger.debug("[Data association] output avg. track length: %s",
mean_3d_track_length)
# dump the 3d point cloud before Bundle Adjustment for offline visualization
points_3d = [
list(triangulated_data.track(j).point3())
for j in range(num_accepted_tracks)
]
# bin edges are halfway between each integer
track_lengths_histogram, _ = np.histogram(track_lengths_3d,
bins=np.linspace(
-0.5, 10.5, 12))
# min possible track len is 2, above 10 is improbable
histogram_dict = {
f"num_len_{i}_tracks": int(track_lengths_histogram[i])
for i in range(2, 11)
}
data_assoc_metrics = {
"mean_2d_track_length":
np.round(mean_2d_track_length, 3),
"accepted_tracks_ratio":
np.round(accepted_tracks_ratio, 3),
"track_cheirality_failure_ratio":
np.round(track_cheirality_failure_ratio, 3),
"num_accepted_tracks":
num_accepted_tracks,
"3d_tracks_length": {
"median": median_3d_track_length,
"mean": mean_3d_track_length,
"min": int(track_lengths_3d.min()),
"max": int(track_lengths_3d.max()),
"track_lengths_histogram": histogram_dict,
},
"mean_accepted_track_avg_error":
np.array(per_accepted_track_avg_errors).mean(),
"per_rejected_track_avg_errors":
per_rejected_track_avg_errors,
"per_accepted_track_avg_errors":
per_accepted_track_avg_errors,
"points_3d":
points_3d,
}
return triangulated_data, data_assoc_metrics
def run(
self,
num_images: int,
cameras: Dict[int, gtsfm_types.CAMERA_TYPE],
corr_idxs_dict: Dict[Tuple[int, int], np.ndarray],
keypoints_list: List[Keypoints],
cameras_gt: List[Optional[gtsfm_types.CALIBRATION_TYPE]],
relative_pose_priors: Dict[Tuple[int, int], Optional[PosePrior]],
images: Optional[List[Image]] = None,
) -> Tuple[GtsfmData, GtsfmMetricsGroup]:
"""Perform the data association.
Args:
num_images: Number of images in the scene.
cameras: dictionary, with image index -> camera mapping.
corr_idxs_dict: dictionary, with key as image pair (i1,i2) and value as matching keypoint indices.
keypoints_list: keypoints for each image.
cameras_gt: list of GT cameras, to be used for benchmarking the tracks.
images: a list of all images in scene (optional and only for track patch visualization)
Returns:
A tuple of GtsfmData with cameras and tracks, and a GtsfmMetricsGroup with data association metrics
"""
# generate tracks for 3D points using pairwise correspondences
tracks_estimator = DsfTracksEstimator()
tracks_2d = tracks_estimator.run(corr_idxs_dict, keypoints_list)
if self.save_track_patches_viz and images is not None:
io_utils.save_track_visualizations(tracks_2d, images, save_dir=os.path.join("plots", "tracks_2d"))
# track lengths w/o triangulation check
track_lengths_2d = np.array(list(map(lambda x: int(x.number_measurements()), tracks_2d)), dtype=np.uint32)
logger.debug("[Data association] input number of tracks: %s", len(tracks_2d))
logger.debug("[Data association] input avg. track length: %s", np.mean(track_lengths_2d))
# Initialize 3D landmark for each track
point3d_initializer = Point3dInitializer(cameras, self.triangulation_options)
# form GtsfmData object after triangulation
triangulated_data = GtsfmData(num_images)
# add all cameras
for i, camera in cameras.items():
triangulated_data.add_camera(i, camera)
exit_codes_wrt_gt = track_utils.classify_tracks2d_with_gt_cameras(tracks=tracks_2d, cameras_gt=cameras_gt)
# add valid tracks where triangulation is successful
exit_codes_wrt_computed: List[TriangulationExitCode] = []
per_accepted_track_avg_errors = []
per_rejected_track_avg_errors = []
for track_2d in tracks_2d:
# triangulate and filter based on reprojection error
sfm_track, avg_track_reproj_error, triangulation_exit_code = point3d_initializer.triangulate(track_2d)
exit_codes_wrt_computed.append(triangulation_exit_code)
if triangulation_exit_code == TriangulationExitCode.CHEIRALITY_FAILURE:
continue
if sfm_track is not None and self.__validate_track(sfm_track):
triangulated_data.add_track(sfm_track)
per_accepted_track_avg_errors.append(avg_track_reproj_error)
else:
per_rejected_track_avg_errors.append(avg_track_reproj_error)
# aggregate the exit codes to get the distribution w.r.t each triangulation exit
# get the exit codes distribution w.r.t. the camera params computed by the upstream modules of GTSFM
exit_codes_wrt_computed_distribution = Counter(exit_codes_wrt_computed)
# compute the exit codes distribution w.r.t. a tuple of exit codes: the exit code when triangulated with the
# ground truth cameras and the exit code when triangulated with the computed cameras.
exit_codes_wrt_gt_and_computed_distribution = None
if exit_codes_wrt_gt is not None:
exit_codes_wrt_gt_and_computed_distribution = Counter(zip(exit_codes_wrt_gt, exit_codes_wrt_computed))
track_cheirality_failure_ratio = exit_codes_wrt_computed_distribution[
TriangulationExitCode.CHEIRALITY_FAILURE
] / len(tracks_2d)
# pick only the largest connected component
# TODO(Ayush): remove this for hilti as disconnected components not an issue?
cam_edges_from_prior = [k for k, v in relative_pose_priors.items() if v is not None]
connected_data = triangulated_data.select_largest_connected_component(extra_camera_edges=cam_edges_from_prior)
num_accepted_tracks = connected_data.number_tracks()
accepted_tracks_ratio = num_accepted_tracks / len(tracks_2d)
mean_3d_track_length, median_3d_track_length = connected_data.get_track_length_statistics()
track_lengths_3d = connected_data.get_track_lengths()
logger.debug("[Data association] output number of tracks: %s", num_accepted_tracks)
logger.debug("[Data association] output avg. track length: %.2f", mean_3d_track_length)
data_assoc_metrics = GtsfmMetricsGroup(
"data_association_metrics",
[
GtsfmMetric(
"2D_track_lengths",
track_lengths_2d,
store_full_data=False,
plot_type=GtsfmMetric.PlotType.HISTOGRAM,
),
GtsfmMetric("accepted_tracks_ratio", accepted_tracks_ratio),
GtsfmMetric("track_cheirality_failure_ratio", track_cheirality_failure_ratio),
GtsfmMetric("num_accepted_tracks", num_accepted_tracks),
GtsfmMetric(
"3d_tracks_length",
track_lengths_3d,
store_full_data=False,
plot_type=GtsfmMetric.PlotType.HISTOGRAM,
),
GtsfmMetric("accepted_track_avg_errors_px", per_accepted_track_avg_errors, store_full_data=False),
GtsfmMetric(
"rejected_track_avg_errors_px",
np.array(per_rejected_track_avg_errors).astype(np.float32),
store_full_data=False,
),
GtsfmMetric(name="number_cameras", data=len(connected_data.get_valid_camera_indices())),
],
)
if exit_codes_wrt_gt_and_computed_distribution is not None:
for (gt_exit_code, computed_exit_code), count in exit_codes_wrt_gt_and_computed_distribution.items():
# Each track has 2 associated exit codes: the triangulation exit codes w.r.t ground truth cameras
# and w.r.t cameras computed by upstream modules of GTSFM. We get the distribution of the number of
# tracks for each pair of (triangulation exit code w.r.t GT cams, triangulation exit code w.r.t
# computed cams)
metric_name = "#tracks triangulated with GT cams: {}, computed cams: {}".format(
gt_exit_code.name, computed_exit_code.name
)
data_assoc_metrics.add_metric(GtsfmMetric(name=metric_name, data=count))
return connected_data, data_assoc_metrics