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