def test_data(self):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3, 5, 6
uv_i1 = Point2(21.23, 45.64)
# translating along X-axis
uv_i2 = Point2(45.7, 45.64)
uv_i3 = Point2(68.35, 45.64)
# add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = Point3(1.0, 6.0, 2.0)
track2 = SfmTrack(pt)
track2.addMeasurement(i1, uv_i1)
track2.addMeasurement(i2, uv_i2)
track2.addMeasurement(i3, uv_i3)
self.data.addTrack(self.tracks)
self.data.addTrack(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)
def apply_Sim3(self, aSb: Similarity3) -> "GtsfmData":
"""Assume current tracks and cameras are in frame "b", then transport them to frame "a".
Returns:
New GtsfmData object which has been transformed from frame a to frame b.
"""
bTi_list = self.get_camera_poses()
aTi_list = [aSb.transformFrom(bTi) if bTi is not None else None for bTi in bTi_list]
aligned_data = GtsfmData(number_images=self.number_images())
# Update the camera poses to their aligned poses, but use the previous calibration.
for i, aTi in enumerate(aTi_list):
if aTi is None:
continue
calibration = self.get_camera(i).calibration()
camera_type = gtsfm_types.get_camera_class_for_calibration(calibration)
aligned_data.add_camera(i, camera_type(aTi, calibration))
# Align estimated tracks to ground truth.
for j in range(self.number_tracks()):
# Align each 3d point
track_b = self.get_track(index=j)
# Place into the "a" reference frame
pt_a = aSb.transformFrom(track_b.point3())
track_a = SfmTrack(pt_a)
# Copy over the 2d measurements directly into the new track.
for k in range(track_b.numberMeasurements()):
i, uv = track_b.measurement(k)
track_a.addMeasurement(i, uv)
aligned_data.add_track(track_a)
return aligned_data
def test_pick_cameras(self) -> None:
"""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.addMeasurement(idx=0, m=np.array([20.0, 5.0]))
track_to_add.addMeasurement(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 test_add_track_with_nonexistant_cameras(self) -> None:
"""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.addMeasurement(idx=0, m=np.array([20.0, 5.0]))
track_to_add.addMeasurement(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_get_track(self) -> None:
"""Testing getter for track."""
expected_track = SfmTrack(
np.array([6.41689062, 0.38897032, -23.58628273]))
expected_track.addMeasurement(0, np.array([383.88000488, 15.2999897]))
expected_track.addMeasurement(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_valid_cameras(self) -> None:
"""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.addMeasurement(idx=0, m=np.array([20.0, 5.0]))
track_to_add.addMeasurement(idx=1, m=np.array([60.0, 50.0]))
self.assertTrue(gtsfm_data.add_track(track_to_add))
def triangulate_two_view_correspondences(
cls,
camera_i1: gtsfm_types.CAMERA_TYPE,
camera_i2: gtsfm_types.CAMERA_TYPE,
keypoints_i1: Keypoints,
keypoints_i2: Keypoints,
corr_idxs: np.ndarray,
) -> Tuple[List[SfmTrack], List[int]]:
"""Triangulate 2-view correspondences to form 3d tracks.
Args:
camera_i1: camera for 1st view.
camera_i2: camera for 2nd view.
keypoints_i1: keypoints for 1st view.
keypoints_i2: keypoints for 2nd view.
corr_idxs: indices of corresponding keypoints.
Returns:
Triangulated 3D points as tracks.
"""
camera_set = (CameraSetCal3Bundler() if isinstance(
camera_i1, PinholeCameraCal3Bundler) else CameraSetCal3Fisheye())
camera_set.append(camera_i1)
camera_set.append(camera_i2)
tracks_3d: List[SfmTrack] = []
valid_indices: List[int] = []
for j, (idx1, idx2) in enumerate(corr_idxs):
track_2d = Point2Vector()
track_2d.append(keypoints_i1.coordinates[idx1])
track_2d.append(keypoints_i2.coordinates[idx2])
try:
triangulated_pt = gtsam.triangulatePoint3(
camera_set,
track_2d,
rank_tol=SVD_DLT_RANK_TOL,
optimize=False)
track_3d = SfmTrack(triangulated_pt)
track_3d.addMeasurement(0, track_2d[0])
track_3d.addMeasurement(1, track_2d[1])
tracks_3d.append(track_3d)
valid_indices.append(j)
except RuntimeError:
pass
# logger.error(e)
return tracks_3d, valid_indices
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.addMeasurement(idx=0, m=np.array([13, 24]))
# in camera 1
track_3d.addMeasurement(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 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 colmap2gtsfm(
cameras: Dict[int, ColmapCamera],
images: Dict[int, ColmapImage],
points3D: Dict[int, ColmapPoint3D],
load_sfmtracks: bool = False,
) -> Tuple[List[Cal3Bundler], List[Pose3], List[str], Optional[List[Point3]]]:
"""Converts COLMAP-formatted variables to GTSfM format.
Args:
cameras: dictionary of COLMAP-formatted Cameras
images: dictionary of COLMAP-formatted Images
points3D: dictionary of COLMAP-formatted Point3Ds
return_tracks (optional): whether or not to return tracks
Returns:
img_fnames: file names of images in images_gtsfm
images_gtsfm: list of N camera poses when each image was taken
cameras_gtsfm: list of N camera calibrations corresponding to the N images in images_gtsfm
sfmtracks_gtsfm: tracks of points in points3D
"""
# Note: Assumes input cameras use `PINHOLE` model
if len(images) == 0 and len(cameras) == 0:
raise RuntimeError("No Image or Camera data provided to loader.")
cameras_gtsfm, images_gtsfm, img_fnames = [], [], []
image_id_to_idx = {
} # keeps track of discrepencies between `image_id` and List index.
for idx, img in enumerate(images.values()):
images_gtsfm.append(Pose3(Rot3(img.qvec2rotmat()), img.tvec).inverse())
img_fnames.append(img.name)
fx, _, cx, cy = cameras[img.camera_id].params[:4]
cameras_gtsfm.append(Cal3Bundler(fx, 0.0, 0.0, cx, cy))
image_id_to_idx[img.id] = idx
if len(points3D) == 0 and load_sfmtracks:
raise RuntimeError("No SfMTrack data provided to loader.")
sfmtracks_gtsfm = None
if len(points3D) > 0 and load_sfmtracks:
sfmtracks_gtsfm = []
for point3D in points3D.values():
sfmtrack = SfmTrack(point3D.xyz)
for (image_id, point2d_idx) in zip(point3D.image_ids,
point3D.point2D_idxs):
sfmtrack.addMeasurement(image_id_to_idx[image_id],
images[image_id].xys[point2d_idx])
sfmtracks_gtsfm.append(sfmtrack)
return img_fnames, images_gtsfm, cameras_gtsfm, sfmtracks_gtsfm
def test_get_average_point_color(self):
"""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.addMeasurement(idx=0, m=np.array([130, 80]))
# in camera 1
track_3d.addMeasurement(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)
self.assertEqual(r, 50)
self.assertEqual(g, 60)
self.assertEqual(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.addMeasurement(idx=0, m=np.random.randn(2))
track_1.addMeasurement(idx=3, m=np.random.randn(2))
track_2 = SfmTrack(
np.random.randn(3)) # track with 3 cameras, which will be retained
track_2.addMeasurement(idx=1, m=np.random.randn(2))
track_2.addMeasurement(idx=2, m=np.random.randn(2))
track_2.addMeasurement(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_get_stats_for_sfmdata_skydio32(self) -> None:
"""Real data, from Skydio-32 sequence with the SIFT front-end, if Sim(3) object is corrupted with negative scale.
All tracks should have NaN reprojection error, from cheirality errors from the negative scale.
"""
fx, k1, k2, px, py = 609.94, -8.82687675e-07, -4.25111629e-06, 507, 380
calib = Cal3Bundler(fx, k1, k2, px, py)
wTi2 = Pose3(
Rot3([
[0.95813172, 0.00187844098, -0.286321635],
[-0.00477926501, 0.999944088, -0.00943285149],
[0.286287907, 0.0104063212, 0.958087127],
]),
Point3(-0.763405555, -0.266165811, -0.118153783),
)
wTi3 = Pose3(
Rot3([
[0.999985712, 0.00523414604, -0.00108604007],
[-0.00524091331, 0.999966264, -0.00632478431],
[0.00105289859, 0.00633038578, 0.999979409],
]),
Point3(-0.76321922 - 0.266172481 - 0.118146617),
)
wTi4 = Pose3(
Rot3([
[0.963327067, 0.00797924699, 0.268211287],
[-0.0070859796, 0.999965656, -0.00429831651],
[-0.268236373, 0.00224014493, 0.963350523],
]),
Point3(-0.76303985 - 0.266175812 - 0.11817041),
)
wTi5 = Pose3(
Rot3([
[0.965261642, 0.00828550155, 0.261153812],
[-0.00485679326, 0.99989337, -0.0137717378],
[-0.261240071, 0.0120249602, 0.965198956],
]),
Point3(-0.762848965 - 0.266179234 - 0.118209764),
)
wTi30 = Pose3(
Rot3([
[0.761797609, -0.0189987841, 0.647536446],
[0.000647893104, 0.999591701, 0.0285659033],
[-0.647814775, -0.0213419024, 0.761498877],
]),
Point3(-0.780003719, -0.266108015, -0.154604541),
)
aligned_cameras = {
2: PinholeCameraCal3Bundler(wTi2, calib),
3: PinholeCameraCal3Bundler(wTi3, calib),
4: PinholeCameraCal3Bundler(wTi4, calib),
5: PinholeCameraCal3Bundler(wTi5, calib),
30: PinholeCameraCal3Bundler(wTi30, calib),
}
t0 = SfmTrack(pt=[-0.7627727, -0.26624048, -0.11879795])
t0.addMeasurement(2, [184.08586121, 441.31314087])
t0.addMeasurement(4, [18.98637581, 453.21853638])
t1 = SfmTrack(pt=[-0.76277714, -0.26603358, -0.11884205])
t1.addMeasurement(2, [213.51266479, 288.06637573])
t1.addMeasurement(4, [50.23059464, 229.30541992])
t2 = SfmTrack(pt=[-0.7633115, -0.2662322, -0.11826181])
t2.addMeasurement(2, [227.52420044, 695.15087891])
t2.addMeasurement(3, [996.67608643, 705.03125])
t3 = SfmTrack(pt=[-0.76323087, -0.26629859, -0.11836833])
t3.addMeasurement(2, [251.37863159, 702.97064209])
t3.addMeasurement(3, [537.9753418, 732.26025391])
t4 = SfmTrack(pt=[-0.70450081, -0.28115719, -0.19063382])
t4.addMeasurement(2, [253.17749023, 490.47991943])
t4.addMeasurement(3, [13.17782784, 507.57717896])
t5 = SfmTrack(pt=[-0.52781989, -0.31926005, -0.40763909])
t5.addMeasurement(2, [253.52301025, 478.41384888])
t5.addMeasurement(3, [10.92995739, 493.31018066])
t6 = SfmTrack(pt=[-0.74893948, -0.27132075, -0.1360136])
t6.addMeasurement(2, [254.64611816, 533.04730225])
t6.addMeasurement(3, [18.78449249, 557.05041504])
aligned_tracks = [t0, t1, t2, t3, t4, t5, t6]
aligned_filtered_data = GtsfmData.from_cameras_and_tracks(
cameras=aligned_cameras, tracks=aligned_tracks, number_images=32)
metrics = metric_utils.get_stats_for_sfmdata(aligned_filtered_data,
suffix="_filtered")
assert metrics[0].name == "number_cameras"
assert np.isclose(metrics[0]._data, np.array(5.0, dtype=np.float32))
assert metrics[1].name == "number_tracks_filtered"
assert np.isclose(metrics[1]._data, np.array(7.0, dtype=np.float32))
assert metrics[2].name == "3d_track_lengths_filtered"
assert metrics[2].summary == {
"min": 2,
"max": 2,
"median": 2.0,
"mean": 2.0,
"stddev": 0.0,
"histogram": {
"1": 7
},
}
assert metrics[3].name == "reprojection_errors_filtered_px"
assert metrics[3].summary == {
"min": np.nan,
"max": np.nan,
"median": np.nan,
"mean": np.nan,
"stddev": np.nan
}
def triangulate(
self, track_2d: SfmTrack2d
) -> Tuple[Optional[SfmTrack], Optional[float], TriangulationExitCode]:
"""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
"""
# Check if we will run RANSAC, or not.
if self.options.mode in [
TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM,
TriangulationSamplingMode.RANSAC_SAMPLE_BIASED_BASELINE,
TriangulationSamplingMode.RANSAC_TOPK_BASELINES,
]:
best_inliers = self.execute_ransac_variant(track_2d)
elif self.options.mode == TriangulationSamplingMode.NO_RANSAC:
best_inliers = np.ones(len(track_2d.measurements),
dtype=bool) # all marked as inliers
# Verify we have at least 2 inliers.
inlier_idxs = (np.where(best_inliers)[0]).tolist()
if len(inlier_idxs) < 2:
return None, None, TriangulationExitCode.INLIERS_UNDERCONSTRAINED
# Extract keypoint measurements corresponding to inlier indices.
inlier_track = track_2d.select_subset(inlier_idxs)
track_cameras, track_measurements = self.extract_measurements(
inlier_track)
# Exit if we do not have at least 2 measurements in cameras with estimated poses.
if track_cameras is None:
return None, None, TriangulationExitCode.POSES_UNDERCONSTRAINED
# Triangulate and check for cheirality failure from GTSAM.
try:
triangulated_pt = gtsam.triangulatePoint3(
track_cameras,
track_measurements,
rank_tol=SVD_DLT_RANK_TOL,
optimize=True,
)
except RuntimeError:
return None, None, TriangulationExitCode.CHEIRALITY_FAILURE
# Compute reprojection errors for each measurement.
reproj_errors, avg_track_reproj_error = reproj_utils.compute_point_reprojection_errors(
self.track_camera_dict, triangulated_pt, inlier_track.measurements)
# Check that all measurements are within reprojection error threshold.
if not np.all(
reproj_errors.flatten() < self.options.reproj_error_threshold):
return None, avg_track_reproj_error, TriangulationExitCode.EXCEEDS_REPROJ_THRESH
# Create a gtsam.SfmTrack with the triangulated 3d point and associated 2d measurements.
track_3d = SfmTrack(triangulated_pt)
for i, uv in inlier_track.measurements:
track_3d.addMeasurement(i, uv)
return track_3d, avg_track_reproj_error, TriangulationExitCode.SUCCESS
def test_sfmTrack_roundtrip(self):
obj = SfmTrack(Point3(1, 1, 0))
obj.addMeasurement(0, Point2(-1, 5))
obj.addMeasurement(1, Point2(6, 2))
self.assertEqualityOnPickleRoundtrip(obj)
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
def test_align_via_Sim3_to_poses_skydio32(self) -> None:
"""Real data, from Skydio-32 sequence with the SIFT front-end.
Tracks should have identical non-NaN reprojection error before and after alignment.
"""
poses_gt = [
Pose3(
Rot3([
[0.696305769, -0.0106830792, -0.717665705],
[0.00546412488, 0.999939148, -0.00958346857],
[0.717724415, 0.00275160848, 0.696321772],
]),
Point3(5.83077801, -0.94815149, 0.397751679),
),
Pose3(
Rot3([
[0.692272397, -0.00529704529, -0.721616549],
[0.00634689669, 0.999979075, -0.00125157022],
[0.721608079, -0.0037136016, 0.692291531],
]),
Point3(5.03853323, -0.97547405, -0.348177392),
),
Pose3(
Rot3([
[0.945991981, -0.00633548292, -0.324128225],
[0.00450436485, 0.999969379, -0.00639931046],
[0.324158843, 0.00459370582, 0.945991552],
]),
Point3(4.13186176, -0.956364218, -0.796029527),
),
Pose3(
Rot3([
[0.999553623, -0.00346470207, -0.0296740626],
[0.00346104216, 0.999993995, -0.00017469881],
[0.0296744897, 7.19175654e-05, 0.999559612],
]),
Point3(3.1113898, -0.928583423, -0.90539337),
),
Pose3(
Rot3([
[0.967850252, -0.00144846042, 0.251522892],
[0.000254511591, 0.999988546, 0.00477934325],
[-0.251526934, -0.00456167299, 0.967839535],
]),
Point3(2.10584013, -0.921303194, -0.809322971),
),
Pose3(
Rot3([
[0.969854065, 0.000629052774, 0.243685716],
[0.000387180179, 0.999991428, -0.00412234326],
[-0.243686221, 0.00409242166, 0.969845508],
]),
Point3(1.0753788, -0.913035975, -0.616584091),
),
Pose3(
Rot3([
[0.998189342, 0.00110235337, 0.0601400045],
[-0.00110890447, 0.999999382, 7.55559042e-05],
[-0.060139884, -0.000142108649, 0.998189948],
]),
Point3(0.029993558, -0.951495122, -0.425525143),
),
Pose3(
Rot3([
[0.999999996, -2.62868666e-05, -8.67178281e-05],
[2.62791334e-05, 0.999999996, -8.91767396e-05],
[8.67201719e-05, 8.91744604e-05, 0.999999992],
]),
Point3(-0.973569417, -0.936340994, -0.253464928),
),
Pose3(
Rot3([
[0.99481227, -0.00153645011, 0.101716252],
[0.000916919443, 0.999980747, 0.00613725239],
[-0.101723724, -0.00601214847, 0.994794525],
]),
Point3(-2.02071256, -0.955446292, -0.240707879),
),
Pose3(
Rot3([
[0.89795602, -0.00978591184, 0.43997636],
[0.00645921401, 0.999938116, 0.00905779513],
[-0.440037771, -0.00529159974, 0.89796366],
]),
Point3(-2.94096695, -0.939974858, 0.0934225593),
),
Pose3(
Rot3([
[0.726299119, -0.00916784876, 0.687318077],
[0.00892018672, 0.999952563, 0.0039118575],
[-0.687321336, 0.00328981905, 0.726346444],
]),
Point3(-3.72843416, -0.897889251, 0.685129502),
),
Pose3(
Rot3([
[0.506756029, -0.000331706105, 0.862089858],
[0.00613841257, 0.999975964, -0.00322354286],
[-0.862068067, 0.00692541035, 0.506745885],
]),
Point3(-4.3909926, -0.890883291, 1.43029524),
),
Pose3(
Rot3([
[0.129316352, -0.00206958814, 0.991601896],
[0.00515932597, 0.999985691, 0.00141424797],
[-0.991590634, 0.00493310721, 0.129325179],
]),
Point3(-4.58510846, -0.922534227, 2.36884523),
),
Pose3(
Rot3([
[0.599853194, -0.00890004681, -0.800060263],
[0.00313716318, 0.999956608, -0.00877161373],
[0.800103615, 0.00275175707, 0.599855085],
]),
Point3(5.71559638, 0.486863076, 0.279141372),
),
Pose3(
Rot3([
[0.762552447, 0.000836438681, -0.646926069],
[0.00211337894, 0.999990607, 0.00378404105],
[0.646923157, -0.00425272942, 0.762543517],
]),
Point3(5.00243443, 0.513321893, -0.466921769),
),
Pose3(
Rot3([
[0.930381645, -0.00340164355, -0.36657678],
[0.00425636616, 0.999989781, 0.00152338305],
[0.366567852, -0.00297761145, 0.930386617],
]),
Point3(4.05404984, 0.493385291, -0.827904571),
),
Pose3(
Rot3([
[0.999996073, -0.00278379707, -0.000323508543],
[0.00278790921, 0.999905063, 0.0134941517],
[0.000285912831, -0.0134950006, 0.999908897],
]),
Point3(3.04724478, 0.491451306, -0.989571061),
),
Pose3(
Rot3([
[0.968578343, -0.002544616, 0.248695527],
[0.000806130148, 0.999974526, 0.00709200332],
[-0.248707238, -0.0066686795, 0.968555721],
]),
Point3(2.05737869, 0.46840177, -0.546344594),
),
Pose3(
Rot3([
[0.968827882, 0.000182770584, 0.247734722],
[-0.000558107079, 0.9999988, 0.00144484904],
[-0.24773416, -0.00153807255, 0.968826821],
]),
Point3(1.14019947, 0.469674641, -0.0491053805),
),
Pose3(
Rot3([
[0.991647805, 0.00197867892, 0.128960146],
[-0.00247518407, 0.999990129, 0.00368991165],
[-0.128951572, -0.00397829284, 0.991642914],
]),
Point3(0.150270471, 0.457867448, 0.103628642),
),
Pose3(
Rot3([
[0.992244594, 0.00477781876, -0.124208847],
[-0.0037682125, 0.999957938, 0.00836195891],
[0.124243574, -0.00782906317, 0.992220862],
]),
Point3(-0.937954641, 0.440532658, 0.154265069),
),
Pose3(
Rot3([
[0.999591078, 0.00215462857, -0.0285137564],
[-0.00183807224, 0.999936443, 0.0111234301],
[0.028535911, -0.0110664711, 0.999531507],
]),
Point3(-1.95622231, 0.448914367, -0.0859439782),
),
Pose3(
Rot3([
[0.931835342, 0.000956922238, 0.362880212],
[0.000941640753, 0.99998678, -0.00505501434],
[-0.362880252, 0.00505214382, 0.931822122],
]),
Point3(-2.85557418, 0.434739285, 0.0793777177),
),
Pose3(
Rot3([
[0.781615218, -0.0109886966, 0.623664238],
[0.00516954657, 0.999924591, 0.011139446],
[-0.623739616, -0.00548270158, 0.781613084],
]),
Point3(-3.67524552, 0.444074681, 0.583718622),
),
Pose3(
Rot3([
[0.521291761, 0.00264805046, 0.853374051],
[0.00659087718, 0.999952868, -0.00712898365],
[-0.853352707, 0.00934076542, 0.521249738],
]),
Point3(-4.35541796, 0.413479707, 1.31179007),
),
Pose3(
Rot3([
[0.320164205, -0.00890839482, 0.947319884],
[0.00458409304, 0.999958649, 0.007854118],
[-0.947350678, 0.00182799903, 0.320191803],
]),
Point3(-4.71617526, 0.476674479, 2.16502998),
),
Pose3(
Rot3([
[0.464861609, 0.0268597443, -0.884976415],
[-0.00947397841, 0.999633409, 0.0253631906],
[0.885333239, -0.00340614699, 0.464945663],
]),
Point3(6.11772094, 1.63029238, 0.491786626),
),
Pose3(
Rot3([
[0.691647251, 0.0216006293, -0.721912024],
[-0.0093228132, 0.999736395, 0.020981541],
[0.722174939, -0.00778156302, 0.691666308],
]),
Point3(5.46912979, 1.68759322, -0.288499782),
),
Pose3(
Rot3([
[0.921208931, 0.00622640471, -0.389018433],
[-0.00686296262, 0.999976419, -0.000246683913],
[0.389007724, 0.00289706631, 0.92122994],
]),
Point3(4.70156942, 1.72186229, -0.806181015),
),
Pose3(
Rot3([
[0.822397705, 0.00276497594, 0.568906142],
[0.00804891535, 0.999831556, -0.016494662],
[-0.568855921, 0.0181442503, 0.822236923],
]),
Point3(-3.51368714, 1.59619714, 0.437437437),
),
Pose3(
Rot3([
[0.726822937, -0.00545541524, 0.686803193],
[0.00913794245, 0.999956756, -0.00172754968],
[-0.686764068, 0.00753159111, 0.726841357],
]),
Point3(-4.29737821, 1.61462527, 1.11537749),
),
Pose3(
Rot3([
[0.402595481, 0.00697612855, 0.915351441],
[0.0114113638, 0.999855006, -0.0126391687],
[-0.915306892, 0.0155338804, 0.4024575],
]),
Point3(-4.6516433, 1.6323107, 1.96579585),
),
]
fx, k1, k2, px, py = 609.94, -8.82687675e-07, -4.25111629e-06, 507, 380
calib = Cal3Bundler(fx, k1, k2, px, py)
unaligned_cameras = {
2:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.681949, -0.568276, 0.460444],
[0.572389, -0.0227514, 0.819667],
[-0.455321, 0.822524, 0.34079],
]),
Point3(-1.52189, 0.78906, -1.60608),
),
calib,
),
4:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.817805393, -0.575044816, 0.022755196],
[0.0478829397, -0.0285875849, 0.998443776],
[-0.573499401, 0.81762229, 0.0509139174],
]),
Point3(-1.22653168, 0.686485651, -1.39294168),
),
calib,
),
3:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.783051568, -0.571905041, 0.244448085],
[0.314861464, -0.0255673164, 0.948793218],
[-0.536369743, 0.819921299, 0.200091385],
]),
Point3(-1.37620079, 0.721408674, -1.49945316),
),
calib,
),
5:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.818916586, -0.572896131, 0.0341415873],
[0.0550548476, -0.0192038786, 0.99829864],
[-0.571265778, 0.819402974, 0.0472670839],
]),
Point3(-1.06370243, 0.663084159, -1.27672831),
),
calib,
),
6:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.798825521, -0.571995242, 0.186277293],
[0.243311017, -0.0240196245, 0.969650869],
[-0.550161372, 0.819905178, 0.158360233],
]),
Point3(-0.896250742, 0.640768239, -1.16984756),
),
calib,
),
7:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.786416666, -0.570215296, 0.237493882],
[0.305475635, -0.0248440676, 0.951875732],
[-0.536873788, 0.821119534, 0.193724669],
]),
Point3(-0.740385043, 0.613956842, -1.05908579),
),
calib,
),
8:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.806252832, -0.57019757, 0.157578877],
[0.211046715, -0.0283979846, 0.977063375],
[-0.55264424, 0.821016617, 0.143234279],
]),
Point3(-0.58333517, 0.549832698, -0.9542864),
),
calib,
),
9:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.821191354, -0.557772774, -0.120558255],
[-0.125347331, -0.0297958331, 0.991665395],
[-0.556716092, 0.829458703, -0.0454472483],
]),
Point3(-0.436483039, 0.55003923, -0.850733187),
),
calib,
),
21:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.778607603, -0.575075476, 0.251114312],
[0.334920968, -0.0424301164, 0.941290407],
[-0.53065822, 0.816999316, 0.225641247],
]),
Point3(-0.736735967, 0.571415247, -0.738663611),
),
calib,
),
17:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.818569806, -0.573904529, 0.0240221722],
[0.0512889176, -0.0313725422, 0.998190969],
[-0.572112681, 0.818321059, 0.0551155579],
]),
Point3(-1.36150982, 0.724829031, -1.16055631),
),
calib,
),
18:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.812668105, -0.582027424, 0.0285417146],
[0.0570298244, -0.0306936169, 0.997900547],
[-0.579929436, 0.812589675, 0.0581366453],
]),
Point3(-1.20484771, 0.762370343, -1.05057127),
),
calib,
),
20:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.748446406, -0.580905382, 0.319963926],
[0.416860654, -0.0368374152, 0.908223651],
[-0.515805363, 0.813137099, 0.269727429],
]),
Point3(569.449421, -907.892555, -794.585647),
),
calib,
),
22:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.826878177, -0.559495019, -0.0569017041],
[-0.0452256802, -0.0346974602, 0.99837404],
[-0.560559647, 0.828107125, 0.00338702978],
]),
Point3(-0.591431172, 0.55422253, -0.654656597),
),
calib,
),
29:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.785759779, -0.574532433, -0.229115805],
[-0.246020939, -0.049553424, 0.967996981],
[-0.567499134, 0.81698038, -0.102409921],
]),
Point3(69.4916073, 240.595227, -493.278045),
),
calib,
),
23:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.783524382, -0.548569702, -0.291823276],
[-0.316457553, -0.051878563, 0.94718701],
[-0.534737468, 0.834493797, -0.132950906],
]),
Point3(-5.93496204, 41.9304933, -3.06881633),
),
calib,
),
10:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.766833992, -0.537641809, -0.350580824],
[-0.389506676, -0.0443270797, 0.919956336],
[-0.510147213, 0.84200736, -0.175423563],
]),
Point3(234.185458, 326.007989, -691.769777),
),
calib,
),
30:
PinholeCameraCal3Bundler(
Pose3(
Rot3([
[-0.754844165, -0.559278755, -0.342662459],
[-0.375790683, -0.0594160018, 0.92479787],
[-0.537579435, 0.826847636, -0.165321923],
]),
Point3(-5.93398168, 41.9107972, -3.07385081),
),
calib,
),
}
t0 = SfmTrack(pt=[-0.89190672, 1.21298076, -1.05838554])
t0.addMeasurement(2, [184.08586121, 441.31314087])
t0.addMeasurement(4, [18.98637581, 453.21853638])
t1 = SfmTrack(pt=[-0.76287111, 1.26476165, -1.22710579])
t1.addMeasurement(2, [213.51266479, 288.06637573])
t1.addMeasurement(4, [50.23059464, 229.30541992])
t2 = SfmTrack(pt=[-1.45773622, 0.86221933, -1.47515461])
t2.addMeasurement(2, [227.52420044, 695.15087891])
t2.addMeasurement(3, [996.67608643, 705.03125])
t3 = SfmTrack(pt=[-1.40486691, 0.93824916, -1.35192298])
t3.addMeasurement(2, [251.37863159, 702.97064209])
t3.addMeasurement(3, [537.9753418, 732.26025391])
t4 = SfmTrack(pt=[55.48969812, 52.24862241, 58.84578119])
t4.addMeasurement(2, [253.17749023, 490.47991943])
t4.addMeasurement(3, [13.17782784, 507.57717896])
t5 = SfmTrack(pt=[230.43166291, 206.44760657, 234.25904211])
t5.addMeasurement(2, [253.52301025, 478.41384888])
t5.addMeasurement(3, [10.92995739, 493.31018066])
t6 = SfmTrack(pt=[11.62742671, 13.43484624, 14.50306349])
t6.addMeasurement(2, [254.64611816, 533.04730225])
t6.addMeasurement(3, [18.78449249, 557.05041504])
unaligned_tracks = [t0, t1, t2, t3, t4, t5, t6]
unaligned_filtered_data = GtsfmData.from_cameras_and_tracks(
cameras=unaligned_cameras,
tracks=unaligned_tracks,
number_images=32)
unaligned_metrics = metrics_utils.get_stats_for_sfmdata(
unaligned_filtered_data, suffix="_filtered")
aligned_filtered_data = unaligned_filtered_data.align_via_Sim3_to_poses(
wTi_list_ref=poses_gt)
aligned_metrics = metrics_utils.get_stats_for_sfmdata(
aligned_filtered_data, suffix="_filtered")
assert unaligned_metrics[3].name == "reprojection_errors_filtered_px"
assert aligned_metrics[3].name == "reprojection_errors_filtered_px"
# Reprojection error should be unaffected by Sim(3) alignment.
for key in ["min", "max", "median", "mean", "stddev"]:
assert np.isclose(unaligned_metrics[3].summary[key],
aligned_metrics[3].summary[key])
class TestSfmData(GtsamTestCase):
"""Tests for SfmData and SfmTrack modules."""
def setUp(self):
"""Initialize SfmData and SfmTrack"""
self.data = SfmData()
# initialize SfmTrack with 3D point
self.tracks = SfmTrack()
def test_tracks(self):
"""Test functions in SfmTrack"""
# measurement is of format (camera_idx, imgPoint)
# create arbitrary camera indices for two cameras
i1, i2 = 4, 5
# create arbitrary image measurements for cameras i1 and i2
uv_i1 = Point2(12.6, 82)
# translating point uv_i1 along X-axis
uv_i2 = Point2(24.88, 82)
# add measurements to the track
self.tracks.addMeasurement(i1, uv_i1)
self.tracks.addMeasurement(i2, uv_i2)
# Number of measurements in the track is 2
self.assertEqual(self.tracks.numberMeasurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1)
np.testing.assert_array_almost_equal(Point3(0., 0., 0.),
self.tracks.point3())
def test_data(self):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3, 5, 6
uv_i1 = Point2(21.23, 45.64)
# translating along X-axis
uv_i2 = Point2(45.7, 45.64)
uv_i3 = Point2(68.35, 45.64)
# add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = Point3(1.0, 6.0, 2.0)
track2 = SfmTrack(pt)
track2.addMeasurement(i1, uv_i1)
track2.addMeasurement(i2, uv_i2)
track2.addMeasurement(i3, uv_i3)
self.data.addTrack(self.tracks)
self.data.addTrack(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)
def test_Balbianello(self):
""" Check that we can successfully read a bundler file and create a
factor graph from it
"""
# The structure where we will save the SfM data
filename = gtsam.findExampleDataFile("Balbianello.out")
sfm_data = SfmData.FromBundlerFile(filename)
# Check number of things
self.assertEqual(5, sfm_data.numberCameras())
self.assertEqual(544, sfm_data.numberTracks())
track0 = sfm_data.track(0)
self.assertEqual(3, track0.numberMeasurements())
# Check projection of a given point
self.assertEqual(0, track0.measurement(0)[0])
camera0 = sfm_data.camera(0)
expected = camera0.project(track0.point3())
actual = track0.measurement(0)[1]
self.gtsamAssertEquals(expected, actual, 1)
# We share *one* noiseModel between all projection factors
model = gtsam.noiseModel.Isotropic.Sigma(2,
1.0) # one pixel in u and v
# Convert to NonlinearFactorGraph
graph = sfm_data.sfmFactorGraph(model)
self.assertEqual(1419, graph.size()) # regression
# Get initial estimate
values = gtsam.initialCamerasAndPointsEstimate(sfm_data)
self.assertEqual(549, values.size()) # regression