In this lab you will run either: \( (i) \) a visual SLAM stack (mono/stereo/RGB-D), or \( (ii) \) a visual–inertial odometry/SLAM stack (VIO/VINS), on a standard dataset (e.g., EuRoC MAV / TUM-VI style logs), and evaluate the output trajectory.

Your deliverables:

• Reproducible run configuration: dataset path, sensor rates, camera intrinsics/extrinsics, IMU noise, and timing assumptions.
• Trajectory outputs: estimated pose stream in a common format (e.g., TUM: \( t, \mathbf{p}, \mathbf{q} \)).
• Quantitative metrics: ATE RMSE and RPE (translation + rotation), with interpretation of failure modes.

A run is considered “successful” if: \( (1) \) tracking does not diverge (no catastrophic drift), \( (2) \) loop closures (if enabled) reduce global drift, and \( (3) \) the final ATE RMSE is consistent with dataset difficulty and sensor modality.

The key to “lab-grade” SLAM/VIO is not running commands; it is controlling assumptions: camera model, frame conventions, sensor time bases, IMU noise, and gravity direction. A single mismatch here typically dominates error more than algorithm choice.

flowchart TD
  A["Select dataset + sequence"] --> B["Verify sensor streams: cam fps, imu rate, timestamps"]
  B --> C["Calibrate camera intrinsics + distortion model"]
  C --> D["Set cam-imu extrinsics (T_ci) + gravity axis convention"]
  D --> E["Set IMU noise + bias random walk (from spec or Allan)"]
  E --> F["Run stack (visual SLAM or VIO) with fixed config"]
  F --> G["Export trajectory to common format (TUM)"]
  G --> H["Evaluate ATE/RPE + inspect residuals + failure patterns"]
  H --> I["Tune: time offset, feature threshold, IMU noise, outlier gating"]
  I --> F
        

Minimum lab artifacts to record: dataset sequence name, commit/version of the stack, full configuration file(s), and the exact trajectory export used for evaluation.

Most stacks assume a pinhole camera (possibly with distortion). In normalized coordinates, with camera-frame point \( \mathbf{P}_c=(X,Y,Z)^\top \), the ideal projection is \( \pi(\mathbf{P}_c) = (X/Z,\; Y/Z)^\top \). Pixel projection (no distortion) is:

\[ \begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} f_x \dfrac{X}{Z} + c_x \\ f_y \dfrac{Y}{Z} + c_y \end{bmatrix}, \quad Z > 0 \]

The key geometric residual used in feature-based stacks is the reprojection residual for a tracked feature: \( \mathbf{r} = \mathbf{z} - \hat{\mathbf{z} } \). With robust loss \( \rho(\cdot) \), the typical cost is:

\[ \min_{\mathcal{X} } \sum_{k}\sum_{j \in \mathcal{F}_k} \rho\!\left( \left\| \mathbf{z}_{k}^{(j)} - \pi\!\big(\mathbf{T}_{cw}(k)\,\mathbf{P}_w^{(j)}\big) \right\|^2_{\mathbf{\Sigma}_{z}^{-1} } \right) \]

For VIO, the IMU adds a continuous-time inertial model. A common discrete-time error-state view uses: position \( \mathbf{p} \), velocity \( \mathbf{v} \), orientation \( \mathbf{R}\in SO(3) \), biases \( \mathbf{b}_a,\mathbf{b}_g \). With measured accel/gyro \( \mathbf{a}_m,\boldsymbol{\omega}_m \):

\[ \dot{\mathbf{p} } = \mathbf{v},\quad \dot{\mathbf{v} } = \mathbf{R}\big(\mathbf{a}_m - \mathbf{b}_a - \mathbf{n}_a\big) + \mathbf{g},\quad \dot{\mathbf{R} } = \mathbf{R}\big[\boldsymbol{\omega}_m - \mathbf{b}_g - \mathbf{n}_g\big]_\times \]

Here \( [\cdot]_\times \) is the skew operator, and IMU noise drives drift unless corrected by vision constraints. In practice, most VIO stacks use preintegration between keyframes (Lesson 3) to form inertial factors.

A feature-based visual SLAM stack (e.g., ORB-family) typically has: tracking (front-end), local mapping (BA in a sliding window), and loop closing (place recognition + pose graph correction). Your lab runbook should be written in terms of inputs/outputs and assumptions:

• Inputs: images (mono/stereo), intrinsics + distortion, optionally depth/RGB-D, and time stamps.
• Key settings: feature thresholding, pyramid levels, keyframe insertion policy, and outlier gates.
• Outputs: camera poses \( \mathbf{T}_{wc}(t) \) and a sparse map (if SLAM, not pure VO).

Monocular note (scale): without inertial or metric depth, the solution is only determined up to a similarity transform. That is, if \( \{\mathbf{p}_k\} \) is a valid trajectory, so is \( \{ s\mathbf{R}\mathbf{p}_k + \mathbf{t} \} \). Therefore evaluation must allow scale alignment.

\[ \text{Mono ambiguity:}\quad \mathbf{p}_k \;\;\text{and}\;\; s\mathbf{R}\mathbf{p}_k + \mathbf{t} \;\; \\ \text{produce identical normalized projections under noiseless pinhole geometry.} \]

Practical lab checks when tracking fails:

• Lighting/motion blur (Lesson 4): reduce exposure time, increase feature pyramid robustness, or switch to direct methods.
• Wrong intrinsics/distortion: reprojection residuals inflate; feature matches become inconsistent.
• Timestamp ordering: even small time jitter can destroy motion prediction.

A VIO stack adds the IMU stream, so configuration expands to: \( (i) \) camera intrinsics/distortion, \( (ii) \) camera–IMU extrinsics, \( (iii) \) IMU noise + bias random walk, \( (iv) \) time offset model or synchronization policy.

