Calibration & ROS-Free System Infrastructure

6.1The Calibration Ladder

Every fusion equation in this book silently assumed three things were known exactly: each sensor’s intrinsics (its internal model — focal length, distortion, bias behavior), the extrinsics between sensors (the fixed T_imu_cam, T_imu_lidar of Phase 1’s frame chains), and a shared clock. Get any rung wrong and the errors don’t add — they masquerade: a 5 ms time offset looks like an IMU bias, a 1° extrinsic error looks like odometry drift, a bad focal length looks like scale drift. Whole weeks have been lost tuning filters to compensate for calibration that was never done. The ladder must be climbed in order — intrinsics first, then extrinsics, then time — because each stage’s estimator assumes the previous stage is solved.

6.2Camera Intrinsics: The Projection Contract

The pinhole model maps a 3D point in the camera frame to a pixel, and calibration is — one more time — the Phase 1 loop, with the model’s parameters as the state and checkerboard (or AprilTag) corners as the measurements:

projection:  [u]   [f_x  0   c_x] [X/Z]
             [v] = [ 0  f_y  c_y]·[Y/Z]     then distortion warps (u,v):
             [1]   [ 0   0    1 ]

radial:      x_d = x(1 + k₁r² + k₂r⁴ + k₃r⁶) + tangential p₁, p₂ terms

calibrate:   argmin_{K, dist, poses}  Σᵢⱼ ‖ u_ij − π(K, dist, T_j, P_i) ‖²
             → LM over intrinsics AND board poses jointly (Phase 1.8)
target RMS reprojection: < 0.5 px. Above 1 px, redo the capture, not the math.

Capture craft matters more than solver choice: fill the whole image with board views (distortion lives at the edges), tilt the board (foreshortening constrains focal length), lock exposure and focus, and prefer AprilTag grids over checkerboards — each tag’s identity is unambiguous, so partial views and motion blur stop silently corrupting corners. Rolling-shutter cameras need the rolling-shutter model or slow motion during capture; fisheye lenses need their own projection models (equidistant, double-sphere), not more radial terms bolted onto pinhole.

6.3IMU Intrinsics: Allan Variance

Phase 2’s filters need two numbers per gyro/accel axis that datasheets state optimistically and temperature ruins: the white noise density (how noisy each sample is) and the bias random walk (how fast the bias wanders). The standard instrument is the Allan variance: record the stationary sensor for hours, average the signal over windows of growing length τ, and study how the averages disagree as a function of τ:

σ²(τ) = ½ ⟨ ( ȳ_{k+1}(τ) − ȳ_k(τ) )² ⟩       ȳ_k: mean over the k-th window

log-log plot of σ(τ) vs τ reads like a spectrogram of error sources:
  slope −½  →  white noise:        σ(τ) = N / √τ      → read N at τ = 1 s
  slope  0  →  bias instability    (the flat floor)
  slope +½  →  rate random walk:   σ(τ) = K·√(τ/3)    → read K on the rise
averaging longer first helps (noise averages out), then hurts (bias wanders)

// allan.cc — simulate a stationary gyro, recover N and K from Allan deviation.
#include <cmath>
#include <iostream>
#include <random>
#include <vector>

