Skip to content

ds_msp.vo

Monocular visual odometry — composes the bearing-vector two-view stack (ds_msp.mvg) into a trajectory estimator, plus Sim(3) alignment and ATE/RPE evaluation metrics.

ds_msp.vo

Monocular visual odometry (Tier 2).

Composes the Tier-1 bearing-vector stack (ds_msp.mvg) into a trajectory estimator, plus the standard evaluation metrics (Sim(3) alignment, ATE, RPE) to report it.

VOResult dataclass

Output of :func:estimate_trajectory.

Source code in ds_msp/vo/odometry.py 
35
36
37
38
39
40
41
42
43
44
@dataclass
class VOResult:
    """Output of :func:`estimate_trajectory`."""
    poses: np.ndarray                       # (N, 4, 4) camera-to-world per frame
    landmarks: Dict[int, np.ndarray] = field(default_factory=dict)  # id -> world xyz

    @property
    def centers(self) -> np.ndarray:
        """(N, 3) camera centres in world frame."""
        return self.poses[:, :3, 3].copy()

centers property 

centers: ndarray

(N, 3) camera centres in world frame.

align_sim3 

align_sim3(src: ndarray, dst: ndarray, *, with_scale: bool = True) -> Tuple[float, np.ndarray, np.ndarray]

Closed-form similarity that best maps src onto dst (Umeyama 1991).

Finds s, R, t minimising Σ ‖dst_i − (s·R·src_i + t)‖² for point sets src, dst of shape (N, 3).

Returns (s, R, t) with s scalar scale, R (3, 3) rotation, t (3,). With with_scale=False the scale is fixed to 1 (rigid SE(3) alignment).

Source code in ds_msp/vo/metrics.py 
25
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
def align_sim3(src: np.ndarray, dst: np.ndarray, *, with_scale: bool = True
               ) -> Tuple[float, np.ndarray, np.ndarray]:
    """Closed-form similarity that best maps ``src`` onto ``dst`` (Umeyama 1991).

    Finds ``s, R, t`` minimising ``Σ ‖dst_i − (s·R·src_i + t)‖²`` for point sets
    ``src, dst`` of shape ``(N, 3)``.

    Returns ``(s, R, t)`` with ``s`` scalar scale, ``R`` (3, 3) rotation, ``t`` (3,).
    With ``with_scale=False`` the scale is fixed to 1 (rigid SE(3) alignment).
    """
    src = np.asarray(src, dtype=np.float64)
    dst = np.asarray(dst, dtype=np.float64)
    if src.shape != dst.shape or src.ndim != 2 or src.shape[1] != 3:
        raise ValueError("src and dst must both be (N, 3) and equal-shaped")
    n = src.shape[0]

    mu_s = src.mean(axis=0)
    mu_d = dst.mean(axis=0)
    sc = src - mu_s
    dc = dst - mu_d

    cov = (dc.T @ sc) / n
    U, D, Vt = np.linalg.svd(cov)
    S = np.eye(3)
    if np.linalg.det(U) * np.linalg.det(Vt) < 0:
        S[2, 2] = -1.0
    R = U @ S @ Vt

    if with_scale:
        var_s = (sc ** 2).sum() / n
        s = float((D * np.diag(S)).sum() / var_s) if var_s > 0 else 1.0
    else:
        s = 1.0
    t = mu_d - s * R @ mu_s
    return s, R, t

apply_sim3 

apply_sim3(s: float, R: ndarray, t: ndarray, pts: ndarray) -> np.ndarray

Apply a similarity s·R·pts + t to an (N, 3) point set.

Source code in ds_msp/vo/metrics.py 
62
63
64
def apply_sim3(s: float, R: np.ndarray, t: np.ndarray, pts: np.ndarray) -> np.ndarray:
    """Apply a similarity ``s·R·pts + t`` to an ``(N, 3)`` point set."""
    return (s * (np.asarray(pts, dtype=np.float64) @ np.asarray(R).T)) + np.asarray(t)

ate_rmse 

