Skip to content

geometry

geometry #

The projective geometry the reconstruction stages share, in OpenCV's frame.

  1. Cameras. A Cameras bundle holds a set of poses as OpenCV world-to-camera extrinsics (x right, y down, z forward) and 3×3 calibrations, stacked so a whole model projects in one call. Cameras.of converts from the OpenGL camera-to-world Pose; pose_from_extrinsics converts back. Keypoint pixels are OpenCV's, integer at the pixel centre, so a Pose's principal point (continuous, as cameras.rays reads it) shifts by half a pixel in the calibration.
  2. Triangulation. triangulated is the N-view DLT on calibrated rays; rejection names why a point fails the geometric checks — too small a triangulation angle, behind a camera, or a reprojection too far off.
  3. Two-view and one-view solves. relative_pose initialises a pair from the essential matrix; absolute_pose registers a view against known points with PnP RANSAC.

FLIP = np.diag([1.0, -1.0, -1.0]) module-attribute #

OpenGL camera axes to OpenCV's: y up becomes down, -z forward becomes z.

Cameras dataclass #

A set of cameras as stacked OpenCV extrinsics and calibrations.

Attributes:

Name Type Description
rotations ndarray

C×3×3 world-to-camera rotations.

translations ndarray

C×3 world-to-camera translations.

calibrations ndarray

C×3×3 pinhole matrices in keypoint pixels.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
@dataclasses.dataclass(frozen=True)
class Cameras:
    """A set of cameras as stacked OpenCV extrinsics and calibrations.

    Attributes:
        rotations: C×3×3 world-to-camera rotations.
        translations: C×3 world-to-camera translations.
        calibrations: C×3×3 pinhole matrices in keypoint pixels.
    """

    rotations: np.ndarray
    translations: np.ndarray
    calibrations: np.ndarray

    @classmethod
    def of(cls, poses: Sequence[cg.Pose]) -> Self:
        """The cameras of ``poses``.

        Raises:
            ValueError: If a pose carries no intrinsics.
        """
        rotations = np.zeros((len(poses), 3, 3))
        translations = np.zeros((len(poses), 3))
        calibrations = np.zeros((len(poses), 3, 3))
        for index, pose in enumerate(poses):
            rotations[index], translations[index] = extrinsics(pose)
            calibrations[index] = calibration(pose)
        return cls(rotations, translations, calibrations)

    def __len__(self) -> int:
        """The number of cameras."""
        return int(self.rotations.shape[0])

    def centres(self) -> np.ndarray:
        """The C×3 camera centres in the world."""
        return -np.einsum("cji,cj->ci", self.rotations, self.translations)

    def in_camera(self, camera: np.ndarray, points: np.ndarray) -> np.ndarray:
        """World ``points`` (M×3) in the frame of the camera at each row of ``camera`` (M)."""
        return np.einsum("mij,mj->mi", self.rotations[camera], points) + self.translations[camera]

    def project(self, camera: np.ndarray, points: np.ndarray) -> np.ndarray:
        """The M×2 pixels of world ``points`` (M×3) seen by the camera at each row of ``camera``."""
        local = self.in_camera(camera, points)
        k = self.calibrations[camera]
        u = k[:, 0, 0] * local[:, 0] / local[:, 2] + k[:, 0, 2]
        v = k[:, 1, 1] * local[:, 1] / local[:, 2] + k[:, 1, 2]
        return np.stack([u, v], axis=1)

    def subset(self, indices: np.ndarray) -> Self:
        """The cameras at ``indices``, in that order."""
        return type(self)(
            self.rotations[indices],
            self.translations[indices],
            self.calibrations[indices],
        )

__len__() #

The number of cameras.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def __len__(self) -> int:
    """The number of cameras."""
    return int(self.rotations.shape[0])

centres() #

The C×3 camera centres in the world.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def centres(self) -> np.ndarray:
    """The C×3 camera centres in the world."""
    return -np.einsum("cji,cj->ci", self.rotations, self.translations)

in_camera(camera, points) #

World points (M×3) in the frame of the camera at each row of camera (M).

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def in_camera(self, camera: np.ndarray, points: np.ndarray) -> np.ndarray:
    """World ``points`` (M×3) in the frame of the camera at each row of ``camera`` (M)."""
    return np.einsum("mij,mj->mi", self.rotations[camera], points) + self.translations[camera]

