def values_to_gtsfm_data(values: Values, initial_data: GtsfmData) -> GtsfmData:
"""Cast results from the optimization to GtsfmData object.
Args:
values: results of factor graph optimization.
initial_data: data used to generate the factor graph; used to extract information about poses and 3d points in
the graph.
Returns:
optimized poses and landmarks.
"""
result = GtsfmData(initial_data.number_images())
# add cameras
for i in initial_data.get_valid_camera_indices():
result.add_camera(i, values.atPinholeCameraCal3Bundler(C(i)))
# add tracks
for j in range(initial_data.number_tracks()):
input_track = initial_data.get_track(j)
# populate the result with optimized 3D point
result_track = SfmTrack(values.atPoint3(P(j)))
for measurement_idx in range(input_track.number_measurements()):
i, uv = input_track.measurement(measurement_idx)
result_track.add_measurement(i, uv)
result.add_track(result_track)
return result
def test_round_trip_images_txt(self) -> None:
"""Starts with a pose. Writes the pose to images.txt (in a temporary directory). Then reads images.txt to recover
that same pose. Checks if the original wTc and recovered wTc match up."""
# fmt: off
# Rotation 45 degrees about the z-axis.
original_wRc = np.array([[np.cos(np.pi / 4), -np.sin(np.pi / 4), 0],
[np.sin(np.pi / 4),
np.cos(np.pi / 4), 0], [0, 0, 1]])
original_wtc = np.array([3, -2, 1])
# fmt: on
# Setup dummy GtsfmData Object with one image
original_wTc = Pose3(Rot3(original_wRc), original_wtc)
default_intrinsics = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0)
camera = PinholeCameraCal3Bundler(original_wTc, default_intrinsics)
gtsfm_data = GtsfmData(number_images=1)
gtsfm_data.add_camera(0, camera)
image = Image(value_array=None, file_name="dummy_image.jpg")
images = [image]
# Perform write and read operations inside a temporary directory
with tempfile.TemporaryDirectory() as tempdir:
images_fpath = os.path.join(tempdir, "images.txt")
io_utils.write_images(gtsfm_data, images, tempdir)
wTi_list, _ = io_utils.read_images_txt(images_fpath)
recovered_wTc = wTi_list[0]
npt.assert_almost_equal(original_wTc.matrix(),
recovered_wTc.matrix(),
decimal=3)
def test_get_camera_valid(self):
"""Test for get_camera for a valid index."""
expected = PinholeCameraCal3Bundler(
Pose3(), Cal3Bundler(fx=900, k1=0, k2=0, u0=100, v0=100))
i = 0
test_data = GtsfmData(1)
test_data.add_camera(index=i, camera=expected)
computed = test_data.get_camera(i)
self.assertTrue(computed.equals(expected, EQUALITY_TOLERANCE))
def test_select_largest_connected_component(self, graph_largest_cc_mock):
"""Test pruning to largest connected component according to tracks.
The function under test calles the graph utility, which has been mocked and we test the call against the mocked
object.
"""
gtsfm_data = GtsfmData(5)
cam = PinholeCameraCal3Bundler(Pose3(), Cal3Bundler())
# add the same camera at all indices
for i in range(gtsfm_data.number_images()):
gtsfm_data.add_camera(i, cam)
# add two tracks to create two connected components
track_1 = SfmTrack(
np.random.randn(3)) # track with 2 cameras, which will be dropped
track_1.add_measurement(idx=0, m=np.random.randn(2))
track_1.add_measurement(idx=3, m=np.random.randn(2))
track_2 = SfmTrack(
np.random.randn(3)) # track with 3 cameras, which will be retained
track_2.add_measurement(idx=1, m=np.random.randn(2))
track_2.add_measurement(idx=2, m=np.random.randn(2))
track_2.add_measurement(idx=4, m=np.random.randn(2))
gtsfm_data.add_track(track_1)
gtsfm_data.add_track(track_2)
largest_component_data = gtsfm_data.select_largest_connected_component(
)
# check the graph util function called with the edges defined by tracks
graph_largest_cc_mock.assert_called_once_with([(0, 3), (1, 2), (1, 4),
(2, 4)])
# check the expected cameras coming just from track_2
expected_camera_indices = [1, 2, 4]
computed_camera_indices = largest_component_data.get_valid_camera_indices(
)
self.assertListEqual(computed_camera_indices, expected_camera_indices)
# check that there is just one track
expected_num_tracks = 1
computed_num_tracks = largest_component_data.number_tracks()
self.assertEqual(computed_num_tracks, expected_num_tracks)
# check the exact track
computed_track = largest_component_data.get_track(0)
self.assertTrue(computed_track.equals(track_2, EQUALITY_TOLERANCE))
def values_to_gtsfm_data(values: Values, initial_data: GtsfmData,
shared_calib: bool) -> GtsfmData:
"""Cast results from the optimization to GtsfmData object.
Args:
values: results of factor graph optimization.
initial_data: data used to generate the factor graph; used to extract information about poses and 3d points in
the graph.
shared_calib: flag indicating if calibrations were shared between the cameras.
Returns:
optimized poses and landmarks.
"""
result = GtsfmData(initial_data.number_images())
is_fisheye_calibration = isinstance(initial_data.get_camera(0),
PinholeCameraCal3Fisheye)
if is_fisheye_calibration:
cal3_value_extraction_lambda = lambda i: values.atCal3Fisheye(
K(0 if shared_calib else i))
else:
cal3_value_extraction_lambda = lambda i: values.atCal3Bundler(
K(0 if shared_calib else i))
camera_class = PinholeCameraCal3Fisheye if is_fisheye_calibration else PinholeCameraCal3Bundler
# add cameras
for i in initial_data.get_valid_camera_indices():
result.add_camera(
i,
camera_class(values.atPose3(X(i)),
cal3_value_extraction_lambda(i)),
)
# add tracks
for j in range(initial_data.number_tracks()):
input_track = initial_data.get_track(j)
# populate the result with optimized 3D point
result_track = SfmTrack(values.atPoint3(P(j)))
for measurement_idx in range(input_track.numberMeasurements()):
i, uv = input_track.measurement(measurement_idx)
result_track.addMeasurement(i, uv)
result.add_track(result_track)
return result
def get_dummy_gtsfm_data(self) -> GtsfmData:
""" """
sfm_result = GtsfmData(self._num_images)
# Assume all default cameras are valid cameras, add toy data for cameras
for i in range(DEFAULT_NUM_CAMERAS):
sfm_result.add_camera(i, DEFAULT_CAMERAS[i])
# Calculate the measurements under each camera for all track points, then add toy data for tracks:
for j in range(DEFAULT_NUM_TRACKS):
world_x = DEFAULT_TRACK_POINTS[j]
track_to_add = SfmTrack(world_x)
for i in range(DEFAULT_NUM_CAMERAS):
uv = sfm_result.get_camera(i).project(world_x)
track_to_add.addMeasurement(idx=i, m=uv)
sfm_result.add_track(track_to_add)
return sfm_result
def test_round_trip_cameras_txt(self) -> None:
"""Creates a two cameras and writes to cameras.txt (in a temporary directory). Then reads cameras.txt to recover
the information. Checks if the original and recovered cameras match up."""
# Create multiple calibration data
k1 = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0)
k2 = Cal3Bundler(fx=200, k1=0.001, k2=0, u0=1000, v0=2000)
k3 = Cal3Bundler(fx=300, k1=0.004, k2=0.001, u0=1001, v0=2002)
original_calibrations = [k1, k2, k3]
gtsfm_data = GtsfmData(number_images=len(original_calibrations))
# Populate gtsfm_data with the generated vales
for i in range(len(original_calibrations)):
camera = PinholeCameraCal3Bundler(Pose3(),
original_calibrations[i])
gtsfm_data.add_camera(i, camera)
# Generate dummy images
image = Image(value_array=np.zeros((240, 320)),
file_name="dummy_image.jpg")
images = [image for i in range(len(original_calibrations))]
# Round trip
with tempfile.TemporaryDirectory() as tempdir:
cameras_fpath = os.path.join(tempdir, "cameras.txt")
io_utils.write_cameras(gtsfm_data, images, tempdir)
recovered_calibrations = io_utils.read_cameras_txt(cameras_fpath)
self.assertEqual(len(original_calibrations),
len(recovered_calibrations))
for i in range(len(recovered_calibrations)):
K_ori = original_calibrations[i]
K_rec = recovered_calibrations[i]
self.assertEqual(K_ori.fx(), K_rec.fx())
self.assertEqual(K_ori.px(), K_rec.px())
self.assertEqual(K_ori.py(), K_rec.py())
self.assertEqual(K_ori.k1(), K_rec.k1())
self.assertEqual(K_ori.k2(), K_rec.k2())
def test_filter_landmarks(self):
"""Tests filtering of SfmData based on reprojection error."""
max_reproj_error = 15
VALID_TRACK_INDICES = [0, 1, 5]
# construct expected data w/ tracks with reprojection errors below the
# threshold
expected_data = GtsfmData(EXAMPLE_DATA.number_images())
for i in EXAMPLE_DATA.get_valid_camera_indices():
expected_data.add_camera(i, EXAMPLE_DATA.get_camera(i))
for j in VALID_TRACK_INDICES:
expected_data.add_track(EXAMPLE_DATA.get_track(j))
# run the fn under test
filtered_sfm_data = EXAMPLE_DATA.filter_landmarks(max_reproj_error)
# compare the SfmData objects
self.assertEqual(filtered_sfm_data, expected_data)
def get_dummy_gtsfm_data(self) -> GtsfmData:
"""Create a new GtsfmData instance, add dummy cameras and tracks, and draw the track points on all images"""
sfm_result = GtsfmData(self._num_images)
# Assume all dummy cameras are valid cameras, add toy data for cameras
for i in range(NUM_CAMERAS):
sfm_result.add_camera(i, CAMERAS[i])
# Calculate the measurements under each camera for all track points, then add toy data for tracks:
for j in range(NUM_TRACKS):
world_x = DUMMY_TRACK_PTS_WORLD[j]
track_to_add = SfmTrack(world_x)
for i in range(NUM_CAMERAS):
u, v = sfm_result.get_camera(i).project(world_x)
# color the track point with (r, g, b) = (255, 255, 255)
self._img_dict[i].value_array[
np.round(v).astype(np.uint32),
np.round(u).astype(np.uint32), :] = 255
track_to_add.addMeasurement(idx=i, m=[u, v])
sfm_result.add_track(track_to_add)
return sfm_result
def read_bal(file_path: str) -> GtsfmData:
"""Read a Bundle Adjustment in the Large" (BAL) file.
See https://grail.cs.washington.edu/projects/bal/ for more details on the format.
Args:
file_name: file path of the BAL file.
Returns:
The data as an GtsfmData object.
"""
sfm_data = gtsam.readBal(file_path)
num_images = sfm_data.number_cameras()
gtsfm_data = GtsfmData(num_images)
for i in range(num_images):
camera = sfm_data.camera(i)
gtsfm_data.add_camera(i, camera)
for j in range(sfm_data.number_tracks()):
gtsfm_data.add_track(sfm_data.track(j))
return gtsfm_data
def bundle_adjust(
self,
keypoints_i1: Keypoints,
keypoints_i2: Keypoints,
verified_corr_idxs: np.ndarray,
camera_intrinsics_i1: gtsfm_types.CALIBRATION_TYPE,
camera_intrinsics_i2: gtsfm_types.CALIBRATION_TYPE,
i2Ri1_initial: Optional[Rot3],
i2Ui1_initial: Optional[Unit3],
i2Ti1_prior: Optional[PosePrior],
) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray]:
"""Refine the relative pose using bundle adjustment on the 2-view scene.
Args:
keypoints_i1: keypoints from image i1.
keypoints_i2: keypoints from image i2.
verified_corr_idxs: indices of verified correspondences between i1 and i2.
camera_intrinsics_i1: intrinsics for i1.
camera_intrinsics_i2: intrinsics for i2.
i2Ri1_initial: the relative rotation to be used as initial rotation between cameras.
i2Ui1_initial: the relative unit direction, to be used to initialize initial translation between cameras.
i2Ti1_prior: prior on the relative pose for cameras (i1, i2).
Returns:
Optimized relative rotation i2Ri1.
Optimized unit translation i2Ui1.
Optimized verified_corr_idxs.
"""
i2Ti1_from_verifier: Optional[Pose3] = (Pose3(
i2Ri1_initial, i2Ui1_initial.point3()) if i2Ri1_initial is not None
else None)
i2Ti1_initial: Optional[
Pose3] = self.__generate_initial_pose_for_bundle_adjustment(
i2Ti1_from_verifier, i2Ti1_prior)
if i2Ti1_initial is None:
return None, None, verified_corr_idxs
# Set the i1 camera pose as the global coordinate system.
camera_class = gtsfm_types.get_camera_class_for_calibration(
camera_intrinsics_i1)
camera_i1 = camera_class(Pose3(), camera_intrinsics_i1)
camera_i2 = camera_class(i2Ti1_initial.inverse(), camera_intrinsics_i2)
# Perform data association to construct 2-view BA input.
start_time = timeit.default_timer()
# TODO: add flag to switch between verified and putative correspondences.
triangulated_tracks, triangulated_indices = self.triangulate_two_view_correspondences(
camera_i1=camera_i1,
camera_i2=camera_i2,
keypoints_i1=keypoints_i1,
keypoints_i2=keypoints_i2,
corr_idxs=verified_corr_idxs,
)
logger.debug("Performed DA in %.6f seconds.",
timeit.default_timer() - start_time)
logger.debug("Triangulated %d correspondences out of %d.",
len(triangulated_tracks), len(verified_corr_idxs))
if len(triangulated_tracks) == 0:
return i2Ti1_initial.rotation(), Unit3(
i2Ti1_initial.translation()), np.array([], dtype=np.uint32)
# Perform 2-view BA.
start_time = timeit.default_timer()
ba_input = GtsfmData(number_images=2)
ba_input.add_camera(0, camera_i1)
ba_input.add_camera(1, camera_i2)
for track in triangulated_tracks:
ba_input.add_track(track)
relative_pose_prior_for_ba = {}
if i2Ti1_prior is not None:
relative_pose_prior_for_ba = {(0, 1): i2Ti1_prior}
_, ba_output, valid_mask = self._ba_optimizer.run(
ba_input,
absolute_pose_priors=[],
relative_pose_priors=relative_pose_prior_for_ba,
verbose=False)
valid_corr_idxs = verified_corr_idxs[triangulated_indices][valid_mask]
wTi1, wTi2 = ba_output.get_camera_poses() # extract the camera poses
if wTi1 is None or wTi2 is None:
logger.warning("2-view BA failed")
return i2Ri1_initial, i2Ui1_initial, valid_corr_idxs
i2Ti1_optimized = wTi2.between(wTi1)
logger.debug("Performed 2-view BA in %.6f seconds.",
timeit.default_timer() - start_time)
return i2Ti1_optimized.rotation(), Unit3(
i2Ti1_optimized.translation()), valid_corr_idxs
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 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_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 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
import gtsam
import numpy as np
from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Pose3, SfmData, SfmTrack
import gtsfm.utils.graph as graph_utils
import gtsfm.utils.io as io_utils
from gtsfm.common.gtsfm_data import GtsfmData
GTSAM_EXAMPLE_FILE = "dubrovnik-3-7-pre" # example data with 3 cams and 7 tracks
EXAMPLE_DATA = io_utils.read_bal(gtsam.findExampleDataFile(GTSAM_EXAMPLE_FILE))
NULL_DATA = SfmData()
# create example with non-consecutive cams
EXAMPLE_WITH_NON_CONSECUTIVE_CAMS = GtsfmData(number_images=5)
EXAMPLE_WITH_NON_CONSECUTIVE_CAMS.add_camera(index=0,
camera=EXAMPLE_DATA.get_camera(0))
EXAMPLE_WITH_NON_CONSECUTIVE_CAMS.add_camera(index=2,
camera=EXAMPLE_DATA.get_camera(1))
EXAMPLE_WITH_NON_CONSECUTIVE_CAMS.add_camera(index=3,
camera=EXAMPLE_DATA.get_camera(2))
EQUALITY_TOLERANCE = 1e-5
class TestGtsfmData(unittest.TestCase):
"""Unit tests for GtsfmData."""
def test_equality_with_same_data(self):
"""Test equality with the same data (same value but not same object)"""
self.assertEqual(EXAMPLE_DATA, copy.deepcopy(EXAMPLE_DATA))
def test_equality_with_different_object(self):
