ds_msp.mvg

Multi-view geometry on bearing vectors — essential matrix estimation, RANSAC relative pose, cheirality/decomposition, and triangulation, all ray-based so they work identically for any central camera. See Two-view geometry on rays.

ds_msp.mvg

Multi-view geometry on bearing vectors.

A fisheye measures rays, not pixels on a plane, so two-view geometry for a wide-FOV camera is done on unit bearing vectors — cam.unproject(pixels) — and never touches a pinhole. The calibrated epipolar constraint f2ᵀ E f1 = 0 holds for any central model, so the eight-point algorithm, pose recovery, and triangulation below work the same for Double Sphere, UCM, EUCM, Kannala-Brandt, … — that is the whole point.

This layer is pure NumPy and model-agnostic: feed it rays, get relative pose and 3D points.

angular_reprojection_error

angular_reprojection_error(f1: ndarray, f2: ndarray, R: ndarray, t: ndarray, X: ndarray) -> np.ndarray

Per-point reprojection error in degrees: max of the two view angles.

View 1 predicts direction X; view 2 predicts R @ X + t. Each predicted direction is compared, as an angle, to the corresponding observed ray — the model-free, FOV-independent error metric :func:refine_two_view minimizes (see the tangent-plane construction in :func:refine_two_view's docstring).

Parameters:

Name	Type	Description	Default
f1	(N, 3) ndarray	Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.	required
f2	(N, 3) ndarray	Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.	required
R	(3, 3), (3,) ndarray	Relative pose, camera 1 to camera 2: X2 = R @ X1 + t.	required
t	(3, 3), (3,) ndarray	Relative pose, camera 1 to camera 2: X2 = R @ X1 + t.	required
X	(N, 3) ndarray	Candidate 3D points in the camera-1 frame (e.g. from :func:~ds_msp.mvg.triangulate_rays).	required

Returns:

Type	Description
(N,) ndarray	Per-point error in degrees: max(angle(f1, X), angle(f2, R @ X + t)). Zero for a perfect fit.

Source code in ds_msp/mvg/bundle.py

def angular_reprojection_error(f1: np.ndarray, f2: np.ndarray,
                               R: np.ndarray, t: np.ndarray, X: np.ndarray) -> np.ndarray:
    """Per-point reprojection error in **degrees**: max of the two view angles.

    View 1 predicts direction ``X``; view 2 predicts ``R @ X + t``. Each predicted direction is
    compared, as an angle, to the corresponding observed ray — the model-free, FOV-independent
    error metric :func:`refine_two_view` minimizes (see the tangent-plane construction in
    :func:`refine_two_view`'s docstring).

    Parameters
    ----------
    f1, f2 : (N, 3) ndarray
        Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and
        camera 2.
    R, t : (3, 3), (3,) ndarray
        Relative pose, camera 1 to camera 2: ``X2 = R @ X1 + t``.
    X : (N, 3) ndarray
        Candidate 3D points in the camera-1 frame (e.g. from :func:`~ds_msp.mvg.triangulate_rays`).

    Returns
    -------
    (N,) ndarray
        Per-point error in degrees: ``max(angle(f1, X), angle(f2, R @ X + t))``. Zero for a
        perfect fit.
    """
    f1, f2 = _as_rays(f1), _as_rays(f2)
    R = np.asarray(R, float)
    t = np.asarray(t, float).reshape(3)
    d1 = X / np.linalg.norm(X, axis=1, keepdims=True)
    d2 = X @ R.T + t
    d2 /= np.linalg.norm(d2, axis=1, keepdims=True)
    a1 = np.degrees(np.arccos(np.clip(np.einsum("ij,ij->i", d1, f1), -1, 1)))
    a2 = np.degrees(np.arccos(np.clip(np.einsum("ij,ij->i", d2, f2), -1, 1)))
    return np.maximum(a1, a2)

decompose_essential

decompose_essential(E: ndarray) -> List[Tuple[np.ndarray, np.ndarray]]

The four (R, t) candidates consistent with an essential matrix.

Every essential matrix factors as E = [t]_× R for exactly two rotations and two signs of t (Hartley & Zisserman, §9.6.2, Result 9.19) — ambiguous without an extra cheirality test, which :func:recover_pose applies to pick the one physical solution.

Parameters:

Name	Type	Description	Default
E	(3, 3) ndarray	Essential matrix, e.g. from :func:essential_from_rays. Need not already be exactly rank 2 / equal-singular-value — only its left/right singular bases are used.	required

Returns:

Type	Description
list of (ndarray, ndarray)	The four (R, t) candidates [(R1, t), (R1, -t), (R2, t), (R2, -t)]. Each R is a (3, 3) rotation matrix (det = +1); t is a (3,) unit vector (translation is recovered only up to scale).

Examples:

>>> import numpy as np
>>> from ds_msp.mvg import essential_from_rays, decompose_essential
>>> rng = np.random.default_rng(0)
>>> X1 = rng.uniform(-2, 2, (10, 3)) + np.array([0, 0, 5])
>>> X2 = X1 + np.array([1.0, 0.0, 0.0])
>>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
>>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
>>> E = essential_from_rays(f1, f2)
>>> candidates = decompose_essential(E)
>>> len(candidates)
4

Source code in ds_msp/mvg/two_view.py

def decompose_essential(E: np.ndarray) -> List[Tuple[np.ndarray, np.ndarray]]:
    """The four ``(R, t)`` candidates consistent with an essential matrix.

    Every essential matrix factors as ``E = [t]_× R`` for exactly two rotations and two signs
    of ``t`` (Hartley & Zisserman, §9.6.2, Result 9.19) — ambiguous without an extra
    cheirality test, which :func:`recover_pose` applies to pick the one physical solution.

    Parameters
    ----------
    E : (3, 3) ndarray
        Essential matrix, e.g. from :func:`essential_from_rays`. Need not already be exactly
        rank 2 / equal-singular-value — only its left/right singular bases are used.

    Returns
    -------
    list of (ndarray, ndarray)
        The four ``(R, t)`` candidates ``[(R1, t), (R1, -t), (R2, t), (R2, -t)]``. Each ``R``
        is a ``(3, 3)`` rotation matrix (``det = +1``); ``t`` is a ``(3,)`` unit vector
        (translation is recovered only up to scale).

    Examples
    --------
    >>> import numpy as np
    >>> from ds_msp.mvg import essential_from_rays, decompose_essential
    >>> rng = np.random.default_rng(0)
    >>> X1 = rng.uniform(-2, 2, (10, 3)) + np.array([0, 0, 5])
    >>> X2 = X1 + np.array([1.0, 0.0, 0.0])
    >>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
    >>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
    >>> E = essential_from_rays(f1, f2)
    >>> candidates = decompose_essential(E)
    >>> len(candidates)
    4
    """
    U, _, Vt = np.linalg.svd(np.asarray(E, float))
    if np.linalg.det(U) < 0:
        U = -U
    if np.linalg.det(Vt) < 0:
        Vt = -Vt
    R1 = U @ _W @ Vt
    R2 = U @ _W.T @ Vt
    t = U[:, 2]
    return [(R1, t), (R1, -t), (R2, t), (R2, -t)]

epipolar_residual

epipolar_residual(E: ndarray, f1: ndarray, f2: ndarray) -> np.ndarray

Algebraic epipolar residual f2ᵀ E f1 for each ray correspondence.

This is the raw algebraic constraint, not an angle — use :func:~ds_msp.mvg.sampson_residual for a threshold that is meaningful in radians (e.g. for RANSAC).

Parameters:

Name	Type	Description	Default
E	(3, 3) ndarray	Essential matrix, e.g. from :func:essential_from_rays.	required
f1	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, one correspondence per row.	required
f2	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, one correspondence per row.	required

Returns:

Type	Description
(N,) ndarray	f2[i] @ E @ f1[i] for each row i. Zero to float64 round-off for an exact E and noise-free correspondences.

Examples:

>>> import numpy as np
>>> from ds_msp.mvg import essential_from_rays, epipolar_residual
>>> rng = np.random.default_rng(0)
>>> X1 = rng.uniform(-2, 2, (10, 3)) + np.array([0, 0, 5])  # points in front of camera 1
>>> X2 = X1 + np.array([1.0, 0.0, 0.0])                    # camera 2 shifted along +x
>>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
>>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
>>> E = essential_from_rays(f1, f2)
>>> float(np.abs(epipolar_residual(E, f1, f2)).max()) < 1e-10
True

Source code in ds_msp/mvg/two_view.py

def epipolar_residual(E: np.ndarray, f1: np.ndarray, f2: np.ndarray) -> np.ndarray:
    """Algebraic epipolar residual ``f2ᵀ E f1`` for each ray correspondence.

    This is the raw algebraic constraint, not an angle — use :func:`~ds_msp.mvg.sampson_residual`
    for a threshold that is meaningful in radians (e.g. for RANSAC).

    Parameters
    ----------
    E : (3, 3) ndarray
        Essential matrix, e.g. from :func:`essential_from_rays`.
    f1, f2 : (N, 3) ndarray
        Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2,
        one correspondence per row.

    Returns
    -------
    (N,) ndarray
        ``f2[i] @ E @ f1[i]`` for each row ``i``. Zero to float64 round-off for an exact ``E``
        and noise-free correspondences.

    Examples
    --------
    >>> import numpy as np
    >>> from ds_msp.mvg import essential_from_rays, epipolar_residual
    >>> rng = np.random.default_rng(0)
    >>> X1 = rng.uniform(-2, 2, (10, 3)) + np.array([0, 0, 5])  # points in front of camera 1
    >>> X2 = X1 + np.array([1.0, 0.0, 0.0])                    # camera 2 shifted along +x
    >>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
    >>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
    >>> E = essential_from_rays(f1, f2)
    >>> float(np.abs(epipolar_residual(E, f1, f2)).max()) < 1e-10
    True
    """
    f1 = _as_rays(f1)
    f2 = _as_rays(f2)
    return np.einsum("ij,jk,ik->i", f2, np.asarray(E, float), f1)

essential_from_rays

essential_from_rays(f1: ndarray, f2: ndarray, *, normalize: bool = False) -> np.ndarray

Essential matrix from >=8 ray correspondences (eight-point on bearing vectors).

Solves f2ᵀ E f1 = 0 in the least-squares (smallest-singular-vector) sense — the eight-point algorithm (Hartley & Zisserman, §11.3) applied to unit bearing vectors instead of pixels — then projects the raw least-squares solution onto the essential manifold (singular values forced to (1, 1, 0)).

Parameters:

Name	Type	Description	Default
f1	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, one correspondence per row, N >= 8.	required
f2	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, one correspondence per row, N >= 8.	required
normalize	bool	Apply spherical whitening (see the module-private _whiten) before the solve and undo it after. Leave off for clean data — it changes nothing in the noise-free limit (verified in Chapter 8, residual 5.69e-16 either way). Turn on for noisy / narrow-baseline rays where conditioning matters; pixel-domain Hartley normalization does not apply to bearing vectors, so this is its spherical analogue.	False

Returns:

Type	Description
(3, 3) ndarray	Essential matrix E with singular values (1, 1, 0), satisfying f2ᵀ E f1 ≈ 0 for the input correspondences.

Raises:

Type	Description
ValueError	If fewer than 8 correspondences are given, or if f1/f2 are not shape (N, 3) or contain a zero-length vector (checked in the shared _as_rays validator).

Examples:

>>> import numpy as np
>>> from ds_msp.mvg import essential_from_rays, epipolar_residual
>>> rng = np.random.default_rng(0)
>>> X1 = rng.uniform(-2, 2, (10, 3)) + np.array([0, 0, 5])
>>> X2 = X1 + np.array([1.0, 0.0, 0.0])
>>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
>>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
>>> E = essential_from_rays(f1, f2)
>>> E.shape
(3, 3)
>>> np.round(np.linalg.svd(E, compute_uv=False), 6)
array([1., 1., 0.])

Source code in ds_msp/mvg/two_view.py

def essential_from_rays(f1: np.ndarray, f2: np.ndarray, *, normalize: bool = False
                        ) -> np.ndarray:
    """Essential matrix from >=8 ray correspondences (eight-point on bearing vectors).

    Solves ``f2ᵀ E f1 = 0`` in the least-squares (smallest-singular-vector) sense — the
    eight-point algorithm (Hartley & Zisserman, §11.3) applied to unit bearing vectors instead
    of pixels — then projects the raw least-squares solution onto the essential manifold
    (singular values forced to ``(1, 1, 0)``).

    Parameters
    ----------
    f1, f2 : (N, 3) ndarray
        Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2,
        one correspondence per row, ``N >= 8``.
    normalize : bool, default False
        Apply spherical whitening (see the module-private ``_whiten``) before the solve and
        undo it after. Leave off for clean data — it changes nothing in the noise-free limit
        (verified in [Chapter 8](../learn/08_two_view_geometry_on_rays.md), residual
        ``5.69e-16`` either way). Turn on for noisy / narrow-baseline rays where conditioning
        matters; pixel-domain Hartley normalization does not apply to bearing vectors, so this
        is its spherical analogue.

    Returns
    -------
    (3, 3) ndarray
        Essential matrix ``E`` with singular values ``(1, 1, 0)``, satisfying
        ``f2ᵀ E f1 ≈ 0`` for the input correspondences.

    Raises
    ------
    ValueError
        If fewer than 8 correspondences are given, or if ``f1``/``f2`` are not shape ``(N, 3)``
        or contain a zero-length vector (checked in the shared ``_as_rays`` validator).

    Examples
    --------
    >>> import numpy as np
    >>> from ds_msp.mvg import essential_from_rays, epipolar_residual
    >>> rng = np.random.default_rng(0)
    >>> X1 = rng.uniform(-2, 2, (10, 3)) + np.array([0, 0, 5])
    >>> X2 = X1 + np.array([1.0, 0.0, 0.0])
    >>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
    >>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
    >>> E = essential_from_rays(f1, f2)
    >>> E.shape
    (3, 3)
    >>> np.round(np.linalg.svd(E, compute_uv=False), 6)
    array([1., 1., 0.])
    """
    f1 = _as_rays(f1)
    f2 = _as_rays(f2)
    if f1.shape[0] < 8:
        raise ValueError(f"need ≥8 correspondences, got {f1.shape[0]}")
    g1, g2 = f1, f2
    T1 = T2 = None
    if normalize:
        g1, T1 = _whiten(f1)
        g2, T2 = _whiten(f2)
    # Each row: coefficients of vec(E) (row-major) in g2ᵀ E g1 = 0  →  kron(g2, g1).
    A = g2[:, [0, 0, 0, 1, 1, 1, 2, 2, 2]] * np.tile(g1, 3)
    _, _, Vt = np.linalg.svd(A)
    E = Vt[-1].reshape(3, 3)
    if normalize:
        E = T2.T @ E @ T1                       # map back to un-whitened coordinates
    # Project onto the essential manifold: equal non-zero singular values, rank 2.
    U, _, Vt2 = np.linalg.svd(E)
    return U @ np.diag([1.0, 1.0, 0.0]) @ Vt2

estimate_relative_pose

estimate_relative_pose(f1: ndarray, f2: ndarray, *, threshold: float = 0.005, max_iters: int = 1000, seed: int = 0, refine: bool = True, robust_kernel: str = 'none', robust_scale: float | str = 'auto')

End-to-end robust two-view relative pose from ray correspondences.

Ties the RANSAC consensus and angular-BA pieces into one call: RANSAC consensus over the eight-point gives an outlier-free inlier set and a well-conditioned initial (R₀, t₀) (a far better seed than a single least-squares eight-point on contaminated data, especially at large rotation where one bad ray skews the essential matrix); the inliers are triangulated and handed to the manifold-correct :func:refine_two_view for a final geometric (angular) bundle adjustment. This is the estimator :mod:ds_msp.vo composes into a trajectory (:func:ds_msp.vo.estimate_trajectory), one consecutive frame pair at a time.

Parameters:

Name	Type	Description	Default
f1	(N, 3) ndarray	Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, N >= 8.	required
f2	(N, 3) ndarray	Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, N >= 8.	required
threshold	float	RANSAC inlier cutoff on the Sampson angle (radians); forwarded to :func:~ds_msp.mvg.ransac_relative_pose.	0.005
max_iters	int	RANSAC iteration budget; forwarded to :func:~ds_msp.mvg.ransac_relative_pose.	1000
seed	int	RNG seed for RANSAC sampling.	0
refine	bool	Run :func:refine_two_view on the RANSAC inliers after triangulation. If False, returns the closed-form RANSAC pose and triangulation unrefined.	True
robust_kernel	str	IRLS kernel passed to :func:refine_two_view (only used when refine=True); lets the refinement additionally down-weight soft mismatches that slipped under the RANSAC threshold.	"none"
robust_scale	float or str	Inlier scale for robust_kernel, or "auto" to re-estimate it each iteration; only used when refine=True.	"auto"

Returns:

Name	Type	Description
R	(3, 3) ndarray	Rotation, camera 1 to camera 2.
t	(3,) ndarray	Unit-length translation direction.
X	(N_inliers, 3) ndarray	Triangulated 3D points (camera-1 frame), inliers only.
inliers	(N,) bool ndarray	Boolean inlier mask over the input correspondences.

Source code in ds_msp/mvg/bundle.py

def estimate_relative_pose(f1: np.ndarray, f2: np.ndarray, *,
                           threshold: float = 0.005, max_iters: int = 1000, seed: int = 0,
                           refine: bool = True, robust_kernel: str = "none",
                           robust_scale: float | str = "auto"):
    """End-to-end **robust** two-view relative pose from ray correspondences.

    Ties the RANSAC consensus and angular-BA pieces into one call: RANSAC consensus over the eight-point gives an
    outlier-free inlier set and a well-conditioned initial ``(R₀, t₀)`` (a far better seed than a
    single least-squares eight-point on contaminated data, especially at large rotation where one
    bad ray skews the essential matrix); the inliers are triangulated and handed to the
    manifold-correct :func:`refine_two_view` for a final geometric (angular) bundle adjustment.
    This is the estimator :mod:`ds_msp.vo` composes into a trajectory
    (:func:`ds_msp.vo.estimate_trajectory`), one consecutive frame pair at a time.

    Parameters
    ----------
    f1, f2 : (N, 3) ndarray
        Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and
        camera 2, ``N >= 8``.
    threshold : float, default 0.005
        RANSAC inlier cutoff on the Sampson angle (radians); forwarded to
        :func:`~ds_msp.mvg.ransac_relative_pose`.
    max_iters : int, default 1000
        RANSAC iteration budget; forwarded to :func:`~ds_msp.mvg.ransac_relative_pose`.
    seed : int, default 0
        RNG seed for RANSAC sampling.
    refine : bool, default True
        Run :func:`refine_two_view` on the RANSAC inliers after triangulation. If ``False``,
        returns the closed-form RANSAC pose and triangulation unrefined.
    robust_kernel : str, default "none"
        IRLS kernel passed to :func:`refine_two_view` (only used when ``refine=True``); lets the
        refinement *additionally* down-weight soft mismatches that slipped under the RANSAC
        threshold.
    robust_scale : float or str, default "auto"
        Inlier scale for ``robust_kernel``, or ``"auto"`` to re-estimate it each iteration; only
        used when ``refine=True``.

    Returns
    -------
    R : (3, 3) ndarray
        Rotation, camera 1 to camera 2.
    t : (3,) ndarray
        Unit-length translation direction.
    X : (N_inliers, 3) ndarray
        Triangulated 3D points (camera-1 frame), inliers only.
    inliers : (N,) bool ndarray
        Boolean inlier mask over the input correspondences.
    """
    f1, f2 = _as_rays(f1), _as_rays(f2)
    R0, t0, inliers = ransac_relative_pose(f1, f2, threshold=threshold,
                                           max_iters=max_iters, seed=seed)
    fin1, fin2 = f1[inliers], f2[inliers]
    X0, _, _ = triangulate_rays(fin1, fin2, R0, t0)
    if refine:
        R, t, X = refine_two_view(fin1, fin2, R0, t0, X0,
                                  robust_kernel=robust_kernel, robust_scale=robust_scale)
    else:
        R, t, X = R0, t0, X0
    return R, t, X, inliers

ransac_relative_pose

ransac_relative_pose(f1: ndarray, f2: ndarray, *, threshold: float = 0.005, max_iters: int = 1000, confidence: float = 0.999, seed: int = 0, refine: bool = True) -> Tuple[np.ndarray, np.ndarray, np.ndarray]

Robust (R, t) from ray correspondences via RANSAC over the eight-point.

Repeatedly samples 8 correspondences, fits :func:~ds_msp.mvg.essential_from_rays, and scores every correspondence with :func:sampson_residual; keeps the largest inlier consensus and (optionally) re-fits on it. The iteration budget is adaptive: once the best inlier fraction seen implies an all-inlier 8-sample has probably already been drawn (at the given confidence), remaining iterations are skipped.

Parameters:

Name	Type	Description	Default
f1	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, N >= 8.	required
f2	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, N >= 8.	required
threshold	float	Inlier cutoff on the Sampson angle (radians); 0.005 rad ~ 0.3 deg.	0.005
max_iters	int	Maximum RANSAC iterations (upper bound; adaptive stopping usually exits sooner).	1000
confidence	float	Target probability of having drawn at least one all-inlier 8-sample; controls the adaptive early stop.	0.999
seed	int	Seed for the internal numpy.random.default_rng (deterministic sampling).	0
refine	bool	Re-fit the essential matrix on all inliers, with spherical whitening (essential_from_rays(..., normalize=True)), before the final pose recovery.	True

Returns:

Name	Type	Description
R	(3, 3) ndarray	Rotation mapping camera 1 to camera 2 (X2 = R @ X1 + t), det(R) = +1.
t	(3,) ndarray	Unit-length translation direction.
inliers	(N,) bool ndarray	Consensus inlier mask over the input correspondences.

Raises:

Type	Description
ValueError	If fewer than 8 correspondences are given.
RuntimeError	If no 8-point sample ever reaches an 8-correspondence consensus (degenerate or all-outlier data).

Examples:

Recovering rotation to ~0.11 deg from 30%-corrupted ray matches (full walkthrough with the naive-vs-RANSAC comparison table: Chapter 8, §5):

>>> import numpy as np
>>> from ds_msp.mvg import ransac_relative_pose
>>> def rot_err_deg(A, B):
...     return np.degrees(np.arccos(np.clip((np.trace(A.T @ B) - 1) / 2, -1, 1)))
>>> rng = np.random.default_rng(3)
>>> R_true = np.eye(3)  # no rotation, for a self-contained deterministic example
>>> t_true = np.array([0.1, 0.0, 0.0])
>>> X1 = np.column_stack([rng.uniform(-2, 2, 120), rng.uniform(-2, 2, 120),
...                       rng.uniform(2, 8, 120)])
>>> X2 = (R_true @ X1.T).T + t_true
>>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
>>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
>>> rng2 = np.random.default_rng(4)
>>> outlier = rng2.random(120) < 0.30
>>> f2[outlier] = rng2.standard_normal((int(outlier.sum()), 3))
>>> f2 /= np.linalg.norm(f2, axis=1, keepdims=True)
>>> R, t, inliers = ransac_relative_pose(f1, f2, threshold=0.005, seed=0)
>>> bool(rot_err_deg(R_true, R) < 1.0)
True
>>> int(inliers.sum()) >= 80
True

Source code in ds_msp/mvg/ransac.py

def ransac_relative_pose(
    f1: np.ndarray, f2: np.ndarray, *,
    threshold: float = 0.005, max_iters: int = 1000, confidence: float = 0.999,
    seed: int = 0, refine: bool = True,
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Robust ``(R, t)`` from ray correspondences via RANSAC over the eight-point.

    Repeatedly samples 8 correspondences, fits :func:`~ds_msp.mvg.essential_from_rays`, and
    scores every correspondence with :func:`sampson_residual`; keeps the largest inlier
    consensus and (optionally) re-fits on it. The iteration budget is adaptive: once the best
    inlier fraction seen implies an all-inlier 8-sample has probably already been drawn (at
    the given ``confidence``), remaining iterations are skipped.

    Parameters
    ----------
    f1, f2 : (N, 3) ndarray
        Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2,
        ``N >= 8``.
    threshold : float, default 0.005
        Inlier cutoff on the Sampson **angle** (radians); ``0.005 rad ~ 0.3 deg``.
    max_iters : int, default 1000
        Maximum RANSAC iterations (upper bound; adaptive stopping usually exits sooner).
    confidence : float, default 0.999
        Target probability of having drawn at least one all-inlier 8-sample; controls the
        adaptive early stop.
    seed : int, default 0
        Seed for the internal ``numpy.random.default_rng`` (deterministic sampling).
    refine : bool, default True
        Re-fit the essential matrix on all inliers, with spherical whitening
        (``essential_from_rays(..., normalize=True)``), before the final pose recovery.

    Returns
    -------
    R : (3, 3) ndarray
        Rotation mapping camera 1 to camera 2 (``X2 = R @ X1 + t``), ``det(R) = +1``.
    t : (3,) ndarray
        Unit-length translation direction.
    inliers : (N,) bool ndarray
        Consensus inlier mask over the input correspondences.

    Raises
    ------
    ValueError
        If fewer than 8 correspondences are given.
    RuntimeError
        If no 8-point sample ever reaches an 8-correspondence consensus (degenerate or
        all-outlier data).

    Examples
    --------
    Recovering rotation to ``~0.11 deg`` from 30%-corrupted ray matches (full walkthrough with
    the naive-vs-RANSAC comparison table: [Chapter 8, §5](../learn/08_two_view_geometry_on_rays.md#5-make-it-robust-ransac-against-wrong-matches)):

    >>> import numpy as np
    >>> from ds_msp.mvg import ransac_relative_pose
    >>> def rot_err_deg(A, B):
    ...     return np.degrees(np.arccos(np.clip((np.trace(A.T @ B) - 1) / 2, -1, 1)))
    >>> rng = np.random.default_rng(3)
    >>> R_true = np.eye(3)  # no rotation, for a self-contained deterministic example
    >>> t_true = np.array([0.1, 0.0, 0.0])
    >>> X1 = np.column_stack([rng.uniform(-2, 2, 120), rng.uniform(-2, 2, 120),
    ...                       rng.uniform(2, 8, 120)])
    >>> X2 = (R_true @ X1.T).T + t_true
    >>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
    >>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
    >>> rng2 = np.random.default_rng(4)
    >>> outlier = rng2.random(120) < 0.30
    >>> f2[outlier] = rng2.standard_normal((int(outlier.sum()), 3))
    >>> f2 /= np.linalg.norm(f2, axis=1, keepdims=True)
    >>> R, t, inliers = ransac_relative_pose(f1, f2, threshold=0.005, seed=0)
    >>> bool(rot_err_deg(R_true, R) < 1.0)
    True
    >>> int(inliers.sum()) >= 80
    True
    """
    f1 = _as_rays(f1)
    f2 = _as_rays(f2)
    n = f1.shape[0]
    if n < 8:
        raise ValueError(f"need ≥8 correspondences, got {n}")
    rng = np.random.default_rng(seed)

    best_inliers = np.zeros(n, dtype=bool)
    best_count = 0
    iters = max_iters
    it = 0
    while it < iters:
        it += 1
        idx = rng.choice(n, 8, replace=False)
        try:
            E = essential_from_rays(f1[idx], f2[idx])
        except (np.linalg.LinAlgError, ValueError):
            continue
        inl = sampson_residual(E, f1, f2) < threshold
        c = int(inl.sum())
        if c > best_count:
            best_count, best_inliers = c, inl
            # adaptive stop: enough iterations to have hit an all-inlier sample
            w = max(best_count / n, 1e-6)
            denom = np.log(max(1.0 - w ** 8, 1e-12))
            if denom < 0:
                iters = min(max_iters, int(np.log(1.0 - confidence) / denom) + 1)

    if best_count < 8:
        raise RuntimeError("RANSAC failed to find an 8-point consensus; check threshold/data")

    fin1, fin2 = f1[best_inliers], f2[best_inliers]
    E = essential_from_rays(fin1, fin2, normalize=refine)
    R, t, _ = recover_pose(fin1, fin2, E)
    return R, t, best_inliers

recover_pose

recover_pose(f1: ndarray, f2: ndarray, E: Optional[ndarray] = None) -> Tuple[np.ndarray, np.ndarray, np.ndarray]

Relative pose (R, t) and triangulated points from ray correspondences.

Computes E (if not supplied), enumerates the four :func:decompose_essential candidates, and via cheirality picks the one with the most :func:triangulate_rays points in front of both cameras.

Parameters:

Name	Type	Description	Default
f1	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, N >= 8 when E is not supplied (needed by :func:essential_from_rays).	required
f2	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2, N >= 8 when E is not supplied (needed by :func:essential_from_rays).	required
E	(3, 3) ndarray	A precomputed essential matrix (e.g. re-fit on a RANSAC inlier set by :func:ransac_relative_pose). If None (default), computed from f1, f2 via :func:essential_from_rays with normalize=False.	None

Returns:

Name	Type	Description
R	(3, 3) ndarray	Rotation mapping camera 1 to camera 2 (X2 = R @ X1 + t), det(R) = +1.
t	(3,) ndarray	Unit-length translation direction; its scale is unobservable from two views.
X	(N, 3) ndarray	Triangulated 3D points, in the camera-1 frame.

Examples:

Exact recovery on noise-free rays (see Chapter 8 for the full walkthrough, including a real Double Sphere camera round-trip asserted to < 1e-3 degrees rotation error):

>>> import numpy as np
>>> from ds_msp.mvg import recover_pose
>>> rng = np.random.default_rng(1)
>>> R_true = np.eye(3)
>>> t_true = np.array([1.0, 0.0, 0.0])
>>> X1 = rng.uniform(-2, 2, (40, 3)) + np.array([0, 0, 5])
>>> X2 = (R_true @ X1.T).T + t_true
>>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
>>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
>>> R, t, X = recover_pose(f1, f2)
>>> bool(np.allclose(R, R_true, atol=1e-8))
True

Source code in ds_msp/mvg/two_view.py

def recover_pose(f1: np.ndarray, f2: np.ndarray, E: Optional[np.ndarray] = None
                 ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Relative pose ``(R, t)`` and triangulated points from ray correspondences.

    Computes ``E`` (if not supplied), enumerates the four :func:`decompose_essential`
    candidates, and via cheirality picks the one with the most :func:`triangulate_rays` points
    in front of **both** cameras.

    Parameters
    ----------
    f1, f2 : (N, 3) ndarray
        Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2,
        ``N >= 8`` when ``E`` is not supplied (needed by :func:`essential_from_rays`).
    E : (3, 3) ndarray, optional
        A precomputed essential matrix (e.g. re-fit on a RANSAC inlier set by
        :func:`ransac_relative_pose`). If ``None`` (default), computed from ``f1``, ``f2`` via
        :func:`essential_from_rays` with ``normalize=False``.

    Returns
    -------
    R : (3, 3) ndarray
        Rotation mapping camera 1 to camera 2 (``X2 = R @ X1 + t``), ``det(R) = +1``.
    t : (3,) ndarray
        Unit-length translation direction; its scale is unobservable from two views.
    X : (N, 3) ndarray
        Triangulated 3D points, in the **camera-1** frame.

    Examples
    --------
    Exact recovery on noise-free rays (see
    [Chapter 8](../learn/08_two_view_geometry_on_rays.md) for the full walkthrough, including a
    real Double Sphere camera round-trip asserted to ``< 1e-3`` degrees rotation error):

    >>> import numpy as np
    >>> from ds_msp.mvg import recover_pose
    >>> rng = np.random.default_rng(1)
    >>> R_true = np.eye(3)
    >>> t_true = np.array([1.0, 0.0, 0.0])
    >>> X1 = rng.uniform(-2, 2, (40, 3)) + np.array([0, 0, 5])
    >>> X2 = (R_true @ X1.T).T + t_true
    >>> f1 = X1 / np.linalg.norm(X1, axis=1, keepdims=True)
    >>> f2 = X2 / np.linalg.norm(X2, axis=1, keepdims=True)
    >>> R, t, X = recover_pose(f1, f2)
    >>> bool(np.allclose(R, R_true, atol=1e-8))
    True
    """
    f1 = _as_rays(f1)
    f2 = _as_rays(f2)
    if E is None:
        E = essential_from_rays(f1, f2)
    best = None
    for R, t in decompose_essential(E):
        _, d1, d2 = triangulate_rays(f1, f2, R, t)
        n_front = int(np.sum((d1 > 0) & (d2 > 0)))
        if best is None or n_front > best[0]:
            best = (n_front, R, t)
    _, R, t = best
    X, _, _ = triangulate_rays(f1, f2, R, t)
    return R, t, X

refine_two_view

refine_two_view(f1: ndarray, f2: ndarray, R0: ndarray, t0: ndarray, X0: ndarray, *, max_nfev: int = 100, robust_kernel: str = 'none', robust_scale: float | str = 1.0) -> Tuple[np.ndarray, np.ndarray, np.ndarray]

Nonlinear refinement of (R, t, X) minimizing the tangent-plane angular residual.

Manifold-correct, in-house solver. Driven by :func:ds_msp.core.optimize.lm_solve, which re-bases the rotation every accepted step (R ← R · exp([δω]_×), δω reset to 0) instead of letting a fixed-base perturbation drift toward the ‖δω‖ = π singularity — so it stays stable at large inter-frame rotation where a flat axis-angle / fixed-base solve wobbles. The Jacobian is analytic (the angular residual differentiated through the tangent basis), replacing the old finite-difference scipy path that stalled at large rotation.

Camera 1 is fixed at the identity (reference frame) and t is kept unit-length, pinning the 7-DOF similarity gauge (the angular error is scale-invariant). The translation gauge makes the δt block rank-deficient by design; the solver's damped-Cholesky floor absorbs it.

Parameters:

Name	Type	Description	Default
f1	(N, 3) ndarray	Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.	required
f2	(N, 3) ndarray	Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.	required
R0	(3, 3), (3,) ndarray	Initial relative pose (e.g. from :func:~ds_msp.mvg.recover_pose / :func:~ds_msp.mvg.ransac_relative_pose). t0 is renormalized to unit length before optimizing.	required
t0	(3, 3), (3,) ndarray	Initial relative pose (e.g. from :func:~ds_msp.mvg.recover_pose / :func:~ds_msp.mvg.ransac_relative_pose). t0 is renormalized to unit length before optimizing.	required
X0	(N, 3) ndarray	Initial 3D points in the camera-1 frame (e.g. from :func:~ds_msp.mvg.triangulate_rays).	required
max_nfev	int	Maximum solver iterations, forwarded to :func:ds_msp.core.optimize.lm_solve as max_iter.	100
robust_kernel	str	IRLS down-weighting kernel for mismatched correspondences (e.g. "cauchy"); see :func:ds_msp.core.optimize.lm_solve. "none" reproduces plain L2.	"none"
robust_scale	float or str	Inlier scale for robust_kernel, or "auto" to re-estimate it from the residuals each iteration.	1.0

Returns:

Name	Type	Description
R	(3, 3) ndarray	Refined rotation, camera 1 to camera 2.
t	(3,) ndarray	Refined unit-length translation direction.
X	(N, 3) ndarray	Refined 3D points, in the camera-1 frame.

Source code in ds_msp/mvg/bundle.py

def refine_two_view(f1: np.ndarray, f2: np.ndarray,
                    R0: np.ndarray, t0: np.ndarray, X0: np.ndarray, *,
                    max_nfev: int = 100, robust_kernel: str = "none",
                    robust_scale: float | str = 1.0
                    ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Nonlinear refinement of ``(R, t, X)`` minimizing the tangent-plane angular residual.

    **Manifold-correct, in-house solver.** Driven by :func:`ds_msp.core.optimize.lm_solve`,
    which *re-bases* the rotation every accepted step (``R ← R · exp([δω]_×)``, ``δω`` reset to 0)
    instead of letting a fixed-base perturbation drift toward the ``‖δω‖ = π`` singularity — so it
    stays stable at large inter-frame rotation where a flat axis-angle / fixed-base solve wobbles.
    The Jacobian is **analytic** (the angular residual differentiated through the tangent basis),
    replacing the old finite-difference ``scipy`` path that stalled at large rotation.

    Camera 1 is fixed at the identity (reference frame) and ``t`` is kept unit-length, pinning the
    7-DOF similarity gauge (the angular error is scale-invariant). The translation gauge makes the
    ``δt`` block rank-deficient by design; the solver's damped-Cholesky floor absorbs it.

    Parameters
    ----------
    f1, f2 : (N, 3) ndarray
        Observed unit (or non-unit; renormalized internally) bearing vectors in camera 1 and
        camera 2.
    R0, t0 : (3, 3), (3,) ndarray
        Initial relative pose (e.g. from :func:`~ds_msp.mvg.recover_pose` /
        :func:`~ds_msp.mvg.ransac_relative_pose`). ``t0`` is renormalized to unit length before
        optimizing.
    X0 : (N, 3) ndarray
        Initial 3D points in the camera-1 frame (e.g. from
        :func:`~ds_msp.mvg.triangulate_rays`).
    max_nfev : int, default 100
        Maximum solver iterations, forwarded to :func:`ds_msp.core.optimize.lm_solve` as
        ``max_iter``.
    robust_kernel : str, default "none"
        IRLS down-weighting kernel for mismatched correspondences (e.g. ``"cauchy"``); see
        :func:`ds_msp.core.optimize.lm_solve`. ``"none"`` reproduces plain L2.
    robust_scale : float or str, default 1.0
        Inlier scale for ``robust_kernel``, or ``"auto"`` to re-estimate it from the residuals
        each iteration.

    Returns
    -------
    R : (3, 3) ndarray
        Refined rotation, camera 1 to camera 2.
    t : (3,) ndarray
        Refined unit-length translation direction.
    X : (N, 3) ndarray
        Refined 3D points, in the camera-1 frame.
    """
    f1, f2 = _as_rays(f1), _as_rays(f2)
    n = f1.shape[0]
    eu1, ev1 = _tangent_basis(f1)
    eu2, ev2 = _tangent_basis(f2)
    R0 = np.asarray(R0, float)
    t0 = np.asarray(t0, float).reshape(3)
    t0 = t0 / np.linalg.norm(t0)
    X0 = np.asarray(X0, float)

    def residual(state):
        """Tangent-plane residual ``(4N,)`` for ``lm_solve``: view-1 then view-2 (u, v) pairs."""
        R, t, X = state
        d1 = X / np.linalg.norm(X, axis=1, keepdims=True)
        d2 = X @ R.T + t
        d2 /= np.linalg.norm(d2, axis=1, keepdims=True)
        r1 = np.stack([np.einsum("ij,ij->i", d1, eu1), np.einsum("ij,ij->i", d1, ev1)], 1)
        r2 = np.stack([np.einsum("ij,ij->i", d2, eu2), np.einsum("ij,ij->i", d2, ev2)], 1)
        return np.concatenate([r1.ravel(), r2.ravel()])

    def jacobian(state):
        """Analytic ``d(residual)/d(δω, δt, δX)``, shape ``(4N, 6 + 3N)``, for ``lm_solve``."""
        R, t, X = state
        # View 1 (camera at identity) sees only X; view 2 sees R·exp(δω)·X + normalize(t+δt).
        J = np.zeros((4 * n, 6 + 3 * n))
        y1 = X
        n1 = np.linalg.norm(y1, axis=1, keepdims=True)
        d1 = y1 / n1
        y2 = X @ R.T + t
        n2 = np.linalg.norm(y2, axis=1, keepdims=True)
        d2 = y2 / n2
        Pt = np.eye(3) - np.outer(t, t)                  # ∂ normalize(t+δt)/∂δt at δt=0
        for i in range(n):
            E1 = np.stack([eu1[i], ev1[i]])              # (2,3)
            P1 = (np.eye(3) - np.outer(d1[i], d1[i])) / n1[i, 0]
            J[2 * i:2 * i + 2, 6 + 3 * i:9 + 3 * i] = E1 @ P1          # ∂r1/∂X_i

            E2 = np.stack([eu2[i], ev2[i]])
            P2 = (np.eye(3) - np.outer(d2[i], d2[i])) / n2[i, 0]
            G = E2 @ P2                                  # (2,3): ∂r2/∂y2
            row = 2 * n + 2 * i
            J[row:row + 2, 0:3] = G @ (-R @ hat(X[i]))   # ∂r2/∂δω
            J[row:row + 2, 3:6] = G @ Pt                 # ∂r2/∂δt
            J[row:row + 2, 6 + 3 * i:9 + 3 * i] = G @ R  # ∂r2/∂X_i
        return J

    def retract(state, delta):
        """Manifold update for ``lm_solve``: ``R`` by SO(3) exp, ``t`` re-normalized, ``X`` flat."""
        R, t, X = state
        R = R @ so3_exp(delta[:3])
        t = t + delta[3:6]
        t = t / np.linalg.norm(t)
        X = X + delta[6:].reshape(n, 3)
        return (R, t, X)

    out = lm_solve((R0, t0, X0), residual, jacobian, retract, block=2,
                   max_iter=max_nfev, robust_kernel=robust_kernel,
                   robust_scale=robust_scale)
    return out.state

relative_pose

relative_pose(f1: ndarray, f2: ndarray) -> Tuple[np.ndarray, np.ndarray]

Convenience: (R, t) from ray correspondences (eight-point + cheirality).

Source code in ds_msp/mvg/two_view.py

def relative_pose(f1: np.ndarray, f2: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
    """Convenience: ``(R, t)`` from ray correspondences (eight-point + cheirality)."""
    R, t, _ = recover_pose(f1, f2)
    return R, t

sampson_residual

sampson_residual(E: ndarray, f1: ndarray, f2: ndarray) -> np.ndarray

Symmetric angular epipolar distance per correspondence (radians, small-angle).

First-order (Sampson) approximation of how far each ray pair is from satisfying f2ᵀ E f1 = 0, with the gradient taken in the tangent planes of the unit rays so the result is an angle in radians, not the unitless algebraic residual of :func:~ds_msp.mvg.epipolar_residual. This is the FOV-independent scoring function :func:ransac_relative_pose thresholds against — a fixed radian cutoff means the same angular tolerance at the image centre and at the rim of a wide-FOV lens, unlike a pixel threshold.

Parameters:

Name	Type	Description	Default
E	(3, 3) ndarray	Essential matrix candidate.	required
f1	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.	required
f2	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.	required

Returns:

Type	Description
(N,) ndarray	Non-negative Sampson angle (radians) per correspondence; small for a correct E and a genuine correspondence, large (up to ~pi) for a mismatch.

Source code in ds_msp/mvg/ransac.py

def sampson_residual(E: np.ndarray, f1: np.ndarray, f2: np.ndarray) -> np.ndarray:
    """Symmetric angular epipolar distance per correspondence (radians, small-angle).

    First-order (Sampson) approximation of how far each ray pair is from satisfying
    ``f2ᵀ E f1 = 0``, with the gradient taken in the **tangent planes** of the unit rays so the
    result is an **angle in radians**, not the unitless algebraic residual of
    :func:`~ds_msp.mvg.epipolar_residual`. This is the FOV-independent scoring function
    :func:`ransac_relative_pose` thresholds against — a fixed radian cutoff means the same
    angular tolerance at the image centre and at the rim of a wide-FOV lens, unlike a pixel
    threshold.

    Parameters
    ----------
    E : (3, 3) ndarray
        Essential matrix candidate.
    f1, f2 : (N, 3) ndarray
        Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.

    Returns
    -------
    (N,) ndarray
        Non-negative Sampson angle (radians) per correspondence; small for a correct ``E`` and
        a genuine correspondence, large (up to ``~pi``) for a mismatch.
    """
    E = np.asarray(E, float)
    f1 = _as_rays(f1)
    f2 = _as_rays(f2)
    num = np.einsum("ij,jk,ik->i", f2, E, f1)          # f2ᵀ E f1
    Ef1 = f1 @ E.T                                      # epipolar normal in cam 2, (N,3)
    Etf2 = f2 @ E                                       # epipolar normal in cam 1, (N,3)
    g2 = Ef1 - np.einsum("ij,ij->i", Ef1, f2)[:, None] * f2     # tangent component at f2
    g1 = Etf2 - np.einsum("ij,ij->i", Etf2, f1)[:, None] * f1   # tangent component at f1
    denom = np.sqrt(np.sum(g1 * g1, axis=1) + np.sum(g2 * g2, axis=1))
    return np.abs(num) / np.maximum(denom, 1e-12)

triangulate_rays

triangulate_rays(f1: ndarray, f2: ndarray, R: ndarray, t: ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]

Midpoint triangulation of ray pairs under pose (R, t).

For each correspondence, finds the two points (one on ray 1, one on ray 2, expressed in camera-1 frame) that minimize the distance between the rays, and returns their midpoint — the closed-form linear method (see e.g. Hartley & Sturm, Triangulation, CVIU 1997, §2.1).

Parameters:

Name	Type	Description	Default
f1	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.	required
f2	(N, 3) ndarray	Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.	required
R	(3, 3), (3,) ndarray	Relative pose mapping a point from camera 1 to camera 2: X2 = R @ X1 + t (the convention returned by :func:recover_pose / :func:decompose_essential).	required
t	(3, 3), (3,) ndarray	Relative pose mapping a point from camera 1 to camera 2: X2 = R @ X1 + t (the convention returned by :func:recover_pose / :func:decompose_essential).	required

Returns:

Name	Type	Description
X	(N, 3) ndarray	Triangulated 3D points, in the camera-1 frame.
depth1	(N,) ndarray	Signed distance of each point along f1 from camera 1's centre.
depth2	(N,) ndarray	Signed distance of each point along f2 from camera 2's centre. A point is in front of both cameras iff depth1 > 0 and depth2 > 0 — the ray-cheirality test used by :func:recover_pose (positive depth along the bearing ray, not z > 0: valid for rays past 90° off-axis on a wide-FOV camera).

Notes

Near-parallel ray pairs (|f1 · d2| -> 1, i.e. almost no parallax) make the 2x2 linear system nearly singular; the denominator is floored at 1e-12 rather than raising, so the result degrades gracefully (a poorly-conditioned but finite point) instead of dividing by zero.

Source code in ds_msp/mvg/two_view.py

def triangulate_rays(f1: np.ndarray, f2: np.ndarray, R: np.ndarray, t: np.ndarray
                     ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Midpoint triangulation of ray pairs under pose ``(R, t)``.

    For each correspondence, finds the two points (one on ray 1, one on ray 2, expressed in
    camera-1 frame) that minimize the distance between the rays, and returns their midpoint —
    the closed-form linear method (see e.g. Hartley & Sturm, *Triangulation*, CVIU 1997, §2.1).

    Parameters
    ----------
    f1, f2 : (N, 3) ndarray
        Unit (or non-unit; renormalized internally) bearing vectors in camera 1 and camera 2.
    R, t : (3, 3), (3,) ndarray
        Relative pose mapping a point from camera 1 to camera 2: ``X2 = R @ X1 + t``
        (the convention returned by :func:`recover_pose` / :func:`decompose_essential`).

    Returns
    -------
    X : (N, 3) ndarray
        Triangulated 3D points, in the **camera-1** frame.
    depth1 : (N,) ndarray
        Signed distance of each point along ``f1`` from camera 1's centre.
    depth2 : (N,) ndarray
        Signed distance of each point along ``f2`` from camera 2's centre. A point is in front
        of both cameras iff ``depth1 > 0`` and ``depth2 > 0`` — the ray-cheirality test used by
        :func:`recover_pose` (positive depth *along the bearing ray*, not ``z > 0``: valid for
        rays past 90° off-axis on a wide-FOV camera).

    Notes
    -----
    Near-parallel ray pairs (``|f1 · d2| -> 1``, i.e. almost no parallax) make the 2x2 linear
    system nearly singular; the denominator is floored at ``1e-12`` rather than raising, so the
    result degrades gracefully (a poorly-conditioned but finite point) instead of dividing by
    zero.
    """
    f1 = _as_rays(f1)
    f2 = _as_rays(f2)
    R = np.asarray(R, float)
    t = np.asarray(t, float).reshape(3)
    # Both rays expressed in camera-1 frame.
    c2 = -R.T @ t                       # camera-2 centre in camera-1 frame
    d2 = f2 @ R                         # = (R.T @ f2.T).T, ray-2 directions in camera-1 frame
    w0 = -c2                            # o1 - o2, with o1 = 0
    b = np.einsum("ij,ij->i", f1, d2)   # f1·d2  (a = f1·f1 = 1, c = d2·d2 = 1)
    d = f1 @ w0                          # f1·w0
    e = d2 @ w0                          # d2·w0
    denom = 1.0 - b * b                  # a*c - b^2
    denom = np.where(np.abs(denom) < 1e-12, 1e-12, denom)
    lam1 = (b * e - d) / denom           # (b*e - c*d)/denom, c = 1
    lam2 = (e - b * d) / denom           # (a*e - b*d)/denom, a = 1
    P1 = lam1[:, None] * f1
    P2 = c2[None, :] + lam2[:, None] * d2
    X = 0.5 * (P1 + P2)
    return X, lam1, lam2