The optimization (or filtering) view is typically: states at keyframes (pose, velocity, biases) linked by IMU preintegration constraints and visual reprojection constraints.

flowchart TD
  Xk["State k: (R_k, p_k, v_k, b_gk, b_ak)"] --> IMU["IMU factor: preintegrated meas"]
  IMU --> Xk1["State k+1: (R_k1, p_k1, v_k1, b_gk1, b_ak1)"]
  Xk --> VIS["Vision factors: reprojection residuals"]
  Xk1 --> VIS
  VIS --> MAP["Landmarks or inverse-depth features"]
  Xk1 --> MARG["Marginalize old states → prior factor"]
        

Time offset sensitivity (lab reality): If camera timestamps are shifted by \( \delta t \), then the predicted pose used for reprojection is evaluated at the wrong time, producing a systematic residual. First-order, the residual perturbation scales like:

\[ \delta \mathbf{r} \approx \frac{\partial \mathbf{r} }{\partial \boldsymbol{\xi} } \frac{\partial \boldsymbol{\xi} }{\partial t}\,\delta t, \quad \text{so}\quad \|\delta \mathbf{r}\| \propto \|\boldsymbol{\omega}\|\,|\delta t| \]

This explains why aggressive angular motion makes poor sync immediately visible in residuals and drift.

Let ground-truth positions be \( \{\mathbf{p}_k^{gt}\}_{k=1}^N \) and estimated positions be \( \{\mathbf{p}_k\}_{k=1}^N \), after time association. We align the estimate to ground truth by a similarity transform \( (s,\mathbf{R},\mathbf{t}) \): \( \mathbf{p}_k^{al} = s\mathbf{R}\mathbf{p}_k + \mathbf{t} \).

The aligned Absolute Trajectory Error (ATE) is:

\[ \text{ATE}_k = \left\| \mathbf{p}_k^{al} - \mathbf{p}_k^{gt} \right\|_2, \quad \text{ATE}_{\text{RMSE} } = \sqrt{\frac{1}{N}\sum_{k=1}^N \text{ATE}_k^2} \]

Relative Pose Error (RPE) over consecutive associated pairs measures local drift. For translation-only (positions), define \( \Delta \mathbf{p}_k = \mathbf{p}_{k+1}-\mathbf{p}_k \) and use aligned estimates:

\[ \text{RPE}^{trans}_k = \left\| (\mathbf{p}_{k+1}^{al}-\mathbf{p}_{k}^{al}) - (\mathbf{p}_{k+1}^{gt}-\mathbf{p}_{k}^{gt}) \right\|_2 \]

Why SVD alignment works (derivation sketch): Consider minimizing \( \sum_k \|\mathbf{p}_k^{gt} - (s\mathbf{R}\mathbf{p}_k + \mathbf{t})\|^2 \). Differentiating w.r.t. \( \mathbf{t} \) yields \( \mathbf{t}^\star = \bar{\mathbf{p} }^{gt} - s\mathbf{R}\bar{\mathbf{p} } \), reducing the problem to centered point sets \( \mathbf{x}_k = \mathbf{p}_k - \bar{\mathbf{p} }, \mathbf{y}_k = \mathbf{p}_k^{gt} - \bar{\mathbf{p} }^{gt} \). The remaining rotation maximization becomes:

\[ \min_{\mathbf{R}\in SO(3)} \sum_k \| \mathbf{y}_k - s\mathbf{R}\mathbf{x}_k \|^2 \;\;\Longleftrightarrow\;\; \max_{\mathbf{R}\in SO(3)} \operatorname{tr}\!\left(\mathbf{R}\mathbf{\Sigma}\right), \quad \mathbf{\Sigma}=\frac{1}{N}\sum_k \mathbf{y}_k\mathbf{x}_k^\top \]

With SVD \( \mathbf{\Sigma}=\mathbf{U}\mathbf{D}\mathbf{V}^\top \), the optimizer is \( \mathbf{R}^\star=\mathbf{U}\mathbf{S}\mathbf{V}^\top \) where \( \mathbf{S}=\operatorname{diag}(1,1,\det(\mathbf{U}\mathbf{V}^\top)) \). If scale is allowed, \( s^\star \) is obtained from the trace ratio (see code files).

This script reads TUM-format trajectories, associates by timestamp, computes Umeyama similarity alignment (optional scale), then reports ATE (RMSE/median/max) and RPE (translation + rotation, approximate).

File: Chapter13_Lesson5.py

#!/usr/bin/env python3
"""
Chapter13_Lesson5.py
Trajectory evaluation utilities for VIO/Visual SLAM labs.

Input trajectory format (TUM):
t tx ty tz qx qy qz qw

Typical usage:
  python Chapter13_Lesson5.py --gt groundtruth.txt --est estimated.txt --allow-scale --plot

Notes:
- "allow-scale" is useful for monocular VO/SLAM where scale is unknown.
- Association matches by nearest timestamp within --max-dt.
"""
from __future__ import annotations

import argparse
import math
from dataclasses import dataclass
from typing import List, Tuple

import numpy as np


@dataclass
class Trajectory:
    t: np.ndarray          # (N,)
    p: np.ndarray          # (N,3)
    q: np.ndarray          # (N,4) [x,y,z,w] unit quaternions


def _read_tum(path: str) -> Trajectory:
    ts, ps, qs = [], [], []
    with open(path, "r", encoding="utf-8") as f:
        for line in f:
            s = line.strip()
            if (not s) or s.startswith("#"):
                continue
            parts = s.split()
            if len(parts) < 8:
                continue
            t = float(parts[0])
            tx, ty, tz = map(float, parts[1:4])
            qx, qy, qz, qw = map(float, parts[4:8])
            ts.append(t)
            ps.append([tx, ty, tz])
            qs.append([qx, qy, qz, qw])
    t = np.asarray(ts, dtype=float)
    p = np.asarray(ps, dtype=float)
    q = np.asarray(qs, dtype=float)
    n = np.linalg.norm(q, axis=1, keepdims=True) + 1e-12
    q = q / n
    return Trajectory(t=t, p=p, q=q)


