Skip to content

bundle

bundle #

bundle_adjust — cameras and points refined together until the reprojections agree.

  1. Parameters. Each camera is a Rodrigues rotation and a translation (world-to-camera), plus a focal length and principal point when intrinsics are refined; each point is its three coordinates.
  2. Residuals. Every observation contributes its reprojection error in pixels, under a pseudo-Huber loss so a wrong match cannot drag the rest (the smooth loss converges where Huber's knee stalls the solver). With a position sigma, every camera also contributes (centre − prior) / sigma, its prior being where it stood on entry; without one, the first camera is held fixed so the model cannot drift as a whole.
  3. Solve. SciPy's trust-region least squares, told which residual depends on which parameter, so the Jacobian is sparse. adjust is that solver over plain arrays, which the incremental reconstruction reuses.

ROBUST_SCALE_PX = 2.0 module-attribute #

Residuals beyond this many pixels count roughly linearly, not quadratically.

adjust(cameras, points, camera, point, pixels, fixed, priors, sigma, refine_intrinsics) #

The cameras and points that best reproject pixels.

Parameters:

Name Type Description Default
cameras Cameras

The cameras to start from.

required
points ndarray

The N×3 points to start from.

required
camera ndarray

The camera of each observation.

required
point ndarray

The point of each observation.

required
pixels ndarray

The M×2 pixel of each observation.

required
fixed ndarray

Per camera, whether its pose is held; intrinsics being refined move regardless.

required
priors ndarray | None

C×3 centres the cameras are drawn to, or None.

required
sigma float | None

The metres one prior residual unit spans; required with priors.

required
refine_intrinsics bool

Whether focal lengths and principal points move.

required

Returns:

Type Description
Cameras

(cameras, points, stats): the refined cameras and points, and the

ndarray

numbers a log line reports — reprojection before and after in

dict[str, float | int]

pixels, the mean and largest camera shift in metres, iterations.

Source code in capturegraph-recipes/capturegraph_recipes/sfm/reconstruction/bundle.py
def adjust(
    cameras: Cameras,
    points: np.ndarray,
    camera: np.ndarray,
    point: np.ndarray,
    pixels: np.ndarray,
    fixed: np.ndarray,
    priors: np.ndarray | None,
    sigma: float | None,
    refine_intrinsics: bool,
) -> tuple[Cameras, np.ndarray, dict[str, float | int]]:
    """The cameras and points that best reproject ``pixels``.

    Args:
        cameras: The cameras to start from.
        points: The N×3 points to start from.
        camera: The camera of each observation.
        point: The point of each observation.
        pixels: The M×2 pixel of each observation.
        fixed: Per camera, whether its pose is held; intrinsics being refined
            move regardless.
        priors: C×3 centres the cameras are drawn to, or ``None``.
        sigma: The metres one prior residual unit spans; required with ``priors``.
        refine_intrinsics: Whether focal lengths and principal points move.

    Returns:
        ``(cameras, points, stats)``: the refined cameras and points, and the
        numbers a log line reports — reprojection before and after in
        pixels, the mean and largest camera shift in metres, iterations.
    """
    width = 9 if refine_intrinsics else 6
    aspect = cameras.calibrations[:, 1, 1] / cameras.calibrations[:, 0, 0]
    block = np.zeros((len(cameras), width))
    block[:, :3] = rotation_vectors(cameras.rotations)
    block[:, 3:6] = cameras.translations
    if refine_intrinsics:
        block[:, 6] = cameras.calibrations[:, 0, 0]
        block[:, 7:9] = cameras.calibrations[:, 0:2, 2]
    base = np.concatenate([block.ravel(), points.ravel()])
    free = np.ones(len(base), dtype=bool)
    free[: block.size] = np.repeat(~fixed, width)
    free[: block.size].reshape(-1, width)[:, 6:] = True

    def unpack(values: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
        full = base.copy()
        full[free] = values
        return full[: block.size].reshape(-1, width), full[block.size :].reshape(-1, 3)

    def reprojection(block: np.ndarray, points: np.ndarray) -> np.ndarray:
        local = rotated(block[camera, :3], points[point]) + block[camera, 3:6]
        if refine_intrinsics:
            focal_x, centre = block[camera, 6], block[camera, 7:9]
        else:
            focal_x = cameras.calibrations[camera, 0, 0]
            centre = cameras.calibrations[camera, 0:2, 2]
        focal = np.stack([focal_x, focal_x * aspect[camera]], axis=1)
        return focal * local[:, :2] / local[:, 2:3] + centre - pixels

    def residuals(values: np.ndarray) -> np.ndarray:
        block, points = unpack(values)
        errors = reprojection(block, points).ravel()
        if priors is None or sigma is None:
            return errors
        centres = -rotated(-block[:, :3], block[:, 3:6])
        return np.concatenate([errors, ((centres - priors) / sigma).ravel()])

    before = float(np.linalg.norm(reprojection(*unpack(base[free])), axis=1).mean())
    result = least_squares(
        residuals,
        base[free],
        jac_sparsity=_sparsity(len(cameras), width, camera, point, free, priors is not None),
        method="trf",
        loss="soft_l1",
        f_scale=ROBUST_SCALE_PX,
        x_scale="jac",
        max_nfev=MAX_ITERATIONS,
    )
    block, points = unpack(result.x)
    refined = Cameras(
        rotation_matrices(block[:, :3]),
        block[:, 3:6],
        _calibrations(cameras, block, aspect) if refine_intrinsics else cameras.calibrations,
    )
    shifts = np.linalg.norm(refined.centres() - cameras.centres(), axis=1)
    return (
        refined,
        points,
        {
            "reprojection_before_px": before,
            "reprojection_after_px": float(
                np.linalg.norm(reprojection(block, points), axis=1).mean()
            ),
            "mean_shift_meters": float(shifts.mean()) if len(shifts) else 0.0,
            "max_shift_meters": float(shifts.max()) if len(shifts) else 0.0,
            "iterations": int(result.nfev),
        },
    )

bundle_adjust(frames, model, position_sigma_meters=None, refine_intrinsics=False) #

model with its cameras and points moved to minimise reprojection error.

The frames supply the images whose keypoints the observations index; their poses play no part — the model's cameras are the starting point, and the priors when a sigma is given.

Parameters:

Name Type Description Default
frames Array[PosedImage]

The posed images the model was built from.

required
model Reconstruction

The reconstruction to refine.

required
position_sigma_meters float | None

How far a camera centre may plausibly move from where the model has it; None trusts the images alone and holds the first camera fixed instead.

None
refine_intrinsics bool

Whether each camera's focal length and principal point are solved for too.

False
Source code in capturegraph-recipes/capturegraph_recipes/sfm/reconstruction/bundle.py
@cg.pure_function("sfm.bundle_adjust/1")
def bundle_adjust(
    frames: cg.Array[PosedImage],
    model: Reconstruction,
    position_sigma_meters: float | None = None,
    refine_intrinsics: bool = False,
) -> Reconstruction:
    """``model`` with its cameras and points moved to minimise reprojection error.

    The frames supply the images whose ``keypoints`` the observations index;
    their poses play no part — the model's cameras are the starting point,
    and the priors when a sigma is given.

    Args:
        frames: The posed images the model was built from.
        model: The reconstruction to refine.
        position_sigma_meters: How far a camera centre may plausibly move
            from where the model has it; ``None`` trusts the images alone
            and holds the first camera fixed instead.
        refine_intrinsics: Whether each camera's focal length and principal
            point are solved for too.
    """
    indices = np.array(sorted(camera_index(key) for key in model.cameras.keys()))
    keys = [camera_key(index) for index in indices.tolist()]
    poses = [model.cameras[key] for key in keys]
    pixels: list[np.ndarray] = []
    for index in cg.logging.progress(indices.tolist(), label="Reading keypoints"):
        pixels.append(keypoints(frames[index].image).array("xy"))
    offsets = np.cumsum([0, *(len(found) for found in pixels)])
    observed = model.observations.arrays()
    camera = np.searchsorted(indices, observed["image"])
    positions, colors = model.points.points()
    cameras = Cameras.of(poses)
    fixed = np.zeros(len(poses), dtype=bool)
    if position_sigma_meters is None:
        fixed[:1] = True
    adjusted, points, stats = adjust(
        cameras,
        positions.astype(np.float64),
        camera,
        observed["point"],
        np.concatenate(pixels)[offsets[camera] + observed["keypoint"]],
        fixed,
        cameras.centres() if position_sigma_meters is not None else None,
        position_sigma_meters,
        refine_intrinsics,
    )
    cg.logging.info(
        "Adjusted",
        reprojection_before_px=stats["reprojection_before_px"],
        reprojection_after_px=stats["reprojection_after_px"],
        mean_shift_meters=stats["mean_shift_meters"],
        max_shift_meters=stats["max_shift_meters"],
        iterations=stats["iterations"],
    )
    return Reconstruction(
        cameras=cg.Map[cg.String, cg.Pose](
            {
                key: pose_from_extrinsics(
                    adjusted.rotations[local],
                    adjusted.translations[local],
                    pose,
                    adjusted.calibrations[local] if refine_intrinsics else None,
                )
                for local, (key, pose) in enumerate(zip(keys, poses, strict=True))
            }
        ),
        points=cg.PointCloud.of_points(points.astype(np.float32), colors),
        observations=model.observations,
    )