int main() {
  const double fs = 100.0, T = 7200.0;        // 100 Hz, two hours
  const double N_true = 0.005;                 // white noise  [rad/s/√Hz]
  const double K_true = 0.0001;                // rate random walk [rad/s·√Hz]
  const size_t n = size_t(fs * T);

  std::mt19937 rng(11);
  std::normal_distribution<double> gauss(0.0, 1.0);
  std::vector<double> w(n);
  double bias = 0.0;
  for (size_t i = 0; i < n; ++i) {             // rate = wandering bias + noise
    bias += K_true * std::sqrt(1.0 / fs) * gauss(rng);
    w[i] = bias + N_true * std::sqrt(fs) * gauss(rng);
  }

  std::cout << "tau(s)      sigma(rad/s)\n";
  for (double tau = 0.01; tau <= 1000.0; tau *= 2.0) {
    const size_t m = size_t(tau * fs);         // samples per window
    if (m < 1 || 2 * m >= n) continue;
    std::vector<double> avg;                   // window means
    for (size_t k = 0; k + m <= n; k += m) {
      double s = 0.0;
      for (size_t i = k; i < k + m; ++i) s += w[i];
      avg.push_back(s / m);
    }
    double acc = 0.0;                          // Allan variance
    for (size_t k = 0; k + 1 < avg.size(); ++k)
      acc += (avg[k + 1] - avg[k]) * (avg[k + 1] - avg[k]);
    const double sigma = std::sqrt(0.5 * acc / (avg.size() - 1));
    std::cout << tau << "\t" << sigma << "\n";
    if (std::abs(tau - 1.28) < 0.1)            // ≈ read-off points
      std::cout << "  → N ≈ σ(1s)·√1s = " << sigma
                << "   (true " << N_true << ")\n";
    if (std::abs(tau - 655.36) < 1.0)
      std::cout << "  → K ≈ σ(τ)·√(3/τ) = " << sigma * std::sqrt(3.0 / tau)
                << "   (true " << K_true << ")\n";
  }
}

6.4Extrinsics: Kalibr, Hand-Eye, and LiDAR Rigs

Extrinsic calibration answers: what rigid transform relates two sensors’ frames — and it needs shared observations of motion or structure. The camera–IMU case is the canon, and Kalibr is its standard tool: wave the rig in front of an AprilTag grid with rich rotation on all axes, and solve a continuous-time batch problem — the trajectory modeled as B-splines so that camera poses (from the target) and IMU samples (rates and accelerations, i.e. spline derivatives) constrain one smooth curve — estimating T_imu_cam, gravity, biases, and, crucially, the camera–IMU time offset in the same optimization. The observability fine print: without rotational excitation about all axes, the extrinsic translation is barely observable — a gentle hand-held figure-eight beats any tripod.

hand-eye (the classical form of every extrinsic problem):
  A_i · X = X · B_i         A_i: sensor-1 motion,  B_i: sensor-2 motion,
                            X = the unknown extrinsic between them
  → rotations first (quaternion linear system), then translation (linear LS)
  → needs ≥ 2 motions with non-parallel rotation axes — excitation, again

LiDAR–IMU rigs use the same principles with geometry instead of tags: LiDAR-Align optimizes the extrinsic (and time offset) by minimizing the crispness of the aggregated point cloud — with the wrong transform, walls smear when the rig moves; with the right one, they focus. Newer target-free tools (the BIFROST line and friends) push this further with continuous-time trajectories, but the acceptance test never changes: aggregate a rotation-rich log through the estimated calibration and look at the walls. Blur is truth.

6.5Time: The Third Dimension of Calibration

Two sensors with perfect intrinsics and extrinsics still disagree if their clocks do. Time errors are vicious because they scale with motion: at a yaw rate of 1 rad/s, a 5 ms offset is a 0.29° attitude error — larger than a good extrinsic calibration’s whole budget — and it vanishes on the bench where the rig sits still. The engineering hierarchy, best to worst: hardware triggering (one signal line exposes the camera and timestamps the lidar sweep — the offset becomes zero by construction); PTP / IEEE 1588 on the network for microsecond-level sync between devices that can’t share a wire; software timestamping at the driver with an estimated constant offset (what Kalibr and VINS-Fusion estimate online); and — the one that fails silently — timestamping on message arrival, where USB buffering injects tens of jittering milliseconds. One more subtlety worth an interview answer: a camera’s honest timestamp is mid-exposure, and a spinning lidar’s points span the whole sweep — which is exactly the deskew problem Phase 2.7 solved, now revealed as a time-calibration statement.

6.6ROS-Free IPC: Iceoryx, ZeroMQ, Fast-DDS

A navigation stack is a set of processes — driver, odometry, SLAM, planner, controller — passing sensor-sized messages at sensor rates. Without ROS, you choose the transport yourself, and the choice is really about copies. A 2 MB point cloud at 20 Hz through a socket API is serialized, copied into the kernel, copied out, deserialized — four traversals of memory per hop, per subscriber. Iceoryx deletes all of them: publishers loan a chunk of shared memory, write the cloud in place, and publish the pointer; subscribers read the same physical bytes. Latency becomes single-digit microseconds and — the deeper win — independent of message size.

