Skip to content

ds_msp.geometry

Shared geometric primitives — resection/PnP, robust pose averaging, and covisibility-graph utilities, consumed by both single-camera (ds_msp.calib) and multi-camera (ds_msp.rig) services.

ds_msp.geometry

Neutral geometry primitives — the shared single-/multi-view geometry layer.

One resection/PnP, robust pose averaging, and covisibility-graph utilities, consumed by both single-camera (calib) and multi-camera (rig) calibration so neither has to import the other. Pure NumPy + stdlib; depends only on core (and data) — never on models, detection, IO, or a service layer, and never on OpenCV.

average_rotation 

average_rotation(Rs: List[ndarray]) -> np.ndarray

Markley SVD quaternion averaging of rotation matrices (geometrytools.cpp:881).

Source code in ds_msp/geometry/averaging.py 
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
def average_rotation(Rs: List[np.ndarray]) -> np.ndarray:
    """Markley SVD quaternion averaging of rotation matrices (geometrytools.cpp:881)."""
    Rs = list(Rs)
    if len(Rs) == 1:
        return np.asarray(Rs[0], float).copy()
    A = np.zeros((4, 4))
    for R in Rs:
        q = _mat_to_quat_xyzw(R)
        if q[3] < 0:                       # antipodal fix on the scalar (cpp:897)
            q = -q
        A += np.outer(q, q)
    A /= len(Rs)
    # A is symmetric PSD: leading eigenvector == leading left-singular vector (cpp uses SVD U[:,0]).
    _, V = np.linalg.eigh(A)
    return _quat_xyzw_to_mat(V[:, -1])

average_transform 

average_transform(Ts: List[ndarray]) -> np.ndarray

Fuse a stack of noisy 4x4 transforms into one (initInterTransform analogue).

Rotation by Markley averaging, translation by component-wise median.

Source code in ds_msp/geometry/averaging.py 
80
81
82
83
84
85
86
87
88
89
90
91
def average_transform(Ts: List[np.ndarray]) -> np.ndarray:
    """Fuse a stack of noisy 4x4 transforms into one (``initInterTransform`` analogue).

    Rotation by Markley averaging, translation by component-wise median.
    """
    Ts = [np.asarray(T, float) for T in Ts]
    R = average_rotation([T[:3, :3] for T in Ts])
    t = average_translation(np.array([T[:3, 3] for T in Ts]))
    out = np.eye(4)
    out[:3, :3] = R
    out[:3, 3] = t
    return out

average_translation 

average_translation(ts: ndarray) -> np.ndarray

Component-wise median translation (McCalib.cpp:843 / geometrytools.cpp:697).

Source code in ds_msp/geometry/averaging.py 
75
76
77
def average_translation(ts: np.ndarray) -> np.ndarray:
    """Component-wise median translation (McCalib.cpp:843 / geometrytools.cpp:697)."""
    return np.median(np.asarray(ts, float).reshape(-1, 3), axis=0)

bundle_adjust 

bundle_adjust(model_cls, params0: ndarray, R0: ndarray, t0: ndarray, X_world_list: Sequence[ndarray], keypoints_list: Sequence[ndarray], visibility_list: Sequence[ndarray], *, kernel: str = 'none', scale: 'float | str' = 1.0, gnc_start: float = 0.0, gnc_iters: int = 0, noise_bound: 'float | None' = None, max_iter: int = 200) -> Tuple[np.ndarray, np.ndarray, np.ndarray, OptResult]

Refine model_cls intrinsics + per-image poses from seeded extrinsics.

Parameters:

Name Type Description Default
model_cls type[CameraModel]

The camera-model class (the parameter vector is in model_cls.param_names order).

 required
params0 (P,) ndarray

Initial intrinsics (clipped into model_cls.param_bounds()).

 required
R0 (n,3,3), (n,3) ndarray

Seeded per-image rotation matrices and translations (object->camera).

 required
t0 (n,3,3), (n,3) ndarray