def _associate_by_time(t_a: np.ndarray, t_b: np.ndarray, max_dt: float) -> List[Tuple[int, int]]:
    pairs: List[Tuple[int, int]] = []
    j = 0
    used_b = set()
    for i, ta in enumerate(t_a):
        while j + 1 < len(t_b) and t_b[j] < ta:
            j += 1
        cand = []
        for jj in (j, j - 1):
            if 0 <= jj < len(t_b) and jj not in used_b:
                cand.append(jj)
        if not cand:
            continue
        jj_best = min(cand, key=lambda jj: abs(t_b[jj] - ta))
        if abs(t_b[jj_best] - ta) <= max_dt:
            pairs.append((i, jj_best))
            used_b.add(jj_best)
    return pairs


def _umeyama_alignment(A: np.ndarray, B: np.ndarray, with_scale: bool):
    if A.shape != B.shape or A.shape[1] != 3:
        raise ValueError("A and B must be (N,3) and same shape.")
    n = A.shape[0]
    if n < 3:
        raise ValueError("Need at least 3 point correspondences.")

    mu_A = A.mean(axis=0)
    mu_B = B.mean(axis=0)
    X = A - mu_A
    Y = B - mu_B

    Sigma = (Y.T @ X) / n
    U, D, Vt = np.linalg.svd(Sigma)
    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_A = (X * X).sum() / n
        s = float(np.trace(np.diag(D) @ S) / (var_A + 1e-12))
    else:
        s = 1.0

    t = mu_B - s * (R @ mu_A)
    return s, R, t