ate_rmse(est: ndarray, gt: ndarray, *, align: bool = True, with_scale: bool = True) -> float

Absolute Trajectory Error (RMSE of camera centres), Sim(3)-aligned by default.

est, gt are (N, 3) camera-centre trajectories in correspondence.

Source code in ds_msp/vo/metrics.py 
67
68
69
70
71
72
73
74
75
76
77
78
79
def ate_rmse(est: np.ndarray, gt: np.ndarray, *, align: bool = True,
             with_scale: bool = True) -> float:
    """Absolute Trajectory Error (RMSE of camera centres), Sim(3)-aligned by default.

    ``est, gt`` are ``(N, 3)`` camera-centre trajectories in correspondence.
    """
    est = np.asarray(est, dtype=np.float64)
    gt = np.asarray(gt, dtype=np.float64)
    if align:
        s, R, t = align_sim3(est, gt, with_scale=with_scale)
        est = apply_sim3(s, R, t, est)
    d = est - gt
    return float(np.sqrt((d ** 2).sum(axis=1).mean()))

estimate_trajectory 

estimate_trajectory(model, frames: Sequence[Frame], *, min_common: int = 8, threshold: float = 0.005, seed: int = 0) -> VOResult

Estimate a monocular camera trajectory from per-frame correspondences.

Parameters:

Name Type Description Default
model CameraModel

Any central DS-MSP model (its unproject lifts pixels to bearing rays).

 required
frames sequence of mapping

One {landmark_id: (u, v)} per frame; ids link the same 3D point across frames.

 required
min_common int

Minimum shared correspondences required between consecutive frames (≥ 8 for the eight-point estimator).

 8
threshold float

Forwarded to the robust two-view estimator (angular RANSAC threshold / RNG seed).

 0.005
seed float

Forwarded to the robust two-view estimator (angular RANSAC threshold / RNG seed).

 0.005

Returns:

Type Description
VOResult

poses (N, 4, 4) camera-to-world (frame 0 is the world origin, global scale 1), and the triangulated landmarks map.
Source code in ds_msp/vo/odometry.py 
 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
def estimate_trajectory(model, frames: Sequence[Frame], *, min_common: int = 8,
                        threshold: float = 0.005, seed: int = 0) -> VOResult:
    """Estimate a monocular camera trajectory from per-frame correspondences.

    Parameters
    ----------
    model : CameraModel
        Any central DS-MSP model (its ``unproject`` lifts pixels to bearing rays).
    frames : sequence of mapping
        One ``{landmark_id: (u, v)}`` per frame; ids link the same 3D point across frames.
    min_common : int
        Minimum shared correspondences required between consecutive frames (≥ 8 for the
        eight-point estimator).
    threshold, seed :
        Forwarded to the robust two-view estimator (angular RANSAC threshold / RNG seed).

    Returns
    -------
    VOResult
        ``poses`` (N, 4, 4) camera-to-world (frame 0 is the world origin, global scale 1),
        and the triangulated ``landmarks`` map.
    """
    n = len(frames)
    if n < 2:
        raise ValueError("need at least 2 frames")

    poses = [np.eye(4)]                      # T_wc[0] = identity (world = frame 0)
    landmarks: Dict[int, np.ndarray] = {}

    for k in range(n - 1):
        fa, fb = frames[k], frames[k + 1]
        common = sorted(set(fa) & set(fb))
        if len(common) < min_common:
            raise ValueError(
                f"frames {k}->{k + 1} share only {len(common)} correspondences "
                f"(need ≥ {min_common})"
            )
        px1 = np.array([fa[i] for i in common], dtype=np.float64)
        px2 = np.array([fb[i] for i in common], dtype=np.float64)
        f1, _ = model.unproject(px1)
        f2, _ = model.unproject(px2)

        # Relative pose (R, t): X_cam{k+1} = R · X_cam{k} + t, with ‖t‖ = 1.
        R, t, _, _ = estimate_relative_pose(f1, f2, threshold=threshold, seed=seed)
        # Re-triangulate the full common set (in camera-k frame) at unit translation scale.
        X_unit, d1, d2 = triangulate_rays(f1, f2, R, t)
        front = (d1 > 0) & (d2 > 0)

        # --- resolve the scale of this pair's unit translation ---
        T_wc_k = poses[k]
        T_cw_k = np.linalg.inv(T_wc_k)
        known = [(j, i) for j, i in enumerate(common)
                 if i in landmarks and front[j]]
        if known:
            # Existing landmarks brought into camera-k frame lie on the same rays as the
            # unit-scale triangulation → ratio of distances along the ray = the scale.
            idx = [j for j, _ in known]
            X_known_camk = _transform_points(T_cw_k, np.array([landmarks[i] for _, i in known]))
            num = np.linalg.norm(X_known_camk, axis=1)
            den = np.linalg.norm(X_unit[idx], axis=1)
            scale = float(np.median(num / np.maximum(den, 1e-12)))
        else:
            scale = 1.0                      # first pair fixes the global gauge

        # Chain the (scaled) relative pose: T_wc[k+1] = T_wc[k] · inv(T_{k+1<-k}).
        T_rel = _rel_transform(R, scale * t)
        T_wc_kp1 = T_wc_k @ np.linalg.inv(T_rel)
        poses.append(T_wc_kp1)

        # Add newly-seen landmarks (scaled, in world frame).
        X_world = _transform_points(T_wc_k, scale * X_unit)
        for j, i in enumerate(common):
            if front[j] and i not in landmarks:
                landmarks[i] = X_world[j]

    return VOResult(poses=np.stack(poses), landmarks=landmarks)