Seeded per-image rotation matrices and translations (object->camera).

 required
kernel robust IRLS kernel name + inlier scale (``"none"`` ⇒ plain L2).
'none'
scale robust IRLS kernel name + inlier scale (``"none"`` ⇒ plain L2).
'none'
gnc_start graduated-non-convexity schedule (0 ⇒ off).
0.0
gnc_iters graduated-non-convexity schedule (0 ⇒ off).
0.0
noise_bound expected per-corner reprojection σ in pixels (e.g. ``~0.3``). When set, run a

median-free GNC-TLS solve (:func:core.optimize.gnc_tls_schur_solve) against the explicit barc2 = (3.03·σ)² inlier band instead of the MAD-capped kernel path — it rejects past 50% gross corner-detection outliers and returns a hard inlier set. kernel/scale/gnc_* are ignored when noise_bound is given.

 None

Returns:

Type Description
tuple

(params, Rb, t, OptResult) — refined intrinsics, refined base rotations, translations, and the raw solver result.
Source code in ds_msp/geometry/calibrate_core.py 
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
def bundle_adjust(model_cls,
                  params0: np.ndarray,
                  R0: np.ndarray,
                  t0: np.ndarray,
                  X_world_list: Sequence[np.ndarray],
                  keypoints_list: Sequence[np.ndarray],
                  visibility_list: Sequence[np.ndarray],
                  *, kernel: str = "none", scale: "float | str" = 1.0,
                  gnc_start: float = 0.0, gnc_iters: int = 0,
                  noise_bound: "float | None" = None,
                  max_iter: int = 200) -> Tuple[np.ndarray, np.ndarray, np.ndarray, OptResult]:
    """Refine ``model_cls`` intrinsics + per-image poses from seeded extrinsics.

    Parameters
    ----------
    model_cls : type[CameraModel]
        The camera-model class (the parameter vector is in ``model_cls.param_names`` order).
    params0 : (P,) ndarray
        Initial intrinsics (clipped into ``model_cls.param_bounds()``).
    R0, t0 : (n,3,3), (n,3) ndarray
        Seeded per-image rotation matrices and translations (object->camera).
    kernel, scale : robust IRLS kernel name + inlier scale (``"none"`` ⇒ plain L2).
    gnc_start, gnc_iters : graduated-non-convexity schedule (0 ⇒ off).
    noise_bound : expected per-corner reprojection σ in pixels (e.g. ``~0.3``). When set, run a
        median-free **GNC-TLS** solve (:func:`core.optimize.gnc_tls_schur_solve`) against the
        explicit ``barc2 = (3.03·σ)²`` inlier band instead of the MAD-capped kernel path — it
        rejects **past 50%** gross corner-detection outliers and returns a hard inlier set.
        ``kernel``/``scale``/``gnc_*`` are ignored when ``noise_bound`` is given.

    Returns
    -------
    tuple
        ``(params, Rb, t, OptResult)`` — refined intrinsics, refined base rotations,
        translations, and the raw solver result.
    """
    cls = model_cls
    P = len(cls.param_names)
    n_img = len(X_world_list)
    sizes = [len(X) for X in X_world_list]
    total = 2 * sum(sizes)
    masks = [np.asarray(v, bool) for v in visibility_list]
    lb_i, ub_i = cls.param_bounds()
    state0 = (np.clip(np.asarray(params0, float), lb_i, ub_i).copy(),
              np.asarray(R0, float).copy(), np.asarray(t0, float).copy())

    def residual(state):
        """Flat ``(2 * sum(sizes),)`` pixel residual ``uv_predicted - uv_observed``, image-major."""
        params, Rb, t = state
        m = cls.from_params(params)
        out = np.zeros((total,))
        row = 0
        for i, (Xw, uv) in enumerate(zip(X_world_list, keypoints_list)):
            N = sizes[i]
            Xc = (Rb[i] @ Xw.T).T + t[i]
            uvp, valid = m.project(Xc)
            mask = masks[i] & valid
            diff = np.zeros_like(uv, dtype=np.float64)
            diff[mask] = uvp[mask] - uv[mask]
            out[row:row + 2 * N] = diff.ravel()
            row += 2 * N
        return out

    def linearize(state):
        """Per-image residual + split Jacobian (shared intrinsics A_i, local pose B_i).

        Feeding the blocks separately lets the solver Schur-complement out the (block-
        diagonal) per-image poses, so the work scales linearly in image count rather than
        cubically in the full ``P + 6·n_img`` dimension.
        """
        params, Rb, t = state
        m = cls.from_params(params)
        r_list, A_list, B_list = [], [], []
        for i, (Xw, uv) in enumerate(zip(X_world_list, keypoints_list)):
            N = sizes[i]
            Xc = (Rb[i] @ Xw.T).T + t[i]
            uvp, J_point, J_param, valid = m.project_jacobian(Xc)
            mask = (masks[i] & valid)
            r_i = np.zeros((N, 2))
            r_i[mask] = uvp[mask] - uv[mask]
            mask3 = mask[:, None, None].astype(np.float64)
            # δω linearized at 0 ⇒ J_r = I, so ∂Xc/∂δω = -R[Xw]_×; ∂Xc/∂δt = I.
            dXc_dw = -np.einsum('ij,njk->nik', Rb[i], hat_batch(Xw))
            J_rvec = np.einsum('nij,njc->nic', J_point, dXc_dw)
            J_ext = np.concatenate([J_rvec, J_point], axis=-1) * mask3
            r_list.append(r_i.ravel())
            A_list.append((J_param * mask3).reshape(2 * N, P))
            B_list.append(J_ext.reshape(2 * N, 6))
        return r_list, A_list, B_list

    def retract(state, d_shared, d_local):
        """Manifold update for ``schur_lm``/``gnc_tls_schur_solve``: intrinsics clipped to
        bounds, each per-image rotation updated by SO(3) exp, translation updated flat."""
        params, Rb, t = state
        params = np.clip(params + d_shared, lb_i, ub_i)          # keep intrinsics valid
        Rb, t = Rb.copy(), t.copy()
        for i in range(n_img):
            Rb[i] = Rb[i] @ so3_exp(d_local[i, :3])
            t[i] = t[i] + d_local[i, 3:]
        return (params, Rb, t)

    if noise_bound is not None:
        out = gnc_tls_schur_solve(state0, residual, linearize, retract,
                                  noise_bound=3.03 * float(noise_bound),   # 2-DoF 99% χ² band
                                  n_groups=n_img, shared_dim=P, local_dim=6, block=2,
                                  inner_max_iter=max_iter)
    else:
        out = schur_lm(state0, residual, linearize, retract,
                       n_groups=n_img, shared_dim=P, local_dim=6, block=2,
                       max_iter=max_iter, robust_kernel=kernel, robust_scale=scale,
                       gnc_start=gnc_start, gnc_iters=gnc_iters)
    params, Rb, t = out.state
    return params, Rb, t, out

