def extract_measurements(
self, track: SfmTrack2d
) -> Tuple[Union[CameraSetCal3Bundler, CameraSetCal3Fisheye],
Point2Vector]:
"""Convert measurements in a track into GTSAM primitive types for triangulation arguments.
Returns None, None if less than 2 measurements were found with estimated camera poses after averaging.
Args:
track: feature track from which measurements are to be extracted.
Returns:
Vector of individual camera calibrations pertaining to track
Vector of 2d points pertaining to track measurements
"""
track_cameras = self._camera_set_class()
track_measurements = Point2Vector() # vector of 2d points
# Compile valid measurements.
for i, uv in track.measurements:
# check for unestimated cameras
if i in self.track_camera_dict and self.track_camera_dict.get(
i) is not None:
track_cameras.append(self.track_camera_dict.get(i))
track_measurements.append(uv)
else:
logger.warning(
"Unestimated cameras found at index %d. Skipping them.", i)
# Triangulation is underconstrained with <2 measurements.
if len(track_cameras) < 2:
return None, None
return track_cameras, track_measurements
def extract_measurements(
self,
track: SfmTrack2d) -> Tuple[CameraSetCal3Bundler, Point2Vector]:
"""Extract measurements in a track for triangulation.
Args:
track: feature track from which measurements are to be extracted.
Returns:
Vector of individual camera calibrations pertaining to track
Vector of 2d points pertaining to track measurements
"""
track_cameras = CameraSetCal3Bundler()
track_measurements = Point2Vector() # vector of 2d points
for i, uv in track.measurements:
# check for unestimated cameras
if self.track_camera_dict.get(i) != None:
track_cameras.append(self.track_camera_dict.get(i))
track_measurements.append(uv)
else:
logger.warning(
"Unestimated cameras found at index {}. Skipping them.".
format(i))
if len(track_cameras) < 2 or len(track_measurements) < 2:
raise Exception("Nb of measurements should not be <= 2. \
number of cameras is: {} \
and number of observations is {}".format(
len(track_cameras), len(track_measurements)))
return track_cameras, track_measurements
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 generate_measurements(self, calibration, camera_model, cal_params, camera_set=None):
"""
Generate vector of measurements for given calibration and camera model.
Args:
calibration: Camera calibration e.g. Cal3_S2
camera_model: Camera model e.g. PinholeCameraCal3_S2
cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
camera_set: Cameraset object (for individual calibrations)
Returns:
list of measurements and list/CameraSet object for cameras
"""
if camera_set is not None:
cameras = camera_set()
else:
cameras = []
measurements = Point2Vector()
for k, pose in zip(cal_params, self.poses):
K = calibration(*k)
camera = camera_model(pose, K)
cameras.append(camera)
z = camera.project(self.landmark)
measurements.append(z)
return measurements, cameras
def generate_measurements(
self,
calibration: Union[Cal3Bundler, Cal3_S2],
camera_model: Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2],
cal_params: Iterable[Iterable[Union[int, float]]],
camera_set: Optional[Union[CameraSetCal3Bundler,
CameraSetCal3_S2]] = None,
) -> Tuple[Point2Vector, Union[CameraSetCal3Bundler, CameraSetCal3_S2,
List[Cal3Bundler], List[Cal3_S2]]]:
"""
Generate vector of measurements for given calibration and camera model.
Args:
calibration: Camera calibration e.g. Cal3_S2
camera_model: Camera model e.g. PinholeCameraCal3_S2
cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
camera_set: Cameraset object (for individual calibrations)
Returns:
list of measurements and list/CameraSet object for cameras
"""
if camera_set is not None:
cameras = camera_set()
else:
cameras = []
measurements = Point2Vector()
for k, pose in zip(cal_params, self.poses):
K = calibration(*k)
camera = camera_model(pose, K)
cameras.append(camera)
z = camera.project(self.landmark)
measurements.append(z)
return measurements, cameras
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 generate_noisy_2d_measurements(
world_point: Point3,
calibrations: List[Cal3Bundler],
per_image_noise_vecs: np.ndarray,
poses: Pose3Vector,
) -> Tuple[List[Keypoints], List[Tuple[int, int]], Dict[
int, PinholeCameraCal3Bundler], ]:
"""
Generate PinholeCameras from specified poses and calibrations, and then generate
1 measurement per camera of a given 3d point.
Args:
world_point: 3d coords of 3d landmark in world frame
calibrations: List of calibrations for each camera
noise_params: List of amounts of noise to be added to each measurement
poses: List of poses for each camera in world frame
Returns:
keypoints_list: List of keypoints in all images (projected measurements in all images)
img_idxs: Tuple of indices for all images
cameras: Dictionary mapping image index i to calibrated PinholeCamera object
"""
keypoints_list = []
measurements = Point2Vector()
cameras = dict()
for i in range(len(poses)):
camera = PinholeCameraCal3Bundler(poses[i], calibrations[i])
# Project landmark into two cameras and triangulate
z = camera.project(world_point)
cameras[i] = camera
measurement = z + per_image_noise_vecs[i]
measurements.append(measurement)
keypoints_list += [Keypoints(coordinates=measurement.reshape(1, 2))]
# Create image indices for each pose - only subsequent pairwise matches
# assumed, e.g. between images (0,1) and images (1,2)
img_idxs = []
for i in range(len(poses) - 1):
img_idxs += [(i, i + 1)]
return keypoints_list, img_idxs, cameras
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 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))
def test_outliers_and_far_landmarks(self) -> None:
"""Check safe triangulation function."""
pose1, pose2 = self.poses
K1 = Cal3_S2(1500, 1200, 0, 640, 480)
# create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
camera1 = PinholeCameraCal3_S2(pose1, K1)
# create second camera 1 meter to the right of first camera
K2 = Cal3_S2(1600, 1300, 0, 650, 440)
camera2 = PinholeCameraCal3_S2(pose2, K2)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(self.landmark)
z2 = camera2.project(self.landmark)
cameras = CameraSetCal3_S2()
measurements = Point2Vector()
cameras.append(camera1)
cameras.append(camera2)
measurements.append(z1)
measurements.append(z2)
landmarkDistanceThreshold = 10 # landmark is closer than that
# all default except landmarkDistanceThreshold:
params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
actual: TriangulationResult = gtsam.triangulateSafe(
cameras, measurements, params)
self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2)
self.assertTrue(actual.valid())
landmarkDistanceThreshold = 4 # landmark is farther than that
params2 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params2)
self.assertTrue(actual.farPoint())
# 3. Add a slightly rotated third camera above with a wrong measurement
# (OUTLIER)
pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1))
K3 = Cal3_S2(700, 500, 0, 640, 480)
camera3 = PinholeCameraCal3_S2(pose3, K3)
z3 = camera3.project(self.landmark)
cameras.append(camera3)
measurements.append(z3 + Point2(10, -10))
landmarkDistanceThreshold = 10 # landmark is closer than that
outlierThreshold = 100 # loose, the outlier is going to pass
params3 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold,
outlierThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params3)
self.assertTrue(actual.valid())
# now set stricter threshold for outlier rejection
outlierThreshold = 5 # tighter, the outlier is not going to pass
params4 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold,
outlierThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params4)
self.assertTrue(actual.outlier())