Visual odometry estimates the relative pose between time indices \(k-1\) and \(k\) using camera measurements (and in later lessons, inertial measurements). The basic output per step is a transform \( \mathbf{T}_{k-1,k} \) describing rotation and translation. Composing these transforms yields a trajectory estimate that is accurate locally but may drift over time.

The AMR-specific considerations are:

• Planar motion prior: ground robots usually have small roll/pitch; yaw dominates. This can be used as a soft constraint (not a substitute for correct estimation).
• Scale ambiguity (monocular): a single moving camera cannot infer metric scale from geometry alone. Scale can be recovered via stereo/RGB-D or by fusing wheel odometry (Chapter 5) / inertial cues (Lesson 3).
• Dynamics + environment: motion blur, low texture floors, specular surfaces, and lighting changes create frequent outliers and degeneracies (pure rotation, low parallax).

flowchart TD
  A["Camera frames I(k-1), I(k)"] --> B["Preprocess (undistort; rectify if stereo)"]
  B --> C["Compute correspondences (features+match or direct alignment)"]
  C --> D["Outlier rejection (RANSAC / robust scoring)"]
  D --> E["Estimate motion (R,t) between frames"]
  E --> F["(Optional) Recover metric scale (stereo / wheel / IMU / known height)"]
  F --> G["Compose: T(0,k) = T(k-1,k) * T(0,k-1)"]
  G --> H["Quality checks: parallax, inlier ratio, residuals"]
  H -->|ok| I["Output delta pose + trajectory update"]
  H -->|bad| J["Fallback: reduce step / reinit / use other sensors"]
        

We assume a calibrated pinhole camera with intrinsic matrix \( \mathbf{K} \):

\[ \mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}. \]

A 3D point in camera coordinates \( \mathbf{X}_c = [X\;Y\;Z]^\top \) projects to the image (pixel coordinates) via \( \pi(\cdot) \):

\[ \begin{aligned} u &= f_x \frac{X}{Z} + c_x, \\ v &= f_y \frac{Y}{Z} + c_y, \end{aligned} \qquad Z > 0. \]

For VO, the fundamental residual is typically a reprojection error. If a landmark \(\mathbf{X}\) (in a chosen world frame) is observed in frame \(k\) as \(\mathbf{u}_{k}\in\mathbb{R}^2\), and the camera pose is \(\mathbf{T}_{c_w}(k) = [\mathbf{R}(k)\;\mathbf{t}(k)]\) (world → camera), then the predicted measurement is \(\hat{\mathbf{u} }_k = \pi\big(\mathbf{K}(\mathbf{R}(k)\mathbf{X} + \mathbf{t}(k))\big)\). The residual is: \( \mathbf{r}_k = \mathbf{u}_k - \hat{\mathbf{u} }_k\).

VO differs from full SLAM in that, at minimum, it only needs the incremental pose between consecutive frames. However, practical VO often maintains a small window of landmarks and poses and refines them via local bundle adjustment (later in this chapter).

Consider two consecutive calibrated frames with relative pose \(\mathbf{R}\in SO(3)\) and translation \(\mathbf{t}\in\mathbb{R}^3\) such that a 3D point satisfies:

\[ \mathbf{X}_2 = \mathbf{R}\mathbf{X}_1 + \mathbf{t}. \]

Let normalized image coordinates be \(\mathbf{x}_i = \mathbf{K}^{-1} \tilde{\mathbf{u} }_i\), where \(\tilde{\mathbf{u} }=[u\;v\;1]^\top\). Because \(\mathbf{x}_2\) and \(\mathbf{X}_2\) are collinear, we have \(\mathbf{x}_2 \times \mathbf{X}_2 = \mathbf{0}\). Substitute \(\mathbf{X}_2 = \mathbf{R}\mathbf{X}_1 + \mathbf{t}\):

\[ \mathbf{0} = \mathbf{x}_2 \times (\mathbf{R}\mathbf{X}_1 + \mathbf{t}) \;\Longleftrightarrow\; \mathbf{x}_2^\top [\mathbf{t}]_\times \mathbf{R}\mathbf{X}_1 = 0, \]

where the skew-symmetric matrix \([\mathbf{t}]_\times\) is defined by \([\mathbf{t}]_\times \mathbf{a} = \mathbf{t}\times \mathbf{a}\) for any vector \(\mathbf{a}\):

\[ [\mathbf{t}]_\times = \begin{bmatrix} 0 & -t_z & t_y \\ t_z & 0 & -t_x \\ -t_y & t_x & 0 \end{bmatrix}. \]

Since \(\mathbf{X}_1\) lies on the ray defined by \(\mathbf{x}_1\), we can replace \(\mathbf{X}_1\) by any scalar multiple of \(\mathbf{x}_1\), yielding the epipolar constraint:

\[ \boxed{\;\mathbf{x}_2^\top \mathbf{E}\,\mathbf{x}_1 = 0\;}, \qquad \mathbf{E} = [\mathbf{t}]_\times \mathbf{R}. \]

Key properties. The essential matrix \(\mathbf{E}\) has rank 2 and two equal nonzero singular values. This motivates enforcing the constraint via SVD: \(\mathbf{E} = \mathbf{U}\,\operatorname{diag}(\sigma,\sigma,0)\,\mathbf{V}^\top\).

Why VO needs robust estimation. Real correspondences include outliers. Therefore, \(\mathbf{E}\) is typically estimated by RANSAC from minimal samples, and then refined on inliers by minimizing a geometric error (e.g., Sampson error).

Monocular scale ambiguity (proof sketch). If \((\mathbf{R},\mathbf{t})\) is a valid relative pose, then for any scalar \(\alpha > 0\), \((\mathbf{R},\alpha\mathbf{t})\) produces the same epipolar constraint because \(\mathbf{E}(\alpha) = [\alpha\mathbf{t}]_\times \mathbf{R} = \alpha [\mathbf{t}]_\times \mathbf{R}\), and the constraint \(\mathbf{x}_2^\top \mathbf{E}\mathbf{x}_1 = 0\) is homogeneous in \(\mathbf{E}\). Thus, geometry alone cannot determine metric translation magnitude from monocular views.