connected_components 

connected_components(nodes: List[int], weights: Dict[Edge, float]) -> List[List[int]]

Union-find connected components. Each component is sorted so comp[0] is the minimum id — MC-Calib's reference choice (min_element).

Source code in ds_msp/geometry/graph.py 
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
def connected_components(nodes: List[int], weights: Dict[Edge, float]) -> List[List[int]]:
    """Union-find connected components. Each component is sorted so ``comp[0]`` is the
    minimum id — MC-Calib's reference choice (``min_element``)."""
    parent = {n: n for n in nodes}

    def find(x: int) -> int:
        """Union-find root of ``x``, with path halving."""
        while parent[x] != x:
            parent[x] = parent[parent[x]]
            x = parent[x]
        return x

    for (i, j) in weights:
        if i in parent and j in parent:
            parent[find(i)] = find(j)

    comps: Dict[int, List[int]] = {}
    for n in nodes:
        comps.setdefault(find(n), []).append(n)
    return [sorted(c) for c in comps.values()]

covis_weights 

covis_weights(pair_counts: Dict[Edge, int]) -> Dict[Edge, float]

pair_counts[(i, j)] = #frames i and j were co-observed -> edge weight 1/N.

Keys are normalized to i < j (undirected). Symmetric duplicates are summed.

