def calculate_triangulation_angle_in_degrees(
camera_1: PinholeCameraCal3Bundler, camera_2: PinholeCameraCal3Bundler,
point_3d: np.ndarray) -> float:
"""Calculates the angle formed at the 3D point by the rays backprojected from 2 cameras.
In the setup with X (point_3d) and two cameras C1 and C2, the triangulation angle is the angle between rays C1-X
and C2-X, i.e. the angle subtendted at the 3d point.
X
/ \
/ \
/ \
C1 C2
References:
- https://github.com/colmap/colmap/blob/dev/src/base/triangulation.cc#L122
Args:
camera_1: the first camera.
camera_2: the second camera.
point_3d: the 3d point which is imaged by the two camera centers, and where the angle between the light rays
associated with the measurements are computed.
Returns:
the angle formed at the 3d point, in degrees.
"""
camera_center_1: np.ndarray = camera_1.pose().translation()
camera_center_2: np.ndarray = camera_2.pose().translation()
# compute the two rays
ray_1 = point_3d - camera_center_1
ray_2 = point_3d - camera_center_2
return geometry_utils.compute_relative_unit_translation_angle(
Unit3(ray_1), Unit3(ray_2))
def compute_keypoint_intersections(
keypoints: Keypoints,
gt_camera: PinholeCameraCal3Bundler,
gt_scene_mesh: Trimesh,
verbose: bool = False) -> Tuple[np.ndarray, np.ndarray]:
"""Computes intersections between ground truth surface mesh and rays originating from image keypoints.
Args:
keypoints: N keypoints computed in image.
gt_camera: ground truth camera.
gt_scene_mesh: ground truth triangular surface mesh.
Returns:
keypoint_ind: (M,) array of keypoint indices whose corresponding ray intersected the ground truth mesh.
intersections_locations: (M, 3), array of ray intersection locations.
"""
num_kpts = len(keypoints)
src = np.repeat(gt_camera.pose().translation().reshape((-1, 3)),
num_kpts,
axis=0) # At_i1A
drc = np.asarray([
gt_camera.backproject(keypoints.coordinates[i], depth=1.0) - src[i, :]
for i in range(num_kpts)
])
start_time = timeit.default_timer()
intersections, keypoint_ind, _ = gt_scene_mesh.ray.intersects_location(
src, drc, multiple_hits=False)
if verbose:
logger.debug("Case %d rays in %.6f seconds.", num_kpts,
timeit.default_timer() - start_time)
return keypoint_ind, intersections
def calculate_triangulation_angles_in_degrees(
camera_1: PinholeCameraCal3Bundler, camera_2: PinholeCameraCal3Bundler,
points_3d: np.ndarray) -> np.ndarray:
"""Vectorized. calculuation of the angles formed at 3D points by the rays backprojected from 2 cameras.
In the setup with X (point_3d) and two cameras C1 and C2, the triangulation angle is the angle between rays C1-X
and C2-X, i.e. the angle subtendted at the 3d point.
X
/ \
/ \
/ \
C1 C2
References:
- https://github.com/colmap/colmap/blob/dev/src/base/triangulation.cc#L122
Args:
camera_1: the first camera.
camera_2: the second camera.
points_3d: (N,3) 3d points which are imaged by the two camera centers, and where the angle between the
light rays associated with the measurements are computed.
Returns:
the angles formed at the 3d points, in degrees.
https://github.com/colmap/colmap/blob/dev/src/base/triangulation.cc#L147
"""
camera_center_1: np.ndarray = camera_1.pose().translation()
camera_center_2: np.ndarray = camera_2.pose().translation()
N = points_3d.shape[0]
# ensure broadcasting is in the correct direction
rays1 = points_3d - camera_center_1.reshape(1, 3)
rays2 = points_3d - camera_center_2.reshape(1, 3)
# normalize rays to unit length
rays1 /= np.linalg.norm(rays1, axis=1).reshape(N, 1)
rays2 /= np.linalg.norm(rays2, axis=1).reshape(N, 1)
dot_products = np.multiply(rays1, rays2).sum(axis=1)
dot_products = np.clip(dot_products, -1, 1)
angles_rad = np.arccos(dot_products)
angles_deg = np.rad2deg(angles_rad)
return angles_deg