The pragmatic architecture stacks them: iceoryx for the fat intra-host pipes (lidar → odometry → mapping), Fast-DDS or ZeroMQ for the thin inter-host ones (telemetry, commands, fleet traffic) — which is, not coincidentally, roughly what ROS 2 itself assembles from these very libraries. Using them directly is not heresy; it is the same parts without the framework contract.

6.7Threads, Queues, and the Real-Time Contract

Inside each process, the same discipline in miniature. The sensor pipeline pattern is one thread per blocking source, handing off through bounded single-producer single-consumer queues, with an explicit drop policy — a localization consumer wants the latest scan, not a growing backlog of stale ones (queue depth 1, overwrite), while a logging consumer wants every scan (deep queue, backpressure). Declaring which consumer is which, per edge, is the design act; middleware QoS is just this decision given names. The hot path — the loop between sensor arrival and actuator command — additionally obeys the real-time contract: no heap allocation, no unbounded locks, no printf; preallocate everything, pin the control thread to a core with elevated priority, and measure the tail latency, not the mean, because the 99.9th percentile is the one that misses the control deadline into a wall.

// ipcbench.cc — ZeroMQ over ipc://: REQ/REP latency + PUSH/PULL throughput.
#include <zmq.h>
#include <chrono>
#include <cstring>
#include <iostream>
#include <sys/wait.h>
#include <unistd.h>
#include <vector>

using clk = std::chrono::steady_clock;
double ms_since(clk::time_point t0) {
  return std::chrono::duration<double, std::milli>(clk::now() - t0).count();
}

int main() {
  const int PINGS = 2000;
  const size_t MSG = 2 * 1024 * 1024;          // a 2 MB "point cloud"
  const int CLOUDS = 500;

  if (fork() == 0) {                            // ---- child: echo + sink ----
    void* ctx = zmq_ctx_new();
    void* rep = zmq_socket(ctx, ZMQ_REP);
    zmq_bind(rep, "ipc:///tmp/lat.ipc");
    char buf[64];
    for (int i = 0; i < PINGS; ++i) {           // echo pings back
      zmq_recv(rep, buf, sizeof buf, 0);
      zmq_send(rep, buf, 8, 0);
    }
    void* pull = zmq_socket(ctx, ZMQ_PULL);
    zmq_bind(pull, "ipc:///tmp/thr.ipc");
    std::vector<char> big(MSG);
    for (int i = 0; i < CLOUDS; ++i)            // swallow the clouds
      zmq_recv(pull, big.data(), MSG, 0);
    zmq_close(rep); zmq_close(pull); zmq_ctx_term(ctx);
    _exit(0);
  }

  void* ctx = zmq_ctx_new();                    // ---- parent: measure ----
  void* req = zmq_socket(ctx, ZMQ_REQ);
  usleep(200000);                               // let the child bind
  zmq_connect(req, "ipc:///tmp/lat.ipc");
  char buf[64] = "ping";
  auto t0 = clk::now();
  for (int i = 0; i < PINGS; ++i) {
    zmq_send(req, buf, 8, 0);
    zmq_recv(req, buf, sizeof buf, 0);
  }
  std::cout << "REQ/REP round trip: "
            << ms_since(t0) * 1000.0 / PINGS << " µs avg\n";

  void* push = zmq_socket(ctx, ZMQ_PUSH);
  zmq_connect(push, "ipc:///tmp/thr.ipc");
  std::vector<char> cloud(MSG, 42);
  t0 = clk::now();
  for (int i = 0; i < CLOUDS; ++i)
    zmq_send(push, cloud.data(), MSG, 0);
  const double s = ms_since(t0) / 1000.0;
  std::cout << "PUSH/PULL 2 MB × " << CLOUDS << ": "
            << (MSG / 1048576.0) * CLOUDS / s << " MB/s  ("
            << CLOUDS / s << " clouds/s)\n";
  zmq_close(req); zmq_close(push); zmq_ctx_term(ctx);
  wait(nullptr);
}

6.8Reading List

6.9Interview Questions