Source code in ds_msp/geometry/graph.py 
18
19
20
21
22
23
24
25
26
27
28
29
def covis_weights(pair_counts: Dict[Edge, int]) -> Dict[Edge, float]:
    """``pair_counts[(i, j)] = #frames i and j were co-observed`` -> edge weight ``1/N``.

    Keys are normalized to ``i < j`` (undirected). Symmetric duplicates are summed.
    """
    w: Dict[Edge, int] = {}
    for (i, j), n in pair_counts.items():
        if i == j:
            continue
        key = (i, j) if i < j else (j, i)
        w[key] = w.get(key, 0) + n
    return {e: 1.0 / n for e, n in w.items() if n > 0}

decompose_P 

decompose_P(P: ndarray) -> Optional[Tuple[np.ndarray, np.ndarray, np.ndarray]]

Factor P = K[R|t] into intrinsics K (K[2,2]=1), rotation R (det +1), and translation t. Returns None if the factorization is degenerate.

Source code in ds_msp/geometry/resection.py 
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
def decompose_P(P: np.ndarray) -> Optional[Tuple[np.ndarray, np.ndarray, np.ndarray]]:
    """Factor ``P = K[R|t]`` into intrinsics ``K`` (``K[2,2]=1``), rotation ``R`` (det +1),
    and translation ``t``. Returns ``None`` if the factorization is degenerate.
    """
    H = P[:, :3]
    if abs(np.linalg.det(H)) < 1e-12:
        return None
    K, R = _rq3(H)
    # force a positive diagonal on K (sign ambiguity of RQ): H = K·S·S·R with S=diag(±1)
    d = np.diag(K)
    S = np.diag(np.where(d >= 0, 1.0, -1.0))
    K = K @ S
    R = S @ R
    lam = K[2, 2]                                           # DLT scale: H = λ·K_true·R
    if abs(lam) < 1e-12:
        return None
    K = K / lam                                            # normalize K[2,2] = 1
    Pn = P / lam                                           # rescale P to match → Pn = K[R|t]
    if np.linalg.det(R) < 0:                                # det(R)=+1 resolves the 1-bit sign
        R = -R
        Pn = -Pn
    if K[0, 0] < 0 or K[1, 1] < 0:
        return None
    t = np.linalg.inv(K) @ Pn[:, 3]
    return K, R, t

dlt_projection 

dlt_projection(X: ndarray, uv: ndarray) -> np.ndarray

Normalized DLT estimate of the 3x4 camera matrix P (uv ~ P·[X;1]).

Solves the 2N x 12 homogeneous system A vec(P) = 0 by SVD (smallest right-singular vector) after Hartley normalization, then de-normalizes. The returned P is only defined up to scale.

Parameters:

Name Type Description Default
X (N, 3) ndarray

