ds_msp.rig

Multi-camera rig calibration — the N-camera analogue of MC-Calib, extending single-camera intrinsics (ds_msp.calib) and two-view geometry (ds_msp.mvg) to a full rig. See the rig calibration guide for the CLI-driven workflow.

ds_msp.rig

Multi-camera rig calibration — the N-camera analogue of MC-Calib.

DS-MSP's single-camera, multi-model intrinsics (calib.bundle) and two-view MVG (mvg) are extended here to a full rig: many cameras + many planar boards observed over many frames, fused into one consistent set of extrinsics + intrinsics.

The math is model-agnostic — every routine composes poses and calls a CameraModel's project / unproject / project_jacobian. So the entire pipeline works for any of DS-MSP's camera models, exactly as it is in MC-Calib.

BoardObs dataclass

One planar board seen by one camera in one frame (cf. BoardObs.cpp).

Source code in ds_msp/data/observations.py

@dataclass
class BoardObs:
    """One planar board seen by one camera in one frame (cf. ``BoardObs.cpp``)."""

    cam_id: int
    frame_id: int
    board_id: int
    corner_ids: np.ndarray      # (K,) int  — board-local corner ids that were detected
    pts_2d: np.ndarray          # (K, 2)     — detected pixels
    T_c_b: Optional[np.ndarray] = None   # (4,4) board->camera from robust PnP
    valid: bool = True          # False if PnP inliers < 4 (BoardObs.cpp:149)
    image_path: Optional[str] = None     # source image this was detected in (for overlays)

Object3D dataclass

Several planar boards fused into one rigid 3D point cloud (cf. Object3D.cpp).

Source code in ds_msp/data/observations.py

@dataclass
class Object3D:
    """Several planar boards fused into one rigid 3D point cloud (cf. ``Object3D.cpp``)."""

    object_id: int
    board_ids: List[int]
    ref_board_id: int                                   # min(board_ids) (McCalib.cpp:898)
    T_co_b: Dict[int, np.ndarray]                       # board_id -> (4,4) board->object
    pts_3d: np.ndarray                                  # (P, 3) all corners in object frame
    pts_obj_2_board: np.ndarray                         # (P, 2) [board_id, corner_id]
    pts_board_2_obj: Dict[Tuple[int, int], int]         # (board_id, corner_id) -> row

    def row_of(self, board_id: int, corner_id: int) -> int:
        """Look up the row into :attr:`pts_3d` for a ``(board_id, corner_id)`` pair.

        Parameters
        ----------
        board_id : int
            One of :attr:`board_ids`.
        corner_id : int
            Board-local corner id (as detected by the board's own numbering).

        Returns
        -------
        int
            Row index into :attr:`pts_3d` / :attr:`pts_obj_2_board`.

        Raises
        ------
        KeyError
            If ``(board_id, corner_id)`` was never fused into this object.
        """
        return self.pts_board_2_obj[(int(board_id), int(corner_id))]

row_of

row_of(board_id: int, corner_id: int) -> int

Look up the row into :attr:pts_3d for a (board_id, corner_id) pair.

Parameters:

Name	Type	Description	Default
board_id	int	One of :attr:board_ids.	required
corner_id	int	Board-local corner id (as detected by the board's own numbering).	required

Returns:

Type	Description
int	Row index into :attr:pts_3d / :attr:pts_obj_2_board.

Raises:

Type	Description
KeyError	If (board_id, corner_id) was never fused into this object.

Source code in ds_msp/data/observations.py

def row_of(self, board_id: int, corner_id: int) -> int:
    """Look up the row into :attr:`pts_3d` for a ``(board_id, corner_id)`` pair.

    Parameters
    ----------
    board_id : int
        One of :attr:`board_ids`.
    corner_id : int
        Board-local corner id (as detected by the board's own numbering).

    Returns
    -------
    int
        Row index into :attr:`pts_3d` / :attr:`pts_obj_2_board`.

    Raises
    ------
    KeyError
        If ``(board_id, corner_id)`` was never fused into this object.
    """
    return self.pts_board_2_obj[(int(board_id), int(corner_id))]

ObjectObs dataclass

One fused object seen by one camera in one frame (cf. Object3DObs.cpp).

Source code in ds_msp/data/observations.py

@dataclass
class ObjectObs:
    """One fused object seen by one camera in one frame (cf. ``Object3DObs.cpp``)."""

    cam_id: int
    frame_id: int
    object_id: int
    point_rows: np.ndarray      # (K,) int  — rows into Object3D.pts_3d
    pts_2d: np.ndarray          # (K, 2)
    T_c_o: Optional[np.ndarray] = None   # (4,4) object->camera from robust PnP
    image_path: Optional[str] = None     # source image this was detected in (for overlays)

RigState dataclass

The optimization variable mutated by the staged global BA (rig.bundle).

Source code in ds_msp/data/observations.py

@dataclass
class RigState:
    """The optimization variable mutated by the staged global BA (``rig.bundle``)."""

    cameras: Dict[int, CameraModel]                     # per-camera intrinsics
    T_c_g: Dict[int, np.ndarray]                        # camera-in-group; ref cam = identity
    ref_cam_id: int
    object_poses: Dict[Tuple[int, int], np.ndarray]     # (object_id, frame_id) -> T_g_o
    objects: Dict[int, Object3D]                        # holds T_co_b board poses
    img_size: Dict[int, Tuple[int, int]] = field(default_factory=dict)  # cam_id -> (w, h)

calibrate_rig

calibrate_rig(obj: Object3D, object_obs: List[ObjectObs], img_size: Dict[int, Tuple[int, int]], *, fix_intrinsics: bool = False, verbose: bool = False, front_end: Optional[Callable] = None, he_approach: int = 0, refine_structure: bool = False, structure_rounds: int = 6, gnc_iters: int = 5, gnc_start: float = 4.0, noise_bound: Optional[float] = None, on_iter=None, objects: Optional[List[Object3D]] = None, reproj_gate_px: Optional[float] = None) -> RigState

Calibrate a multi-camera rig from fused-object observations.

Returns a :class:RigState with per-camera intrinsics, T_c_g extrinsics (reference camera = identity), and per-frame object poses.

noise_bound (per-corner reprojection σ in pixels) makes the global joint BA run the median-free GNC-TLS solver (barc = 3.03·σ inlier band), which rejects gross-outlier corners past the 50% breakdown of the MAD auto-scale; the cheaper stages (group refine, structure polish) stay on the legacy reweighting so the cost stays bounded. The per-frame pose seed (:func:_gated_pnp) always uses a high-breakdown RANSAC PnP. This low-level primitive defaults to None (legacy path); the config-driven product entry :func:~ds_msp.rig.calib_param.calibrate_from_config supplies a noise_bound (config default 1.0 px) so a real calibration is robust by default.

on_iter(it, max_iter, rms, cost, rig_snapshot) — optional live-progress callback, forwarded to every stage's underlying solver (:func:bundle.refine / core.optimize.lm_solve / schur_lm); fires once per solver iteration across every stage in sequence.

Source code in ds_msp/rig/calibrate.py

def calibrate_rig(obj: Object3D, object_obs: List[ObjectObs],
                  img_size: Dict[int, Tuple[int, int]],
                  *, fix_intrinsics: bool = False, verbose: bool = False,
                  front_end: Optional[Callable] = None, he_approach: int = 0,
                  refine_structure: bool = False, structure_rounds: int = 6,
                  gnc_iters: int = 5, gnc_start: float = 4.0,
                  noise_bound: Optional[float] = None, on_iter=None,
                  objects: Optional[List[Object3D]] = None,
                  reproj_gate_px: Optional[float] = None) -> RigState:
    """Calibrate a multi-camera rig from fused-object observations.

    Returns a :class:`RigState` with per-camera intrinsics, ``T_c_g`` extrinsics
    (reference camera = identity), and per-frame object poses.

    ``noise_bound`` (per-corner reprojection σ in pixels) makes the **global joint** BA run the
    median-free **GNC-TLS** solver (``barc = 3.03·σ`` inlier band), which rejects gross-outlier
    corners past the 50% breakdown of the MAD auto-scale; the cheaper stages (group refine,
    structure polish) stay on the legacy reweighting so the cost stays bounded. The per-frame
    pose seed (:func:`_gated_pnp`) always uses a high-breakdown RANSAC PnP. This low-level
    primitive defaults to ``None`` (legacy path); the config-driven product entry
    :func:`~ds_msp.rig.calib_param.calibrate_from_config` supplies a ``noise_bound`` (config
    default ``1.0`` px) so a real calibration is robust by default.

    ``on_iter(it, max_iter, rms, cost, rig_snapshot)`` — optional live-progress callback,
    forwarded to every stage's underlying solver (:func:`bundle.refine` / ``core.optimize.lm_solve`` /
    ``schur_lm``); fires once per solver iteration across every stage in sequence.
    """
    obs_by_cam: Dict[int, List[ObjectObs]] = defaultdict(list)
    for o in object_obs:
        obs_by_cam[o.cam_id].append(o)
    cam_ids = sorted(obs_by_cam)

    # Multi-object rig: ``objects`` carries every covisibility component (MC-Calib keeps them
    # all instead of dropping the non-co-observed boards). Default to the single fused object so
    # every existing single-object caller is unchanged. The object map drives the object-aware
    # front-end and, below, the merge stage that fuses non-co-observed objects into one.
    if objects is None:
        objects = [obj]
    objects_by_id = {o.object_id: o for o in objects}

    # 1. per-camera intrinsics + object poses (T_c_o). Default to the robust from-scratch
    #    front-end (RANSAC-DLT seed + per-model Cauchy refine), not the plain-L2
    #    cv2.calibrateCamera path (`_front_end_opencv`), which collapses the focal under
    #    gross outliers — so a direct calibrate_rig caller gets the robust behaviour the
    #    high-level entry points already use, without having to know to pass front_end.
    #    This has no on_iter callback of its own (multi-start seed dispersion runs 2 full
    #    per-camera solves each -- NFR-PERF-001 -- measured ~19s on an 8-camera real rig) so
    #    a live view would otherwise sit frozen on the previous stage's label for that whole
    #    stretch; set_stage still fires here even without per-iteration progress inside it.
    if on_iter is not None and hasattr(on_iter, "set_stage"):
        on_iter.set_stage("(0) per-camera front-end intrinsics")
    fe = front_end or make_bundle_front_end(RadTanModel)
    cameras = fe(objects_by_id, obs_by_cam, img_size)
    if verbose:
        print(f"[front-end] calibrated {len(cameras)} cameras")

    # 2. camera-group covisibility -> extrinsics init (T_c_g; ref cam = identity)
    groups, extr = init_camera_groups(object_obs, cam_ids)
    if len(groups) > 1:
        # Non-overlapping groups: link with hand-eye, then re-base every camera to the
        # global reference (group 0's reference camera).
        from .handeye import link_groups
        extr = link_groups(groups, extr, object_obs, he_approach=he_approach)

    # 2b. Multi-object merge (MC-Calib merge3DObjects): fuse rigidly-linked objects that are
    #     never co-observed at the board level (each camera sees a different board) into ONE
    #     rigid object, using the extrinsics just recovered; re-seed poses and re-group. This
    #     reduces the non-overlapping multi-object rig to the ordinary single-object case, so
    #     everything below (staged BA, structure refine, IO) is unchanged. See
    #     docs/RIG_MULTIOBJECT_IMPLEMENTATION_PLAN.md.
    if len(objects) > 1:
        objects, objects_by_id, groups, extr = _merge_and_relink(
            objects, object_obs, cameras, cam_ids, he_approach, verbose)
        # The live-view animator (if any) cached the PRE-merge object's pts_3d/point_rows via
        # bind_scene() before this stage ran (calib_param.py/cli.py bind it right after
        # detection); a merge changes both, so every on_iter callback from here on would index
        # a stale, smaller point cloud. Re-bind to the fused object + relabeled obs.
        if on_iter is not None and hasattr(on_iter, "bind_scene"):
            on_iter.bind_scene(objects_by_id.get(0, objects[0]), object_obs)
    obj = objects_by_id.get(0, objects[0])           # primary (largest) object
    if verbose:
        print(f"[groups] {len(groups)} group(s): {groups}")
    ref_cam = groups[0][0]

    # 3. per-frame object-in-group poses (average over the cameras that see it)
    by_fo: Dict[Tuple[int, int], List[Tuple[int, np.ndarray]]] = defaultdict(list)
    for o in object_obs:
        if o.T_c_o is not None and o.cam_id in extr:
            by_fo[(o.object_id, o.frame_id)].append((o.cam_id, o.T_c_o))
    object_poses: Dict[Tuple[int, int], np.ndarray] = {}
    for key, lst in by_fo.items():
        object_poses[key] = average_object_pose_in_group(lst, extr, ref_cam)

    rig = RigState(cameras=cameras, T_c_g=extr, ref_cam_id=ref_cam,
                   object_poses=object_poses, objects=objects_by_id, img_size=img_size)

    # 4. hierarchical refinement, MC-Calib's staged structure, every stage an analytic-
    #    Jacobian BA (no autodiff), robust IRLS weighting (no rejection):
    #    (a) per-object — refine each frame's object pose with cameras+intrinsics fixed
    #        (estimatePoseAllObjects / computeAllObjPoseInCameraGroup): a metric BA warm-up
    #        of the closed-form averaged object poses before any extrinsic moves.
    if on_iter is not None and hasattr(on_iter, "set_stage"):
        on_iter.set_stage("(a) per-object pose warm-up")
    rig = bundle.refine(rig, object_obs, fix_intrinsics=True, fix_extrinsics=True,
                    robust_kernel="huber", robust_scale="auto", verbose=verbose,
                    on_iter=on_iter)
    #    (b) per-camera-group — refine each group's extrinsics + its object poses, intrinsics
    #        fixed (calibrateCameraGroup / refineAllCameraGroupAndObjects). Single group ->
    #        whole-rig poses-only; multiple groups -> each independently before the joint pass.
    if on_iter is not None and hasattr(on_iter, "set_stage"):
        on_iter.set_stage("(b) per-camera-group extrinsics" if len(groups) > 1
                           else "(b) rig extrinsics")
    rig = bundle.refine_groups(rig, object_obs, groups,
                           robust_kernel="huber", robust_scale="auto", verbose=verbose,
                           on_iter=on_iter)
    #    (c) global joint — full rig + (optionally) intrinsics with a redescending Cauchy
    #        kernel and a short GNC anneal (refineAllCameraGroupAndObjectsAndIntrinsics).
    # Graduated non-convexity: anneal the robust scale from ``gnc_start``×MAD down. A wider
    # start + more steps escapes the bad-data basin under heavy (≈50%) gross-outlier
    # contamination, where a MAD scale is itself corrupted — measurably more robust at the
    # breakdown point with negligible cost on clean data.
    if on_iter is not None and hasattr(on_iter, "set_stage"):
        on_iter.set_stage("(c) global joint BA" + (" + intrinsics" if fix_intrinsics is False
                                                     else ""))
    rig = bundle.refine(rig, object_obs, fix_intrinsics=fix_intrinsics, robust_kernel="cauchy",
                    robust_scale="auto", gnc_iters=gnc_iters, gnc_start=gnc_start,
                    noise_bound=noise_bound, verbose=verbose, on_iter=on_iter)

    #    (c.5) gross-outlier rejection (MC-Calib's ``ransac_threshold``). The robust BA above
    #        recovers the correct rig even with mis-detections, but a *self-consistent* blunder —
    #        e.g. a ChArUco board a different OpenCV build mis-decodes (wrong corner ids -> a
    #        plausible-but-wrong pose, low per-board RMS) — stays in the object set and inflates
    #        every reported metric while contributing nothing true. It only reveals itself once
    #        composed with the assembled rig, so gate on the rig reprojection, hard-drop, and
    #        re-solve. Down-weighting alone cannot: ``per_observation_errors`` still counts it,
    #        so a correct calibration reads as a 25px failure. Off unless ``reproj_gate_px`` set
    #        (the config-driven entry passes ``ransac_threshold``).
    if reproj_gate_px and reproj_gate_px > 0:
        kept, ndrop = _reject_outlier_observations(rig, object_obs, reproj_gate_px)
        if ndrop:
            object_obs[:] = kept                         # clean the caller's set (metrics/save)
            live = {(o.object_id, o.frame_id) for o in object_obs}
            rig.object_poses = {k: v for k, v in rig.object_poses.items() if k in live}
            if verbose:
                print(f"[outliers] dropped {ndrop} board observation(s) reprojecting > "
                      f"{reproj_gate_px:.1f}px through the rig (mis-detections); re-solving")
            rig = bundle.refine(rig, object_obs, fix_intrinsics=fix_intrinsics,
                            robust_kernel="cauchy", robust_scale="auto", gnc_iters=gnc_iters,
                            gnc_start=gnc_start, noise_bound=noise_bound, verbose=verbose,
                            on_iter=on_iter)

    #    (d) object-structure refinement (MC-Calib's refineObject), multi-board only: alternate
    #        re-triangulating the fused object's 3-D points (gauge anchored on the reference
    #        board) with the joint BA. A reconstructed nominal object carries inter-board pose
    #        and physical-board error that no camera/pose update can absorb; freeing the
    #        structure removes it — the dominant reprojection win on a real multi-board target.
    if refine_structure and len(obj.board_ids) > 1:
        prev = float("inf")
        for round_i in range(structure_rounds):
            rig = bundle.refine_object_structure(rig, object_obs)
            # a light joint polish each round (no GNC, capped iters): the structure step did
            # the heavy lifting, so the BA only has to absorb it — full-length GNC passes here
            # were ~80% of total runtime for <0.5% extra accuracy.
            if on_iter is not None and hasattr(on_iter, "set_stage"):
                on_iter.set_stage(f"(d) structure refinement {round_i + 1}/{structure_rounds}")
            rig = bundle.refine(rig, object_obs, fix_intrinsics=fix_intrinsics,
                            robust_kernel="cauchy", robust_scale="auto", max_iter=25,
                            verbose=verbose, on_iter=on_iter)
            cur = max(bundle.reprojection_rms(rig, object_obs).values())
            if prev - cur < 0.003 * prev:           # converged -> stop early
                break
            prev = cur
    return rig

load_camera

load_camera(path: str, cam_id: int)

Read one camera's calibrated intrinsics back into a ready CameraModel instance -- the ds_msp.io.mccalib analogue of :func:ds_msp.io.kalibr.load_kalibr.

Needs camera_model (or the legacy distortion_type/disto_type int) to be present in the file to know which model to reconstruct -- see :func:_camera_model_field. Unlike Kalibr's single-camera camchain, one MC-Calib file holds every camera in the rig, so this takes the 0-based cam_id (matching camera_<cam_id> in the file, the same indexing :func:save_mccalib_cameras writes).

Source code in ds_msp/io/mccalib.py

def load_camera(path: str, cam_id: int):
    """Read one camera's calibrated intrinsics back into a ready ``CameraModel`` instance --
    the ``ds_msp.io.mccalib`` analogue of :func:`ds_msp.io.kalibr.load_kalibr`.

    Needs ``camera_model`` (or the legacy ``distortion_type``/``disto_type`` int) to be
    present in the file to know which model to reconstruct -- see :func:`_camera_model_field`.
    Unlike Kalibr's single-camera camchain, one MC-Calib file holds every camera in the rig,
    so this takes the 0-based ``cam_id`` (matching ``camera_<cam_id>`` in the file, the same
    indexing :func:`save_mccalib_cameras` writes)."""
    from ..models.registry import model_class
    fs = cv2.FileStorage(str(path), cv2.FILE_STORAGE_READ)
    cn = fs.getNode(f"camera_{cam_id}")
    if cn.empty():
        fs.release()
        raise KeyError(f"no camera_{cam_id} in {path}")
    name = _camera_model_field(cn)
    K = cn.getNode("camera_matrix").mat()
    dist_node = cn.getNode("distortion_vector")
    dist = dist_node.mat() if not dist_node.empty() else None
    fs.release()
    if name is None:
        raise ValueError(f"camera_{cam_id} in {path} states no camera_model/distortion_type "
                         f"-- cannot tell which model to reconstruct")
    layout = _DISTORTION_LAYOUT[name]
    dist = np.asarray(dist, dtype=float).ravel() if dist is not None else np.zeros(0)
    if len(dist) != len(layout):
        raise ValueError(f"camera_{cam_id}: model {name!r} expects {len(layout)} distortion "
                         f"values {layout}, file has {len(dist)}")
    kwargs = dict(zip(layout, dist.tolist()))
    if name == "ocam":
        kwargs["cx"], kwargs["cy"] = float(K[0, 2]), float(K[1, 2])
    else:
        kwargs.update(fx=float(K[0, 0]), fy=float(K[1, 1]), cx=float(K[0, 2]), cy=float(K[1, 2]))
    return model_class(name)(**kwargs)