In Lessons 1 and 2 of this chapter, we defined what to measure (localization/SLAM metrics and navigation robustness metrics). In this lesson, we define where and how to measure: on standard datasets and simulator benchmark suites with controlled protocols.

We focus on AMR-relevant benchmark settings: wheeled indoor/outdoor datasets, long-term localization sequences, and simulation scenarios for navigation stress tests. The central principle is that a metric value is only meaningful together with the protocol that produced it: \( \text{result} = (\text{dataset}, \text{split}, \text{metric}, \text{seed}, \text{hardware budget}) \).

By the end of this lesson, students should be able to (i) select an appropriate benchmark family, (ii) define a reproducible evaluation protocol, (iii) compute trajectory metrics consistently, and (iv) compare methods using paired statistics instead of isolated single-number claims.

A standard dataset is not merely a collection of files; it is a controlled measurement instrument. It provides synchronized sensor streams, coordinate conventions, calibration files, and some form of reference trajectory or labels. Without shared datasets, two algorithms may be tested under different lighting, motion profiles, and sensor noise conditions, making direct comparison invalid.

Let \( M(\mathcal{A}, \mathcal{D}) \) denote a metric measured for algorithm \( \mathcal{A} \) on dataset \( \mathcal{D} \). If datasets differ in distribution, then the difference \( M(\mathcal{A}_1,\mathcal{D}_1)-M(\mathcal{A}_2,\mathcal{D}_2) \) confounds algorithm performance and domain shift. A fair comparison requires \( \mathcal{D}_1=\mathcal{D}_2 \) (or a paired protocol across a shared benchmark set).

This is why benchmark papers standardize not only the data but also split rules, file formats, and evaluation scripts.

flowchart TD
  A["Raw sensor logs"] --> B["Dataset packaging"]
  B --> C["Calibration + timestamps + frames"]
  C --> D["Reference labels / ground truth"]
  D --> E["Protocol: splits + metrics + seeds"]
  E --> F["Reproducible benchmark result"]
  F --> G["Cross-paper comparison"]
        

For AMR, benchmark resources can be grouped along three axes:

• Environment: indoor, outdoor campus, urban road, warehouse, agricultural field.
• Sensor modality: wheel odometry, IMU, LiDAR, RGB-D, stereo, GNSS/RTK.
• Ground truth type: motion capture, laser tracker, RTK/INS, map alignment, or simulator truth.

Typical examples used in robotics evaluation include:

• Indoor RGB-D / visual SLAM: TUM RGB-D, ICL-NUIM (synthetic but widely used), and related RGB-D benchmarks.
• Visual–inertial: EuRoC MAV for synchronized stereo + IMU with accurate reference trajectories.
• Outdoor road/long-term: KITTI odometry and Oxford RobotCar (including long-term condition changes).
• Long-term campus robotics: NCLT for repeated traversals and seasonal changes.
• Simulator suites: Gazebo, Webots, CARLA, and AirSim for controlled scenario generation and repeatable stress tests.

A key design question is \( \mathcal{D}_{\text{real}} \) vs. \( \mathcal{D}_{\text{sim}} \). Simulator benchmarks offer exact state access and systematic perturbation, while real datasets capture sensor artifacts and domain complexity. In practice, modern AMR evaluation uses both.

A benchmark protocol is a tuple \( \Pi = (\mathcal{S}, \mathcal{M}, \mathcal{B}, \mathcal{R}) \), where \( \mathcal{S} \) is the split policy, \( \mathcal{M} \) the metric set, \( \mathcal{B} \) the compute/time budget, and \( \mathcal{R} \) the randomness specification (seeds and repeated runs).

For deterministic SLAM pipelines, repeated runs may be unnecessary, but for stochastic components (particle filters, randomized map initializations, learning-based modules), we must estimate expectation and variability:

\[ \hat{\mu}_m = \frac{1}{K}\sum_{k=1}^{K} m_k, \qquad \hat{\sigma}_m^2 = \frac{1}{K-1}\sum_{k=1}^{K}(m_k-\hat{\mu}_m)^2 \]

If the benchmark is sequence-based, the split rule should prevent leakage. For example, if multiple traversals of the same route exist, then placing the same traversal in both training and test sets is invalid. More subtly, adjacent segments from one continuous run can also leak temporal information.

A conservative rule is to split by session (entire traversal) rather than by frame. Formally, let \( s(i) \) map frame \( i \) to session ID. A valid split requires \( s(i)=s(j) \Rightarrow \text{split}(i)=\text{split}(j) \).

We reuse the trajectory notions introduced in previous lessons. Let the reference trajectory be \( \mathbf{p}_{gt}(t) = [x_{gt}(t), y_{gt}(t), \psi_{gt}(t)]^\top \) and the estimated trajectory be \( \mathbf{p}_{est}(t) = [x_{est}(t), y_{est}(t), \psi_{est}(t)]^\top \).

Since timestamps often differ, we evaluate both trajectories on a common time grid using interpolation: \( \tilde{\mathbf{p}}_{gt}(t_k) \) is the interpolated ground truth at estimator timestamps.

Absolute Trajectory Error (ATE): after optional rigid alignment in \( SE(2) \), the translational RMSE is

\[ \operatorname{ATE}_{\text{RMSE}} = \sqrt{\frac{1}{N}\sum_{k=1}^{N} \left\| \mathbf{r}_k \right\|_2^2}, \qquad \mathbf{r}_k = \mathbf{t}_{est,k}^{aligned} - \tilde{\mathbf{t}}_{gt,k} \]

where \( \mathbf{t}=[x,y]^\top \). The yaw error is \( e_{\psi,k} = \operatorname{wrap}(\psi_{est,k}-\tilde{\psi}_{gt,k}) \).

Relative Pose Error (RPE): for a time horizon \( \Delta \),

\[ \Delta \mathbf{T}_{gt,k} = \mathbf{T}_{gt}(t_k)^{-1}\mathbf{T}_{gt}(t_k+\Delta), \qquad \Delta \mathbf{T}_{est,k} = \mathbf{T}_{est}(t_k)^{-1}\mathbf{T}_{est}(t_k+\Delta) \]

\[ \mathbf{E}_k = \Delta \mathbf{T}_{est,k}^{-1}\Delta \mathbf{T}_{gt,k}, \qquad \operatorname{RPE}_{\text{RMSE}}(\Delta) = \sqrt{\frac{1}{N_\Delta}\sum_{k}\left\| \operatorname{trans}(\mathbf{E}_k)\right\|_2^2} \]

Segment-based drift metrics used in outdoor benchmarks normalize by path length \( L \), e.g. drift percentage \( 100 \times \frac{\|\operatorname{trans}(\mathbf{E})\|}{L} \).

Proposition 1 (Paired comparison reduces nuisance variance): Let \( m_{a,s} \) be the metric of algorithm \( a \) on sequence \( s \), and suppose