World-frame 3D points, genuinely non-coplanar (see :func:_is_coplanar / :func:ransac_pnp_normalized's planar branch for coplanar targets, where this general DLT is degenerate).

 required
uv (N, 2) ndarray

Corresponding image points (pixels, or normalized coordinates with K = I).

 required

Returns:

Type Description
(3, 4) ndarray

Camera matrix P such that [u, v, 1]ᵀ ~ P @ [X, 1]ᵀ up to scale.
Notes

Needs at least 6 correspondences for A to be non-degenerate (2N >= 11 free unknowns), but this is not checked: fewer points still run without error and silently return a meaningless P. Callers needing that guarantee should go through :func:ransac_resection, which enforces min_sample before calling this.

Source code in ds_msp/geometry/resection.py 
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
def dlt_projection(X: np.ndarray, uv: np.ndarray) -> np.ndarray:
    """Normalized DLT estimate of the 3x4 camera matrix ``P`` (``uv ~ P·[X;1]``).

    Solves the 2N x 12 homogeneous system ``A vec(P) = 0`` by SVD (smallest right-singular
    vector) after Hartley normalization, then de-normalizes. The returned ``P`` is only defined
    up to scale.

    Parameters
    ----------
    X : (N, 3) ndarray
        World-frame 3D points, genuinely non-coplanar (see :func:`_is_coplanar` /
        :func:`ransac_pnp_normalized`'s planar branch for coplanar targets, where this general
        DLT is degenerate).
    uv : (N, 2) ndarray
        Corresponding image points (pixels, or normalized coordinates with ``K = I``).

    Returns
    -------
    (3, 4) ndarray
        Camera matrix ``P`` such that ``[u, v, 1]ᵀ ~ P @ [X, 1]ᵀ`` up to scale.

    Notes
    -----
    Needs **at least 6** correspondences for ``A`` to be non-degenerate (``2N >= 11`` free
    unknowns), but this is **not checked**: fewer points still run without error and silently
    return a meaningless ``P``. Callers needing that guarantee should go through
    :func:`ransac_resection`, which enforces ``min_sample`` before calling this.
    """
    X = np.asarray(X, float)
    uv = np.asarray(uv, float)
    U, Xn = _normalize_3d(X)
    T, un = _normalize_2d(uv)
    n = len(X)
    A = np.zeros((2 * n, 12))
    Xh = Xn                                   # (n,4) homogeneous, normalized
    u, v = un[:, 0], un[:, 1]
    A[0::2, 0:4] = -Xh
    A[0::2, 8:12] = u[:, None] * Xh
    A[1::2, 4:8] = -Xh
    A[1::2, 8:12] = v[:, None] * Xh
    _, _, Vt = np.linalg.svd(A)
    Pn = Vt[-1].reshape(3, 4)
    # de-normalize: u = T·P_real·X  and  un = T·u, Xn = U·X  ⇒  P_real = T⁻¹·Pn·U
    P = np.linalg.inv(T) @ Pn @ U
    return P

intrinsics_seed 

intrinsics_seed(objpts_list: List[ndarray], imgpts_list: List[ndarray], w: int, h: int, *, thresh_px: float = 3.0, seed: int = 0) -> Tuple[np.ndarray, List[Optional[Tuple[np.ndarray, np.ndarray, np.ndarray]]]]

Robust pinhole intrinsic seed K from a genuinely-3D target, no OpenCV.

RANSAC-resects every view, RQ-decomposes each into K_i, R_i, t_i, and returns the robust median K over the views whose decomposition is plausible plus the per-view (K_i, R_i, t_i) (None for views that failed). Gross outliers are rejected inside each view's RANSAC, so the focal seed never sees the blunders.

Source code in ds_msp/geometry/resection.py 
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
def intrinsics_seed(objpts_list: List[np.ndarray], imgpts_list: List[np.ndarray],
                    w: int, h: int, *, thresh_px: float = 3.0, seed: int = 0
                    ) -> Tuple[np.ndarray, List[Optional[Tuple[np.ndarray, np.ndarray, np.ndarray]]]]:
    """Robust pinhole intrinsic seed ``K`` from a genuinely-3D target, no OpenCV.

    RANSAC-resects every view, RQ-decomposes each into ``K_i, R_i, t_i``, and returns
    the **robust median** ``K`` over the views whose decomposition is plausible plus the
    per-view ``(K_i, R_i, t_i)`` (``None`` for views that failed). Gross outliers are
    rejected inside each view's RANSAC, so the focal seed never sees the blunders.
    """
    diag = float(np.hypot(w, h))
    Ks, poses = [], []
    for i, (X, uv) in enumerate(zip(objpts_list, imgpts_list)):
        P, inl = ransac_resection(np.asarray(X, float), np.asarray(uv, float),
                                  thresh_px=thresh_px, seed=seed + i)
        dec = decompose_P(P) if P is not None and inl.sum() >= 6 else None
        if dec is None:
            poses.append(None)
            continue
        K, R, t = dec
        fx, fy = K[0, 0], K[1, 1]
        # only let plausibly-focused views vote for the consensus intrinsics
        if 0.2 * diag < fx < 5.0 * diag and 0.2 * diag < fy < 5.0 * diag:
            Ks.append([fx, fy, K[0, 2], K[1, 2]])
        poses.append((K, R, t))
    if Ks:
        fx, fy, cx, cy = np.median(np.array(Ks), axis=0)
    else:
        fx = fy = float(w)
        cx, cy = w / 2.0, h / 2.0
    K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1.0]])
    return K, poses

