ds_msp.calib

Generic calibration (bundle adjustment) for any CameraModel. See Calibrate any model for task recipes and the capstone for a full worked example.

ds_msp.calib

Generic calibration (bundle adjustment) for any CameraModel.

calibrate and AprilGridTarget are dependency-light. detect_aprilgrid needs the optional aprilgrid backend (pip install ds_msp[calib]) and is imported lazily so this package stays importable without it.

load_camera re-exports :func:ds_msp.io.kalibr.load_kalibr under a name that doesn't require knowing the output is Kalibr-formatted — ds-msp-calibrate's output is meant to be loaded straight back into a ready :class:~ds_msp.core.contracts.CameraModel instance, not inspected as a file format.

AprilGridTarget

A Kalibr-style AprilGrid: a rows x cols grid of AprilTags.

Geometry follows Kalibr's convention exactly so detections from a board calibrated by Kalibr/Basalt line up:

• Tag ids run row-major from the bottom-left corner: id = row * cols + col.
• Each tag's four corners are ordered counter-clockwise starting bottom-left (BL, BR, TR, TL) — the same order the AprilGrid detector returns them.
• Tags are squares of side tag_size (metres) separated by a gap of tag_spacing * tag_size (tag_spacing is Kalibr's gap/size ratio).

tag_size only sets absolute scale, which affects the recovered extrinsic translations, not the intrinsics — so a slightly wrong board size still yields correct fx, fy, cx, cy and distortion.

Source code in ds_msp/calib/targets.py

class AprilGridTarget:
    """A Kalibr-style AprilGrid: a ``rows x cols`` grid of AprilTags.

    Geometry follows Kalibr's convention exactly so detections from a board
    calibrated by Kalibr/Basalt line up:

    - Tag ids run row-major from the bottom-left corner: ``id = row * cols + col``.
    - Each tag's four corners are ordered **counter-clockwise starting bottom-left**
      ``(BL, BR, TR, TL)`` — the same order the AprilGrid detector returns them.
    - Tags are squares of side ``tag_size`` (metres) separated by a gap of
      ``tag_spacing * tag_size`` (``tag_spacing`` is Kalibr's gap/size ratio).

    ``tag_size`` only sets absolute scale, which affects the recovered *extrinsic*
    translations, not the intrinsics — so a slightly wrong board size still yields
    correct ``fx, fy, cx, cy`` and distortion.
    """

    def __init__(self, tag_rows: int = 6, tag_cols: int = 6,
                 tag_size: float = 0.088, tag_spacing: float = 0.3) -> None:
        self.tag_rows = int(tag_rows)
        self.tag_cols = int(tag_cols)
        self.tag_size = float(tag_size)
        self.tag_spacing = float(tag_spacing)

    @property
    def n_tags(self) -> int:
        """Total tag count, ``tag_rows * tag_cols``."""
        return self.tag_rows * self.tag_cols

    def object_points(self, tag_id: int) -> np.ndarray:
        """3D board coordinates (metres) of one tag's 4 corners, ``(4, 3)``.

        Order matches the detector: bottom-left, bottom-right, top-right, top-left.
        """
        if not 0 <= tag_id < self.n_tags:
            raise ValueError(f"tag_id {tag_id} out of range [0, {self.n_tags})")
        row, col = divmod(tag_id, self.tag_cols)
        pitch = self.tag_size * (1.0 + self.tag_spacing)
        s = self.tag_size
        x0, y0 = col * pitch, row * pitch
        return np.array([[x0,     y0,     0.0],
                         [x0 + s, y0,     0.0],
                         [x0 + s, y0 + s, 0.0],
                         [x0,     y0 + s, 0.0]], dtype=np.float64)

    def all_object_points(self) -> np.ndarray:
        """Every corner of the board, ``(n_tags * 4, 3)`` in tag-id order."""
        return np.concatenate([self.object_points(t) for t in range(self.n_tags)])

    def build_correspondences(
        self, detections_per_image: List[Mapping[int, np.ndarray]],
        *, min_corners: int = 8,
    ) -> Tuple[List[np.ndarray], List[np.ndarray], List[np.ndarray]]:
        """Turn per-image ``{tag_id: (4, 2) pixels}`` into calibration inputs.

        Returns ``(X_world_list, keypoints_list, visibility_list)`` ready for
        ``ds_msp.calib.bundle.calibrate``. Images with fewer than ``min_corners``
        detected corners are dropped (too few to constrain a pose).
        """
        X_world_list: List[np.ndarray] = []
        keypoints_list: List[np.ndarray] = []
        visibility_list: List[np.ndarray] = []
        for det in detections_per_image:
            obj, pix = [], []
            for tag_id, corners in det.items():
                corners = np.asarray(corners, dtype=np.float64).reshape(-1, 2)
                if corners.shape[0] != 4:
                    continue
                obj.append(self.object_points(int(tag_id)))
                pix.append(corners)
            if not obj:
                continue
            X = np.concatenate(obj)
            uv = np.concatenate(pix)
            if len(X) < min_corners:
                continue
            X_world_list.append(X)
            keypoints_list.append(uv)
            visibility_list.append(np.ones(len(X), dtype=bool))
        return X_world_list, keypoints_list, visibility_list

n_tags property

n_tags: int

Total tag count, tag_rows * tag_cols.

all_object_points

all_object_points() -> np.ndarray

Every corner of the board, (n_tags * 4, 3) in tag-id order.

Source code in ds_msp/calib/targets.py

def all_object_points(self) -> np.ndarray:
    """Every corner of the board, ``(n_tags * 4, 3)`` in tag-id order."""
    return np.concatenate([self.object_points(t) for t in range(self.n_tags)])

build_correspondences

build_correspondences(detections_per_image: List[Mapping[int, ndarray]], *, min_corners: int = 8) -> Tuple[List[np.ndarray], List[np.ndarray], List[np.ndarray]]

Turn per-image {tag_id: (4, 2) pixels} into calibration inputs.

Returns (X_world_list, keypoints_list, visibility_list) ready for ds_msp.calib.bundle.calibrate. Images with fewer than min_corners detected corners are dropped (too few to constrain a pose).

Source code in ds_msp/calib/targets.py

def build_correspondences(
    self, detections_per_image: List[Mapping[int, np.ndarray]],
    *, min_corners: int = 8,
) -> Tuple[List[np.ndarray], List[np.ndarray], List[np.ndarray]]:
    """Turn per-image ``{tag_id: (4, 2) pixels}`` into calibration inputs.

    Returns ``(X_world_list, keypoints_list, visibility_list)`` ready for
    ``ds_msp.calib.bundle.calibrate``. Images with fewer than ``min_corners``
    detected corners are dropped (too few to constrain a pose).
    """
    X_world_list: List[np.ndarray] = []
    keypoints_list: List[np.ndarray] = []
    visibility_list: List[np.ndarray] = []
    for det in detections_per_image:
        obj, pix = [], []
        for tag_id, corners in det.items():
            corners = np.asarray(corners, dtype=np.float64).reshape(-1, 2)
            if corners.shape[0] != 4:
                continue
            obj.append(self.object_points(int(tag_id)))
            pix.append(corners)
        if not obj:
            continue
        X = np.concatenate(obj)
        uv = np.concatenate(pix)
        if len(X) < min_corners:
            continue
        X_world_list.append(X)
        keypoints_list.append(uv)
        visibility_list.append(np.ones(len(X), dtype=bool))
    return X_world_list, keypoints_list, visibility_list

object_points

object_points(tag_id: int) -> np.ndarray

3D board coordinates (metres) of one tag's 4 corners, (4, 3).

Order matches the detector: bottom-left, bottom-right, top-right, top-left.

Source code in ds_msp/calib/targets.py

def object_points(self, tag_id: int) -> np.ndarray:
    """3D board coordinates (metres) of one tag's 4 corners, ``(4, 3)``.

    Order matches the detector: bottom-left, bottom-right, top-right, top-left.
    """
    if not 0 <= tag_id < self.n_tags:
        raise ValueError(f"tag_id {tag_id} out of range [0, {self.n_tags})")
    row, col = divmod(tag_id, self.tag_cols)
    pitch = self.tag_size * (1.0 + self.tag_spacing)
    s = self.tag_size
    x0, y0 = col * pitch, row * pitch
    return np.array([[x0,     y0,     0.0],
                     [x0 + s, y0,     0.0],
                     [x0 + s, y0 + s, 0.0],
                     [x0,     y0 + s, 0.0]], dtype=np.float64)

calibrate

calibrate(init_model: CameraModel, X_world_list: List[ndarray], keypoints_list: List[ndarray], visibility_list: List[ndarray], *, max_nfev: int = 200, verbose: int = 0, robust: str = 'cauchy', robust_scale: 'float | str' = 'auto', gnc: bool = False, multi_start: bool = True, n_restarts: int = 4, seed: int = 0, force_multistart: bool = True, loss: str | None = None, f_scale: float | None = None) -> Dict

Calibrate any model from checkerboard correspondences — robust by default.

Parameters:

Name	Type	Description	Default
robust	str	Redescending IRLS kernel applied in the BA loop: "cauchy" (default), "huber", "geman_mcclure", "barron", or "none" for plain L2. A robust kernel keeps every corner but down-weights large residuals instead of letting one mis-localized corner drag the L2 fit.	'cauchy'
robust_scale	float | str	Inlier scale (px) where down-weighting begins, or "auto" (default) to re-estimate it each iteration from the residual MAD (50%-breakdown).	'auto'
gnc	bool	Run a graduated-non-convexity anneal (default False): start with a wide kernel that dissolves spurious minima, then sharpen. Useful only for pathological bad-init basins; with two-fold seeding it is unnecessary and re-admits gross outliers, so it is off by default (enable it for a known-hard initialization).	False
multi_start	bool	Model-aware multi-start auto-init (default True): screen the base seed plus n_restarts seeds with the shape parameters dispersed across their bounds, keep the one with the lowest robust (median) reprojection, then refine it fully. This is what makes a poor init_model (only the type + a rough focal) converge — it rescues wrong-basin shape seeds (the DS ξ fold, etc.) and is a no-op when the base seed is already good. Determinism is preserved via seed. Applies to every model, including RadTan/KB/OCam: an earlier version of this code skipped dispersion for models with no documented degenerate basin on the theory that a plain distortion tail is well-conditioned — true on rich, real data, measurably false at scale on thinner synthetic data (56/100 RadTan cameras with ~2x the reprojection error in one real test — see :func:_shape_seeds's docstring). Set multi_start=False only when you have independently verified it is safe for your data, not by model class alone.	True
n_restarts	int	Number of dispersed shape seeds for multi_start (default 4).	4
force_multistart	bool	Kept for API stability (default True, matching the always-disperse default above); see :func:_shape_seeds.	True
loss	str	SciPy-style loss name ("linear", "huber", "soft_l1", "cauchy") for backward compatibility. Passing this (with or without f_scale) reproduces the pre-robust-default behaviour: fixed scale, GNC off.	None
f_scale	float	Fixed inlier scale (px) for the legacy loss-based path, in place of the modern robust/robust_scale auto-scaling.	None

Returns:

Type	Description
dict	The refined model, per-image poses as (rvec, tvec), success, n_obs, and reprojection statistics over valid observations — rms_px, mean_px, median_px, p95_px, max_px (all in pixels, independent of the robust kernel so they stay comparable across configurations).

Source code in ds_msp/calib/bundle.py

def calibrate(init_model: CameraModel,
              X_world_list: List[np.ndarray],
              keypoints_list: List[np.ndarray],
              visibility_list: List[np.ndarray],
              *, max_nfev: int = 200, verbose: int = 0,
              robust: str = "cauchy", robust_scale: "float | str" = "auto",
              gnc: bool = False, multi_start: bool = True, n_restarts: int = 4,
              seed: int = 0, force_multistart: bool = True,
              loss: str | None = None, f_scale: float | None = None) -> Dict:
    """Calibrate any model from checkerboard correspondences — robust by default.

    Parameters
    ----------
    robust : str
        Redescending IRLS kernel applied in the BA loop: ``"cauchy"`` (default),
        ``"huber"``, ``"geman_mcclure"``, ``"barron"``, or ``"none"`` for plain L2.
        A robust kernel keeps *every* corner but **down-weights** large residuals instead of
        letting one mis-localized corner drag the L2 fit.
    robust_scale : float | str
        Inlier scale (px) where down-weighting begins, or ``"auto"`` (default) to
        re-estimate it each iteration from the residual MAD (50%-breakdown).
    gnc : bool
        Run a graduated-non-convexity anneal (default ``False``): start with a wide kernel
        that dissolves spurious minima, then sharpen. Useful only for pathological bad-init
        basins; with two-fold seeding it is unnecessary and re-admits gross outliers, so it is
        off by default (enable it for a known-hard initialization).
    multi_start : bool
        Model-aware multi-start auto-init (default ``True``): screen the base seed plus
        ``n_restarts`` seeds with the **shape** parameters dispersed across their bounds, keep
        the one with the lowest robust (median) reprojection, then refine it fully. This is what
        makes a *poor* ``init_model`` (only the type + a rough focal) converge — it rescues
        wrong-basin shape seeds (the DS ``ξ`` fold, etc.) and is a no-op when the base seed is
        already good. Determinism is preserved via ``seed``. Applies to **every** model,
        including RadTan/KB/OCam: an earlier version of this code skipped dispersion for models
        with no *documented* degenerate basin on the theory that a plain distortion tail is
        well-conditioned — true on rich, real data, measurably **false** at scale on thinner
        synthetic data (56/100 RadTan cameras with ~2x the reprojection error in one real test —
        see :func:`_shape_seeds`'s docstring). Set ``multi_start=False`` only when you have
        independently verified it is safe for your data, not by model class alone.
    n_restarts : int
        Number of dispersed shape seeds for ``multi_start`` (default 4).
    force_multistart : bool
        Kept for API stability (default ``True``, matching the always-disperse default above);
        see :func:`_shape_seeds`.
    loss : str, optional
        SciPy-style loss name (``"linear"``, ``"huber"``, ``"soft_l1"``, ``"cauchy"``) for
        backward compatibility. Passing this (with or without ``f_scale``) reproduces the
        pre-robust-default behaviour: fixed scale, GNC off.
    f_scale : float, optional
        Fixed inlier scale (px) for the legacy ``loss``-based path, in place of the modern
        ``robust``/``robust_scale`` auto-scaling.

    Returns
    -------
    dict
        The refined ``model``, per-image ``poses`` as ``(rvec, tvec)``, ``success``, ``n_obs``,
        and reprojection statistics over valid observations — ``rms_px``, ``mean_px``,
        ``median_px``, ``p95_px``, ``max_px`` (all in pixels, independent of the robust kernel
        so they stay comparable across configurations).
    """
    # Back-compat: an explicit loss/f_scale pins the legacy fixed-scale, no-GNC path.
    legacy = loss is not None or f_scale is not None
    if loss is not None:
        robust = _LOSS_TO_KERNEL.get(loss, loss)
    if legacy:
        gnc = False
        robust_scale = f_scale if f_scale is not None else 1.0

    cls = type(init_model)
    n_img = len(X_world_list)
    masks = [np.asarray(v, bool) for v in visibility_list]

    kernel = robust
    if kernel == "none":
        scale_arg, gnc_start, gnc_iters = 1.0, 0.0, 0
    else:
        scale_arg = robust_scale
        # Auto-scale GNC: start the kernel ~3x wide and anneal to the calibrated scale over
        # ~20% of the iteration budget (a few annealing steps, then the sharp robust fit).
        gnc_start, gnc_iters = (3.0, max(8, max_nfev // 5)) if gnc else (0.0, 0)

    fit_kw = dict(kernel=kernel, scale=scale_arg, gnc_start=gnc_start, gnc_iters=gnc_iters)

    # Model-aware multi-start auto-init: cheaply screen the base seed + shape-dispersed seeds
    # (a short BA each), score by the *robust* (median) reprojection so a wrong shape basin is
    # rejected, then run the full refine from the winning seed. With a single seed this reduces
    # to one full fit, so non-multistart behaviour is unchanged.
    seeds = _shape_seeds(cls, init_model.params, n_restarts, seed,
                         force=force_multistart) if multi_start else [
        np.asarray(init_model.params, float)]
    if len(seeds) > 1:
        screen_iter = max(15, max_nfev // 5)
        best_seed, best_score = seeds[0], float("inf")
        for p0 in seeds:
            pr, Rr, tr, _ = _seed_and_fit(cls, p0, X_world_list, keypoints_list,
                                          visibility_list, max_iter=screen_iter, **fit_kw)
            mr = cls.from_params(pr)
            poses_r = [(cv2.Rodrigues(Rr[i])[0].ravel(), tr[i]) for i in range(n_img)]
            er = _reproj_errors(mr, poses_r, X_world_list, keypoints_list, masks)
            score = float(np.median(er)) if er.size else float("inf")
            if score < best_score:
                best_seed, best_score = p0, score
    else:
        best_seed = seeds[0]

    params, Rb, t, out = _seed_and_fit(cls, best_seed, X_world_list, keypoints_list,
                                       visibility_list, max_iter=max_nfev, **fit_kw)
    model = cls.from_params(params)

    # Absolute (rvec, tvec) per image so downstream code (cv2.Rodrigues) is unaffected.
    poses = [(cv2.Rodrigues(Rb[i])[0].ravel(), np.asarray(t[i], float)) for i in range(n_img)]

    # True reprojection stats over valid observations — computed directly so they mean the
    # same thing under any kernel. mean/p95 expose flipped views that the median hides.
    errs = _reproj_errors(model, poses, X_world_list, keypoints_list, masks)
    if errs.size:
        stats = {
            "rms_px": float(np.sqrt(np.mean(errs ** 2))),
            "mean_px": float(np.mean(errs)),
            "median_px": float(np.median(errs)),
            "p95_px": float(np.percentile(errs, 95)),
            "max_px": float(np.max(errs)),
        }
    else:
        nan = float("nan")
        stats = {"rms_px": nan, "mean_px": nan, "median_px": nan, "p95_px": nan, "max_px": nan}

    return {"model": model, "poses": poses, "success": bool(out.success),
            "n_obs": int(errs.size), **stats}

estimate_relative_pose

estimate_relative_pose(poses_from: Sequence[Pose], poses_to: Sequence[Pose]) -> Dict

Estimate the rigid transform T_to_from between two cameras.

Given board poses observed by each camera on the same frames (poses_from[i] and poses_to[i] are the board seen at frame i by the "from" and "to" cameras), returns the transform that maps a point in the from camera's frame into the to camera's frame — e.g. pass cam0 then cam1 to get Kalibr's T_cn_cnm1 (= T_cam1_cam0).

Each frame yields one estimate T_to_board ∘ (T_from_board)^-1; the rotations are averaged on SO(3) (chordal / SVD projection) and the translation by the component-wise median (robust to per-frame PnP noise).

Returns {T, R, t, n, rot_rms_deg, t_std_mm} where T is 4x4 and rot_rms_deg / t_std_mm report how consistent the per-frame estimates are.

Source code in ds_msp/calib/stereo.py

def estimate_relative_pose(poses_from: Sequence[Pose],
                           poses_to: Sequence[Pose]) -> Dict:
    """Estimate the rigid transform ``T_to_from`` between two cameras.

    Given board poses observed by each camera on the **same frames**
    (``poses_from[i]`` and ``poses_to[i]`` are the board seen at frame ``i`` by the
    "from" and "to" cameras), returns the transform that maps a point in the *from*
    camera's frame into the *to* camera's frame — e.g. pass cam0 then cam1 to get
    Kalibr's ``T_cn_cnm1`` (= ``T_cam1_cam0``).

    Each frame yields one estimate ``T_to_board ∘ (T_from_board)^-1``; the rotations
    are averaged on SO(3) (chordal / SVD projection) and the translation by the
    component-wise median (robust to per-frame PnP noise).

    Returns ``{T, R, t, n, rot_rms_deg, t_std_mm}`` where ``T`` is 4x4 and
    ``rot_rms_deg`` / ``t_std_mm`` report how consistent the per-frame estimates are.
    """
    if len(poses_from) != len(poses_to) or not poses_from:
        raise ValueError("poses_from and poses_to must be non-empty and the same length")

    rots: List[np.ndarray] = []
    trans: List[np.ndarray] = []
    for (rvf, tvf), (rvt, tvt) in zip(poses_from, poses_to):
        Rf, _ = cv2.Rodrigues(np.asarray(rvf, dtype=np.float64))
        Rt, _ = cv2.Rodrigues(np.asarray(rvt, dtype=np.float64))
        R = Rt @ Rf.T
        t = np.asarray(tvt, dtype=np.float64).ravel() - R @ np.asarray(tvf, dtype=np.float64).ravel()
        rots.append(R)
        trans.append(t)
    trans = np.asarray(trans)

    # rotation: average the matrices, then project back onto SO(3) via SVD
    U, _, Vt = np.linalg.svd(np.mean(rots, axis=0))
    R = U @ Vt
    if np.linalg.det(R) < 0:
        U = U.copy()
        U[:, -1] *= -1.0
        R = U @ Vt
    t = np.median(trans, axis=0)

    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t

    rot_rms = float(np.sqrt(np.mean([_geodesic_deg(R, Ri) ** 2 for Ri in rots])))
    return {
        "T": T, "R": R, "t": t, "n": len(rots),
        "rot_rms_deg": rot_rms,
        "t_std_mm": np.std(trans, axis=0) * 1000.0,
    }

relative_pose_error

relative_pose_error(T_a: ndarray, T_b: ndarray) -> Dict

Compare two rigid transforms: rotation angle (deg) and translation error (mm).

Source code in ds_msp/calib/stereo.py

def relative_pose_error(T_a: np.ndarray, T_b: np.ndarray) -> Dict:
    """Compare two rigid transforms: rotation angle (deg) and translation error (mm)."""
    T_a, T_b = np.asarray(T_a, float), np.asarray(T_b, float)
    return {
        "rot_deg": _geodesic_deg(T_a[:3, :3], T_b[:3, :3]),
        "trans_mm": float(np.linalg.norm(T_a[:3, 3] - T_b[:3, 3]) * 1000.0),
    }