def get_average_point_color(track: SfmTrack,
images: List[Image]) -> Tuple[int, int, int]:
"""
Args:
track: 3d point/landmark and its corresponding 2d measurements in various cameras
images: list of all images for this scene
Returns:
r: red color intensity, in range [0,255]
g: green color intensity, in range [0,255]
b: blue color intensity, in range [0,255]
"""
rgb_measurements = []
for k in range(track.number_measurements()):
# process each measurement
i, uv_measured = track.measurement(k)
u, v = np.round(uv_measured).astype(np.int32)
# ensure round did not push us out of bounds
u = np.clip(u, 0, images[i].width - 1)
v = np.clip(v, 0, images[i].height - 1)
rgb_measurements += [images[i].value_array[v, u]]
r, g, b = np.array(rgb_measurements).mean(axis=0).astype(np.uint8)
return r, g, b
def compute_track_reprojection_errors(
track_camera_dict: Dict[int, PinholeCameraCal3Bundler],
track: SfmTrack) -> Tuple[np.ndarray, float]:
"""Compute reprojection errors for measurements in the tracks.
Args:
track_camera_dict: Dict of cameras, with camera indices as keys.
track: 3d point/landmark and its corresponding 2d measurements in various cameras
Returns:
reprojection errors for each measurement (measured in pixels).
avg_track_reproj_error: average reprojection error of all meausurements in track.
"""
errors = []
for k in range(track.numberMeasurements()):
# process each measurement
i, uv_measured = track.measurement(k)
# get the camera associated with the measurement
camera = track_camera_dict[i]
# Project to camera
uv_reprojected, success_flag = camera.projectSafe(track.point3())
# Projection error in camera
if success_flag:
errors.append(np.linalg.norm(uv_measured - uv_reprojected))
else:
# failure in projection
errors.append(np.nan)
errors = np.array(errors)
avg_track_reproj_error = np.nan if np.isnan(errors).all() else np.nanmean(
errors)
return errors, avg_track_reproj_error
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 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 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_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 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 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 add_track(self, track: SfmTrack) -> bool:
"""Add a track, after checking if all the cameras in the track are already added.
Args:
track: track to add.
Returns:
Flag indicating the success of adding operation.
"""
# check if all cameras are already added
for j in range(track.number_measurements()):
i, _ = track.measurement(j)
if i not in self._cameras:
return False
self._tracks.append(track)
return True
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_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 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_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 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_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 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_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)
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
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])
def setUp(self):
"""Initialize SfmData and SfmTrack"""
self.data = SfmData()
# initialize SfmTrack with 3D point
self.tracks = SfmTrack()
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
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_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 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(self) -> None:
"""Ensure that alignment of a SFM result to ground truth camera poses works correctly.
Consider a simple example, wih 3 estimated poses and 2 points.
When fitting the Similarity(3), all correspondences should have no noise, and alignment should be exact.
GT: ===========================================
|
. (pose 3)
.
X . .
|
. (pose 2). . (pose 0)
. .(pose 1) .
--X . . ----X . . --- X . .
|
|
|
Estimate: =====================================
| . (pose 3)
| .
| X . .
|
| . . (pose 0)
| .(pose 1) .
| X . . --- X . .
|
---------------------------
|
|
"""
dummy_calibration = Cal3Bundler(fx=900, k1=0, k2=0, u0=100, v0=100)
# fmt: off
wTi_list_gt = [
Pose3(Rot3(), np.array([3, 0, 0])), # wTi0
Pose3(Rot3(), np.array([0, 0, 0])), # wTi1
Pose3(Rot3(), np.array([0, -3, 0])), # wTi2
Pose3(Rot3(), np.array([0, 3, 0])), # wTi3
]
# points_gt = [
# np.array([1, 1, 0]),
# np.array([3, 3, 0])
# ]
# pose graph is scaled by a factor of 2, and shifted also.
wTi_list_est = [
Pose3(Rot3(), np.array([8, 2, 0])), # wTi0
Pose3(Rot3(), np.array([2, 2, 0])), # wTi1
None, # wTi2
Pose3(Rot3(), np.array([2, 8, 0])), # wTi3
]
points_est = [np.array([4, 4, 0]), np.array([8, 8, 0])]
# fmt: on
def add_dummy_measurements_to_track(track: SfmTrack) -> SfmTrack:
"""Add some dummy 2d measurements in three views in cameras 0,1,3."""
track.addMeasurement(0, np.array([100, 200]))
track.addMeasurement(1, np.array([300, 400]))
track.addMeasurement(3, np.array([500, 600]))
return track
sfm_result = GtsfmData(number_images=4)
gt_gtsfm_data = GtsfmData(number_images=4)
for gtsfm_data, wTi_list in zip([sfm_result, gt_gtsfm_data],
[wTi_list_est, wTi_list_gt]):
for i, wTi in enumerate(wTi_list):
if wTi is None:
continue
gtsfm_data.add_camera(
i, PinholeCameraCal3Bundler(wTi, dummy_calibration))
for pt in points_est:
track = SfmTrack(pt)
track = add_dummy_measurements_to_track(track)
gtsfm_data.add_track(track)
aligned_sfm_result = sfm_result.align_via_Sim3_to_poses(
wTi_list_ref=gt_gtsfm_data.get_camera_poses())
# tracks and poses should match GT now, after applying estimated scale and shift.
assert aligned_sfm_result == gt_gtsfm_data
# 3d points from tracks should now match the GT.
assert np.allclose(
aligned_sfm_result.get_track(0).point3(), np.array([1.0, 1.0,
0.0]))
assert np.allclose(
aligned_sfm_result.get_track(1).point3(), np.array([3.0, 3.0,
0.0]))
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)