mean_transform 

mean_transform(Ts: List[ndarray]) -> np.ndarray

Markley rotation averaging but arithmetic-mean translation — the convention CameraGroupObs::computeObjectsPose uses (CameraGroupObs.cpp:95), distinct from :func:average_transform's median.

Source code in ds_msp/geometry/averaging.py 
140
141
142
143
144
145
146
147
148
149
150
def mean_transform(Ts: List[np.ndarray]) -> np.ndarray:
    """Markley rotation averaging but *arithmetic-mean* translation — the convention
    ``CameraGroupObs::computeObjectsPose`` uses (CameraGroupObs.cpp:95), distinct from
    :func:`average_transform`'s median."""
    Ts = [np.asarray(T, float) for T in Ts]
    R = average_rotation([T[:3, :3] for T in Ts])
    t = np.mean(np.array([T[:3, 3] for T in Ts]), axis=0)
    out = np.eye(4)
    out[:3, :3] = R
    out[:3, 3] = t
    return out

ransac_pnp_normalized 

ransac_pnp_normalized(X: ndarray, pn: ndarray, *, focal: float = 1.0, thresh_px: float = 3.0, max_iters: int = 300, confidence: float = 0.999, min_sample: int = 6, seed: int = 0) -> Tuple[Optional[np.ndarray], np.ndarray]

RANSAC pose on the normalized plane. thresh_px is interpreted in pixels via focal (the model focal) so the gate matches the pixel-domain blunders. Returns (T_cam_obj (4,4) | None, inlier_mask).

Branches on target geometry: a coplanar board uses the IPPE planar solver (the general 3×4 DLT is degenerate for coplanar points — it returns garbage poses, ~1700 px reprojection on a wide-FOV board); a non-coplanar (fused multi-board) object uses the DLT.

Source code in ds_msp/geometry/resection.py 
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
def ransac_pnp_normalized(X: np.ndarray, pn: np.ndarray, *, focal: float = 1.0,
                          thresh_px: float = 3.0, max_iters: int = 300,
                          confidence: float = 0.999, min_sample: int = 6,
                          seed: int = 0) -> Tuple[Optional[np.ndarray], np.ndarray]:
    """RANSAC pose on the normalized plane. ``thresh_px`` is interpreted in pixels via
    ``focal`` (the model focal) so the gate matches the pixel-domain blunders. Returns
    ``(T_cam_obj (4,4) | None, inlier_mask)``.

    Branches on target geometry: a **coplanar** board uses the IPPE planar solver (the general
    3×4 DLT is degenerate for coplanar points — it returns garbage poses, ~1700 px reprojection
    on a wide-FOV board); a non-coplanar (fused multi-board) object uses the DLT."""
    X = np.asarray(X, float)
    pn = np.asarray(pn, float)
    n = len(X)
    if n < min_sample:
        return None, np.zeros(n, bool)
    coplanar = _is_coplanar(X)
    min_sample = 4 if coplanar else min_sample
    solve = _pose_planar_normalized if coplanar else _pose_dlt_normalized
    thr = thresh_px / max(focal, 1e-9)       # normalized-plane tolerance
    rng = np.random.default_rng(seed)
    best_inl = np.zeros(n, bool)
    iters, it = max_iters, 0

    def _err(R, t):
        Xc = X @ R.T + t
        z = Xc[:, 2]
        with np.errstate(divide="ignore", invalid="ignore"):
            proj = Xc[:, :2] / z[:, None]
        e = np.linalg.norm(proj - pn, axis=1)
        e[~np.isfinite(e) | (z <= 0)] = np.inf
        return e

    while it < iters and it < max_iters:
        it += 1
        sample = rng.choice(n, min_sample, replace=False)
        sol = solve(X[sample], pn[sample])
        if sol is None:
            continue
        R, t = sol
        inl = _err(R, t) < thr
        if inl.sum() > best_inl.sum():
            best_inl = inl
            frac = float(np.clip(inl.mean(), 1e-6, 1.0))
            if frac >= 1.0:
                break
            den = np.log1p(-frac ** min_sample)
            if den < -1e-12:
                iters = min(max_iters, int(np.log1p(-confidence) / den) + 1)
    if best_inl.sum() < min_sample:
        return None, best_inl
    sol = solve(X[best_inl], pn[best_inl])
    if sol is None:
        return None, best_inl
    R, t = sol
    best_inl = _err(R, t) < thr
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T, best_inl

