def test_pick_cameras(self):
"""Test picking cameras."""
obj = copy.deepcopy(EXAMPLE_DATA)
# add a new track with just camera 0 and 2
track_to_add = SfmTrack(np.array([0, -2.0, 5.0]))
track_to_add.add_measurement(idx=0, m=np.array([20.0, 5.0]))
track_to_add.add_measurement(idx=2, m=np.array([60.0, 50.0]))
obj.add_track(track_to_add)
# pick the cameras at index 0 and 2, and hence dropping camera at index 1.
cams_to_pick = [0, 2]
computed = GtsfmData.from_selected_cameras(obj, cams_to_pick)
# test the camera has actually been dropped
self.assertListEqual(computed.get_valid_camera_indices(), cams_to_pick)
# test the camera objects
self.assertEqual(computed.get_camera(0), obj.get_camera(0))
self.assertEqual(computed.get_camera(2), obj.get_camera(2))
# check the track
self.assertEqual(computed.number_tracks(), 1)
self.assertTrue(
computed.get_track(0).equals(track_to_add, EQUALITY_TOLERANCE))
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_get_track(self):
"""Testing getter for track."""
expected_track = SfmTrack(
np.array([6.41689062, 0.38897032, -23.58628273]))
expected_track.add_measurement(0, np.array([383.88000488, 15.2999897]))
expected_track.add_measurement(1, np.array([559.75, 106.15000153]))
computed = EXAMPLE_DATA.get_track(1)
# comparing just the point because track equality is failing
np.testing.assert_allclose(computed.point3(), expected_track.point3())
def test_add_track_with_nonexistant_cameras(self):
"""Testing track addition where some cameras are not in tracks, resulting in failure."""
gtsfm_data = copy.deepcopy(EXAMPLE_DATA)
# add a track on camera #0 and #1, which exists in the data
track_to_add = SfmTrack(np.array([0, -2.0, 5.0]))
track_to_add.add_measurement(idx=0, m=np.array([20.0, 5.0]))
track_to_add.add_measurement(idx=3, m=np.array(
[60.0, 50.0])) # this camera does not exist
self.assertFalse(gtsfm_data.add_track(track_to_add))
def test_add_track_with_valid_cameras(self):
"""Testing track addition when all cameras in track are already present."""
gtsfm_data = copy.deepcopy(EXAMPLE_DATA)
# add a track on camera #0 and #1, which exists in the data
track_to_add = SfmTrack(np.array([0, -2.0, 5.0]))
track_to_add.add_measurement(idx=0, m=np.array([20.0, 5.0]))
track_to_add.add_measurement(idx=1, m=np.array([60.0, 50.0]))
self.assertTrue(gtsfm_data.add_track(track_to_add))
def run(
self,
cameras: Dict[int, PinholeCameraCal3Bundler],
corr_idxs_dict: Dict[Tuple[int, int], np.ndarray],
keypoints_list: List[Keypoints],
) -> SfmData:
"""Perform the data association.
Args:
cameras: dictionary with image index as key, and camera object w/ intrinsics + extrinsics as value.
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
"""
available_cams = np.array(list(cameras.keys()), dtype=np.uint32)
# form few tracks randomly
tracks = []
num_tracks = random.randint(5, 10)
for _ in range(num_tracks):
# obtain 3D points for the track randomly
point_3d = np.random.rand(3, 1)
# create GTSAM's SfmTrack object
sfmTrack = SfmTrack(point_3d)
# randomly select cameras for this track
selected_cams = np.random.choice(available_cams, self.min_track_len, replace=False)
# for each selected camera, randomly select a point
for cam_idx in selected_cams:
measurement_idx = random.randint(0, len(keypoints_list[cam_idx]) - 1)
measurement = keypoints_list[cam_idx].coordinates[measurement_idx]
sfmTrack.add_measurement(cam_idx, measurement)
tracks.append(sfmTrack)
# create the final SfmData object
sfm_data = SfmData()
for cam in cameras.values():
sfm_data.add_camera(cam)
for track in tracks:
sfm_data.add_track(track)
return sfm_data
def test_compute_track_reprojection_errors():
"""Ensure that reprojection error is computed properly within a track.
# For camera 0:
# [13] = [10,0,3] [1,0,0 | 0] [1]
# [24] = [0,10,4] * [0,1,0 | 0] *[2]
# [1] = [0, 0,1] [0,0,1 | 0] [1]
# [1]
# For camera 1:
# [-7] = [10,0,3] [1,0,0 |-2] [1]
# [44] = [0,10,4] * [0,1,0 | 2] *[2]
# [1] = [0, 0,1] [0,0,1 | 0] [1]
# [1]
"""
wTi0 = Pose3(Rot3.RzRyRx(0, 0, 0), np.zeros((3, 1)))
wTi1 = Pose3(Rot3.RzRyRx(0, 0, 0), np.array([2, -2, 0]))
f = 10
k1 = 0
k2 = 0
u0 = 3
v0 = 4
K0 = Cal3Bundler(f, k1, k2, u0, v0)
K1 = Cal3Bundler(f, k1, k2, u0, v0)
track_camera_dict = {
0: PinholeCameraCal3Bundler(wTi0, K0),
1: PinholeCameraCal3Bundler(wTi1, K1)
}
triangulated_pt = np.array([1, 2, 1])
track_3d = SfmTrack(triangulated_pt)
# in camera 0
track_3d.add_measurement(idx=0, m=np.array([13, 24]))
# in camera 1
track_3d.add_measurement(idx=1, m=np.array(
[-8, 43])) # should be (-7,44), 1 px error in each dim
errors, avg_track_reproj_error = reproj_utils.compute_track_reprojection_errors(
track_camera_dict, track_3d)
expected_errors = np.array([0, np.sqrt(2)])
np.testing.assert_allclose(errors, expected_errors)
assert avg_track_reproj_error == np.sqrt(2) / 2
def test_get_average_point_color():
""" Ensure 3d point color is computed as mean of RGB per 2d measurement."""
# random point; 2d measurements below are dummy locations (not actual projection)
triangulated_pt = np.array([1, 2, 1])
track_3d = SfmTrack(triangulated_pt)
# in camera 0
track_3d.add_measurement(idx=0, m=np.array([130, 80]))
# in camera 1
track_3d.add_measurement(idx=1, m=np.array([10, 60]))
img0 = np.zeros((100, 200, 3), dtype=np.uint8)
img0[80, 130] = np.array([40, 50, 60])
img1 = np.zeros((100, 200, 3), dtype=np.uint8)
img1[60, 10] = np.array([60, 70, 80])
images = {0: Image(img0), 1: Image(img1)}
r, g, b = image_utils.get_average_point_color(track_3d, images)
assert r == 50
assert g == 60
assert b == 70
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 test_sfmTrack_roundtrip(self):
obj = SfmTrack(Point3(1, 1, 0))
obj.add_measurement(0, Point2(-1, 5))
obj.add_measurement(1, Point2(6, 2))
self.assertEqualityOnPickleRoundtrip(obj)
def triangulate(
self, track_2d: SfmTrack2d
) -> Tuple[Optional[SfmTrack], Optional[float], bool]:
"""Triangulates 3D point according to the configured triangulation mode.
Args:
track: feature track from which measurements are to be extracted
Returns:
track with inlier measurements and 3D landmark. None returned if triangulation fails or has high error.
avg_track_reproj_error: reprojection error of 3d triangulated point to each image plane
Note: this may be "None" if the 3d point could not be triangulated successfully
due to a cheirality exception or insufficient number of RANSAC inlier measurements
is_cheirality_failure: boolean representing whether the selected 2d measurements lead
to a cheirality exception upon triangulation
"""
if self.mode in [
TriangulationParam.RANSAC_SAMPLE_UNIFORM,
TriangulationParam.RANSAC_SAMPLE_BIASED_BASELINE,
TriangulationParam.RANSAC_TOPK_BASELINES,
]:
best_inliers = self.execute_ransac_variant(track_2d)
elif self.mode == TriangulationParam.NO_RANSAC:
best_inliers = np.ones(len(track_2d.measurements),
dtype=bool) # all marked as inliers
inlier_idxs = (np.where(best_inliers)[0]).tolist()
is_cheirality_failure = False
if len(inlier_idxs) < 2:
return None, None, is_cheirality_failure
inlier_track = track_2d.select_subset(inlier_idxs)
camera_track, measurement_track = self.extract_measurements(
inlier_track)
try:
triangulated_pt = gtsam.triangulatePoint3(
camera_track,
measurement_track,
rank_tol=SVD_DLT_RANK_TOL,
optimize=True,
)
except RuntimeError:
is_cheirality_failure = True
return None, None, is_cheirality_failure
# compute reprojection errors for each measurement
reproj_errors = self.compute_track_reprojection_errors(
inlier_track.measurements, triangulated_pt)
# all the measurements should have error < threshold
if not np.all(reproj_errors < self.reproj_error_thresh):
return None, reproj_errors.mean(), is_cheirality_failure
track_3d = SfmTrack(triangulated_pt)
for i, uv in inlier_track.measurements:
track_3d.add_measurement(i, uv)
avg_track_reproj_error = reproj_errors.mean()
return track_3d, avg_track_reproj_error, is_cheirality_failure