After a robust initial motion estimate (e.g., via \(\mathbf{E}\)), VO refines pose by minimizing a sum of residuals. A common objective over inlier correspondences \(\{(\mathbf{u}_i,\mathbf{X}_i)\}_{i=1}^N\) is:

\[ \min_{\mathbf{R},\mathbf{t} } \sum_{i=1}^N \rho\!\left(\left\|\mathbf{u}_i - \pi\big(\mathbf{K}(\mathbf{R}\mathbf{X}_i + \mathbf{t})\big)\right\|_2^2\right), \]

where \(\rho(\cdot)\) is a robust loss (e.g., Huber) to limit the influence of remaining outliers. Linearize around a current estimate \((\mathbf{R}_0,\mathbf{t}_0)\). Using a small rotation perturbation \(\delta\boldsymbol{\theta}\) and translation perturbation \(\delta\mathbf{t}\), approximate: \(\mathbf{R} \approx (\mathbf{I} + [\delta\boldsymbol{\theta}]_\times)\mathbf{R}_0\), \(\mathbf{t} \approx \mathbf{t}_0 + \delta\mathbf{t}\).

Stack residuals \(\mathbf{r}\) and Jacobian \(\mathbf{J}\). The Gauss–Newton step solves:

\[ (\mathbf{J}^\top \mathbf{W}\mathbf{J})\,\delta = -\mathbf{J}^\top \mathbf{W}\mathbf{r}, \qquad \delta = \begin{bmatrix}\delta\boldsymbol{\theta}\\\delta\mathbf{t}\end{bmatrix}, \]

where \(\mathbf{W}\) is a diagonal weight matrix induced by \(\rho\) (iteratively reweighted least squares). This is the same computational template used later for graph optimization (Chapter 12), now applied locally to VO.

flowchart TD
  A["Input correspondences (inliers)"] --> B["Initialize motion (E/RANSAC or constant-velocity)"]
  B --> C["Iterate: compute residuals r and Jacobian J"]
  C --> D["Compute weights W from robust loss"]
  D --> E["Solve (J^T W J) delta = -J^T W r"]
  E --> F["Update pose (R,t)"]
  F --> G["Check stop: delta small / cost decrease"]
  G -->|continue| C
  G -->|stop| H["Return refined pose + covariance proxy (J^T W J)^(-1)"]
        

Engineering note (AMR planar prior). For a ground robot, it is often reasonable to regularize roll/pitch drift by adding a prior term such as \(\lambda\|[\phi\;\theta]\|^2\) (roll and pitch angles), or by parameterizing the motion directly as \(\delta = [\delta x\;\delta y\;\delta\psi]^\top\). This reduces drift in benign conditions, but it must be applied cautiously because terrain slopes and ramps violate strict planarity.

Even with correct mathematics, VO can fail. You should implement quality gates to decide whether to accept an update:

• Parallax gate: if most inliers exhibit tiny pixel motion, translation is weakly observable (near pure rotation). A typical heuristic is median pixel displacement must be > some threshold.
• Inlier ratio: accept only if \( \#\text{inliers} / \#\text{matches} \) is sufficiently large.
• Cheirality: after decomposing \(\mathbf{E}\), choose the solution that yields most points with depth \(Z > 0\) in both views.
• Robust residual: accept only if the robust cost decreases and the Gauss–Newton step is well-conditioned.

Outliers and dynamic objects. For AMR scenes (people, forklifts), many matches belong to moving objects. RANSAC helps, but additional logic (e.g., semantic masking, motion segmentation) can improve robustness. Later lessons treat outlier handling more systematically at the SLAM level.

Scale recovery options (preview). This lesson emphasizes pure VO. Metric scale is recovered by either:

• Stereo/RGB-D: triangulate points with known baseline/depth, giving metric translation.
• Wheel odometry: use the incremental wheel displacement as a scale factor for visual translation direction.
• IMU: accelerations constrain scale over time (Lesson 3).

The following implementations are intentionally minimal and instructional. They show the canonical feature-based VO loop: detect → match → robust motion → compose. For research-grade systems you will add: keyframe logic, local mapping, better models (rolling shutter, exposure), and sensor fusion (next lessons).

6.1 Python (OpenCV)

Code File: Chapter13_Lesson1.py

#!/usr/bin/env python3
# Chapter13_Lesson1.py
"""
Autonomous Mobile Robots (Control Engineering)
Chapter 13 - Visual and Visual–Inertial SLAM (AMR Focus)
Lesson 1  - Visual Odometry for Mobile Robots

Minimal feature-based monocular VO:
  - ORB keypoints + descriptor matching
  - Essential matrix with RANSAC
  - Relative pose via recoverPose
  - Pose chaining to get a trajectory (up to unknown global scale)

AMR note:
  - Monocular VO has scale ambiguity. This script optionally accepts per-frame
    scale factors from wheel odometry (or any external source) to metrify the trajectory.

Dependencies:
  pip install numpy opencv-python

Usage examples:
  python Chapter13_Lesson1.py --images path/to/frames --K path/to/K.json
  python Chapter13_Lesson1.py --images path/to/frames --scale_csv odom_scales.csv
"""

from __future__ import annotations
import argparse
import glob
import json
import os
from dataclasses import dataclass
from typing import List, Optional, Tuple

import numpy as np
import cv2


@dataclass
class Intrinsics:
    fx: float
    fy: float
    cx: float
    cy: float

    def K(self) -> np.ndarray:
        return np.array([[self.fx, 0.0, self.cx],
                         [0.0, self.fy, self.cy],
                         [0.0, 0.0, 1.0]], dtype=np.float64)


def load_intrinsics(path: Optional[str]) -> Intrinsics:
    # If no intrinsics file is provided, use a common VGA-like placeholder.
    if path is None:
        return Intrinsics(fx=525.0, fy=525.0, cx=319.5, cy=239.5)

    with open(path, "r", encoding="utf-8") as f:
        d = json.load(f)
    return Intrinsics(fx=float(d["fx"]), fy=float(d["fy"]), cx=float(d["cx"]), cy=float(d["cy"]))


def load_scales(path: Optional[str]) -> Optional[List[float]]:
    """
    Load per-step scales (meters) from a CSV with one number per line.
    scale[i] multiplies the translation t estimated between frame i and i+1.
    """
    if path is None:
        return None
    scales: List[float] = []
    with open(path, "r", encoding="utf-8") as f:
        for line in f:
            line = line.strip()
            if not line:
                continue
            scales.append(float(line.split(",")[0]))
    return scales


def list_images(folder: str) -> List[str]:
    exts = ("*.png", "*.jpg", "*.jpeg", "*.bmp")
    files: List[str] = []
    for e in exts:
        files.extend(glob.glob(os.path.join(folder, e)))
    files.sort()
    if len(files) < 2:
        raise ValueError("Need at least 2 images in --images folder.")
    return files


def robust_match(des1: np.ndarray, des2: np.ndarray, ratio: float = 0.75) -> List[cv2.DMatch]:
    bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
    knn = bf.knnMatch(des1, des2, k=2)
    good: List[cv2.DMatch] = []
    for m, n in knn:
        if m.distance < ratio * n.distance:
            good.append(m)
    return good


def enforce_planar_rotation(R: np.ndarray) -> np.ndarray:
    """
    Optional AMR heuristic: project rotation onto pure yaw (zero roll/pitch).
    This is NOT a substitute for proper estimation; it's a pragmatic constraint.
    """
    # yaw from rotation matrix
    yaw = np.arctan2(R[1, 0], R[0, 0])
    cy = float(np.cos(yaw))
    sy = float(np.sin(yaw))
    Rz = np.array([[cy, -sy, 0.0],
                   [sy,  cy, 0.0],
                   [0.0, 0.0, 1.0]], dtype=np.float64)
    return Rz


def main() -> None:
    ap = argparse.ArgumentParser()
    ap.add_argument("--images", required=True, help="Folder containing a time-ordered image sequence")
    ap.add_argument("--K", default=None, help="Camera intrinsics JSON: {fx,fy,cx,cy}")
    ap.add_argument("--scale_csv", default=None, help="CSV with per-step scale (meters), one per line")
    ap.add_argument("--planar", action="store_true", help="Enforce planar yaw-only rotation (AMR heuristic)")
    ap.add_argument("--max_frames", type=int, default=0, help="Process only first N frames (0 = all)")
    args = ap.parse_args()

    intr = load_intrinsics(args.K)
    K = intr.K()

    scales = load_scales(args.scale_csv)

    files = list_images(args.images)
    if args.max_frames and args.max_frames > 1:
        files = files[: args.max_frames]

    orb = cv2.ORB_create(nfeatures=2000)

    img0 = cv2.imread(files[0], cv2.IMREAD_GRAYSCALE)
    if img0 is None:
        raise RuntimeError(f"Cannot read: {files[0]}")
    kp0, des0 = orb.detectAndCompute(img0, None)

    # World frame = camera frame at t=0
    T_cw = np.eye(4, dtype=np.float64)  # transform world -> current camera
    traj = []  # camera centers in world

    def camera_center_world(T_cw_: np.ndarray) -> np.ndarray:
        R_cw = T_cw_[:3, :3]
        t_cw = T_cw_[:3, 3]
        C_w = -R_cw.T @ t_cw
        return C_w

    traj.append(camera_center_world(T_cw))

    for i in range(1, len(files)):
        img1 = cv2.imread(files[i], cv2.IMREAD_GRAYSCALE)
        if img1 is None:
            raise RuntimeError(f"Cannot read: {files[i]}")

        kp1, des1 = orb.detectAndCompute(img1, None)
        if des0 is None or des1 is None or len(kp0) < 20 or len(kp1) < 20:
            print(f"[WARN] Not enough features at frame {i}. Skipping.")
            kp0, des0 = kp1, des1
            continue

        matches = robust_match(des0, des1, ratio=0.75)
        if len(matches) < 20:
            print(f"[WARN] Not enough matches at frame {i}. Skipping.")
            kp0, des0 = kp1, des1
            continue

        pts0 = np.float32([kp0[m.queryIdx].pt for m in matches])
        pts1 = np.float32([kp1[m.trainIdx].pt for m in matches])

        E, inliers = cv2.findEssentialMat(
            pts0, pts1, K, method=cv2.RANSAC, prob=0.999, threshold=1.0
        )
        if E is None:
            print(f"[WARN] Essential matrix failed at frame {i}. Skipping.")
            kp0, des0 = kp1, des1
            continue

        _, R, t, inliers_pose = cv2.recoverPose(E, pts0, pts1, K, mask=inliers)
        if args.planar:
            R = enforce_planar_rotation(R)

        # scale (meters). If not provided, keep unit scale.
        s = 1.0
        if scales is not None:
            if (i - 1) < len(scales):
                s = float(scales[i - 1])
            else:
                # if scales shorter, keep last scale
                s = float(scales[-1])

        # Build relative transform world->cam: T_12 maps cam0 coords -> cam1 coords:
        T_12 = np.eye(4, dtype=np.float64)
        T_12[:3, :3] = R
        T_12[:3, 3] = (s * t.reshape(3))

        # Chain: (world->cam_i) = (cam_{i-1}->cam_i) * (world->cam_{i-1})
        T_cw = T_12 @ T_cw

        traj.append(camera_center_world(T_cw))

        kp0, des0 = kp1, des1

        if i % 10 == 0 or i == len(files) - 1:
            C = traj[-1]
            print(f"[INFO] frame {i:4d}/{len(files)-1}: C_w = {C[0]: .3f}, {C[1]: .3f}, {C[2]: .3f}")

    traj = np.array(traj, dtype=np.float64)
    out = os.path.join(args.images, "vo_trajectory_xyz.csv")
    np.savetxt(out, traj, delimiter=",", header="x,y,z", comments="")
    print("[DONE] trajectory saved to:", out)


if __name__ == "__main__":
    main()
      

6.2 C++ (OpenCV)

Code File: Chapter13_Lesson1.cpp

// Chapter13_Lesson1.cpp
/*
Autonomous Mobile Robots (Control Engineering)
Chapter 13 - Visual and Visual–Inertial SLAM (AMR Focus)
Lesson 1  - Visual Odometry for Mobile Robots

Minimal feature-based monocular VO in C++ (OpenCV):
  - ORB features
  - Descriptor matching (ratio test)
  - Essential matrix (RANSAC)
  - Pose recovery + chaining into a trajectory

Build (example):
  g++ -O2 -std=c++17 Chapter13_Lesson1.cpp `pkg-config --cflags --libs opencv4` -o vo_cpp

Run:
  ./vo_cpp --images path/to/frames --fx 525 --fy 525 --cx 319.5 --cy 239.5

Notes:
  - Monocular VO has unknown scale unless you fuse an external scale (wheel odom) or use stereo/RGB-D.
*/

#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <string>

struct Intrinsics {
    double fx = 525.0, fy = 525.0, cx = 319.5, cy = 239.5;
    cv::Mat K() const {
        return (cv::Mat_<double>(3,3) << fx, 0.0, cx,
                                         0.0, fy, cy,
                                         0.0, 0.0, 1.0);
    }
};

static std::vector<std::string> list_images(const std::string& folder) {
    std::vector<std::string> files;
    std::vector<std::string> patterns = {"/*.png","/*.jpg","/*.jpeg","/*.bmp"};
    for (const auto& p : patterns) {
        std::vector<cv::String> tmp;
        cv::glob(folder + p, tmp, false);
        for (const auto& s : tmp) files.push_back((std::string)s);
    }
    std::sort(files.begin(), files.end());
    return files;
}

static std::vector<cv::DMatch> ratio_match(const cv::Mat& d1, const cv::Mat& d2, double ratio=0.75) {
    cv::BFMatcher bf(cv::NORM_HAMMING, false);
    std::vector<std::vector<cv::DMatch>> knn;
    bf.knnMatch(d1, d2, knn, 2);
    std::vector<cv::DMatch> good;
    good.reserve(knn.size());
    for (const auto& pair : knn) {
        if (pair.size() < 2) continue;
        if (pair[0].distance < ratio * pair[1].distance) good.push_back(pair[0]);
    }
    return good;
}

static cv::Matx44d make_T(const cv::Mat& R, const cv::Mat& t, double scale=1.0) {
    cv::Matx44d T = cv::Matx44d::eye();
    for (int r=0; r<3; ++r) for (int c=0; c<3; ++c) T(r,c) = R.at<double>(r,c);
    T(0,3) = scale * t.at<double>(0);
    T(1,3) = scale * t.at<double>(1);
    T(2,3) = scale * t.at<double>(2);
    return T;
}

static cv::Vec3d camera_center_world(const cv::Matx44d& T_cw) {
    cv::Matx33d R;
    cv::Vec3d t(T_cw(0,3), T_cw(1,3), T_cw(2,3));
    for (int r=0; r<3; ++r) for (int c=0; c<3; ++c) R(r,c) = T_cw(r,c);
    cv::Vec3d Cw = -(R.t() * t);
    return Cw;
}

int main(int argc, char** argv) {
    const cv::String keys =
        "{help h ? |      | help }"
        "{images   |      | folder of frames }"
        "{fx       |525.0 | fx }"
        "{fy       |525.0 | fy }"
        "{cx       |319.5 | cx }"
        "{cy       |239.5 | cy }"
        "{max_frames|0    | process only first N frames (0=all)}";
    cv::CommandLineParser parser(argc, argv, keys);
    if (parser.has("help") || !parser.has("images")) {
        std::cout << "Usage: vo_cpp --images <folder> [--fx ...]\n";
        return 0;
    }

    Intrinsics intr;
    intr.fx = parser.get<double>("fx");
    intr.fy = parser.get<double>("fy");
    intr.cx = parser.get<double>("cx");
    intr.cy = parser.get<double>("cy");
    int max_frames = parser.get<int>("max_frames");

    std::string folder = parser.get<std::string>("images");
    auto files = list_images(folder);
    if (files.size() < 2) {
        std::cerr << "Need at least 2 images in folder.\n";
        return 1;
    }
    if (max_frames > 1 && (size_t)max_frames < files.size()) files.resize((size_t)max_frames);

    cv::Ptr<cv::ORB> orb = cv::ORB::create(2000);
    cv::Mat img0 = cv::imread(files[0], cv::IMREAD_GRAYSCALE);
    if (img0.empty()) { std::cerr << "Cannot read: " << files[0] << "\n"; return 1; }
    std::vector<cv::KeyPoint> kp0;
    cv::Mat des0;
    orb->detectAndCompute(img0, cv::noArray(), kp0, des0);

    cv::Mat K = intr.K();

    cv::Matx44d T_cw = cv::Matx44d::eye();
    std::vector<cv::Vec3d> traj;
    traj.push_back(camera_center_world(T_cw));

    for (size_t i=1; i<files.size(); ++i) {
        cv::Mat img1 = cv::imread(files[i], cv::IMREAD_GRAYSCALE);
        if (img1.empty()) { std::cerr << "Cannot read: " << files[i] << "\n"; break; }
        std::vector<cv::KeyPoint> kp1;
        cv::Mat des1;
        orb->detectAndCompute(img1, cv::noArray(), kp1, des1);

        if (des0.empty() || des1.empty() || kp0.size() < 20 || kp1.size() < 20) {
            std::cerr << "[WARN] low features at frame " << i << "\n";
            kp0 = kp1; des0 = des1.clone();
            continue;
        }

        auto matches = ratio_match(des0, des1, 0.75);
        if (matches.size() < 20) {
            std::cerr << "[WARN] low matches at frame " << i << "\n";
            kp0 = kp1; des0 = des1.clone();
            continue;
        }

        std::vector<cv::Point2f> p0, p1;
        p0.reserve(matches.size());
        p1.reserve(matches.size());
        for (const auto& m : matches) {
            p0.push_back(kp0[m.queryIdx].pt);
            p1.push_back(kp1[m.trainIdx].pt);
        }

        cv::Mat inliers;
        cv::Mat E = cv::findEssentialMat(p0, p1, K, cv::RANSAC, 0.999, 1.0, inliers);
        if (E.empty()) {
            std::cerr << "[WARN] E failed at frame " << i << "\n";
            kp0 = kp1; des0 = des1.clone();
            continue;
        }

        cv::Mat R, t;
        cv::recoverPose(E, p0, p1, K, R, t, inliers);

        cv::Matx44d T_12 = make_T(R, t, 1.0); // unit scale
        T_cw = T_12 * T_cw;

        traj.push_back(camera_center_world(T_cw));
        kp0 = kp1; des0 = des1.clone();

        if (i % 10 == 0 || i == files.size()-1) {
            auto C = traj.back();
            std::cout << "[INFO] frame " << i << "/" << (files.size()-1)
                      << " C_w = " << C[0] << ", " << C[1] << ", " << C[2] << "\n";
        }
    }

    std::ofstream out(folder + "/vo_trajectory_xyz.csv");
    out << "x,y,z\n";
    for (const auto& p : traj) out << p[0] << "," << p[1] << "," << p[2] << "\n";
    out.close();
    std::cout << "[DONE] trajectory saved to: " << folder << "/vo_trajectory_xyz.csv\n";
    return 0;
}
      

6.3 Java (OpenCV Java)

Code File: Chapter13_Lesson1.java

// Chapter13_Lesson1.java
/*
Autonomous Mobile Robots (Control Engineering)
Chapter 13 - Visual and Visual–Inertial SLAM (AMR Focus)
Lesson 1  - Visual Odometry for Mobile Robots

Minimal monocular VO in Java using OpenCV Java bindings.
This is a teaching-oriented skeleton:
  - ORB detect/extract
  - Descriptor match (ratio test)
  - Essential matrix (RANSAC)
  - recoverPose
  - Pose chaining (trajectory up to scale)

Prereqs:
  - Install OpenCV with Java bindings and set java.library.path accordingly.
  - Ensure opencv-xxx.jar is on the classpath.

Run (example):
  javac -cp .:opencv-490.jar Chapter13_Lesson1.java
  java  -cp .:opencv-490.jar -Djava.library.path=/path/to/opencv/lib Chapter13_Lesson1 --images /path/to/frames
*/

import org.opencv.core.*;
import org.opencv.features2d.*;
import org.opencv.calib3d.Calib3d;
import org.opencv.imgcodecs.Imgcodecs;

import java.io.*;
import java.nio.file.*;
import java.util.*;

public class Chapter13_Lesson1 {

    static class Intrinsics {
        double fx = 525.0, fy = 525.0, cx = 319.5, cy = 239.5;
        Mat K() {
            Mat K = Mat.eye(3,3, CvType.CV_64F);
            K.put(0,0, fx); K.put(1,1, fy);
            K.put(0,2, cx); K.put(1,2, cy);
            return K;
        }
    }

    static List<String> listImages(String folder) throws IOException {
        List<String> files = new ArrayList<>();
        String[] exts = new String[]{".png",".jpg",".jpeg",".bmp"};
        try (DirectoryStream<Path> stream = Files.newDirectoryStream(Paths.get(folder))) {
            for (Path p : stream) {
                String name = p.toString().toLowerCase();
                for (String e : exts) {
                    if (name.endsWith(e)) { files.add(p.toString()); break; }
                }
            }
        }
        Collections.sort(files);
        return files;
    }

    static List<DMatch> ratioTest(List<MatOfDMatch> knn, double ratio) {
        List<DMatch> good = new ArrayList<>();
        for (MatOfDMatch m : knn) {
            DMatch[] arr = m.toArray();
            if (arr.length < 2) continue;
            if (arr[0].distance < ratio * arr[1].distance) good.add(arr[0]);
        }
        return good;
    }

    static Matx44d makeT(Mat R, Mat t, double scale) {
        Matx44d T = Matx44d.eye();
        for (int r=0; r<3; r++) for (int c=0; c<3; c++) T.put(r,c, R.get(r,c)[0]);
        T.put(0,3, scale * t.get(0,0)[0]);
        T.put(1,3, scale * t.get(1,0)[0]);
        T.put(2,3, scale * t.get(2,0)[0]);
        return T;
    }

    static double[] cameraCenterWorld(Matx44d Tcw) {
        // Cw = -R^T t
        double[][] R = new double[3][3];
        double[] t = new double[]{Tcw.get(0,3), Tcw.get(1,3), Tcw.get(2,3)};
        for (int r=0; r<3; r++) for (int c=0; c<3; c++) R[r][c] = Tcw.get(r,c);
        double[] C = new double[3];
        for (int i=0; i<3; i++) {
            C[i] = -(R[0][i]*t[0] + R[1][i]*t[1] + R[2][i]*t[2]);
        }
        return C;
    }

    public static void main(String[] args) throws Exception {
        System.loadLibrary(Core.NATIVE_LIBRARY_NAME);

        String folder = null;
        for (int i=0; i<args.length; i++) {
            if (args[i].equals("--images") && i+1 < args.length) folder = args[i+1];
        }
        if (folder == null) {
            System.out.println("Usage: java Chapter13_Lesson1 --images <folder>");
            return;
        }

        Intrinsics intr = new Intrinsics();
        Mat K = intr.K();

        List<String> files = listImages(folder);
        if (files.size() < 2) throw new RuntimeException("Need at least 2 frames.");

        ORB orb = ORB.create(2000);

        Mat img0 = Imgcodecs.imread(files.get(0), Imgcodecs.IMREAD_GRAYSCALE);
        MatOfKeyPoint kp0 = new MatOfKeyPoint();
        Mat des0 = new Mat();
        orb.detectAndCompute(img0, new Mat(), kp0, des0);

        DescriptorMatcher bf = DescriptorMatcher.create(DescriptorMatcher.BRUTEFORCE_HAMMING);

        Matx44d Tcw = Matx44d.eye();
        List<double[]> traj = new ArrayList<>();
        traj.add(cameraCenterWorld(Tcw));

        for (int i=1; i<files.size(); i++) {
            Mat img1 = Imgcodecs.imread(files.get(i), Imgcodecs.IMREAD_GRAYSCALE);
            MatOfKeyPoint kp1 = new MatOfKeyPoint();
            Mat des1 = new Mat();
            orb.detectAndCompute(img1, new Mat(), kp1, des1);

            if (des0.empty() || des1.empty()) {
                kp0 = kp1; des0 = des1;
                continue;
            }

            List<MatOfDMatch> knn = new ArrayList<>();
            bf.knnMatch(des0, des1, knn, 2);
            List<DMatch> good = ratioTest(knn, 0.75);
            if (good.size() < 20) {
                kp0 = kp1; des0 = des1;
                continue;
            }

            KeyPoint[] k0 = kp0.toArray();
            KeyPoint[] k1 = kp1.toArray();
            List<Point> p0 = new ArrayList<>();
            List<Point> p1 = new ArrayList<>();
            for (DMatch m : good) {
                Point a = k0[m.queryIdx].pt;
                Point b = k1[m.trainIdx].pt;
                p0.add(a); p1.add(b);
            }
            MatOfPoint2f P0 = new MatOfPoint2f(); P0.fromList(p0);
            MatOfPoint2f P1 = new MatOfPoint2f(); P1.fromList(p1);

            Mat inliers = new Mat();
            Mat E = Calib3d.findEssentialMat(P0, P1, K, Calib3d.RANSAC, 0.999, 1.0, inliers);
            if (E.empty()) {
                kp0 = kp1; des0 = des1;
                continue;
            }

            Mat R = new Mat();
            Mat t = new Mat();
            Calib3d.recoverPose(E, P0, P1, K, R, t, inliers);

            Matx44d T12 = makeT(R, t, 1.0);
            Tcw = T12.mul(Tcw); // NOTE: Matx.mul is elementwise; we want matrix multiply.

            // Workaround: explicit multiply for Matx44d
            double[][] A = new double[4][4];
            double[][] B = new double[4][4];
            for (int r=0; r<4; r++) for (int c=0; c<4; c++) { A[r][c] = T12.get(r,c); B[r][c] = Tcw.get(r,c); }
            double[][] C = new double[4][4];
            for (int r=0; r<4; r++) for (int c=0; c<4; c++) {
                double s = 0;
                for (int k=0; k<4; k++) s += A[r][k]*B[k][c];
                C[r][c] = s;
            }
            Matx44d Tnew = Matx44d.eye();
            for (int r=0; r<4; r++) for (int c=0; c<4; c++) Tnew.put(r,c, C[r][c]);
            Tcw = Tnew;

            traj.add(cameraCenterWorld(Tcw));
            kp0 = kp1; des0 = des1;

            if (i % 10 == 0 || i == files.size()-1) {
                double[] cw = traj.get(traj.size()-1);
                System.out.printf("[INFO] frame %d/%d Cw = %.3f, %.3f, %.3f%n", i, files.size()-1, cw[0], cw[1], cw[2]);
            }
        }

        try (PrintWriter out = new PrintWriter(new File(folder, "vo_trajectory_xyz.csv"))) {
            out.println("x,y,z");
            for (double[] p : traj) out.printf(Locale.US, "%f,%f,%f%n", p[0], p[1], p[2]);
        }
        System.out.println("[DONE] trajectory saved to: " + folder + "/vo_trajectory_xyz.csv");
    }
}
      

6.4 MATLAB / Simulink (Computer Vision Toolbox)

Code File: Chapter13_Lesson1.m

% Chapter13_Lesson1.m
% Autonomous Mobile Robots (Control Engineering)
% Chapter 13 - Visual and Visual–Inertial SLAM (AMR Focus)
% Lesson 1  - Visual Odometry for Mobile Robots
%
% Feature-based monocular VO using MATLAB Computer Vision Toolbox:
%   - ORB detection / extraction
%   - Feature matching
%   - Essential matrix estimation (RANSAC)
%   - Relative pose recovery
%   - Trajectory chaining (up to unknown scale)
%
% AMR note:
%   Monocular VO has scale ambiguity. In practice, you metrify by:
%     (i) stereo/RGB-D, or (ii) wheel odometry / IMU / known camera height.
%
% Requirements:
%   - Computer Vision Toolbox
%   - (Optional) Simulink if you run the model-generation part at the end.
%
% Usage:
%   Set "imageFolder" below to a folder of sequential frames.
%   (Optionally) Set intrinsics.
%
% Output:
%   - vo_trajectory_xyz.csv saved inside imageFolder

clear; clc;

%% 1) Load image sequence
imageFolder = fullfile(pwd, "frames"); % <-- change this
imds = imageDatastore(imageFolder, "FileExtensions", {".png",".jpg",".jpeg",".bmp"});
if numel(imds.Files) < 2
    error("Need at least 2 images in imageFolder.");