def _quat_mul(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
    x1, y1, z1, w1 = q1
    x2, y2, z2, w2 = q2
    return np.array([
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2,
        w1*w2 - x1*x2 - y1*y2 - z1*z2
    ], dtype=float)


def _quat_conj(q: np.ndarray) -> np.ndarray:
    return np.array([-q[0], -q[1], -q[2], q[3]], dtype=float)


def _quat_to_rot(q: np.ndarray) -> np.ndarray:
    x, y, z, w = q
    xx, yy, zz = x*x, y*y, z*z
    xy, xz, yz = x*y, x*z, y*z
    wx, wy, wz = w*x, w*y, w*z
    return np.array([
        [1 - 2*(yy + zz), 2*(xy - wz),     2*(xz + wy)],
        [2*(xy + wz),     1 - 2*(xx + zz), 2*(yz - wx)],
        [2*(xz - wy),     2*(yz + wx),     1 - 2*(xx + yy)]
    ], dtype=float)


def _so3_log(R: np.ndarray) -> np.ndarray:
    tr = np.trace(R)
    cos_theta = (tr - 1.0) / 2.0
    cos_theta = float(np.clip(cos_theta, -1.0, 1.0))
    theta = math.acos(cos_theta)
    if theta < 1e-12:
        return np.zeros(3)
    w = (1.0 / (2.0 * math.sin(theta))) * np.array([
        R[2, 1] - R[1, 2],
        R[0, 2] - R[2, 0],
        R[1, 0] - R[0, 1]
    ], dtype=float)
    return theta * w


def _ate(gt: Trajectory, est: Trajectory, max_dt: float, allow_scale: bool):
    pairs = _associate_by_time(est.t, gt.t, max_dt=max_dt)
    if len(pairs) < 3:
        raise RuntimeError(f"Too few associated poses: {len(pairs)}")

    A = np.array([est.p[i] for i, _ in pairs], dtype=float)
    B = np.array([gt.p[j] for _, j in pairs], dtype=float)

    s, R, t = _umeyama_alignment(A, B, with_scale=allow_scale)
    A_aligned = (s * (R @ A.T)).T + t.reshape(1, 3)

    e = A_aligned - B
    se = np.sum(e * e, axis=1)
    rmse = float(np.sqrt(np.mean(se)))
    med = float(np.median(np.sqrt(se)))
    mx = float(np.max(np.sqrt(se)))

    return {
        "n_pairs": int(len(pairs)),
        "scale": float(s),
        "R": R,
        "t": t,
        "rmse": rmse,
        "median": med,
        "max": mx,
        "aligned_est_positions": A_aligned,
        "gt_positions": B,
        "assoc_pairs": pairs,
    }


def _rpe(gt: Trajectory, est: Trajectory, pairs, R_align, t_align, s_align):
    if len(pairs) < 2:
        raise RuntimeError("Need at least 2 associated poses for RPE.")

    A = np.array([est.p[i] for i, _ in pairs], dtype=float)
    B = np.array([gt.p[j] for _, j in pairs], dtype=float)
    A_aligned = (s_align * (R_align @ A.T)).T + t_align.reshape(1, 3)

    dt = []
    dr = []
    for k in range(len(pairs) - 1):
        dA = A_aligned[k + 1] - A_aligned[k]
        dB = B[k + 1] - B[k]
        dt.append(np.linalg.norm(dA - dB))

        qi, qj = est.q[pairs[k][0]], est.q[pairs[k + 1][0]]
        qgi, qgj = gt.q[pairs[k][1]], gt.q[pairs[k + 1][1]]

        dq_est = _quat_mul(_quat_conj(qi), qj)
        dq_gt = _quat_mul(_quat_conj(qgi), qgj)

        R_est = _quat_to_rot(dq_est)
        R_gt = _quat_to_rot(dq_gt)
        R_est_al = R_align @ R_est @ R_align.T
        dR = R_est_al.T @ R_gt
        w = _so3_log(dR)
        dr.append(np.linalg.norm(w))

    dt = np.asarray(dt, dtype=float)
    dr = np.asarray(dr, dtype=float)
    return {
        "trans_rmse": float(np.sqrt(np.mean(dt*dt))),
        "rot_rmse_rad": float(np.sqrt(np.mean(dr*dr))),
        "trans_mean": float(np.mean(dt)),
        "rot_mean_rad": float(np.mean(dr)),
        "n_edges": int(len(dt)),
    }


def main() -> None:
    ap = argparse.ArgumentParser()
    ap.add_argument("--gt", required=True)
    ap.add_argument("--est", required=True)
    ap.add_argument("--max-dt", type=float, default=0.02)
    ap.add_argument("--allow-scale", action="store_true")
    ap.add_argument("--plot", action="store_true")
    args = ap.parse_args()

    gt = _read_tum(args.gt)
    est = _read_tum(args.est)

    ate = _ate(gt, est, max_dt=args.max_dt, allow_scale=args.allow_scale)
    rpe = _rpe(gt, est, ate["assoc_pairs"], ate["R"], ate["t"], ate["scale"])

    print("=== ATE (aligned) ===")
    print("pairs:", ate["n_pairs"])
    print("scale:", ate["scale"])
    print("rmse [m]:", ate["rmse"])
    print("median [m]:", ate["median"])
    print("max [m]:", ate["max"])
    print("=== RPE (consecutive) ===")
    print("edges:", rpe["n_edges"])
    print("trans_rmse [m]:", rpe["trans_rmse"])
    print("rot_rmse [rad]:", rpe["rot_rmse_rad"])

    if args.plot:
        import matplotlib.pyplot as plt
        A = ate["aligned_est_positions"]
        B = ate["gt_positions"]
        plt.figure()
        plt.plot(B[:, 0], B[:, 1], label="gt")
        plt.plot(A[:, 0], A[:, 1], label="est_aligned")
        plt.xlabel("x [m]"); plt.ylabel("y [m]")
        plt.axis("equal")
        plt.legend()
        plt.title("Trajectory (top-down)")
        plt.show()


if __name__ == "__main__":
    main()
      

Optional diagnostic for time offset estimation via cross-correlation:

File: Chapter13_Lesson5_Ex1.py

#!/usr/bin/env python3
"""
Chapter13_Lesson5_Ex1.py
Estimate an approximate camera-IMU time offset from two time series by cross-correlation.

Input CSV format (two files):
t,value  (header optional)
"""
from __future__ import annotations

import argparse
import numpy as np


def _read_csv_tv(path: str) -> tuple[np.ndarray, np.ndarray]:
    t, v = [], []
    with open(path, "r", encoding="utf-8") as f:
        for line in f:
            s = line.strip()
            if not s:
                continue
            if s.lower().startswith("t"):
                continue
            parts = s.split(",")
            if len(parts) < 2:
                continue
            t.append(float(parts[0]))
            v.append(float(parts[1]))
    return np.asarray(t, float), np.asarray(v, float)


def _resample_uniform(t: np.ndarray, v: np.ndarray, dt: float) -> tuple[np.ndarray, np.ndarray]:
    t0, t1 = t.min(), t.max()
    tu = np.arange(t0, t1, dt)
    vu = np.interp(tu, t, v)
    return tu, vu


def main() -> None:
    ap = argparse.ArgumentParser()
    ap.add_argument("--imu", required=True)
    ap.add_argument("--vis", required=True)
    ap.add_argument("--dt", type=float, default=0.002)
    ap.add_argument("--max-shift", type=float, default=0.2)
    args = ap.parse_args()

    t_i, v_i = _read_csv_tv(args.imu)
    t_v, v_v = _read_csv_tv(args.vis)

    ti, vi = _resample_uniform(t_i, v_i, args.dt)
    tv, vv = _resample_uniform(t_v, v_v, args.dt)

    t0 = max(ti.min(), tv.min())
    t1 = min(ti.max(), tv.max())
    mi = (ti >= t0) & (ti <= t1)
    mv = (tv >= t0) & (tv <= t1)
    vi = vi[mi]
    vv = vv[mv]

    n = min(len(vi), len(vv))
    vi, vv = vi[:n], vv[:n]

    vi = (vi - vi.mean()) / (vi.std() + 1e-12)
    vv = (vv - vv.mean()) / (vv.std() + 1e-12)

    max_lag = int(round(args.max_shift / args.dt))
    lags = np.arange(-max_lag, max_lag + 1)
    corr = np.zeros_like(lags, dtype=float)

    for k, lag in enumerate(lags):
        if lag >= 0:
            a = vi[lag:]
            b = vv[:len(a)]
        else:
            a = vi[:lag]
            b = vv[-lag:]
        corr[k] = float(np.dot(a, b) / max(len(a), 1))

    best = int(np.argmax(corr))
    best_lag = lags[best]
    dt_est = -best_lag * args.dt
    print("Estimated time offset (VIS relative to IMU) [s]:", dt_est)
    print("Peak correlation:", corr[best])


if __name__ == "__main__":
    main()
      

This is a standalone C++ evaluator (Eigen-based) to validate results without Python dependencies.

File: Chapter13_Lesson5.cpp

// Chapter13_Lesson5.cpp
// Minimal ATE evaluation (TUM format) + Umeyama alignment.
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter13_Lesson5.cpp -I /usr/include/eigen3 -o ate_eval
//
// Run:
//   ./ate_eval groundtruth.txt estimated.txt 0.02 1
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <unordered_set>
#include <cmath>

struct Traj {
  std::vector<double> t;
  std::vector<Eigen::Vector3d> p;
};

static Traj read_tum(const std::string& path) {
  Traj tr;
  std::ifstream f(path);
  if (!f) throw std::runtime_error("Cannot open " + path);
  std::string line;
  while (std::getline(f, line)) {
    if (line.empty() || line[0] == '#') continue;
    std::istringstream iss(line);
    double tt, tx, ty, tz, qx, qy, qz, qw;
    if (!(iss >> tt >> tx >> ty >> tz >> qx >> qy >> qz >> qw)) continue;
    tr.t.push_back(tt);
    tr.p.emplace_back(tx, ty, tz);
  }
  return tr;
}

static std::vector<std::pair<int,int>> associate_by_time(
  const std::vector<double>& ta,
  const std::vector<double>& tb,
  double max_dt)
{
  std::vector<std::pair<int,int>> pairs;
  int j = 0;
  std::unordered_set<int> used;
  for (int i = 0; i < (int)ta.size(); ++i) {
    double t = ta[i];
    while (j + 1 < (int)tb.size() && tb[j] < t) j++;
    std::vector<int> cand;
    for (int jj : {j, j-1}) {
      if (0 <= jj && jj < (int)tb.size() && used.find(jj) == used.end()) cand.push_back(jj);
    }
    if (cand.empty()) continue;
    int best = cand[0];
    for (int jj : cand) if (std::abs(tb[jj] - t) < std::abs(tb[best] - t)) best = jj;
    if (std::abs(tb[best] - t) <= max_dt) {
      pairs.emplace_back(i, best);
      used.insert(best);
    }
  }
  return pairs;
}

static void umeyama(
  const Eigen::MatrixXd& A, // 3xN
  const Eigen::MatrixXd& B, // 3xN
  bool with_scale,
  double& s,
  Eigen::Matrix3d& R,
  Eigen::Vector3d& t)
{
  const int N = (int)A.cols();
  if (N < 3) throw std::runtime_error("Need >=3 points");
  Eigen::Vector3d muA = A.rowwise().mean();
  Eigen::Vector3d muB = B.rowwise().mean();
  Eigen::MatrixXd X = A.colwise() - muA;
  Eigen::MatrixXd Y = B.colwise() - muB;

  Eigen::Matrix3d Sigma = (Y * X.transpose()) / (double)N;

  Eigen::JacobiSVD<Eigen::Matrix3d> svd(Sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  Eigen::Vector3d D = svd.singularValues();

  Eigen::Matrix3d S = Eigen::Matrix3d::Identity();
  if ((U.determinant() * V.determinant()) < 0.0) S(2,2) = -1.0;

  R = U * S * V.transpose();

  if (with_scale) {
    double varA = (X.array() * X.array()).sum() / (double)N;
    s = (D.transpose() * S.diagonal()) / (varA + 1e-12);
  } else {
    s = 1.0;
  }

  t = muB - s * (R * muA);
}

int main(int argc, char** argv) {
  if (argc < 5) {
    std::cerr << "Usage: " << argv[0] << " gt.txt est.txt max_dt allow_scale(0/1)\n";
    return 2;
  }
  const std::string gt_path = argv[1];
  const std::string est_path = argv[2];
  const double max_dt = std::stod(argv[3]);
  const bool allow_scale = (std::stoi(argv[4]) != 0);

  Traj gt = read_tum(gt_path);
  Traj est = read_tum(est_path);

  auto pairs = associate_by_time(est.t, gt.t, max_dt);
  if ((int)pairs.size() < 3) {
    std::cerr << "Too few associated poses: " << pairs.size() << "\n";
    return 3;
  }

  const int N = (int)pairs.size();
  Eigen::MatrixXd A(3, N), B(3, N);
  for (int k = 0; k < N; ++k) {
    A.col(k) = est.p[pairs[k].first];
    B.col(k) = gt.p[pairs[k].second];
  }

  double s;
  Eigen::Matrix3d R;
  Eigen::Vector3d t;
  umeyama(A, B, allow_scale, s, R, t);

  Eigen::MatrixXd A_al = (s * (R * A)).colwise() + t;
  Eigen::MatrixXd E = A_al - B;
  Eigen::VectorXd se = E.colwise().squaredNorm();

  double rmse = std::sqrt(se.mean());
  double mx = std::sqrt(se.maxCoeff());

  std::vector<double> e;
  e.reserve(N);
  for (int k = 0; k < N; ++k) e.push_back(std::sqrt(se(k)));
  std::nth_element(e.begin(), e.begin() + e.size()/2, e.end());
  double med = e[e.size()/2];

  std::cout << "pairs " << N << "\n";
  std::cout << "scale " << s << "\n";
  std::cout << "rmse_m " << rmse << "\n";
  std::cout << "median_m " << med << "\n";
  std::cout << "max_m " << mx << "\n";
  return 0;
}
      

Java can be used for evaluation pipelines in robotics backends (e.g., server-side batch evaluation). This implementation uses EJML SVD.

File: Chapter13_Lesson5.java

// Chapter13_Lesson5.java
// ATE evaluation in Java using EJML for SVD (Umeyama alignment).
//
// Dependency (Gradle):
//   implementation "org.ejml:ejml-simple:0.43"
import org.ejml.simple.SimpleMatrix;

import java.io.BufferedReader;
import java.io.FileReader;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Locale;

public class Chapter13_Lesson5 {

    static class Traj {
        final double[] t;
        final double[][] p; // Nx3
        Traj(double[] t, double[][] p) { this.t = t; this.p = p; }
    }

    static Traj readTum(String path) throws Exception {
        List<Double> ts = new ArrayList<>();
        List<double[]> ps = new ArrayList<>();
        try (BufferedReader br = new BufferedReader(new FileReader(path))) {
            String line;
            while ((line = br.readLine()) != null) {
                line = line.trim();
                if (line.isEmpty() || line.startsWith("#")) continue;
                String[] parts = line.split("\\s+");
                if (parts.length < 8) continue;
                double tt = Double.parseDouble(parts[0]);
                double tx = Double.parseDouble(parts[1]);
                double ty = Double.parseDouble(parts[2]);
                double tz = Double.parseDouble(parts[3]);
                ts.add(tt);
                ps.add(new double[]{tx, ty, tz});
            }
        }
        double[] t = new double[ts.size()];
        double[][] p = new double[ts.size()][3];
        for (int i = 0; i < ts.size(); i++) {
            t[i] = ts.get(i);
            p[i] = ps.get(i);
        }
        return new Traj(t, p);
    }

    static List<int[]> associateByTime(double[] ta, double[] tb, double maxDt) {
        List<int[]> pairs = new ArrayList<>();
        int j = 0;
        HashSet<Integer> used = new HashSet<>();
        for (int i = 0; i < ta.length; i++) {
            double t = ta[i];
            while (j + 1 < tb.length && tb[j] < t) j++;
            int[] cand = new int[]{j, j - 1};
            int best = -1;
            double bestErr = Double.POSITIVE_INFINITY;
            for (int jj : cand) {
                if (0 <= jj && jj < tb.length && !used.contains(jj)) {
                    double err = Math.abs(tb[jj] - t);
                    if (err < bestErr) { bestErr = err; best = jj; }
                }
            }
            if (best >= 0 && bestErr <= maxDt) {
                pairs.add(new int[]{i, best});
                used.add(best);
            }
        }
        return pairs;
    }

    static class Align {
        final double s;
        final SimpleMatrix R; // 3x3
        final SimpleMatrix t; // 3x1
        Align(double s, SimpleMatrix R, SimpleMatrix t) { this.s = s; this.R = R; this.t = t; }
    }

    static Align umeyama(SimpleMatrix A, SimpleMatrix B, boolean withScale) {
        int N = A.numCols();
        if (N < 3) throw new IllegalArgumentException("Need >=3 points");

        SimpleMatrix muA = A.mean(1);
        SimpleMatrix muB = B.mean(1);

        SimpleMatrix X = A.minus(muA.mult(SimpleMatrix.ones(1, N)));
        SimpleMatrix Y = B.minus(muB.mult(SimpleMatrix.ones(1, N)));

        SimpleMatrix Sigma = Y.mult(X.transpose()).divide(N);

        var svd = Sigma.svd();
        SimpleMatrix U = svd.getU();
        SimpleMatrix V = svd.getV();
        SimpleMatrix W = svd.getW();

        SimpleMatrix S = SimpleMatrix.identity(3);
        if (U.determinant() * V.determinant() < 0.0) {
            S.set(2, 2, -1.0);
        }

        SimpleMatrix R = U.mult(S).mult(V.transpose());

        double s;
        if (withScale) {
            double varA = X.elementMult(X).elementSum() / N;
            double tr = 0.0;
            for (int i = 0; i < 3; i++) tr += W.get(i, i) * S.get(i, i);
            s = tr / (varA + 1e-12);
        } else {
            s = 1.0;
        }

        SimpleMatrix t = muB.minus(R.mult(muA).scale(s));
        return new Align(s, R, t);
    }

    public static void main(String[] args) throws Exception {
        Locale.setDefault(Locale.US);
        if (args.length < 4) {
            System.err.println("Usage: Chapter13_Lesson5 gt.txt est.txt max_dt allowScale(true/false)");
            System.exit(2);
        }
        String gtPath = args[0];
        String estPath = args[1];
        double maxDt = Double.parseDouble(args[2]);
        boolean allowScale = Boolean.parseBoolean(args[3]);

        Traj gt = readTum(gtPath);
        Traj est = readTum(estPath);

        List<int[]> pairs = associateByTime(est.t, gt.t, maxDt);
        if (pairs.size() < 3) {
            System.err.println("Too few associated poses: " + pairs.size());
            System.exit(3);
        }

        int N = pairs.size();
        SimpleMatrix A = new SimpleMatrix(3, N);
        SimpleMatrix B = new SimpleMatrix(3, N);
        for (int k = 0; k < N; k++) {
            int i = pairs.get(k)[0];
            int j = pairs.get(k)[1];
            A.set(0, k, est.p[i][0]); A.set(1, k, est.p[i][1]); A.set(2, k, est.p[i][2]);
            B.set(0, k, gt.p[j][0]);  B.set(1, k, gt.p[j][1]);  B.set(2, k, gt.p[j][2]);
        }

        Align al = umeyama(A, B, allowScale);
        SimpleMatrix A_al = al.R.mult(A).scale(al.s).plus(al.t.mult(SimpleMatrix.ones(1, N)));

        double sum = 0.0, max = 0.0;
        double[] errs = new double[N];
        for (int k = 0; k < N; k++) {
            double dx = A_al.get(0,k) - B.get(0,k);
            double dy = A_al.get(1,k) - B.get(1,k);
            double dz = A_al.get(2,k) - B.get(2,k);
            double e = Math.sqrt(dx*dx + dy*dy + dz*dz);
            errs[k] = e;
            sum += e*e;
            if (e > max) max = e;
        }
        double rmse = Math.sqrt(sum / N);

        java.util.Arrays.sort(errs);
        double median = errs[N/2];

        System.out.println("pairs " + N);
        System.out.println("scale " + al.s);
        System.out.println("rmse_m " + rmse);
        System.out.println("median_m " + median);
        System.out.println("max_m " + max);
    }
}
      

MATLAB is frequently used in AMR for sensor characterization and parameter extraction. The following file provides: \( (1) \) ATE evaluation (TUM format) and \( (2) \) Allan deviation computation to estimate IMU noise regimes for VIO tuning. For Simulink usage, the ATE portion can be wrapped in a MATLAB Function block for automated scoring pipelines.

File: Chapter13_Lesson5.m

% Chapter13_Lesson5.m
% MATLAB utilities for VIO/Visual SLAM lab:
%  1) ATE evaluation (TUM format) with optional similarity scale.
%  2) Allan variance-based IMU noise parameter estimation (gyro/accel).
function demo_Chapter13_Lesson5()
  disp("Run functions: ate_eval_tum, allan_imu");