of(poses) classmethod #

The cameras of poses.

Raises:

Type Description
ValueError

If a pose carries no intrinsics.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
@classmethod
def of(cls, poses: Sequence[cg.Pose]) -> Self:
    """The cameras of ``poses``.

    Raises:
        ValueError: If a pose carries no intrinsics.
    """
    rotations = np.zeros((len(poses), 3, 3))
    translations = np.zeros((len(poses), 3))
    calibrations = np.zeros((len(poses), 3, 3))
    for index, pose in enumerate(poses):
        rotations[index], translations[index] = extrinsics(pose)
        calibrations[index] = calibration(pose)
    return cls(rotations, translations, calibrations)

project(camera, points) #

The M×2 pixels of world points (M×3) seen by the camera at each row of camera.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def project(self, camera: np.ndarray, points: np.ndarray) -> np.ndarray:
    """The M×2 pixels of world ``points`` (M×3) seen by the camera at each row of ``camera``."""
    local = self.in_camera(camera, points)
    k = self.calibrations[camera]
    u = k[:, 0, 0] * local[:, 0] / local[:, 2] + k[:, 0, 2]
    v = k[:, 1, 1] * local[:, 1] / local[:, 2] + k[:, 1, 2]
    return np.stack([u, v], axis=1)

subset(indices) #

The cameras at indices, in that order.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def subset(self, indices: np.ndarray) -> Self:
    """The cameras at ``indices``, in that order."""
    return type(self)(
        self.rotations[indices],
        self.translations[indices],
        self.calibrations[indices],
    )

absolute_pose(points, pixels, calibration) #

The world-to-camera pose seeing world points (N×3) at pixels (N×2), by PnP RANSAC.

Returns:

Type Description
ndarray

(rotation, translation, inliers); inliers is empty when no

ndarray