\[ m_{a,s} = \mu_a + \gamma_s + \varepsilon_{a,s} \]

where \( \gamma_s \) is a sequence-difficulty term common to all algorithms and \( \varepsilon_{a,s} \) is zero-mean noise. Then the paired difference \( d_s = m_{1,s}-m_{2,s} \) satisfies

\[ d_s = (\mu_1-\mu_2) + (\varepsilon_{1,s}-\varepsilon_{2,s}) \]

and the nuisance term \( \gamma_s \) cancels exactly. Therefore, estimating \( \mu_1-\mu_2 \) from paired differences has lower variance than comparing unpaired averages when sequence difficulty varies strongly.

Proof: Substitute the additive model for both algorithms and subtract. The shared sequence effect appears with opposite signs and cancels. The remaining variance is

\[ \operatorname{Var}(d_s) = \operatorname{Var}(\varepsilon_{1,s}) + \operatorname{Var}(\varepsilon_{2,s}) - 2\operatorname{Cov}(\varepsilon_{1,s},\varepsilon_{2,s}) \]

while an unpaired comparison includes the extra term \( \operatorname{Var}(\gamma_s) \). Hence paired benchmarking is statistically preferable.

Proposition 2 (Interpolation error bound): Assume the translational ground-truth trajectory \( \mathbf{t}_{gt}(t) \) is twice differentiable and \( \|\ddot{\mathbf{t}}_{gt}(t)\|_2 \leq A_{max} \) over an interval of width \( h \). Linear interpolation at any time inside the interval has error

\[ \left\| \mathbf{t}_{gt}(t) - \mathbf{t}_{lin}(t) \right\|_2 \leq \frac{A_{max}h^2}{8} \]

Proof sketch: Apply the scalar linear interpolation remainder to each coordinate: \( f(t)-f_{lin}(t)=\frac{f''(\xi)}{2}(t-t_i)(t-t_{i+1}) \). The maximum magnitude of \( (t-t_i)(t-t_{i+1}) \) on the interval is \( h^2/4 \). Bound each coordinate and combine with the vector norm. This justifies reporting timestamp resolution and synchronization quality in benchmark protocols.

Proposition 3 (Monotonic capped normalization): Define a lower-is-better normalized subscore for metric \( m_j \) with cap \( c_j > 0 \) by

\[ s_j = \max\!\left(0,\,1-\frac{m_j}{c_j}\right), \qquad S = 100\sum_{j=1}^{q} w_j s_j,\quad w_j \ge 0,\ \sum_j w_j = 1 \]

Then \( S \) is monotone non-increasing in each metric \( m_j \). Therefore, improving any single error metric (holding others fixed) cannot decrease the aggregate score.

Proof: Each \( s_j \) is piecewise linear with derivative \( -1/c_j \) when \( 0 \le m_j \le c_j \) and derivative \( 0 \) when \( m_j \ge c_j \). Since the weights are nonnegative, the weighted sum preserves monotonicity.

A benchmark report should include uncertainty, not only a mean. For sequence-level paired differences \( d_1,\dots,d_n \), a confidence interval for the mean difference is

\[ \bar{d} \pm t_{n-1,1-\alpha/2}\frac{s_d}{\sqrt{n}}, \qquad s_d^2 = \frac{1}{n-1}\sum_{i=1}^{n}(d_i-\bar{d})^2 \]

If normality is questionable (common in robotics due to occasional failures), a nonparametric alternative is bootstrap resampling of sequences. For a failure-rate metric \( \hat{p} = f/n \), report a binomial interval rather than only a percentage.

For simulated benchmarks, seed sensitivity matters. If scenario generation uses random pedestrian trajectories, noise, or map perturbations, then two algorithms must be evaluated under the same seed set: \( \Omega = \{\omega_1,\dots,\omega_K\} \).

Simulation is indispensable for AMR stress testing because it enables controlled variation of one factor at a time (density of dynamic obstacles, friction, lighting, sensor dropout, etc.). The main risk is domain gap: a method tuned to simulation artifacts may not transfer.

Let \( \mathcal{L}_{real}(\theta) \) and \( \mathcal{L}_{sim}(\theta) \) be expected losses of a parameterized method. The transfer gap is \( \Delta_{gap}(\theta)=\mathcal{L}_{real}(\theta)-\mathcal{L}_{sim}(\theta) \). Even when \( \mathcal{L}_{sim} \) is small, the real-world ranking may change if \( \Delta_{gap} \) differs across methods.

Therefore a robust evaluation pipeline is:

1. Use simulation for broad coverage and controlled ablations.
2. Validate on real datasets or field logs.
3. Report both results side-by-side, not simulation alone.

In practice, Gazebo and Webots are common for physics-grounded mobile robot simulation, while CARLA and AirSim are widely used for vehicle-scale sensing and scenario generation.

flowchart TD
  A["Algorithm candidate"] --> B["Sim benchmark sweep"]
  B --> C["Ablation + stress tests"]
  C --> D["Select robust configuration"]
  D --> E["Real dataset replay"]
  E --> F["Field validation"]
  F --> G["Final benchmark report"]
        

Before publishing or comparing results, verify the following checklist:

• Coordinate frames: Are all trajectories expressed in the same frame convention (ENU/map/base)?
• Timestamps: Are time units consistent (seconds vs milliseconds)?
• Alignment policy: Did you use global rigid alignment, similarity alignment, or none? State it explicitly.
• Missing data: How are dropped frames or invalid sensor packets handled?
• Failure definition: What counts as a failure (timeout, collision, divergence, lost tracking)?
• Budget constraints: CPU/GPU, real-time factor, and memory cap must be fixed.
• Seeds and repeats: Use identical seeds across methods for simulated scenarios.
• Split hygiene: Session-level split to prevent leakage.

This lesson’s code examples implement a minimal but rigorous benchmark harness around these ideas.

The Python reference code loads trajectories, interpolates timestamps, computes ATE/RPE, detects split leakage, and produces plots. It is suitable for offline benchmark scripts or ROS bag export post-processing.

Chapter19_Lesson3.py

# Chapter19_Lesson3.py
# Standard Datasets and Simulated Benchmarks for AMR
# Python reference implementation (NumPy/Pandas/Matplotlib)
#
# This script demonstrates:
# 1) loading trajectories from CSV
# 2) timestamp association + linear interpolation
# 3) ATE and RPE metrics
# 4) normalized benchmark score aggregation
# 5) a simple leakage checker for train/val/test splits

import math
from dataclasses import dataclass
from pathlib import Path
from typing import Iterable, Tuple, Dict, List

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt


@dataclass
class TrajectorySE2:
    t: np.ndarray  # shape (N,)
    x: np.ndarray  # shape (N,)
    y: np.ndarray  # shape (N,)
    yaw: np.ndarray  # shape (N,)

    @classmethod
    def from_csv(cls, path: str) -> "TrajectorySE2":
        df = pd.read_csv(path)
        required = {"t", "x", "y", "yaw"}
        if not required.issubset(df.columns):
            raise ValueError(f"{path} must contain columns {sorted(required)}")
        df = df.sort_values("t").reset_index(drop=True)
        return cls(
            t=df["t"].to_numpy(float),
            x=df["x"].to_numpy(float),
            y=df["y"].to_numpy(float),
            yaw=df["yaw"].to_numpy(float),
        )

    def to_matrix(self) -> np.ndarray:
        # Returns N x 3 matrix [x, y, yaw]
        return np.column_stack([self.x, self.y, self.yaw])


def wrap_angle(a: np.ndarray) -> np.ndarray:
    return (a + np.pi) % (2.0 * np.pi) - np.pi


def interp_se2(ref: TrajectorySE2, tq: np.ndarray) -> np.ndarray:
    """
    Linear interpolation in x, y and wrapped interpolation in yaw.
    Returns array of shape (len(tq), 3).
    """
    if np.any(tq < ref.t[0]) or np.any(tq > ref.t[-1]):
        raise ValueError("Query timestamps must lie inside reference interval")

    xq = np.interp(tq, ref.t, ref.x)
    yq = np.interp(tq, ref.t, ref.y)

    # Wrapped yaw interpolation: interpolate sin/cos then recover angle
    c = np.interp(tq, ref.t, np.cos(ref.yaw))
    s = np.interp(tq, ref.t, np.sin(ref.yaw))
    yawq = np.arctan2(s, c)
    return np.column_stack([xq, yq, yawq])


