def test_eq_check_with_missing_measurements(self) -> None:
"""Tests the __eq__ function with one track having subset of measurements of the other."""
track_1 = SfmTrack2d(SAMPLE_MEASUREMENTS)
# dropping the last measurement
track_2 = SfmTrack2d(SAMPLE_MEASUREMENTS[:3])
self.assertNotEqual(track_1, track_2)
self.assertNotEqual(track_2, track_1)
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 test_eq_check_with_same_measurements(self) -> None:
"""Tests the __eq__ function with the same set of measurements but with different ordering."""
# construct two tracks with different ordering of measurements
track_1 = SfmTrack2d(SAMPLE_MEASUREMENTS)
track_2 = SfmTrack2d([
SAMPLE_MEASUREMENTS[0],
SAMPLE_MEASUREMENTS[3],
SAMPLE_MEASUREMENTS[1],
SAMPLE_MEASUREMENTS[2],
])
self.assertEqual(track_1, track_2)
def __runWithCheiralityException(self, obj: Point3dInitializer) -> bool:
"""Run the initialization in a a-cheiral setup, and check that the result is a None track."""
cameras = obj.track_camera_dict
# flip the cameras first
yaw = np.pi
camera_flip_pose = Pose3(Rot3.RzRyRx(yaw, 0, 0), np.zeros((3, 1)))
flipped_cameras = {
i: PinholeCameraCal3Bundler(cam.pose().compose(camera_flip_pose),
cam.calibration())
for i, cam in cameras.items()
}
obj_with_flipped_cameras = Point3dInitializer(
flipped_cameras,
obj.mode,
obj.reproj_error_thresh,
obj.num_ransac_hypotheses,
)
sfm_track = obj_with_flipped_cameras.triangulate(
SfmTrack2d(MEASUREMENTS))
return sfm_track is None
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 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)
def __runWithSingleOutlier(self, obj: Point3dInitializer) -> bool:
"""Run the initialization for a track with all inlier measurements except one, and checks for correctness of
the estimated point."""
sfm_track = obj.triangulate(SfmTrack2d(get_track_with_one_outlier()))
point3d = sfm_track.point3()
return np.array_equal(point3d, LANDMARK_POINT)
def __runWithTwoMeasurements(self, obj: Point3dInitializer) -> bool:
"""Run the initialization with a track with all correct measurements, and checks for correctness of the
recovered 3D point."""
sfm_track = obj.triangulate(SfmTrack2d(MEASUREMENTS[:2]))
point3d = sfm_track.point3()
return np.allclose(point3d, LANDMARK_POINT)
def __runWithDuplicateMeasurements(self, obj: Point3dInitializer) -> bool:
"""Run the initialization for a track with all inlier measurements except one, and checks for correctness of
the estimated point."""
sfm_track = obj.triangulate(
SfmTrack2d(get_track_with_duplicate_measurements()))
point3d = sfm_track.point3()
return np.allclose(point3d, LANDMARK_POINT, atol=1, rtol=1e-1)
def testSimpleTriangulationWithOutlierMeasurements(self):
sfm_track = self.simple_triangulation_initializer.triangulate(
SfmTrack2d(get_track_with_one_outlier()))
self.assertIsNone(sfm_track)
def __runWithOneMeasurement(self, obj: Point3dInitializer) -> bool:
"""Run the initialization with a track with all correct measurements, and checks for a None track as a result."""
sfm_track = obj.triangulate(SfmTrack2d(MEASUREMENTS[:1]))
return sfm_track is None
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