end

function out = ate_eval_tum(gtPath, estPath, maxDt, allowScale)
  gt = read_tum(gtPath);
  est = read_tum(estPath);

  pairs = associate_by_time(est.t, gt.t, maxDt);
  if size(pairs,1) < 3
    error("Too few associated poses.");
  end

  A = est.p(pairs(:,1), :);
  B = gt.p(pairs(:,2), :);

  [s,R,t] = umeyama(A, B, allowScale);
  A_al = (s*(R*A') + t)';
  e = A_al - B;
  se = sum(e.^2,2);
  out.n_pairs = size(pairs,1);
  out.scale = s;
  out.R = R;
  out.t = t;
  out.rmse = sqrt(mean(se));
  out.median = median(sqrt(se));
  out.max = max(sqrt(se));
end

function tr = read_tum(path)
  fid = fopen(path,'r');
  if fid < 0, error("Cannot open file"); end
  t = [];
  p = [];
  while true
    line = fgetl(fid);
    if ~ischar(line), break; end
    line = strtrim(line);
    if isempty(line) || startsWith(line,"#"), continue; end
    parts = split(line);
    if numel(parts) < 8, continue; end
    tt = str2double(parts{1});
    tx = str2double(parts{2});
    ty = str2double(parts{3});
    tz = str2double(parts{4});
    t(end+1,1) = tt; %#ok<AGROW>
    p(end+1,:) = [tx ty tz]; %#ok<AGROW>
  end
  fclose(fid);
  tr.t = t;
  tr.p = p;
end

function pairs = associate_by_time(ta, tb, maxDt)
  pairs = [];
  j = 1;
  used = false(numel(tb),1);
  for i = 1:numel(ta)
    t = ta(i);
    while (j+1) <= numel(tb) && tb(j) < t
      j = j+1;
    end
    cand = [j, j-1];
    best = -1;
    bestErr = inf;
    for c = cand
      if c >= 1 && c <= numel(tb) && ~used(c)
        err = abs(tb(c)-t);
        if err < bestErr
          bestErr = err;
          best = c;
        end
      end
    end
    if best > 0 && bestErr <= maxDt
      pairs(end+1,:) = [i, best]; %#ok<AGROW>
      used(best) = true;
    end
  end
end

function [s,R,t] = umeyama(A, B, withScale)
  n = size(A,1);
  muA = mean(A,1);
  muB = mean(B,1);
  X = A - muA;
  Y = B - muB;
  Sigma = (Y'*X)/n;

  [U,D,V] = svd(Sigma);
  S = eye(3);
  if det(U)*det(V) < 0
    S(3,3) = -1;
  end
  R = U*S*V';
  if withScale
    varA = sum(sum(X.^2))/n;
    s = trace(D*S)/(varA + 1e-12);
  else
    s = 1.0;
  end
  t = muB' - s*R*muA';
end

function av = allan_imu(t, acc, gyro)
  dt = median(diff(t));
  if ~isfinite(dt) || dt <= 0, error("Invalid dt"); end

  maxM = floor(numel(t)/10);
  Ms = unique(round(logspace(0, log10(maxM), 50)));
  taus = Ms * dt;

  av.taus = taus;
  av.acc = cell(1,3);
  av.gyro = cell(1,3);

  for ax = 1:3
    av.acc{ax} = allan_dev_1d(acc(:,ax), Ms);
    av.gyro{ax} = allan_dev_1d(gyro(:,ax), Ms);
  end
end

function sigma = allan_dev_1d(x, Ms)
  x = x(:);
  N = numel(x);
  sigma = nan(numel(Ms),1);
  for k = 1:numel(Ms)
    m = Ms(k);
    K = floor(N/(2*m));
    if K < 2, continue; end
    y = zeros(2*K,1);
    for i = 1:(2*K)
      idx = (i-1)*m + (1:m);
      y(i) = mean(x(idx));
    end
    d = diff(y);
    sigma(k) = sqrt(0.5*mean(d.^2));
  end
end
      

Symbolic Jacobians are useful when debugging custom residuals or verifying sign conventions. This Wolfram Language content computes a first-order Jacobian of pinhole reprojection under a small \( \mathfrak{se}(3) \) perturbation.

File: Chapter13_Lesson5.nb

(* Chapter13_Lesson5.nb
   Wolfram Language content (saved as .nb for convenience): symbolic Jacobian
   of the pinhole reprojection residual with respect to a small se(3) perturbation.
*)

ClearAll["Global`*"];

fx = Symbol["fx"]; fy = Symbol["fy"]; cx = Symbol["cx"]; cy = Symbol["cy"];
Pc = {X, Y, Z};

proj[pc_] := {fx*pc[[1]]/pc[[3]] + cx, fy*pc[[2]]/pc[[3]] + cy};

rho = {rx, ry, rz};
phi = {px, py, pz};
skew[w_] := { {0, -w[[3]], w[[2]]}, {w[[3]], 0, -w[[1]]}, {-w[[2]], w[[1]], 0} };

Pc2 = (IdentityMatrix[3] + skew[phi]).Pc + rho;

r = proj[Pc2] - proj[Pc];
J = D[r, { {rx, ry, rz, px, py, pz} }] // Simplify;

Print["Jacobian dr/d(delta) (2x6):"];
MatrixForm[J]

test = J /. {fx -> 400, fy -> 420, cx -> 320, cy -> 240, X -> 1.2, Y -> -0.4, Z -> 3.5};
Print["Numeric Jacobian example:"];
MatrixForm[N[test]]
      

Problem 1 (Similarity Alignment & Scale): Let \( \{\mathbf{x}_k\}_{k=1}^N \) and \( \{\mathbf{y}_k\}_{k=1}^N \) be corresponding 3D point sets (estimated and ground truth). Show that the optimal translation is \( \mathbf{t}^\star = \bar{\mathbf{y} } - s\mathbf{R}\bar{\mathbf{x} } \) for any fixed \( (s,\mathbf{R}) \).

Solution: Minimize \( J(\mathbf{t})=\sum_{k=1}^N\|\mathbf{y}_k-(s\mathbf{R}\mathbf{x}_k+\mathbf{t})\|^2 \). Differentiate and set to zero:

\[ \frac{\partial J}{\partial \mathbf{t} } = -2\sum_{k=1}^N \big(\mathbf{y}_k - s\mathbf{R}\mathbf{x}_k - \mathbf{t}\big) = \mathbf{0} \;\;\Longrightarrow\;\; N\mathbf{t} = \sum_{k=1}^N \mathbf{y}_k - s\mathbf{R}\sum_{k=1}^N \mathbf{x}_k \]

Divide by \( N \) to get \( \mathbf{t}^\star = \bar{\mathbf{y} } - s\mathbf{R}\bar{\mathbf{x} } \).

Problem 2 (Rotation by Trace Maximization): With centered points \( \mathbf{x}_k, \mathbf{y}_k \), prove that minimizing \( \sum_k \|\mathbf{y}_k - \mathbf{R}\mathbf{x}_k\|^2 \) over \( \mathbf{R}\in SO(3) \) is equivalent to maximizing \( \operatorname{tr}(\mathbf{R}\mathbf{\Sigma}) \) where \( \mathbf{\Sigma}=\frac{1}{N}\sum_k \mathbf{y}_k\mathbf{x}_k^\top \).

Solution: Expand: \( \|\mathbf{y}_k - \mathbf{R}\mathbf{x}_k\|^2 = \|\mathbf{y}_k\|^2 + \|\mathbf{x}_k\|^2 - 2\mathbf{y}_k^\top \mathbf{R}\mathbf{x}_k \). Summing over \( k \), the first two terms are constant in \( \mathbf{R} \), so minimizing is equivalent to maximizing \( \sum_k \mathbf{y}_k^\top \mathbf{R}\mathbf{x}_k \). Using trace cyclicity:

\[ \sum_{k=1}^N \mathbf{y}_k^\top \mathbf{R}\mathbf{x}_k = \sum_{k=1}^N \operatorname{tr}\!\left(\mathbf{y}_k^\top \mathbf{R}\mathbf{x}_k\right) = \sum_{k=1}^N \operatorname{tr}\!\left(\mathbf{R}\mathbf{x}_k\mathbf{y}_k^\top\right) = N\,\operatorname{tr}\!\left(\mathbf{R}\mathbf{\Sigma}^\top\right) \]

Maximizing \( \operatorname{tr}(\mathbf{R}\mathbf{\Sigma}^\top) \) is equivalent to maximizing \( \operatorname{tr}(\mathbf{R}\mathbf{\Sigma}) \) after redefining \( \mathbf{\Sigma} \) accordingly.

Problem 3 (Time Offset as Systematic Residual): Suppose the true camera pose is \( \mathbf{T}(t) \) but the pipeline uses \( \mathbf{T}(t+\delta t) \). Using a first-order Taylor approximation, show that the induced state perturbation is \( \delta \boldsymbol{\xi} \approx \dot{\boldsymbol{\xi} }\,\delta t \), and therefore the residual scales with \( |\delta t| \) and angular rate.

Solution: Let a minimal pose perturbation be parameterized by \( \boldsymbol{\xi}(t) \). Then \( \boldsymbol{\xi}(t+\delta t) \approx \boldsymbol{\xi}(t) + \dot{\boldsymbol{\xi} }(t)\delta t \). A residual \( \mathbf{r}(\boldsymbol{\xi}) \) thus changes as \( \delta \mathbf{r} \approx \frac{\partial \mathbf{r} }{\partial \boldsymbol{\xi} } \dot{\boldsymbol{\xi} }\delta t \). Since \( \dot{\boldsymbol{\xi} } \) contains angular velocity components, fast turns amplify time-offset error.

Problem 4 (ATE Interpretation for Monocular): Explain why evaluating monocular VO without similarity alignment can produce arbitrarily large ATE even if the reconstruction is “geometrically correct.”

Solution: Monocular reconstruction is only defined up to similarity: \( \mathbf{p}_k \mapsto s\mathbf{R}\mathbf{p}_k + \mathbf{t} \). If the estimated scale differs from ground-truth scale, then \( \|\mathbf{p}_k - \mathbf{p}_k^{gt}\| \) grows with distance traveled, yielding large ATE. Similarity alignment removes this gauge freedom, isolating shape and drift errors.

Problem 5 (Robust Loss & Outliers): Let a robust kernel be Huber: \( \rho(s) = \begin{cases} s & s \le \delta^2 \\ 2\delta\sqrt{s} - \delta^2 & s > \delta^2 \end{cases} \). Show that large residuals receive smaller incremental influence than pure least squares.

Solution: For least squares, the penalty grows linearly in \( s \) with derivative \( \rho'(s)=1 \). For Huber, when \( s > \delta^2 \): \( \rho'(s) = \frac{\delta}{\sqrt{s} } \), which decreases as residual magnitude increases, reducing outlier leverage.

You now have a full lab protocol for running a VIO/Visual SLAM stack with disciplined configuration control: sensor models and timing, execution with fixed assumptions, and evaluation using similarity-aligned ATE and local RPE. The accompanying Python/C++/Java/MATLAB/Mathematica tooling enables cross-validation and rapid diagnosis of the dominant failure modes: scale gauge (monocular), time offset, wrong intrinsics/extrinsics, and insufficient outlier handling.