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