def test_TriangulationExample(self):
""" Tests triangulation with shared Cal3_S2 calibration"""
# Some common constants
sharedCal = (1500, 1200, 0, 640, 480)
measurements, _ = self.generate_measurements(Cal3_S2,
PinholeCameraCal3_S2,
(sharedCal, sharedCal))
triangulated_landmark = gtsam.triangulatePoint3(self.poses,
Cal3_S2(sharedCal),
measurements,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
# Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements_noisy = Point2Vector()
measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
triangulated_landmark = gtsam.triangulatePoint3(self.poses,
Cal3_S2(sharedCal),
measurements_noisy,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
def test_triangulation_skipped(self):
"""Estimate spatial point from image measurements"""
triangulated = gtsam.triangulatePoint3(self.cameras,
self.measurements,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(triangulated, self.origin)
def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification"""
rectified = gtsam.Point2Vector([
k.calibration().calibrate(pt)
for k, pt in zip(self.cameras, self.measurements)
])
shared_cal = gtsam.Cal3_S2()
triangulated = gtsam.triangulatePoint3(self.poses,
shared_cal,
rectified,
rank_tol=1e-9,
optimize=False)
self.gtsamAssertEquals(triangulated, self.origin)
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_distinct_Ks(self):
""" Tests triangulation with individual Cal3_S2 calibrations """
# two camera parameters
K1 = (1500, 1200, 0, 640, 480)
K2 = (1600, 1300, 0, 650, 440)
measurements, cameras = self.generate_measurements(Cal3_S2,
PinholeCameraCal3_S2,
(K1, K2),
camera_set=CameraSetCal3_S2)
triangulated_landmark = gtsam.triangulatePoint3(cameras,
measurements,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
def test_distinct_Ks_Bundler(self) -> None:
"""Tests triangulation with individual Cal3Bundler calibrations"""
# two camera parameters
K1 = (1500, 0, 0, 640, 480)
K2 = (1600, 0, 0, 650, 440)
measurements, cameras = self.generate_measurements(
calibration=Cal3Bundler,
camera_model=PinholeCameraCal3Bundler,
cal_params=(K1, K2),
camera_set=CameraSetCal3Bundler)
triangulated_landmark = gtsam.triangulatePoint3(cameras,
measurements,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
def execute_ransac_variant(self, track_2d: SfmTrack2d) -> np.ndarray:
""" Execute RANSAC algorithm to find best subset 2d measurements for a 3d point.
RANSAC chooses one of 3 different sampling schemes to execute.
Args:
track: feature track with N 2d measurements in separate images
Returns:
best_inliers: boolean array of length N. Indices of measurements
are set to true if they correspond to the best RANSAC hypothesis
"""
# Generate all possible matches
measurement_pairs = self.generate_measurement_pairs(track_2d)
# limit the number of samples to the number of available pairs
num_hypotheses = min(self.num_ransac_hypotheses,
len(measurement_pairs))
# Sampling
samples = self.sample_ransac_hypotheses(track_2d, measurement_pairs,
num_hypotheses)
# Initialize the best output containers
best_num_votes = 0
best_error = MAX_TRACK_REPROJ_ERROR
best_inliers = np.zeros(len(track_2d.measurements), dtype=bool)
for sample_idxs in samples:
k1, k2 = measurement_pairs[sample_idxs]
i1, uv1 = track_2d.measurements[k1]
i2, uv2 = track_2d.measurements[k2]
# check for unestimated cameras
if self.track_camera_dict.get(
i1) is None or self.track_camera_dict.get(i2) is None:
logger.warning(
"Unestimated cameras found at indices {} or {}. Skipping them."
.format(i1, i2))
continue
camera_estimates = CameraSetCal3Bundler()
camera_estimates.append(self.track_camera_dict.get(i1))
camera_estimates.append(self.track_camera_dict.get(i2))
img_measurements = Point2Vector()
img_measurements.append(uv1)
img_measurements.append(uv2)
# triangulate point for track
try:
triangulated_pt = gtsam.triangulatePoint3(
camera_estimates,
img_measurements,
rank_tol=SVD_DLT_RANK_TOL,
optimize=True,
)
except RuntimeError:
# TODO: handle cheirality exception properly?
logger.info(
"Cheirality exception from GTSAM's triangulatePoint3() likely due to outlier, skipping track"
)
continue
errors = self.compute_track_reprojection_errors(
track_2d.measurements, triangulated_pt)
# The best solution should correspond to the one with most inliers
# If the inlier number are the same, check the average error of inliers
is_inlier = errors < self.reproj_error_thresh
# tally the number of votes
inlier_errors = errors[is_inlier]
if inlier_errors.size > 0:
# only tally error over the inlier measurements
avg_error = inlier_errors.mean()
num_votes = is_inlier.astype(int).sum()
if (num_votes > best_num_votes) or (num_votes == best_num_votes
and
avg_error < best_error):
best_num_votes = num_votes
best_error = avg_error
best_inliers = is_inlier
return best_inliers
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 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_triangulation_robust_three_poses(self) -> None:
"""Ensure triangulation with a robust model works."""
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0))
pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1))
camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
camera3 = PinholeCameraCal3_S2(pose3, sharedCal)
z1: Point2 = camera1.project(landmark)
z2: Point2 = camera2.project(landmark)
z3: Point2 = camera3.project(landmark)
poses = gtsam.Pose3Vector([pose1, pose2, pose3])
measurements = Point2Vector([z1, z2, z3])
# noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=False)
self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
# Add outlier
measurements[0] += Point2(100, 120) # very large pixel noise!
# now estimate does not match landmark
actual2 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=False)
# DLT is surprisingly robust, but still off (actual error is around 0.26m)
self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
# Again with nonlinear optimization
actual3 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=True)
# result from nonlinear (but non-robust optimization) is close to DLT and still off
self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
# Again with nonlinear optimization, this time with robust loss
model = gtsam.noiseModel.Robust.Create(
gtsam.noiseModel.mEstimator.Huber.Create(1.345),
gtsam.noiseModel.Unit.Create(2))
actual4 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=True,
model=model)
# using the Huber loss we now have a quite small error!! nice!
self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