end

I0 = readimage(imds, 1);
if size(I0,3) == 3, I0g = rgb2gray(I0); else, I0g = I0; end

%% 2) Camera intrinsics (edit for your camera)
fx = 525; fy = 525; cx = 319.5; cy = 239.5;
K = [fx 0 cx; 0 fy cy; 0 0 1];
intr = cameraIntrinsics([fx fy], [cx cy], size(I0g));

%% 3) Detect ORB features in first frame
p0 = detectORBFeatures(I0g, "ScaleFactor", 1.2, "NumLevels", 8);
[f0, v0] = extractFeatures(I0g, p0);
Tcw = eye(4); % world->camera
traj = cameraCenterWorld(Tcw);

%% 4) Process sequence
for k = 2:numel(imds.Files)
    I1 = readimage(imds, k);
    if size(I1,3) == 3, I1g = rgb2gray(I1); else, I1g = I1; end

    p1 = detectORBFeatures(I1g, "ScaleFactor", 1.2, "NumLevels", 8);
    [f1, v1] = extractFeatures(I1g, p1);

    idxPairs = matchFeatures(f0, f1, "Unique", true, "MaxRatio", 0.75, "MatchThreshold", 50);
    if size(idxPairs,1) < 20
        f0 = f1; v0 = v1;
        continue;
    end

    m0 = v0(idxPairs(:,1));
    m1 = v1(idxPairs(:,2));

    % Estimate Essential Matrix with RANSAC
    [E, inliers] = estimateEssentialMatrix(m0, m1, intr, ...
        "Confidence", 99.9, "MaxNumTrials", 2000);

    in0 = m0(inliers);
    in1 = m1(inliers);
    if numel(in0) < 20
        f0 = f1; v0 = v1;
        continue;
    end

    % Recover relative camera pose
    [orient, loc] = relativeCameraPose(E, intr, in0, in1);

    % relativeCameraPose returns orientation and location of camera2 in camera1 coordinates.
    % Convert to (world->cam) update: X2 = R*X1 + t
    R = orient';
    t = -R * loc'; % consistent with X2 = R X1 + t convention

    T12 = eye(4);
    T12(1:3,1:3) = R;
    T12(1:3,4) = t; % unit scale

    Tcw = T12 * Tcw;
    traj(end+1,:) = cameraCenterWorld(Tcw); %#ok<AGROW>

    if mod(k,10)==0 || k==numel(imds.Files)
        C = traj(end,:);
        fprintf("[INFO] frame %d/%d: Cw = %.3f, %.3f, %.3f\n", k-1, numel(imds.Files)-1, C(1), C(2), C(3));
    end

    f0 = f1; v0 = v1;