rpe_rmse 

rpe_rmse(est_poses: ndarray, gt_poses: ndarray, *, delta: int = 1) -> Tuple[float, float]

Relative Pose Error over a fixed step delta.

est_poses, gt_poses are (N, 4, 4) camera-to-world poses in correspondence. For each i the relative motion inv(P_i)·P_{i+delta} is compared between estimate and ground truth; the error E_i = inv(ΔGT)·ΔEST.

Returns (trans_rmse, rot_rmse_deg) — translational RMSE (same units as the poses) and rotational RMSE in degrees.

Source code in ds_msp/vo/metrics.py 
 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
def rpe_rmse(est_poses: np.ndarray, gt_poses: np.ndarray, *, delta: int = 1
             ) -> Tuple[float, float]:
    """Relative Pose Error over a fixed step ``delta``.

    ``est_poses, gt_poses`` are ``(N, 4, 4)`` camera-to-world poses in correspondence.
    For each ``i`` the relative motion ``inv(P_i)·P_{i+delta}`` is compared between
    estimate and ground truth; the error ``E_i = inv(ΔGT)·ΔEST``.

    Returns ``(trans_rmse, rot_rmse_deg)`` — translational RMSE (same units as the
    poses) and rotational RMSE in degrees.
    """
    est_poses = np.asarray(est_poses, dtype=np.float64)
    gt_poses = np.asarray(gt_poses, dtype=np.float64)
    n = len(est_poses)
    if n != len(gt_poses):
        raise ValueError("est_poses and gt_poses must have equal length")
    if delta < 1 or delta >= n:
        raise ValueError(f"delta must be in [1, {n - 1}], got {delta}")

    trans_sq, rot_sq = [], []
    for i in range(n - delta):
        d_est = np.linalg.inv(est_poses[i]) @ est_poses[i + delta]
        d_gt = np.linalg.inv(gt_poses[i]) @ gt_poses[i + delta]
        err = np.linalg.inv(d_gt) @ d_est
        trans_sq.append((err[:3, 3] ** 2).sum())
        cos = (np.trace(err[:3, :3]) - 1.0) / 2.0
        rot_sq.append(np.arccos(np.clip(cos, -1.0, 1.0)) ** 2)
    trans_rmse = float(np.sqrt(np.mean(trans_sq)))
    rot_rmse_deg = float(np.degrees(np.sqrt(np.mean(rot_sq))))
    return trans_rmse, rot_rmse_deg