pose explains enough of the correspondences.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def absolute_pose(
    points: np.ndarray,
    pixels: np.ndarray,
    calibration: np.ndarray,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """The world-to-camera pose seeing world ``points`` (N×3) at ``pixels`` (N×2), by PnP RANSAC.

    Returns:
        ``(rotation, translation, inliers)``; ``inliers`` is empty when no
        pose explains enough of the correspondences.
    """
    if len(points) < 6:
        return np.eye(3), np.zeros(3), np.zeros(0, dtype=np.int64)
    found, vector, translation, inliers = cv2.solvePnPRansac(
        points.reshape(-1, 1, 3).astype(np.float64),
        pixels.reshape(-1, 1, 2).astype(np.float64),
        calibration,
        None,
        iterationsCount=PNP_ITERATIONS,
        reprojectionError=PNP_THRESHOLD_PX,
        confidence=RANSAC_CONFIDENCE,
        flags=cv2.SOLVEPNP_EPNP,
    )
    if not found or inliers is None:
        return np.eye(3), np.zeros(3), np.zeros(0, dtype=np.int64)
    rotation, _ = cv2.Rodrigues(vector)
    return rotation, translation.reshape(3), inliers.reshape(-1)

calibration(pose) #

The 3×3 pinhole matrix of pose in keypoint pixels.

Raises:

Type Description
ValueError

If pose carries no intrinsics.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def calibration(pose: cg.Pose) -> np.ndarray:
    """The 3×3 pinhole matrix of ``pose`` in keypoint pixels.

    Raises:
        ValueError: If ``pose`` carries no intrinsics.
    """
    known = intrinsics(pose)
    if known is None:
        raise ValueError("a pose without intrinsics has no calibration")
    return np.array(
        [
            [known.focal_x, 0.0, known.center_x - 0.5],
            [0.0, known.focal_y, known.center_y - 0.5],
            [0.0, 0.0, 1.0],
        ]
    )

calibration_of(focal, width, height) #

A 3×3 pinhole matrix in keypoint pixels with focal and the centre of the image.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def calibration_of(focal: float, width: int, height: int) -> np.ndarray:
    """A 3×3 pinhole matrix in keypoint pixels with ``focal`` and the centre of the image."""
    return np.array(
        [
            [focal, 0.0, width / 2.0 - 0.5],
            [0.0, focal, height / 2.0 - 0.5],
            [0.0, 0.0, 1.0],
        ]
    )

extrinsics(pose) #

The OpenCV world-to-camera (rotation, translation) of pose.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def extrinsics(pose: cg.Pose) -> tuple[np.ndarray, np.ndarray]:
    """The OpenCV world-to-camera ``(rotation, translation)`` of ``pose``."""
    rotation = (
        FLIP
        @ quaternion_matrix(
            pose.quaternion_w,
            pose.quaternion_x,
            pose.quaternion_y,
            pose.quaternion_z,
        ).T
    )
    return rotation, -rotation @ position(pose)

pose_from_extrinsics(rotation, translation, like, calibration=None) #

A Pose at the OpenCV world-to-camera (rotation, translation).

Parameters:

Name Type Description Default
rotation ndarray

The 3×3 world-to-camera rotation.

required
translation ndarray

The world-to-camera translation.

required
like Pose

The pose whose intrinsics, tracking state and time carry over.

required
calibration ndarray | None

A 3×3 matrix in keypoint pixels replacing like's intrinsics, else they stay.

None
Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def pose_from_extrinsics(
    rotation: np.ndarray,
    translation: np.ndarray,
    like: cg.Pose,
    calibration: np.ndarray | None = None,
) -> cg.Pose:
    """A ``Pose`` at the OpenCV world-to-camera ``(rotation, translation)``.

    Args:
        rotation: The 3×3 world-to-camera rotation.
        translation: The world-to-camera translation.
        like: The pose whose intrinsics, tracking state and time carry over.
        calibration: A 3×3 matrix in keypoint pixels replacing ``like``'s
            intrinsics, else they stay.
    """
    camera_to_world = np.eye(4)
    camera_to_world[:3, :3] = rotation.T @ FLIP
    camera_to_world[:3, 3] = -rotation.T @ translation
    pose = pose_from_matrix(camera_to_world, like)
    if calibration is None:
        return pose
    return dataclasses.replace(
        pose,
        focal_x=float(calibration[0, 0]),
        focal_y=float(calibration[1, 1]),
        center_x=float(calibration[0, 2] + 0.5),
        center_y=float(calibration[1, 2] + 0.5),
    )

rejection(cameras, views, pixels, point, min_angle_degrees, max_error_px) #

Why point fails as the triangulation of pixels in views, or None.

Returns:

Type Description
str | None

"angle" when no two views see it at min_angle_degrees apart

str | None

(a point the DLT could not place at all counts as this: parallel

str | None

rays are what leave it unplaced),

str | None

"behind" when a view has it at non-positive depth,

str | None

"reprojection" when the mean error exceeds max_error_px.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def rejection(
    cameras: Cameras,
    views: np.ndarray,
    pixels: np.ndarray,
    point: np.ndarray,
    min_angle_degrees: float,
    max_error_px: float,
) -> str | None:
    """Why ``point`` fails as the triangulation of ``pixels`` in ``views``, or ``None``.

    Returns:
        ``"angle"`` when no two views see it at ``min_angle_degrees`` apart
        (a point the DLT could not place at all counts as this: parallel
        rays are what leave it unplaced),
        ``"behind"`` when a view has it at non-positive depth,
        ``"reprojection"`` when the mean error exceeds ``max_error_px``.
    """
    if not np.all(np.isfinite(point)):
        return "angle"
    if triangulation_angle_degrees(point, cameras.centres()[views]) < min_angle_degrees:
        return "angle"
    if cameras.in_camera(views, np.broadcast_to(point, (len(views), 3)))[:, 2].min() <= 0.0:
        return "behind"
    errors = np.linalg.norm(
        cameras.project(views, np.broadcast_to(point, (len(views), 3))) - pixels, axis=1
    )
    if errors.mean() > max_error_px:
        return "reprojection"
    return None

relative_pose(pixels_a, pixels_b, calibration_a, calibration_b) #

The pose of view b relative to a at unit baseline, from the essential matrix.

Returns:

Type Description
ndarray

(rotation, translation, inliers): b's world-to-camera pose in

ndarray

a's frame and the indices of the correspondences in front of both.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def relative_pose(
    pixels_a: np.ndarray,
    pixels_b: np.ndarray,
    calibration_a: np.ndarray,
    calibration_b: np.ndarray,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """The pose of view ``b`` relative to ``a`` at unit baseline, from the essential matrix.

    Returns:
        ``(rotation, translation, inliers)``: ``b``'s world-to-camera pose in
        ``a``'s frame and the indices of the correspondences in front of both.
    """
    normalized_a = cv2.undistortPoints(
        pixels_a.reshape(-1, 1, 2).astype(np.float64), calibration_a, None
    )
    normalized_b = cv2.undistortPoints(
        pixels_b.reshape(-1, 1, 2).astype(np.float64), calibration_b, None
    )
    focal = float(np.sqrt(calibration_a[0, 0] * calibration_b[0, 0]))
    essential, mask = cv2.findEssentialMat(
        normalized_a,
        normalized_b,
        np.eye(3),
        method=cv2.USAC_MAGSAC,
        prob=RANSAC_CONFIDENCE,
        threshold=ESSENTIAL_THRESHOLD_PX / focal,
    )
    if essential is None or essential.shape != (3, 3):
        return np.eye(3), np.zeros(3), np.zeros(0, dtype=np.int64)
    _, rotation, translation, mask = cv2.recoverPose(
        essential, normalized_a, normalized_b, np.eye(3), mask=mask
    )
    return rotation, translation.reshape(3), np.flatnonzero(mask.reshape(-1))

rotated(vectors, points) #

points (M×3) each turned by the Rodrigues vector on the same row of vectors.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def rotated(vectors: np.ndarray, points: np.ndarray) -> np.ndarray:
    """``points`` (M×3) each turned by the Rodrigues vector on the same row of ``vectors``."""
    angle = np.linalg.norm(vectors, axis=1, keepdims=True)
    axis = np.divide(vectors, angle, out=np.zeros_like(vectors), where=angle > 0)
    cosine, sine = np.cos(angle), np.sin(angle)
    along = np.sum(points * axis, axis=1, keepdims=True)
    return cosine * points + sine * np.cross(axis, points) + (1.0 - cosine) * along * axis

rotation_matrices(vectors) #

The C×3×3 rotation matrices of C×3 Rodrigues vectors.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def rotation_matrices(vectors: np.ndarray) -> np.ndarray:
    """The C×3×3 rotation matrices of C×3 Rodrigues vectors."""
    return Rotation.from_rotvec(vectors).as_matrix()

rotation_vectors(rotations) #

The C×3 Rodrigues vectors of C×3×3 rotation matrices.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def rotation_vectors(rotations: np.ndarray) -> np.ndarray:
    """The C×3 Rodrigues vectors of C×3×3 rotation matrices."""
    return Rotation.from_matrix(rotations).as_rotvec()

triangulated(cameras, views, pixels) #

The world point the DLT places on the rays of pixels (V×2) seen by views (V).

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def triangulated(cameras: Cameras, views: np.ndarray, pixels: np.ndarray) -> np.ndarray:
    """The world point the DLT places on the rays of ``pixels`` (V×2) seen by ``views`` (V)."""
    k = cameras.calibrations[views]
    x = (pixels[:, 0] - k[:, 0, 2]) / k[:, 0, 0]
    y = (pixels[:, 1] - k[:, 1, 2]) / k[:, 1, 1]
    matrices = np.concatenate(
        [cameras.rotations[views], cameras.translations[views][:, :, None]], axis=2
    )
    rows = np.concatenate(
        [
            x[:, None] * matrices[:, 2] - matrices[:, 0],
            y[:, None] * matrices[:, 2] - matrices[:, 1],
        ]
    )
    _, _, vt = np.linalg.svd(rows)
    homogeneous = vt[-1]
    return homogeneous[:3] / homogeneous[3]

triangulation_angle_degrees(point, centres) #

The widest angle at point between rays from any two of centres.

A point lying on a camera centre subtends no angle at all.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/camera/geometry.py
def triangulation_angle_degrees(point: np.ndarray, centres: np.ndarray) -> float:
    """The widest angle at ``point`` between rays from any two of ``centres``.

    A point lying on a camera centre subtends no angle at all.
    """
    rays = centres - point
    lengths = np.linalg.norm(rays, axis=1, keepdims=True)
    if lengths.min() == 0.0:
        return 0.0
    cosines = np.clip((rays / lengths) @ (rays / lengths).T, -1.0, 1.0)
    return math.degrees(math.acos(float(cosines.min())))