def classify_tracks3d_with_gt_cameras(
tracks: List[SfmTrack], cameras_gt: List[PinholeCameraCal3Bundler], reproj_error_thresh_px: float = 3
) -> List[TriangulationExitCode]:
"""Classifies the 3D tracks w.r.t ground truth cameras by performing triangulation and collecting exit codes.
Args:
tracks: list of 3d tracks, of length J.
cameras_gt: cameras with GT params.
reproj_error_thresh_px (optional): Reprojection error threshold (in pixels) for a track to be considered an
all-inlier one. Defaults to 3.
Returns:
The triangulation exit code for each input track, as list of length J (same as input).
"""
# convert the 3D tracks to 2D tracks
tracks_2d: List[SfmTrack2d] = []
for track_3d in tracks:
num_measurements = track_3d.numberMeasurements()
measurements: List[SfmMeasurement] = []
for k in range(num_measurements):
i, uv = track_3d.measurement(k)
measurements.append(SfmMeasurement(i, uv))
tracks_2d.append(SfmTrack2d(measurements))
return classify_tracks2d_with_gt_cameras(tracks_2d, cameras_gt, reproj_error_thresh_px)
def test_compute_point_reprojection_errors():
"""Ensure a hypothesized 3d point is projected correctly and compared w/ 2 measurements.
# 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)
}
point3d = np.array([1, 2, 1])
measurements = [
SfmMeasurement(i=1, uv=np.array([-8, 43])),
SfmMeasurement(i=0, uv=np.array([13, 24]))
]
errors, avg_track_reproj_error = reproj_utils.compute_point_reprojection_errors(
track_camera_dict, point3d, measurements)
expected_errors = np.array([np.sqrt(2), 0])
np.testing.assert_allclose(errors, expected_errors)
assert avg_track_reproj_error == np.sqrt(2) / 2
def test_eq_check_with_different_measurements(self) -> None:
"""Tests the __eq__ function with one measurement having different value of the 2d point."""
track_1 = SfmTrack2d(SAMPLE_MEASUREMENTS)
# changing the value of the last measurement
old_measurement = SAMPLE_MEASUREMENTS[-1]
track_2 = SfmTrack2d(
SAMPLE_MEASUREMENTS[:3] +
[SfmMeasurement(old_measurement.i, np.random.rand(2))])
self.assertNotEqual(track_1, track_2)
self.assertNotEqual(track_2, track_1)
def get_track_with_duplicate_measurements() -> List[SfmMeasurement]:
"""Generates a track with 2 measurements in an image."""
new_measurements = copy.deepcopy(MEASUREMENTS)
new_measurements.append(
SfmMeasurement(
new_measurements[0].i,
new_measurements[0].uv + Point2(2.0, -3.0),
))
return new_measurements
def get_track_with_one_outlier() -> List[SfmMeasurement]:
"""Generates a track with outlier measurement."""
# perturb one measurement
idx_to_perturb = 5
perturbed_measurements = copy.deepcopy(MEASUREMENTS)
original_measurement = perturbed_measurements[idx_to_perturb]
perturbed_measurements[idx_to_perturb] = SfmMeasurement(
original_measurement.i,
perturbed_measurements[idx_to_perturb].uv + Point2(20.0, -10.0),
)
return perturbed_measurements
def testSimpleTriangulationOnDoorDataset(self):
"""Test the tracks of the door dataset using simple triangulation initialization. Using computed tracks with
ground truth camera params.
Expecting failures on 2 tracks which have incorrect matches."""
with open(DOOR_TRACKS_PATH, "rb") as handle:
tracks = pickle.load(handle)
loader = FolderLoader(DOOR_DATASET_PATH, image_extension="JPG")
camera_dict = {
i: PinholeCameraCal3Bundler(loader.get_camera_pose(i),
loader.get_camera_intrinsics(i))
for i in range(len(loader))
}
initializer = Point3dInitializer(camera_dict,
TriangulationParam.NO_RANSAC,
reproj_error_thresh=1e5)
# tracks which have expected failures
# (both tracks have incorrect measurements)
expected_failures = [
SfmTrack2d(measurements=[
SfmMeasurement(i=1,
uv=np.array([1252.22729492, 1487.29431152])),
SfmMeasurement(i=2,
uv=np.array([1170.96679688, 1407.35876465])),
SfmMeasurement(i=4, uv=np.array([263.32104492, 1489.76965332
])),
]),
SfmTrack2d(measurements=[
SfmMeasurement(i=6, uv=np.array([1142.34545898, 735.92169189
])),
SfmMeasurement(i=7, uv=np.array([1179.84155273, 763.04095459
])),
SfmMeasurement(i=9, uv=np.array([216.54107666, 774.74017334])),
]),
]
for track_2d in tracks:
triangulated_track = initializer.triangulate(track_2d)
if triangulated_track is None:
# assert we have failures which are already expected
self.assertIn(track_2d, expected_failures)
Unit tests for the FeatureTrackGenerator class.
Authors: Sushmita Warrier, Xiaolong Wu, John Lambert
"""
import copy
from typing import Dict, List, Tuple
import numpy as np
from gtsam.utils.test_case import GtsamTestCase
from gtsfm.common.keypoints import Keypoints
from gtsfm.common.sfm_track import SfmMeasurement, SfmTrack2d
SAMPLE_MEASUREMENTS = [
SfmMeasurement(0, np.random.rand(2)),
SfmMeasurement(2, np.random.rand(2)),
SfmMeasurement(3, np.random.rand(2)),
SfmMeasurement(5, np.random.rand(2)),
]
def get_dummy_keypoints_list() -> List[Keypoints]:
""" """
img1_kp_coords = np.array([[1, 1], [2, 2], [3, 3]])
img1_kp_scale = np.array([6.0, 9.0, 8.5])
img2_kp_coords = np.array([
[1, 1],
[2, 2],
[3, 3],
[4, 4],
# Generate 8 camera poses arranged in a circle of radius 40 m
CAMERAS = {
i: PinholeCameraCal3Bundler(pose, CALIBRATION)
for i, pose in enumerate(
SFMdata.createPoses(
Cal3_S2(
CALIBRATION.fx(),
CALIBRATION.fx(),
0,
CALIBRATION.px(),
CALIBRATION.py(),
)))
}
LANDMARK_POINT = Point3(0.0, 0.0, 0.0)
MEASUREMENTS = [
SfmMeasurement(i, cam.project(LANDMARK_POINT))
for i, cam in CAMERAS.items()
]
def get_track_with_one_outlier() -> List[SfmMeasurement]:
"""Generates a track with outlier measurement."""
# perturb one measurement
idx_to_perturb = 5
perturbed_measurements = copy.deepcopy(MEASUREMENTS)
original_measurement = perturbed_measurements[idx_to_perturb]
perturbed_measurements[idx_to_perturb] = SfmMeasurement(
original_measurement.i,
perturbed_measurements[idx_to_perturb].uv + Point2(20.0, -10.0),
def run(self, matches_dict: Dict[Tuple[int, int], np.ndarray],
keypoints_list: List[Keypoints]) -> List[SfmTrack2d]:
"""Estimate tracks from feature correspondences.
Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We create a singleton for union-find
set elements from camera index of a detection and the index of that detection in that camera's keypoint list,
i.e. (i,k).
Args:
matches_dict: Dict of pairwise matches of type:
key: indices for the matched pair of images
val: feature indices, as array of Nx2 shape; N being number of features. A row is (feature_idx1,
feature_idx2).
keypoints_list: List of keypoints for each image.
Returns:
list of all valid SfmTrack2d generated by the matches.
"""
# check to ensure dimensions of coordinates are correct
dims_valid = all([kps.coordinates.ndim == 2 for kps in keypoints_list])
if not dims_valid:
raise Exception(
"Dimensions for Keypoint coordinates incorrect. Array needs to be 2D"
)
# Generate the DSF to form tracks
dsf = gtsam.DSFMapIndexPair()
track_2d_list = []
# for DSF finally
# measurement_idxs represented by ks
for (i1, i2), k_pairs in matches_dict.items():
for (k1, k2) in k_pairs:
dsf.merge(gtsam.IndexPair(i1, k1), gtsam.IndexPair(i2, k2))
key_set = dsf.sets()
erroneous_track_count = 0
# create a landmark map: a list of tracks
# Each track is represented as a list of (camera_idx, measurements)
for set_id in key_set:
index_pair_set = key_set[
set_id] # key_set is a wrapped C++ map, so this unusual syntax is required
# Initialize track from measurements
track_measurements = []
for index_pair in gtsam.IndexPairSetAsArray(index_pair_set):
# camera_idx is represented by i
# measurement_idx is represented by k
i = index_pair.i()
k = index_pair.j()
# add measurement in this track
track_measurements += [
SfmMeasurement(i, keypoints_list[i].coordinates[k])
]
track_2d = SfmTrack2d(track_measurements)
# Skip erroneous track that had repeated measurements within the same image (i.e., violates transitivity).
# This is an expected result from an incorrect correspondence slipping through.
if track_2d.validate_unique_cameras():
track_2d_list += [track_2d]
else:
erroneous_track_count += 1
erroneous_track_pct = erroneous_track_count / len(
key_set) * 100 if len(key_set) > 0 else np.NaN
logger.info(
f"DSF Union-Find: {erroneous_track_pct:.2f}% of tracks discarded from multiple obs. in a single image."
)
return track_2d_list