def rigid_align_2d(est_xy: np.ndarray, gt_xy: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
    """
    Umeyama-style rigid alignment without scale in 2D.
    est_xy, gt_xy: N x 2 arrays.
    Returns (R, t) so that aligned = est_xy @ R.T + t is aligned to gt_xy.
    """
    if est_xy.shape != gt_xy.shape or est_xy.shape[1] != 2:
        raise ValueError("Inputs must both be N x 2")
    mu_e = est_xy.mean(axis=0)
    mu_g = gt_xy.mean(axis=0)
    E = est_xy - mu_e
    G = gt_xy - mu_g
    H = E.T @ G
    U, _, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1.0
        R = Vt.T @ U.T
    t = mu_g - mu_e @ R.T
    return R, t


def compute_ate_rmse(gt: TrajectorySE2, est: TrajectorySE2, align: bool = True) -> Dict[str, float]:
    # Associate by estimator timestamps (common in benchmark tools after synchronization)
    gt_interp = interp_se2(gt, est.t)
    est_pose = est.to_matrix().copy()

    if align:
        R, t = rigid_align_2d(est_pose[:, :2], gt_interp[:, :2])
        est_pose[:, :2] = est_pose[:, :2] @ R.T + t

    e_xy = est_pose[:, :2] - gt_interp[:, :2]
    e_yaw = wrap_angle(est_pose[:, 2] - gt_interp[:, 2])

    trans_rmse = float(np.sqrt(np.mean(np.sum(e_xy**2, axis=1))))
    yaw_rmse = float(np.sqrt(np.mean(e_yaw**2)))
    return {"ATE_trans_rmse_m": trans_rmse, "ATE_yaw_rmse_rad": yaw_rmse}


def relative_motion(pose: np.ndarray, idx0: int, idx1: int) -> np.ndarray:
    """
    pose: N x 3 [x, y, yaw], returns relative SE2 as [dx_body, dy_body, dyaw]
    """
    x0, y0, th0 = pose[idx0]
    x1, y1, th1 = pose[idx1]
    dxw = x1 - x0
    dyw = y1 - y0
    c = math.cos(th0)
    s = math.sin(th0)
    dx = c * dxw + s * dyw
    dy = -s * dxw + c * dyw
    dth = ((th1 - th0 + np.pi) % (2*np.pi)) - np.pi
    return np.array([dx, dy, dth], dtype=float)


def compute_rpe(gt: TrajectorySE2, est: TrajectorySE2, delta_t: float = 1.0) -> Dict[str, float]:
    gt_pose = interp_se2(gt, est.t)
    est_pose = est.to_matrix()
    pairs: List[Tuple[int, int]] = []
    for i in range(len(est.t)):
        target_t = est.t[i] + delta_t
        j = int(np.searchsorted(est.t, target_t))
        if j < len(est.t):
            pairs.append((i, j))
    if not pairs:
        raise ValueError("No valid pairs for the requested delta_t")

    e_trans = []
    e_yaw = []
    for i, j in pairs:
        d_gt = relative_motion(gt_pose, i, j)
        d_est = relative_motion(est_pose, i, j)
        diff = d_est - d_gt
        diff[2] = ((diff[2] + np.pi) % (2*np.pi)) - np.pi
        e_trans.append(float(np.linalg.norm(diff[:2])))
        e_yaw.append(float(abs(diff[2])))

    return {
        "RPE_trans_mean_m": float(np.mean(e_trans)),
        "RPE_trans_rmse_m": float(np.sqrt(np.mean(np.square(e_trans)))),
        "RPE_yaw_mean_rad": float(np.mean(e_yaw)),
        "num_pairs": len(pairs),
    }


def normalized_score(metrics: Dict[str, float], caps: Dict[str, float]) -> float:
    """
    Convert a dictionary of 'lower-is-better' metrics into [0,100].
    score_j = max(0, 1 - m_j / cap_j)
    final score = 100 * weighted average (uniform weights here)
    """
    vals = []
    for k, cap in caps.items():
        if cap <= 0:
            raise ValueError(f"cap for {k} must be positive")
        m = metrics[k]
        vals.append(max(0.0, 1.0 - m / cap))
    return 100.0 * float(np.mean(vals))


def leakage_check(splits: pd.DataFrame) -> pd.DataFrame:
    """
    splits columns: sequence_id, split in {train,val,test}
    Flags sequences appearing in more than one split.
    """
    bad = (
        splits.groupby("sequence_id")["split"]
        .nunique()
        .reset_index(name="num_splits")
        .query("num_splits > 1")
    )
    return bad


def make_demo_data(out_dir: Path, n: int = 600, dt: float = 0.1) -> None:
    out_dir.mkdir(parents=True, exist_ok=True)
    t = np.arange(n) * dt
    # Ground truth: smooth path
    x = 0.5 * t + 2.0*np.sin(0.15*t)
    y = 1.5*np.cos(0.10*t)
    yaw = np.arctan2(np.gradient(y, dt), np.gradient(x, dt))
    gt = pd.DataFrame({"t": t, "x": x, "y": y, "yaw": yaw})

    # Estimated trajectory: drift + noise + slight time warp
    t_est = t.copy()
    t_est = t_est + 0.01*np.sin(0.05*t_est)  # small timestamp distortion
    x_e = x + 0.03*t + 0.08*np.random.randn(n)
    y_e = y - 0.02*t + 0.08*np.random.randn(n)
    yaw_e = yaw + np.deg2rad(2.0)*np.sin(0.2*t) + np.deg2rad(1.0)*np.random.randn(n)
    est = pd.DataFrame({"t": t_est, "x": x_e, "y": y_e, "yaw": yaw_e})

    gt.to_csv(out_dir / "gt_traj.csv", index=False)
    est.to_csv(out_dir / "est_traj.csv", index=False)

    splits = pd.DataFrame({
        "sequence_id": ["S01", "S02", "S03", "S02", "S04"],
        "split": ["train", "train", "val", "test", "test"]
    })
    splits.to_csv(out_dir / "splits.csv", index=False)


def main() -> None:
    np.random.seed(7)
    data_dir = Path("benchmark_demo")
    make_demo_data(data_dir)

    gt = TrajectorySE2.from_csv(str(data_dir / "gt_traj.csv"))
    est = TrajectorySE2.from_csv(str(data_dir / "est_traj.csv"))

    ate = compute_ate_rmse(gt, est, align=True)
    rpe = compute_rpe(gt, est, delta_t=1.0)
    print("ATE:", ate)
    print("RPE:", rpe)

    caps = {
        "ATE_trans_rmse_m": 2.0,
        "RPE_trans_rmse_m": 0.8,
    }
    score = normalized_score(
        {"ATE_trans_rmse_m": ate["ATE_trans_rmse_m"], "RPE_trans_rmse_m": rpe["RPE_trans_rmse_m"]},
        caps
    )
    print(f"Normalized aggregate score: {score:.2f}/100")

    splits = pd.read_csv(data_dir / "splits.csv")
    bad = leakage_check(splits)
    if len(bad) > 0:
        print("\nData leakage warning:")
        print(bad.to_string(index=False))

    # Visualization
    gt_interp = interp_se2(gt, est.t)
    R, tvec = rigid_align_2d(est.to_matrix()[:, :2], gt_interp[:, :2])
    est_aligned = est.to_matrix().copy()
    est_aligned[:, :2] = est_aligned[:, :2] @ R.T + tvec
    e = est_aligned[:, :2] - gt_interp[:, :2]
    e_norm = np.linalg.norm(e, axis=1)

    plt.figure(figsize=(7, 5))
    plt.plot(gt.x, gt.y, label="Ground truth")
    plt.plot(est_aligned[:, 0], est_aligned[:, 1], label="Estimate (aligned)")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")
    plt.title("Trajectory Comparison")
    plt.axis("equal")
    plt.legend()
    plt.tight_layout()
    plt.savefig(data_dir / "trajectory_compare.png", dpi=140)

    plt.figure(figsize=(7, 3))
    plt.plot(est.t, e_norm)
    plt.xlabel("time [s]")
    plt.ylabel("pos error [m]")
    plt.title("Instantaneous Position Error")
    plt.tight_layout()
    plt.savefig(data_dir / "error_over_time.png", dpi=140)
    plt.show()


if __name__ == "__main__":
    main()

In production AMR stacks, evaluation tools are often written in C++ for integration with ROS 2, SLAM backends, and large log-processing pipelines. The following code uses Eigen and implements CSV trajectory loading, interpolation, rigid alignment, and ATE/RPE computation.

Chapter19_Lesson3.cpp

// Chapter19_Lesson3.cpp
// Standard Datasets and Simulated Benchmarks for AMR
// C++17 example (Eigen) for SE(2) trajectory benchmark metrics.
//
// Build example:
//   g++ -std=c++17 Chapter19_Lesson3.cpp -O2 -I /path/to/eigen -o ch19_l3
//
// CSV format expected: t,x,y,yaw

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>

struct TrajectorySE2 {
  std::vector<double> t, x, y, yaw;
};

static double WrapAngle(double a) {
  while (a > M_PI) a -= 2.0 * M_PI;
  while (a < -M_PI) a += 2.0 * M_PI;
  return a;
}

TrajectorySE2 LoadCSV(const std::string& path) {
  std::ifstream fin(path);
  if (!fin) throw std::runtime_error("Cannot open file: " + path);

  TrajectorySE2 tr;
  std::string line;
  std::getline(fin, line);  // header
  while (std::getline(fin, line)) {
    if (line.empty()) continue;
    std::stringstream ss(line);
    std::string cell;
    std::vector<double> row;
    while (std::getline(ss, cell, ',')) row.push_back(std::stod(cell));
    if (row.size() != 4) throw std::runtime_error("CSV row must have 4 columns");
    tr.t.push_back(row[0]);
    tr.x.push_back(row[1]);
    tr.y.push_back(row[2]);
    tr.yaw.push_back(row[3]);
  }
  return tr;
}

size_t FindSegment(const std::vector<double>& t, double tq) {
  if (tq < t.front() || tq > t.back()) throw std::runtime_error("Query time out of bounds");
  auto it = std::lower_bound(t.begin(), t.end(), tq);
  if (it == t.begin()) return 0;
  if (it == t.end()) return t.size() - 2;
  size_t j = static_cast<size_t>(it - t.begin());
  if (*it == tq) return (j == t.size() - 1) ? j - 1 : j;
  return j - 1;
}

Eigen::Vector3d InterpPose(const TrajectorySE2& tr, double tq) {
  size_t i = FindSegment(tr.t, tq);
  size_t j = i + 1;
  double a = (tq - tr.t[i]) / (tr.t[j] - tr.t[i]);

  double x = (1.0 - a) * tr.x[i] + a * tr.x[j];
  double y = (1.0 - a) * tr.y[i] + a * tr.y[j];
  double c = (1.0 - a) * std::cos(tr.yaw[i]) + a * std::cos(tr.yaw[j]);
  double s = (1.0 - a) * std::sin(tr.yaw[i]) + a * std::sin(tr.yaw[j]);
  double yaw = std::atan2(s, c);
  return {x, y, yaw};
}

std::pair<Eigen::Matrix2d, Eigen::Vector2d> Align2D(
    const std::vector<Eigen::Vector2d>& est,
    const std::vector<Eigen::Vector2d>& gt) {
  if (est.size() != gt.size() || est.empty()) throw std::runtime_error("Invalid alignment input");

  Eigen::Vector2d mu_e = Eigen::Vector2d::Zero();
  Eigen::Vector2d mu_g = Eigen::Vector2d::Zero();
  for (size_t i = 0; i < est.size(); ++i) {
    mu_e += est[i];
    mu_g += gt[i];
  }
  mu_e /= static_cast<double>(est.size());
  mu_g /= static_cast<double>(gt.size());

  Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
  for (size_t i = 0; i < est.size(); ++i) {
    H += (est[i] - mu_e) * (gt[i] - mu_g).transpose();
  }

  Eigen::JacobiSVD<Eigen::Matrix2d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix2d R = svd.matrixV() * svd.matrixU().transpose();
  if (R.determinant() < 0.0) {
    Eigen::Matrix2d V = svd.matrixV();
    V.col(1) *= -1.0;
    R = V * svd.matrixU().transpose();
  }
  Eigen::Vector2d t = mu_g - R * mu_e;
  return {R, t};
}

struct Metrics {
  double ate_rmse_m = 0.0;
  double rpe_rmse_m = 0.0;
  double rpe_yaw_mean_rad = 0.0;
  size_t pairs = 0;
};

Eigen::Vector3d RelativeSE2(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
  double dxw = p1.x() - p0.x();
  double dyw = p1.y() - p0.y();
  double c = std::cos(p0.z()), s = std::sin(p0.z());
  double dx = c * dxw + s * dyw;
  double dy = -s * dxw + c * dyw;
  double dth = WrapAngle(p1.z() - p0.z());
  return {dx, dy, dth};
}

Metrics Evaluate(const TrajectorySE2& gt, const TrajectorySE2& est, double delta_t) {
  std::vector<Eigen::Vector3d> gt_i, est_p;
  std::vector<Eigen::Vector2d> gt_xy, est_xy;
  gt_i.reserve(est.t.size());
  est_p.reserve(est.t.size());
  for (size_t i = 0; i < est.t.size(); ++i) {
    Eigen::Vector3d g = InterpPose(gt, est.t[i]);
    Eigen::Vector3d e(est.x[i], est.y[i], est.yaw[i]);
    gt_i.push_back(g);
    est_p.push_back(e);
    gt_xy.push_back(g.head<2>());
    est_xy.push_back(e.head<2>());
  }

  auto [R, t] = Align2D(est_xy, gt_xy);
  std::vector<Eigen::Vector3d> est_aligned = est_p;
  for (auto& p : est_aligned) p.head<2>() = R * p.head<2>() + t;

  Metrics m;
  double sse = 0.0;
  for (size_t i = 0; i < est_aligned.size(); ++i) {
    Eigen::Vector2d e = est_aligned[i].head<2>() - gt_i[i].head<2>();
    sse += e.squaredNorm();
  }
  m.ate_rmse_m = std::sqrt(sse / static_cast<double>(est_aligned.size()));

  std::vector<double> rpe_trans, rpe_yaw;
  for (size_t i = 0; i < est.t.size(); ++i) {
    double target = est.t[i] + delta_t;
    auto it = std::lower_bound(est.t.begin(), est.t.end(), target);
    if (it == est.t.end()) continue;
    size_t j = static_cast<size_t>(it - est.t.begin());

    Eigen::Vector3d d_gt = RelativeSE2(gt_i[i], gt_i[j]);
    Eigen::Vector3d d_est = RelativeSE2(est_aligned[i], est_aligned[j]);
    Eigen::Vector3d diff = d_est - d_gt;
    diff.z() = WrapAngle(diff.z());

    rpe_trans.push_back(diff.head<2>().norm());
    rpe_yaw.push_back(std::abs(diff.z()));
  }

  m.pairs = rpe_trans.size();
  if (!rpe_trans.empty()) {
    double sse_r = 0.0, sum_y = 0.0;
    for (double v : rpe_trans) sse_r += v * v;
    for (double v : rpe_yaw) sum_y += v;
    m.rpe_rmse_m = std::sqrt(sse_r / static_cast<double>(rpe_trans.size()));
    m.rpe_yaw_mean_rad = sum_y / static_cast<double>(rpe_yaw.size());
  }
  return m;
}

int main(int argc, char** argv) {
  try {
    if (argc < 3) {
      std::cerr << "Usage: " << argv[0] << " gt.csv est.csv [delta_t]\n";
      return 1;
    }
    double delta_t = (argc >= 4) ? std::stod(argv[3]) : 1.0;
    TrajectorySE2 gt = LoadCSV(argv[1]);
    TrajectorySE2 est = LoadCSV(argv[2]);
    Metrics m = Evaluate(gt, est, delta_t);

    std::cout << std::fixed << std::setprecision(6);
    std::cout << "ATE_trans_rmse_m=" << m.ate_rmse_m << "\n";
    std::cout << "RPE_trans_rmse_m=" << m.rpe_rmse_m << "\n";
    std::cout << "RPE_yaw_mean_rad=" << m.rpe_yaw_mean_rad << "\n";
    std::cout << "Pairs=" << m.pairs << "\n";
    return 0;
  } catch (const std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 2;
  }
}

Java is useful in enterprise data pipelines, benchmarking dashboards, and log processing services. The example below provides a pure-Java evaluator for basic SE(2) trajectory metrics. In larger projects, libraries such as EJML or Apache Commons Math can be added for matrix and statistics utilities.

Chapter19_Lesson3.java

// Chapter19_Lesson3.java
// Standard Datasets and Simulated Benchmarks for AMR
// Pure Java reference implementation for simple trajectory metric computation.
//
// CSV format: t,x,y,yaw
// Compile: javac Chapter19_Lesson3.java
// Run:     java Chapter19_Lesson3 gt.csv est.csv

import java.io.BufferedReader;
import java.io.FileReader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

public class Chapter19_Lesson3 {

    static class TrajectorySE2 {
        List<Double> t = new ArrayList<>();
        List<Double> x = new ArrayList<>();
        List<Double> y = new ArrayList<>();
        List<Double> yaw = new ArrayList<>();
    }

    static class Pose {
        double x, y, yaw;
        Pose(double x, double y, double yaw) {
            this.x = x; this.y = y; this.yaw = yaw;
        }
    }

    static class Metrics {
        double ateRmse;
        double rpeRmse;
        double rpeYawMean;
        int pairs;
    }

    static TrajectorySE2 loadCsv(String path) throws IOException {
        TrajectorySE2 tr = new TrajectorySE2();
        try (BufferedReader br = new BufferedReader(new FileReader(path))) {
            String line = br.readLine(); // header
            if (line == null) throw new IOException("Empty file");
            while ((line = br.readLine()) != null) {
                if (line.trim().isEmpty()) continue;
                String[] parts = line.split(",");
                if (parts.length != 4) throw new IOException("Expected 4 columns in " + path);
                tr.t.add(Double.parseDouble(parts[0].trim()));
                tr.x.add(Double.parseDouble(parts[1].trim()));
                tr.y.add(Double.parseDouble(parts[2].trim()));
                tr.yaw.add(Double.parseDouble(parts[3].trim()));
            }
        }
        return tr;
    }

    static double wrapAngle(double a) {
        while (a > Math.PI) a -= 2.0 * Math.PI;
        while (a < -Math.PI) a += 2.0 * Math.PI;
        return a;
    }

    static int findSegment(List<Double> t, double tq) {
        if (tq < t.get(0) || tq > t.get(t.size()-1)) {
            throw new IllegalArgumentException("query time out of bounds");
        }
        int j = Collections.binarySearch(t, tq);
        if (j >= 0) {
            if (j == t.size() - 1) return j - 1;
            return j;
        }
        j = -j - 1;
        return Math.max(0, j - 1);
    }

    static Pose interpPose(TrajectorySE2 tr, double tq) {
        int i = findSegment(tr.t, tq);
        int j = i + 1;
        double ti = tr.t.get(i), tj = tr.t.get(j);
        double a = (tq - ti) / (tj - ti);

        double x = (1.0 - a) * tr.x.get(i) + a * tr.x.get(j);
        double y = (1.0 - a) * tr.y.get(i) + a * tr.y.get(j);

        double c = (1.0 - a) * Math.cos(tr.yaw.get(i)) + a * Math.cos(tr.yaw.get(j));
        double s = (1.0 - a) * Math.sin(tr.yaw.get(i)) + a * Math.sin(tr.yaw.get(j));
        double yaw = Math.atan2(s, c);
        return new Pose(x, y, yaw);
    }

    static double[] relativeSE2(Pose p0, Pose p1) {
        double dxw = p1.x - p0.x;
        double dyw = p1.y - p0.y;
        double c = Math.cos(p0.yaw), s = Math.sin(p0.yaw);
        double dx = c * dxw + s * dyw;
        double dy = -s * dxw + c * dyw;
        double dyaw = wrapAngle(p1.yaw - p0.yaw);
        return new double[]{dx, dy, dyaw};
    }

    static Metrics evaluate(TrajectorySE2 gt, TrajectorySE2 est, double deltaT) {
        List<Pose> gtInterp = new ArrayList<>();
        List<Pose> estPose = new ArrayList<>();

        for (int i = 0; i < est.t.size(); i++) {
            Pose g = interpPose(gt, est.t.get(i));
            Pose e = new Pose(est.x.get(i), est.y.get(i), est.yaw.get(i));
            gtInterp.add(g);
            estPose.add(e);
        }

        // For simplicity, no global rigid alignment in this Java example (compare in common frame)
        double sse = 0.0;
        for (int i = 0; i < estPose.size(); i++) {
            double ex = estPose.get(i).x - gtInterp.get(i).x;
            double ey = estPose.get(i).y - gtInterp.get(i).y;
            sse += ex*ex + ey*ey;
        }

        Metrics m = new Metrics();
        m.ateRmse = Math.sqrt(sse / estPose.size());

        double sseRpe = 0.0;
        double sumYaw = 0.0;
        int pairs = 0;
        for (int i = 0; i < est.t.size(); i++) {
            double target = est.t.get(i) + deltaT;
            int j = -1;
            for (int k = i + 1; k < est.t.size(); k++) {
                if (est.t.get(k) >= target) { j = k; break; }
            }
            if (j < 0) continue;

            double[] dgt = relativeSE2(gtInterp.get(i), gtInterp.get(j));
            double[] dest = relativeSE2(estPose.get(i), estPose.get(j));

            double dx = dest[0] - dgt[0];
            double dy = dest[1] - dgt[1];
            double dyaw = wrapAngle(dest[2] - dgt[2]);

            sseRpe += dx*dx + dy*dy;
            sumYaw += Math.abs(dyaw);
            pairs++;
        }

        m.pairs = pairs;
        if (pairs > 0) {
            m.rpeRmse = Math.sqrt(sseRpe / pairs);
            m.rpeYawMean = sumYaw / pairs;
        }
        return m;
    }

    public static void main(String[] args) {
        if (args.length < 2) {
            System.err.println("Usage: java Chapter19_Lesson3 gt.csv est.csv [delta_t]");
            System.exit(1);
        }
        try {
            double deltaT = (args.length >= 3) ? Double.parseDouble(args[2]) : 1.0;
            TrajectorySE2 gt = loadCsv(args[0]);
            TrajectorySE2 est = loadCsv(args[1]);
            Metrics m = evaluate(gt, est, deltaT);

            System.out.printf("ATE_trans_rmse_m = %.6f%n", m.ateRmse);
            System.out.printf("RPE_trans_rmse_m = %.6f%n", m.rpeRmse);
            System.out.printf("RPE_yaw_mean_rad = %.6f%n", m.rpeYawMean);
            System.out.printf("Pairs = %d%n", m.pairs);

            // Example normalized score (lower-is-better metrics)
            double capATE = 2.0, capRPE = 0.8;
            double sATE = Math.max(0.0, 1.0 - m.ateRmse / capATE);
            double sRPE = Math.max(0.0, 1.0 - m.rpeRmse / capRPE);
            double score = 100.0 * 0.5 * (sATE + sRPE);
            System.out.printf("Normalized score = %.2f/100%n", score);

        } catch (Exception ex) {
            ex.printStackTrace();
            System.exit(2);
        }
    }
}

MATLAB is widely used in control engineering labs. This script computes the same metrics and plots. In Simulink workflows, the metric functions can be wrapped in a MATLAB Function block for hardware-in-the-loop or replay test harnesses.

Chapter19_Lesson3.m

% Chapter19_Lesson3.m
% Standard Datasets and Simulated Benchmarks for AMR
% MATLAB/Simulink-oriented script for trajectory benchmark metrics (SE(2)).
%
% CSV columns required: t, x, y, yaw

clear; clc;

gtFile  = 'gt_traj.csv';
estFile = 'est_traj.csv';

if ~isfile(gtFile) || ~isfile(estFile)
    fprintf('Demo CSV files not found. Generating synthetic example...\n');
    n = 500; dt = 0.1; t = (0:n-1)' * dt;
    x  = 0.5*t + 1.5*sin(0.15*t);
    y  = 1.2*cos(0.11*t);
    yw = atan2(gradient(y, dt), gradient(x, dt));
    gt = table(t, x, y, yw, 'VariableNames', {'t','x','y','yaw'});
    writetable(gt, gtFile);

    est = gt;
    est.t   = est.t + 0.005*sin(0.04*est.t);
    est.x   = est.x + 0.03*est.t + 0.05*randn(size(est.x));
    est.y   = est.y - 0.02*est.t + 0.05*randn(size(est.y));
    est.yaw = est.yaw + deg2rad(1.5)*sin(0.2*est.t) + deg2rad(0.8)*randn(size(est.yaw));
    writetable(est, estFile);
end

gt  = readtable(gtFile);
est = readtable(estFile);

% Interpolate GT onto estimator timestamps
tq = est.t;
gt_i.x = interp1(gt.t, gt.x, tq, 'linear');
gt_i.y = interp1(gt.t, gt.y, tq, 'linear');
c = interp1(gt.t, cos(gt.yaw), tq, 'linear');
s = interp1(gt.t, sin(gt.yaw), tq, 'linear');
gt_i.yaw = atan2(s, c);

% 2D rigid alignment (Kabsch without scale)
E = [est.x, est.y];
G = [gt_i.x, gt_i.y];
muE = mean(E,1); muG = mean(G,1);
H = (E - muE)' * (G - muG);
[U,~,V] = svd(H);
R = V * U';
if det(R) < 0
    V(:,end) = -V(:,end);
    R = V * U';
end
tvec = muG' - R * muE';
E_aligned = (R * E')' + tvec';

% ATE
e_xy = E_aligned - G;
ate_rmse = sqrt(mean(sum(e_xy.^2,2)));

% RPE for delta_t = 1s
delta_t = 1.0;
e_rpe = [];
e_yaw = [];
for i = 1:length(tq)
    j = find(tq >= tq(i) + delta_t, 1, 'first');
    if isempty(j), continue; end

    d_gt = relSE2([gt_i.x(i), gt_i.y(i), gt_i.yaw(i)], [gt_i.x(j), gt_i.y(j), gt_i.yaw(j)]);
    d_es = relSE2([E_aligned(i,1), E_aligned(i,2), est.yaw(i)], [E_aligned(j,1), E_aligned(j,2), est.yaw(j)]);

    diff = d_es - d_gt;
    diff(3) = wrapAngle(diff(3));
    e_rpe(end+1,1) = norm(diff(1:2)); %#ok<AGROW>
    e_yaw(end+1,1) = abs(diff(3)); %#ok<AGROW>
end

rpe_rmse = sqrt(mean(e_rpe.^2));
rpe_yaw_mean = mean(e_yaw);

fprintf('ATE_trans_rmse_m = %.6f\n', ate_rmse);
fprintf('RPE_trans_rmse_m = %.6f\n', rpe_rmse);
fprintf('RPE_yaw_mean_rad = %.6f\n', rpe_yaw_mean);

% Normalized benchmark score
capATE = 2.0; capRPE = 0.8;
score = 100 * mean([max(0, 1 - ate_rmse/capATE), max(0, 1 - rpe_rmse/capRPE)]);
fprintf('Normalized score = %.2f / 100\n', score);

% Plots
figure;
plot(gt.x, gt.y, 'LineWidth', 1.5); hold on;
plot(E_aligned(:,1), E_aligned(:,2), 'LineWidth', 1.2);
axis equal; grid on;
xlabel('x [m]'); ylabel('y [m]');
legend('Ground truth','Estimate (aligned)');
title('Trajectory comparison');

figure;
plot(tq, sqrt(sum(e_xy.^2,2)), 'LineWidth', 1.2); grid on;
xlabel('time [s]'); ylabel('error [m]');
title('Position error over time');

% ---- local functions ----
function d = relSE2(p0, p1)
    % p = [x y yaw]
    dxw = p1(1) - p0(1);
    dyw = p1(2) - p0(2);
    c = cos(p0(3)); s = sin(p0(3));
    dx = c*dxw + s*dyw;
    dy = -s*dxw + c*dyw;
    dth = wrapAngle(p1(3) - p0(3));
    d = [dx, dy, dth];
end

function a = wrapAngle(a)
    a = mod(a + pi, 2*pi) - pi;
end

Mathematica is convenient for symbolic derivation and rapid metric prototyping. The notebook below defines interpolation, ATE, RPE, and normalized scoring in Wolfram Language.

Chapter19_Lesson3.nb

Notebook[{
  Cell["Chapter19_Lesson3.nb — Standard Datasets and Simulated Benchmarks for AMR", "Title"],
  Cell["Wolfram Mathematica notebook for SE(2) trajectory metrics (ATE/RPE) and normalized scoring.", "Text"],
  Cell[BoxData[
    ToBoxes[
      ClearAll[WrapAngle, InterpYaw, InterpPose, RelativeSE2, ATErmse, RPErmse, NormalizeScore]
    ]
  ], "Input"],
  Cell[BoxData[
    ToBoxes[
      WrapAngle[a_] := Mod[a + Pi, 2 Pi] - Pi
    ]
  ], "Input"],
  Cell[BoxData[
    ToBoxes[
      InterpYaw[t_, ts_, yaws_] := Module[{c, s},
        c = Interpolation[Transpose[{ts, Cos /@ yaws}], InterpolationOrder -> 1][t];
        s = Interpolation[Transpose[{ts, Sin /@ yaws}], InterpolationOrder -> 1][t];
        ArcTan[c, s]
      ]
    ]
  ], "Input"],
  Cell[BoxData[
    ToBoxes[
      InterpPose[t_, traj_List] := Module[{ts, xs, ys, yaws, fx, fy},
        ts = traj[[All, 1]]; xs = traj[[All, 2]]; ys = traj[[All, 3]]; yaws = traj[[All, 4]];
        fx = Interpolation[Transpose[{ts, xs}], InterpolationOrder -> 1];
        fy = Interpolation[Transpose[{ts, ys}], InterpolationOrder -> 1];
        {fx[t], fy[t], InterpYaw[t, ts, yaws]}
      ]
    ]
  ], "Input"],
  Cell[BoxData[
    ToBoxes[
      RelativeSE2[p0_, p1_] := Module[{dxw, dyw, c, s},
        dxw = p1[[1]] - p0[[1]];
        dyw = p1[[2]] - p0[[2]];
        c = Cos[p0[[3]]]; s = Sin[p0[[3]]];
        {c dxw + s dyw, -s dxw + c dyw, WrapAngle[p1[[3]] - p0[[3]]]}
      ]
    ]
  ], "Input"],
  Cell[BoxData[
    ToBoxes[
      ATErmse[gt_List, est_List] := Module[{tq, gtI, errs},
        tq = est[[All, 1]];
        gtI = InterpPose[#, gt] & /@ tq;
        errs = Norm /@ (est[[All, {2, 3}]] - gtI[[All, {1, 2}]]);
        Sqrt[Mean[errs^2]]
      ]
    ]
  ], "Input"],
  Cell[BoxData[
    ToBoxes[
      RPErmse[gt_List, est_List, deltaT_:1.0] := Module[{tq, gtI, pairs, diffs},
        tq = est[[All, 1]];
        gtI = InterpPose[#, gt] & /@ tq;
        pairs = Select[
          Table[{i, FirstPosition[tq, _?(# >= tq[[i]] + deltaT &), Missing["NotFound"]]}, {i, Length[tq]}],
          #[[2]] =!= Missing["NotFound"] &
        ];
        diffs = Table[
          Module[{dgt, des, d},
            dgt = RelativeSE2[gtI[[pairs[[k, 1]]]], gtI[[pairs[[k, 2, 1]]]]];
            des = RelativeSE2[est[[pairs[[k, 1]], {2, 3, 4}]], est[[pairs[[k, 2, 1]], {2, 3, 4}]]];
            d = des - dgt;
            d[[3]] = WrapAngle[d[[3]]];
            {Norm[d[[1 ;; 2]]], Abs[d[[3]]]}
          ],
          {k, Length[pairs]}
        ];
        <|
          "RPETransRMSE" -> Sqrt[Mean[diffs[[All, 1]]^2]],
          "RPEYawMean" -> Mean[diffs[[All, 2]]],
          "Pairs" -> Length[pairs]
        |>
      ]
    ]
  ], "Input"],
  Cell[BoxData[
    ToBoxes[
      NormalizeScore[metrics_Association, caps_Association] := Module[{keys, vals},
        keys = Keys[caps];
        vals = Table[Max[0, 1 - metrics[key]/caps[key]], {key, keys}];
        100 Mean[vals]
      ]
    ]
  ], "Input"],
  Cell["Demo synthetic trajectories", "Section"],
  Cell[BoxData[
    ToBoxes[
      Module[{n = 400, dt = 0.1, t, gt, est, metrics},
        SeedRandom[11];
        t = N[Range[0, n - 1] dt];
        gt = Table[
          {tt, 0.4 tt + 1.2 Sin[0.14 tt], 1.0 Cos[0.10 tt], 0.2 Sin[0.08 tt]},
          {tt, t}
        ];
        est = Table[
          {tt + 0.004 Sin[0.05 tt],
           (0.4 tt + 1.2 Sin[0.14 tt]) + 0.03 tt + RandomVariate[NormalDistribution[0, 0.05]],
           (1.0 Cos[0.10 tt]) - 0.02 tt + RandomVariate[NormalDistribution[0, 0.05]],
           0.2 Sin[0.08 tt] + RandomVariate[NormalDistribution[0, 0.02]]},
          {tt, t}
        ];
        metrics = <|
          "ATE_trans_rmse_m" -> ATErmse[gt, est],
          "RPE_trans_rmse_m" -> RPErmse[gt, est]["RPETransRMSE"]
        |>;
        <|
          "ATE_trans_rmse_m" -> metrics["ATE_trans_rmse_m"],
          "RPE" -> RPErmse[gt, est],
          "NormalizedScore" -> NormalizeScore[metrics, <|"ATE_trans_rmse_m" -> 2.0, "RPE_trans_rmse_m" -> 0.8|>]
        |>
      ]
    ]
  ], "Input"]
},
WindowTitle -> "Chapter19_Lesson3.nb"
]

Problem 1 (Paired vs unpaired comparison): Suppose two SLAM methods are evaluated on the same set of \( n \) sequences, with additive model \( m_{a,s}=\mu_a+\gamma_s+\varepsilon_{a,s} \). Show that the paired estimator \( \hat{\delta}=\frac{1}{n}\sum_s(m_{1,s}-m_{2,s}) \) is unbiased for \( \mu_1-\mu_2 \).

Solution: By linearity of expectation,

\[ \mathbb{E}[\hat{\delta}] = \frac{1}{n}\sum_s \mathbb{E}\!\left[(\mu_1+\gamma_s+\varepsilon_{1,s})-(\mu_2+\gamma_s+\varepsilon_{2,s})\right] = \mu_1-\mu_2 \]

assuming \( \mathbb{E}[\varepsilon_{a,s}] = 0 \). The sequence term \( \gamma_s \) cancels exactly, which is the key benefit of paired benchmarking.

Problem 2 (Interpolation error bound): Let a scalar trajectory component \( f(t) \) satisfy \( |f''(t)| \le M \) on \( [t_i,t_{i+1}] \) with step \( h=t_{i+1}-t_i \). Prove that the linear interpolation error obeys \( |f(t)-f_{lin}(t)| \le Mh^2/8 \).

Solution: The interpolation remainder is \( f(t)-f_{lin}(t)=\frac{f''(\xi)}{2}(t-t_i)(t-t_{i+1}) \) for some \( \xi \in [t_i,t_{i+1}] \). Hence

\[ |f(t)-f_{lin}(t)| \le \frac{M}{2}\max_{t\in[t_i,t_{i+1}]}\left|(t-t_i)(t-t_{i+1})\right| = \frac{M}{2}\cdot\frac{h^2}{4} = \frac{Mh^2}{8} \]

The maximum occurs at the midpoint. This motivates reporting benchmark timestamp resolution.

Problem 3 (Normalized aggregate score): Consider subscores \( s_j = \max(0,1-m_j/c_j) \) with \( c_j > 0 \) and \( S=100\sum_j w_j s_j \). Show that \( S \) is Lipschitz in each metric \( m_j \).

Solution: For fixed \( j \), the function \( s_j(m_j) \) has slope either \( 0 \) or \( -1/c_j \), so it is \( 1/c_j \)-Lipschitz. Multiplying by \( 100w_j \) gives

\[ |S(m_j)-S(m_j')| \le 100\frac{w_j}{c_j}|m_j-m_j'| \]

Therefore small metric perturbations (e.g., due to timestamp jitter) cannot arbitrarily change the aggregate score if caps are chosen sensibly.

Problem 4 (Session split leakage): Let frames be indexed by \( i \) and session labels be \( s(i) \). Provide a formal condition for a split function \( \sigma(i)\in\{\text{train},\text{val},\text{test}\} \) to be session-consistent, and explain why random frame-wise splitting violates it.

Solution: A split is session-consistent if

\[ s(i)=s(j) \;\Rightarrow\; \sigma(i)=\sigma(j) \]

for all frame indices \( i,j \). Random frame-wise splitting typically assigns different frames from one traversal to different subsets, violating this implication and causing temporal/data leakage.

Problem 5 (Confidence interval for paired metric differences): Given paired differences \( d_1,\dots,d_n \), derive the \( 100(1-\alpha)\% \) t-interval for the mean difference.

Solution: Let \( \bar{d}=\frac{1}{n}\sum_i d_i \) and \( s_d^2=\frac{1}{n-1}\sum_i(d_i-\bar{d})^2 \). Under the usual t-model assumptions,

\[ \frac{\bar{d}-\mu_d}{s_d/\sqrt{n}} \sim t_{n-1} \]

so the interval is

\[ \bar{d} \pm t_{n-1,1-\alpha/2}\frac{s_d}{\sqrt{n}} \]

This is the recommended way to report benchmark advantages on a fixed suite of sequences.

Standard datasets and simulated benchmarks are the infrastructure that makes AMR evaluation scientific. We formalized benchmark protocols, derived trajectory metric equations and fairness guarantees, and implemented a multi-language evaluation harness. In the next lesson, we will use these tools for systematic stress testing and ablation studies.