end

%% 5) Save trajectory
out = fullfile(imageFolder, "vo_trajectory_xyz.csv");
writematrix(traj, out);
fprintf("[DONE] trajectory saved to: %s\n", out);

%% 6) (Optional) Simulink: programmatically create a simple VO model skeleton
% This creates a Simulink model with a MATLAB Function block placeholder where
% you would implement a VO step (or call a System object).
%
% Uncomment if you have Simulink:
%
% modelName = "Chapter13_Lesson1_VO_Model";
% if bdIsLoaded(modelName), close_system(modelName,0); end
% new_system(modelName); open_system(modelName);
% add_block("simulink/User-Defined Functions/MATLAB Function", modelName + "/VO_Step");
% set_param(modelName + "/VO_Step", "Position", [200 120 380 200]);
% add_block("simulink/Sources/Constant", modelName + "/FrameIndex");
% set_param(modelName + "/FrameIndex", "Position", [50 140 120 180], "Value", "1");
% add_line(modelName, "FrameIndex/1", "VO_Step/1");
% save_system(modelName);
% fprintf("[INFO] Simulink model created: %s.slx\n", modelName);

%% Helper
function Cw = cameraCenterWorld(Tcw)
Rcw = Tcw(1:3,1:3);
tcw = Tcw(1:3,4);
Cw = -(Rcw')*tcw;
Cw = Cw(:).';
end
      

6.5 Wolfram Mathematica (Eight-Point + Essential Decomposition)

Code File: Chapter13_Lesson1.nb

(* Chapter13_Lesson1.nb
   Autonomous Mobile Robots (Control Engineering)
   Chapter 13 - Visual and Visual–Inertial SLAM (AMR Focus)
   Lesson 1  - Visual Odometry for Mobile Robots

   This notebook demonstrates the normalized eight-point algorithm for
   estimating the essential matrix E from normalized correspondences,
   enforcing rank-2, and decomposing E into candidate (R, t).
*)

Notebook[{
  Cell["Chapter 13 - Lesson 1: Visual Odometry (Mathematica)", "Title"],
  Cell["Normalized Eight-Point Algorithm + Essential Decomposition", "Section"],

  Cell[BoxData @ ToBoxes @ HoldForm[
    ClearAll[NormalizePoints2D, EightPointE, EnforceEssential, DecomposeE, Skew];

    Skew[t_] := { {0, -t[[3]], t[[2]]}, {t[[3]], 0, -t[[1]]}, {-t[[2]], t[[1]], 0} };

    NormalizePoints2D[pts_] := Module[{mu, s, T, p},
      mu = Mean[pts];
      s = Sqrt[2]/Mean[Norm /@ (pts - mu)];
      T = { {s, 0, -s mu[[1]]}, {0, s, -s mu[[2]]}, {0, 0, 1} };
      p = (T . Append[#, 1]&) /@ pts;
      {T, Most /@ p}
    ];

    EightPointE[pts1_, pts2_] := Module[{T1, T2, p1, p2, A, v, E},
      {T1, p1} = NormalizePoints2D[pts1];
      {T2, p2} = NormalizePoints2D[pts2];
      A = Table[
        With[{x1=p1[[i,1]], y1=p1[[i,2]], x2=p2[[i,1]], y2=p2[[i,2]]},
          {x2 x1, x2 y1, x2, y2 x1, y2 y1, y2, x1, y1, 1}
        ],
        {i, Length[p1]}
      ];
      v = Last[SingularValueDecomposition[A]][[All, -1]];
      E = Partition[v, 3];
      (* denormalize *)
      E = Transpose[T2] . E . T1;
      E
    ];

    EnforceEssential[E_] := Module[{U, S, V},
      {U, S, V} = SingularValueDecomposition[E];
      (* enforce singular values (1,1,0) up to scale *)
      U . DiagonalMatrix[{1,1,0}] . Transpose[V]
    ];

    DecomposeE[E_] := Module[{U, S, V, W, R1, R2, t},
      {U, S, V} = SingularValueDecomposition[E];
      W = { {0, -1, 0}, {1, 0, 0}, {0, 0, 1} };
      R1 = U . W . Transpose[V];
      R2 = U . Transpose[W] . Transpose[V];
      t  = U[[All, 3]];
      {R1, R2, t}
    ];
  ], "Input"],

  Cell["Example (synthetic correspondences)", "Subsection"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    SeedRandom[0];
    pts1 = RandomReal[{-1,1}, {50,2}];
    (* create a small motion and project with a simple affine approximation for demo *)
    Rtrue = { {1, -0.02, 0}, {0.02, 1, 0}, {0,0,1} };
    ttrue = {0.1, 0.0, 0.0};
    pts2 = (Most /@ ((# + {0.01, -0.02})&) /@ pts1)) + RandomReal[{-0.002,0.002}, {50,2}];

    E0 = EightPointE[pts1, pts2];
    E  = EnforceEssential[E0];
    {R1, R2, t} = DecomposeE[E];

    Print["E (rank-2 enforced):"]; MatrixForm[E]
  ], "Input"],

  Cell["Notes", "Text"],
  Cell["In practice you must use calibrated normalized image coordinates, robust estimation (RANSAC), and a cheirality test to pick the correct (R, t).", "Text"]
},
WindowSize->{900,700},
WindowMargins->{ {Automatic, 50}, {Automatic, 50} }
]
      

Problem 1 (Derive the Epipolar Constraint): Starting from \(\mathbf{X}_2 = \mathbf{R}\mathbf{X}_1 + \mathbf{t}\) and the fact that \(\mathbf{x}_2\) is collinear with \(\mathbf{X}_2\), show that \(\mathbf{x}_2^\top [\mathbf{t}]_\times \mathbf{R}\mathbf{x}_1 = 0\) for calibrated rays.

Solution: Collinearity gives \(\mathbf{x}_2 \times \mathbf{X}_2 = \mathbf{0}\). Substitute \(\mathbf{X}_2 = \mathbf{R}\mathbf{X}_1 + \mathbf{t}\): \(\mathbf{x}_2 \times (\mathbf{R}\mathbf{X}_1 + \mathbf{t})=\mathbf{0}\). Use the identity \(\mathbf{a}\times\mathbf{b} = [\mathbf{a}]_\times\mathbf{b}\): \([\mathbf{x}_2]_\times(\mathbf{R}\mathbf{X}_1 + \mathbf{t})=\mathbf{0}\). Left-multiply by \(\mathbf{x}_2^\top\) and use \(\mathbf{x}_2^\top[\mathbf{x}_2]_\times=\mathbf{0}^\top\), obtaining \(\mathbf{x}_2^\top[\mathbf{t}]_\times\mathbf{R}\mathbf{X}_1=0\). Since \(\mathbf{X}_1\) lies along ray \(\mathbf{x}_1\), replace \(\mathbf{X}_1\) by a scalar multiple of \(\mathbf{x}_1\), yielding \(\mathbf{x}_2^\top\mathbf{E}\mathbf{x}_1=0\) with \(\mathbf{E}=[\mathbf{t}]_\times\mathbf{R}\).

Problem 2 (Sampson Approximation): For a correspondence \((\mathbf{x}_1,\mathbf{x}_2)\), the algebraic epipolar error is \(e = \mathbf{x}_2^\top\mathbf{E}\mathbf{x}_1\). Show that the first-order (Sampson) approximation of geometric error is

\[ e_{\text{S} }^2 = \frac{(\mathbf{x}_2^\top\mathbf{E}\mathbf{x}_1)^2} {(\mathbf{E}\mathbf{x}_1)_1^2 + (\mathbf{E}\mathbf{x}_1)_2^2 + (\mathbf{E}^\top\mathbf{x}_2)_1^2 + (\mathbf{E}^\top\mathbf{x}_2)_2^2 }. \]

Solution: The epipolar lines are \(\ell_2 = \mathbf{E}\mathbf{x}_1\) in image 2 and \(\ell_1 = \mathbf{E}^\top\mathbf{x}_2\) in image 1. The squared point-to-line distance (homogeneous coordinates) is approximately \(d^2(\mathbf{x},\ell) \approx \frac{(\ell^\top\mathbf{x})^2}{\ell_1^2+\ell_2^2}\). Because \(\ell_2^\top\mathbf{x}_2 = \mathbf{x}_2^\top\mathbf{E}\mathbf{x}_1 = e\) and \(\ell_1^\top\mathbf{x}_1 = \mathbf{x}_2^\top\mathbf{E}\mathbf{x}_1 = e\), a symmetric first-order approximation sums both distances, yielding the stated denominator.

Problem 3 (Scale Ambiguity Formalization): Prove that monocular two-view geometry cannot determine the magnitude of \(\mathbf{t}\). More precisely, show that if \((\mathbf{R},\mathbf{t})\) is feasible, then so is \((\mathbf{R},\alpha\mathbf{t})\) for any \(\alpha > 0\), with identical epipolar constraints.

Solution: The essential matrix is \(\mathbf{E}=[\mathbf{t}]_\times\mathbf{R}\). Replace \(\mathbf{t}\) by \(\alpha\mathbf{t}\): \(\mathbf{E}(\alpha) = [\alpha\mathbf{t}]_\times\mathbf{R} = \alpha[\mathbf{t}]_\times\mathbf{R} = \alpha\mathbf{E}\). The epipolar constraint is homogeneous: \(\mathbf{x}_2^\top (\alpha\mathbf{E})\mathbf{x}_1 = \alpha\,\mathbf{x}_2^\top\mathbf{E}\mathbf{x}_1 = 0\). Thus the same correspondences are consistent for any \(\alpha\), and translation magnitude is unobservable without additional metric information.

Problem 4 (Gauss–Newton Normal Equations): Consider minimizing \(\min_\delta \|\mathbf{r} + \mathbf{J}\delta\|_2^2\). Derive the normal equations and the closed-form solution when \(\mathbf{J}^\top\mathbf{J}\) is invertible.

Solution: Expand \(\|\mathbf{r}+ \mathbf{J}\delta\|^2 = (\mathbf{r}+ \mathbf{J}\delta)^\top(\mathbf{r}+ \mathbf{J}\delta)\). Differentiate w.r.t. \(\delta\) and set to zero: \(2\mathbf{J}^\top(\mathbf{r}+ \mathbf{J}\delta)=0\), hence \(\mathbf{J}^\top\mathbf{J}\delta = -\mathbf{J}^\top\mathbf{r}\). If \(\mathbf{J}^\top\mathbf{J}\) is invertible, then \(\delta^\star = -(\mathbf{J}^\top\mathbf{J})^{-1}\mathbf{J}^\top\mathbf{r}\).

Problem 5 (Huber IRLS Weight): The Huber loss for a scalar residual \(e\) with threshold \(\delta_H\) is

\[ \rho(e) = \begin{cases} \tfrac{1}{2}e^2, & |e| \le \delta_H \\ \delta_H(|e| - \tfrac{1}{2}\delta_H), & |e| > \delta_H \end{cases}. \]

Derive the IRLS weight \(w(e)\) such that minimizing \(\sum_i \rho(e_i)\) is approximated by minimizing \(\sum_i w(e_i)e_i^2\) at each iteration.

Solution: Choose \(w(e)=\frac{\psi(e)}{e}\), where \(\psi(e)=\frac{d\rho}{de}\). For Huber: \(\psi(e)=e\) if |e| \le \delta_H, and \(\psi(e)=\delta_H\,\operatorname{sign}(e)\) if |e| > \delta_H. Therefore \(w(e)=1\) for |e| \le \delta_H, and \(w(e)=\frac{\delta_H}{|e|}\) for |e| > \delta_H.

We formulated visual odometry as incremental pose estimation from images, derived the calibrated two-view epipolar constraint, and connected practical VO pipelines to robust nonlinear least squares (Gauss–Newton / IRLS). For AMR, the key takeaways are the scale ambiguity of monocular VO and the need for quality gates and robustness in dynamic environments. In Lesson 2 we compare feature-based and direct VO methods; in Lesson 3 we fuse vision with inertial measurements to stabilize scale and dynamics.