ransac_resection 

ransac_resection(X: ndarray, uv: ndarray, *, thresh_px: float = 3.0, max_iters: int = 300, confidence: float = 0.999, min_sample: int = 6, seed: int = 0) -> Tuple[Optional[np.ndarray], np.ndarray]

RANSAC a 3x4 camera matrix robust to gross outliers.

Samples min_sample correspondences, fits a DLT P, scores by reprojection inliers, and refits P on the full consensus set. Returns (P | None, inlier_mask).

Source code in ds_msp/geometry/resection.py 
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
def ransac_resection(X: np.ndarray, uv: np.ndarray, *, thresh_px: float = 3.0,
                     max_iters: int = 300, confidence: float = 0.999,
                     min_sample: int = 6, seed: int = 0
                     ) -> Tuple[Optional[np.ndarray], np.ndarray]:
    """RANSAC a 3x4 camera matrix robust to gross outliers.

    Samples ``min_sample`` correspondences, fits a DLT ``P``, scores by reprojection
    inliers, and refits ``P`` on the full consensus set. Returns ``(P | None, inlier_mask)``.
    """
    X = np.asarray(X, float)
    uv = np.asarray(uv, float)
    n = len(X)
    if n < min_sample:
        return None, np.zeros(n, bool)
    rng = np.random.default_rng(seed)
    best_inl = np.zeros(n, bool)
    iters = max_iters
    it = 0
    while it < iters and it < max_iters:
        it += 1
        sample = rng.choice(n, min_sample, replace=False)
        try:
            P = dlt_projection(X[sample], uv[sample])
        except np.linalg.LinAlgError:
            continue
        inl = _reproj_err(P, X, uv) < thresh_px
        if inl.sum() > best_inl.sum():
            best_inl = inl
            frac = float(np.clip(inl.mean(), 1e-6, 1.0))
            if frac >= 1.0:
                break
            den = np.log1p(-frac ** min_sample)
            if den < -1e-12:
                iters = min(max_iters, int(np.log1p(-confidence) / den) + 1)
    if best_inl.sum() < min_sample:
        return None, best_inl
    try:
        P = dlt_projection(X[best_inl], uv[best_inl])
    except np.linalg.LinAlgError:
        return None, best_inl
    # final inlier set under the refit
    best_inl = _reproj_err(P, X, uv) < thresh_px
    return P, best_inl

robust_average_transform 

robust_average_transform(Ts: List[ndarray], *, iters: int = 3) -> np.ndarray

Outlier-robust fusion of noisy relative transforms.

A handful of per-frame relative poses can be grossly wrong (a frame whose object pose was estimated from an outlier-corrupted view), and plain :func:average_transform weights every sample equally, so the Markley rotation average is dragged into a wrong basin — the dominant failure mode of rig-extrinsics init under gross outliers. This estimator instead selects an inlier consensus: start from the (robust) median estimate, score each sample by its rotation+translation deviation, keep the samples within a MAD-based gate, and re-fuse — iterated a few times. Falls back to the plain average when too few samples survive. The translation scale (metric, from the boards) sets the gate, so SE(3) — not Sim(3) — is all that is needed.

Source code in ds_msp/geometry/averaging.py 
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
def robust_average_transform(Ts: List[np.ndarray], *, iters: int = 3) -> np.ndarray:
    """Outlier-robust fusion of noisy relative transforms.

    A handful of per-frame relative poses can be grossly wrong (a frame whose object pose
    was estimated from an outlier-corrupted view), and plain :func:`average_transform`
    weights every sample equally, so the Markley rotation average is dragged into a wrong
    basin — the dominant failure mode of rig-extrinsics init under gross outliers. This
    estimator instead **selects an inlier consensus**: start from the (robust) median
    estimate, score each sample by its rotation+translation deviation, keep the samples
    within a MAD-based gate, and re-fuse — iterated a few times. Falls back to the plain
    average when too few samples survive. The translation scale (metric, from the boards)
    sets the gate, so SE(3) — not Sim(3) — is all that is needed.
    """
    Ts = [np.asarray(T, float) for T in Ts]
    if len(Ts) <= 2:
        return average_transform(Ts)
    Rs = [T[:3, :3] for T in Ts]
    ts = np.array([T[:3, 3] for T in Ts])
    keep = np.ones(len(Ts), bool)
    est = average_transform(Ts)
    for _ in range(iters):
        Re, te = est[:3, :3], est[:3, 3]
        ang = np.array([_rot_angle(Re, R) for R in Rs])          # rotation residual (rad)
        dist = np.linalg.norm(ts - te, axis=1)                   # translation residual
        # MAD-based gates (1.4826·MAD ≈ σ); floor avoids over-tight gates on clean data.
        a_thr = max(np.median(ang) + 3.0 * 1.4826 * np.median(np.abs(ang - np.median(ang))),
                    np.deg2rad(2.0))
        d_med = float(np.median(dist))
        d_thr = max(d_med + 3.0 * 1.4826 * float(np.median(np.abs(dist - d_med))),
                    0.02 * max(d_med, 1e-6) + 1e-3)
        new_keep = (ang <= a_thr) & (dist <= d_thr)
        if new_keep.sum() < max(3, int(0.3 * len(Ts))):
            break                                                # too aggressive — stop
        est = average_transform([Ts[i] for i in range(len(Ts)) if new_keep[i]])
        if np.array_equal(new_keep, keep):
            break
        keep = new_keep
    return est

shortest_path 

shortest_path(src: int, dst: int, weights: Dict[Edge, float]) -> List[int]

Dijkstra shortest path src -> dst by edge weight. Returns the node list [src, ..., dst] ([src] if equal; raises if disconnected).

Source code in ds_msp/geometry/graph.py 
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
def shortest_path(src: int, dst: int, weights: Dict[Edge, float]) -> List[int]:
    """Dijkstra shortest path ``src -> dst`` by edge weight. Returns the node list
    ``[src, ..., dst]`` (``[src]`` if equal; raises if disconnected)."""
    if src == dst:
        return [src]
    adj = _adjacency(weights)
    dist = {src: 0.0}
    prev: Dict[int, int] = {}
    pq: List[Tuple[float, int]] = [(0.0, src)]
    while pq:
        d, u = heapq.heappop(pq)
        if u == dst:
            break
        if d > dist.get(u, float("inf")):
            continue
        for v, w in adj.get(u, ()):
            nd = d + w
            if nd < dist.get(v, float("inf")):
                dist[v] = nd
                prev[v] = u
                heapq.heappush(pq, (nd, v))
    if dst not in prev and dst != src:
        raise ValueError(f"no path from {src} to {dst}")
    path = [dst]
    while path[-1] != src:
        path.append(prev[path[-1]])
    return path[::-1]