In Chapter 12, we treated SLAM back-ends as nonlinear least squares over poses (and possibly landmarks). In Chapter 13, we focus on the front-end tracking step that provides relative motion constraints. Two measurement models dominate:

• Feature-based: convert images into discrete measurements (keypoints + descriptors), then estimate motion by minimizing geometric residuals.
• Direct: avoid descriptors; estimate motion by minimizing photometric residuals between pixel intensities after warping.

flowchart TD
  A["Input images (I_{t-1}, I_t)"] --> B["Preprocess (undistort, grayscale, pyramid)"]
  B --> C{"Choose measurement \nrepresentation"}
  C --> F["Sparse features"]
  C --> D["Pixel intensities"]
  F --> F1["Detect + describe (e.g., ORB)"]
  F1 --> F2["Match + RANSAC geometry checks"]
  F2 --> F3["Minimize reprojection error → pose update"]
  D --> D1["Select pixels/patches (high gradient)"]
  D1 --> D2["Minimize photometric error → pose update"]
  F3 --> K["Keyframe decision + local map update"]
  D2 --> K
        

Both paradigms typically feed a graph-based back-end (Chapter 12) by producing relative pose constraints and quality scores (inlier ratio, residual statistics, covariance approximations).

Let the robot carry a monocular camera. Between consecutive frames we estimate a relative pose \( \mathbf{T} \in SE(3) \) (rotation + translation). In this AMR course, you can interpret \( \mathbf{T} \) as “the rigid transform mapping points from the previous camera frame to the current camera frame”. For optimization, we parameterize a small update by a 6-vector \( \delta\boldsymbol{\theta}\in\mathbb{R}^6 \) (3 for small rotation, 3 for translation).

Both feature and direct methods can be expressed as Maximum Likelihood estimation with Gaussian noise: choose \( \hat{\mathbf{T}} \) that minimizes a sum of squared residuals. The difference is what the “residual” is computed from.

When outliers exist (mismatches, moving objects), we replace squared loss by a robust loss \( \rho(\cdot) \): this converts the problem into Iteratively Reweighted Least Squares (IRLS).

Feature-based pipelines extract a set of correspondences \( (\mathbf{u}_i, \mathbf{u}'_i) \): pixel locations in frame \( t-1 \) and frame \( t \). A calibrated pinhole camera model maps a 3D point \( \mathbf{P} \) in the camera frame to pixel coordinates:

\[ \mathbf{u} = \pi\!\left(\mathbf{K}\mathbf{P}\right),\quad \pi\!\left(\begin{bmatrix}X\\Y\\Z\end{bmatrix}\right)= \begin{bmatrix}X/Z\\Y/Z\end{bmatrix},\quad \mathbf{K}= \begin{bmatrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 & 1 \end{bmatrix} \]

If we have a set of 3D map points \( \mathbf{P}_j \) (from triangulation or a local map) and their observed pixels \( \mathbf{u}'_j \): the standard residual is reprojection error:

\[ \mathbf{r}_j(\mathbf{T}) = \mathbf{u}'_j - \pi\!\left(\mathbf{K}\left(\mathbf{R}\mathbf{P}_j+\mathbf{t}\right)\right), \qquad \mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{t}\\ \mathbf{0}^\top & 1\end{bmatrix} \]

The (robust) objective becomes:

\[ \min_{\mathbf{T}} \sum_{j=1}^{N} \rho\!\left( \mathbf{r}_j(\mathbf{T})^\top \mathbf{\Sigma}_j^{-1}\mathbf{r}_j(\mathbf{T}) \right) \]

Gauss–Newton step (core derivation). Linearize each residual around current estimate \( \mathbf{T}_0 \): \( \mathbf{r}_j(\mathbf{T}_0 \oplus \delta\boldsymbol{\theta}) \approx \mathbf{r}_j(\mathbf{T}_0)+\mathbf{J}_j\delta\boldsymbol{\theta} \): where \( \mathbf{J}_j \) is the Jacobian. Stacking residuals gives:

\[ \delta\boldsymbol{\theta}^\star = \arg\min_{\delta\boldsymbol{\theta}} \left\lVert \mathbf{W}^{1/2}\left(\mathbf{r}+\mathbf{J}\delta\boldsymbol{\theta}\right)\right\rVert_2^2 \Rightarrow \left(\mathbf{J}^\top\mathbf{W}\mathbf{J}\right)\delta\boldsymbol{\theta} = -\mathbf{J}^\top\mathbf{W}\mathbf{r} \]

This same normal-equation structure is the backbone of bundle adjustment (multi-frame, multi-point) that you will use implicitly when running modern SLAM stacks. The main front-end differences are:

• Pros: robust to moderate illumination changes; good long-term matching with descriptors; natural outlier rejection via RANSAC.
• Cons: needs texture/keypoints; matching can be heavy; failures under motion blur and repeated patterns.

Direct methods treat pixel intensities as measurements. Let a pixel in the reference image be \( \mathbf{p} = (x,y)^\top \): and let \( I_{t-1}(\mathbf{p}) \) be its intensity. If we can warp \( \mathbf{p} \) into the current image under pose \( \mathbf{T} \) (using a depth estimate), we define a photometric residual: \( r(\mathbf{p},\mathbf{T}) \):

\[ r(\mathbf{p},\mathbf{T}) = I_t\!\left(\omega(\mathbf{p},\mathbf{T},d(\mathbf{p}))\right) - I_{t-1}(\mathbf{p}) \]

Here, \( d(\mathbf{p}) \): is the (inverse) depth associated with pixel \( \mathbf{p} \), and \( \omega(\cdot) \): is the warp induced by the 3D motion and projection. The direct objective (with robust loss) is:

\[ \min_{\mathbf{T}} \sum_{\mathbf{p}\in\Omega} \rho\!\left( \frac{r(\mathbf{p},\mathbf{T})^2}{\sigma_I^2} \right) \]

Key linearization (why gradients matter). Using first-order Taylor expansion of the current image around the warped location:

\[ I_t(\mathbf{p}+\delta\mathbf{p}) \approx I_t(\mathbf{p}) + \nabla I_t(\mathbf{p})^\top \delta\mathbf{p} \]

Combining with warp parameterization gives the Jacobian for Gauss–Newton:

\[ \frac{\partial r}{\partial \boldsymbol{\theta}} = \nabla I_t(\omega)^\top \frac{\partial \omega}{\partial \boldsymbol{\theta}} \]

Important practical condition. If \( \lVert \nabla I \rVert \) is small (flat region), the problem becomes ill-conditioned: pixels with \( \lVert \nabla I \rVert \; < \; \epsilon \) are typically rejected.

Illumination model extension. If brightness constancy fails, a common model is affine intensity change: \( I_t(\cdot) \approx a\,I_{t-1}(\cdot)+b \): then residual becomes:

\[ r(\mathbf{p},\mathbf{T},a,b) = I_t\!\left(\omega(\mathbf{p},\mathbf{T},d(\mathbf{p}))\right) - \left(a\,I_{t-1}(\mathbf{p})+b\right) \]

This lets the optimizer absorb exposure changes, which matters for AMRs moving between lighting zones.

Both paradigms are robust nonlinear least squares problems. With robust loss \( \rho \), IRLS turns the problem into repeated weighted least squares. Define scalar residuals (photometric) or stacked residuals (reprojection). At iteration \( k \), weights are: \( w_i^{(k)} = \psi(r_i^{(k)}) / r_i^{(k)} \): where \( \psi = \rho' \).

\[ \left(\mathbf{J}^\top\mathbf{W}\mathbf{J}\right)\delta = -\mathbf{J}^\top\mathbf{W}\mathbf{r}, \qquad \mathbf{W}=\operatorname{diag}(w_1,\dots,w_M) \]

Interpretation for AMR pipelines: feature-based methods build \( \mathbf{r} \) from sparse correspondences (often tens to thousands), while direct methods build \( \mathbf{r} \) from many pixels/patches (hundreds to tens of thousands), but typically keep \( \mathbf{J}^\top\mathbf{W}\mathbf{J} \) small via low-dimensional motion parameters and sparse pixel selection.

For ground robots, motion is often smooth and dominated by yaw + translation, but the camera still experiences vibration and motion blur. Key considerations:

• Texture and scene structure: feature-based prefers corner-rich scenes; direct can exploit weak texture if gradients exist broadly (but struggles on uniform floors/walls).
• Motion blur: descriptors degrade; direct methods can also fail if gradients vanish, but coarse-to-fine helps.
• Lighting changes: features are often more invariant; direct requires photometric calibration or affine brightness terms.
• Dynamic objects: both need robust loss; feature pipelines often rely on RANSAC; direct uses robust weights + pixel selection.
• Compute budget: feature matching can dominate CPU; direct can be efficient if sparse direct pixels are used.

flowchart TD
  S["Initialize pose from motion model"] --> P["Build image pyramid"]
  P --> L["Process levels (coarse to fine)"]
  L --> R["Compute residual r = I_t(w(x)) - I_ref(x)"]
  R --> J["Compute Jacobian J = grad(I_t) * d(w)/d(theta)"]
  J --> N["Solve (J^T W J) delta = -J^T W r"]
  N --> U["Update parameters: theta = theta + delta"]
  U --> C{"Stop if ||delta|| is small"}
  C -->|"yes"| O["Return pose + quality metrics"]
  C -->|"no"| R
  

File: Chapter13_Lesson2.py

The script demonstrates: (1) a feature-based relative pose estimate via ORB + Essential matrix + recoverPose, and (2) a direct translation-only photometric alignment via Gauss–Newton (coarse-to-fine). The direct part is intentionally simplified to isolate the core idea: minimizing intensity residuals.

# Chapter13_Lesson2.py
"""
Autonomous Mobile Robots — Chapter 13, Lesson 2
Feature-Based vs Direct Methods (minimal runnable demo)

Dependencies:
  pip install opencv-python numpy

Usage:
  python Chapter13_Lesson2.py --img1 path/to/frame1.png --img2 path/to/frame2.png
  (Optionally provide camera intrinsics via --fx --fy --cx --cy)
"""
import argparse
import numpy as np
import cv2

def load_gray(path: str) -> np.ndarray:
    img = cv2.imread(path, cv2.IMREAD_GRAYSCALE)
    if img is None:
        raise FileNotFoundError(f"Could not read image: {path}")
    return img

def feature_based_relative_pose(I1, I2, K, nfeatures=2000):
    """ORB + matching + Essential matrix (calibrated) + recoverPose.
    Returns (R, t_unit, inlier_mask, pts1, pts2) where t has unit norm (scale ambiguous for monocular).
    """
    orb = cv2.ORB_create(nfeatures=nfeatures)
    k1, d1 = orb.detectAndCompute(I1, None)
    k2, d2 = orb.detectAndCompute(I2, None)
    if d1 is None or d2 is None or len(k1) < 8 or len(k2) < 8:
        raise RuntimeError("Not enough features/descriptors to estimate pose.")

    bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
    matches = bf.knnMatch(d1, d2, k=2)

    # Lowe ratio test
    good = []
    for m, n in matches:
        if m.distance < 0.75 * n.distance:
            good.append(m)
    if len(good) < 8:
        raise RuntimeError("Not enough good matches after ratio test.")

    pts1 = np.float32([k1[m.queryIdx].pt for m in good])
    pts2 = np.float32([k2[m.trainIdx].pt for m in good])

    # Robust Essential matrix with RANSAC
    E, inliers = cv2.findEssentialMat(
        pts1, pts2, K, method=cv2.RANSAC, prob=0.999, threshold=1.0
    )
    if E is None:
        raise RuntimeError("findEssentialMat failed.")

    # recoverPose expects inliers mask
    inliers = inliers.astype(bool).ravel()
    pts1_in = pts1[inliers]
    pts2_in = pts2[inliers]
    if len(pts1_in) < 8:
        raise RuntimeError("Not enough inliers to recover pose.")

    _, R, t, _ = cv2.recoverPose(E, pts1_in, pts2_in, K)
    t = t.ravel()
    t_unit = t / (np.linalg.norm(t) + 1e-12)
    return R, t_unit, inliers, pts1, pts2

def robust_huber_weights(r, delta):
    """Huber weights for residual vector r (1D)."""
    a = np.abs(r)
    w = np.ones_like(r, dtype=np.float32)
    mask = a > delta
    w[mask] = (delta / (a[mask] + 1e-12)).astype(np.float32)
    return w

def direct_align_translation(I_ref, I_cur, levels=4, iters=10, huber_delta=5.0):
    """Coarse-to-fine direct alignment for 2D translation (u,v) in pixels.
    Minimizes sum rho(I_cur(x+u,y+v) - I_ref(x,y)).
    """
    # Build pyramids
    pyr_ref = [I_ref.astype(np.float32)]
    pyr_cur = [I_cur.astype(np.float32)]
    for _ in range(1, levels):
        pyr_ref.append(cv2.pyrDown(pyr_ref[-1]))
        pyr_cur.append(cv2.pyrDown(pyr_cur[-1]))

    u, v = 0.0, 0.0  # translation at current pyramid scale
    for lvl in reversed(range(levels)):
        R = pyr_ref[lvl]
        C = pyr_cur[lvl]
        # scale translation to this level
        scale = 2.0 ** lvl
        u_lvl = u / scale
        v_lvl = v / scale

        for _ in range(iters):
            # Warp current image by (u_lvl, v_lvl)
            M = np.array([[1.0, 0.0, u_lvl],
                          [0.0, 1.0, v_lvl]], dtype=np.float32)
            Cw = cv2.warpAffine(C, M, (R.shape[1], R.shape[0]), flags=cv2.INTER_LINEAR)

            # Compute residual and gradients
            r = (Cw - R)

            # Use gradients of warped current image (forward-additive LK)
            Ix = cv2.Sobel(Cw, cv2.CV_32F, 1, 0, ksize=3)
            Iy = cv2.Sobel(Cw, cv2.CV_32F, 0, 1, ksize=3)

            # Exclude a border to avoid interpolation artifacts
            b = 10
            r0 = r[b:-b, b:-b].reshape(-1)
            gx = Ix[b:-b, b:-b].reshape(-1)
            gy = Iy[b:-b, b:-b].reshape(-1)

            # Reject near-zero gradient pixels
            g2 = gx*gx + gy*gy
            mask = g2 > 1e-4
            r0 = r0[mask]
            gx = gx[mask]
            gy = gy[mask]
            if r0.size < 200:
                break

            w = robust_huber_weights(r0, huber_delta)
            # Normal equations for translation: A = [gx gy], solve (A^T W A) d = -A^T W r
            A11 = float(np.sum(w * gx * gx))
            A12 = float(np.sum(w * gx * gy))
            A22 = float(np.sum(w * gy * gy))
            b1  = float(np.sum(w * gx * r0))
            b2  = float(np.sum(w * gy * r0))

            H = np.array([[A11, A12],
                          [A12, A22]], dtype=np.float64)
            g = np.array([b1, b2], dtype=np.float64)

            # Solve with damping if ill-conditioned
            lam = 1e-3 * (H[0,0] + H[1,1] + 1e-12)
            H_damped = H + lam * np.eye(2)

            try:
                duv = -np.linalg.solve(H_damped, g)
            except np.linalg.LinAlgError:
                break

            u_lvl += float(duv[0])
            v_lvl += float(duv[1])

            if np.linalg.norm(duv) < 1e-3:
                break

        # bring back to full-res units
        u = u_lvl * scale
        v = v_lvl * scale

    return u, v

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--img1", required=True, help="Path to reference image (t-1).")
    ap.add_argument("--img2", required=True, help="Path to current image (t).")
    ap.add_argument("--fx", type=float, default=525.0)
    ap.add_argument("--fy", type=float, default=525.0)
    ap.add_argument("--cx", type=float, default=319.5)
    ap.add_argument("--cy", type=float, default=239.5)
    ap.add_argument("--no_vis", action="store_true", help="Disable visualization windows.")
    args = ap.parse_args()

    I1 = load_gray(args.img1)
    I2 = load_gray(args.img2)

    K = np.array([[args.fx, 0.0, args.cx],
                  [0.0, args.fy, args.cy],
                  [0.0, 0.0, 1.0]], dtype=np.float64)

    print("=== Feature-based (ORB + Essential + recoverPose) ===")
    R, t_unit, inliers, pts1, pts2 = feature_based_relative_pose(I1, I2, K)
    print("R =\n", R)
    print("t (unit norm) =", t_unit, "(monocular scale unknown)")

    print("\n=== Direct (translation-only photometric alignment) ===")
    u, v = direct_align_translation(I1, I2, levels=4, iters=15, huber_delta=5.0)
    print(f"Estimated pixel translation: u={u:.3f}, v={v:.3f}")

    if not args.no_vis:
        # Draw inlier matches
        orb = cv2.ORB_create(nfeatures=2000)
        k1, d1 = orb.detectAndCompute(I1, None)
        k2, d2 = orb.detectAndCompute(I2, None)
        bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
        matches = bf.knnMatch(d1, d2, k=2)
        good = []
        for m, n in matches:
            if m.distance < 0.75 * n.distance:
                good.append(m)
        # Mask by inliers computed earlier (aligned with 'good' list)
        inlier_list = [m for m, ok in zip(good, inliers.tolist()) if ok]
        vis = cv2.drawMatches(I1, k1, I2, k2, inlier_list[:60], None, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
        cv2.imshow("Inlier matches (top 60)", vis)

        # Visualize direct alignment by warping I2 back toward I1
        M = np.array([[1.0, 0.0, -u],
                      [0.0, 1.0, -v]], dtype=np.float32)
        I2w = cv2.warpAffine(I2, M, (I1.shape[1], I1.shape[0]))
        diff = cv2.absdiff(I1, I2w)
        cv2.imshow("Direct alignment: abs(I1 - warp(I2))", diff)

        cv2.waitKey(0)

if __name__ == "__main__":
    main()

File: Chapter13_Lesson2.cpp

// Chapter13_Lesson2.cpp
/*
Autonomous Mobile Robots — Chapter 13, Lesson 2
Feature-Based vs Direct Methods (C++ / OpenCV)

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

Run:
  ./lesson2 --img1 frame1.png --img2 frame2.png --fx 525 --fy 525 --cx 319.5 --cy 239.5
*/
#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <opencv2/opencv.hpp>

static cv::Mat loadGray(const std::string& path) {
    cv::Mat img = cv::imread(path, cv::IMREAD_GRAYSCALE);
    if (img.empty()) throw std::runtime_error("Could not read image: " + path);
    return img;
}

static void featureBasedRelativePose(
    const cv::Mat& I1, const cv::Mat& I2, const cv::Mat& K,
    cv::Mat& R, cv::Mat& t_unit, cv::Mat& inlierMask,
    std::vector<cv::Point2f>& pts1, std::vector<cv::Point2f>& pts2
) {
    auto orb = cv::ORB::create(2000);
    std::vector<cv::KeyPoint> k1, k2;
    cv::Mat d1, d2;
    orb->detectAndCompute(I1, cv::noArray(), k1, d1);
    orb->detectAndCompute(I2, cv::noArray(), k2, d2);
    if (d1.empty() || d2.empty()) throw std::runtime_error("Not enough descriptors.");

    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 (auto& pair : knn) {
        if (pair.size() < 2) continue;
        if (pair[0].distance < 0.75f * pair[1].distance) good.push_back(pair[0]);
    }
    if (good.size() < 8) throw std::runtime_error("Not enough good matches.");

    pts1.clear(); pts2.clear();
    pts1.reserve(good.size()); pts2.reserve(good.size());
    for (auto& m : good) {
        pts1.push_back(k1[m.queryIdx].pt);
        pts2.push_back(k2[m.trainIdx].pt);
    }

    cv::Mat E = cv::findEssentialMat(pts1, pts2, K, cv::RANSAC, 0.999, 1.0, inlierMask);
    if (E.empty()) throw std::runtime_error("findEssentialMat failed.");

    cv::Mat t;
    cv::recoverPose(E, pts1, pts2, K, R, t, inlierMask);
    double nrm = cv::norm(t);
    t_unit = (nrm > 0) ? (t / nrm) : t;
}

static inline float huberWeight(float r, float delta) {
    float a = std::fabs(r);
    return (a <= delta) ? 1.0f : (delta / (a + 1e-12f));
}

static cv::Point2d directAlignTranslation(
    const cv::Mat& Iref_u8, const cv::Mat& Icur_u8,
    int levels = 4, int iters = 15, float huberDelta = 5.0f
) {
    std::vector<cv::Mat> pyrRef, pyrCur;
    pyrRef.reserve(levels); pyrCur.reserve(levels);
    pyrRef.push_back(Iref_u8);
    pyrCur.push_back(Icur_u8);
    for (int i = 1; i < levels; ++i) {
        cv::Mat r, c;
        cv::pyrDown(pyrRef.back(), r);
        cv::pyrDown(pyrCur.back(), c);
        pyrRef.push_back(r); pyrCur.push_back(c);
    }

    double u = 0.0, v = 0.0;

    for (int lvl = levels - 1; lvl >= 0; --lvl) {
        cv::Mat Rf, Cf;
        pyrRef[lvl].convertTo(Rf, CV_32F);
        pyrCur[lvl].convertTo(Cf, CV_32F);

        double scale = std::pow(2.0, lvl);
        double uL = u / scale;
        double vL = v / scale;

        for (int it = 0; it < iters; ++it) {
            cv::Mat M = (cv::Mat_<float>(2,3) << 1.f, 0.f, (float)uL, 0.f, 1.f, (float)vL);
            cv::Mat Cw;
            cv::warpAffine(Cf, Cw, M, Rf.size(), cv::INTER_LINEAR);

            cv::Mat r = Cw - Rf;

            cv::Mat Ix, Iy;
            cv::Sobel(Cw, Ix, CV_32F, 1, 0, 3);
            cv::Sobel(Cw, Iy, CV_32F, 0, 1, 3);

            const int b = 10;
            double A11=0, A12=0, A22=0, b1=0, b2=0;
            int count = 0;
            for (int y = b; y < r.rows - b; ++y) {
                const float* pr  = r.ptr<float>(y);
                const float* pgx = Ix.ptr<float>(y);
                const float* pgy = Iy.ptr<float>(y);
                for (int x = b; x < r.cols - b; ++x) {
                    float gx = pgx[x], gy = pgy[x];
                    float g2 = gx*gx + gy*gy;
                    if (g2 <= 1e-4f) continue;
                    float ri = pr[x];
                    float w  = huberWeight(ri, huberDelta);
                    A11 += w*gx*gx;
                    A12 += w*gx*gy;
                    A22 += w*gy*gy;
                    b1  += w*gx*ri;
                    b2  += w*gy*ri;
                    count++;
                }
            }
            if (count < 500) break;

            cv::Mat H = (cv::Mat_<double>(2,2) << A11, A12, A12, A22);
            cv::Mat g = (cv::Mat_<double>(2,1) << b1, b2);
            double lam = 1e-3 * (H.at<double>(0,0) + H.at<double>(1,1) + 1e-12);
            H += lam * cv::Mat::eye(2,2,CV_64F);

            cv::Mat duv;
            if (!cv::solve(H, -g, duv, cv::DECOMP_CHOLESKY)) break;

            uL += duv.at<double>(0,0);
            vL += duv.at<double>(1,0);

            if (cv::norm(duv) < 1e-3) break;
        }

        u = uL * scale;
        v = vL * scale;
    }

    return cv::Point2d(u, v);
}

int main(int argc, char** argv) {
    const cv::String keys =
        "{help h | | show help }"
        "{img1  | | path to reference image }"
        "{img2  | | path to current image }"
        "{fx    |525| fx }"
        "{fy    |525| fy }"
        "{cx    |319.5| cx }"
        "{cy    |239.5| cy }"
        "{no_vis|0| disable visualization }";

    cv::CommandLineParser parser(argc, argv, keys);
    if (parser.has("help") || !parser.has("img1") || !parser.has("img2")) {
        std::cout << "Usage: ./lesson2 --img1 a.png --img2 b.png [--fx ... --fy ... --cx ... --cy ...]\n";
        return 0;
    }

    std::string img1 = parser.get<cv::String>("img1");
    std::string img2 = parser.get<cv::String>("img2");
    double fx = parser.get<double>("fx");
    double fy = parser.get<double>("fy");
    double cx = parser.get<double>("cx");
    double cy = parser.get<double>("cy");
    bool noVis = parser.get<int>("no_vis") != 0;

    cv::Mat I1 = loadGray(img1);
    cv::Mat I2 = loadGray(img2);

    cv::Mat K = (cv::Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);

    std::cout << "=== Feature-based (ORB + Essential + recoverPose) ===\n";
    cv::Mat R, t_unit, inliers;
    std::vector<cv::Point2f> pts1, pts2;
    featureBasedRelativePose(I1, I2, K, R, t_unit, inliers, pts1, pts2);
    std::cout << "R=\n" << R << "\n";
    std::cout << "t (unit norm)=\n" << t_unit.t() << " (monocular scale unknown)\n";

    std::cout << "\n=== Direct (translation-only photometric alignment) ===\n";
    cv::Point2d uv = directAlignTranslation(I1, I2);
    std::cout << "Estimated pixel translation: u=" << uv.x << ", v=" << uv.y << "\n";

    if (!noVis) {
        cv::Mat M = (cv::Mat_<float>(2,3) << 1.f, 0.f, (float)-uv.x, 0.f, 1.f, (float)-uv.y);
        cv::Mat I2w;
        cv::warpAffine(I2, I2w, M, I1.size(), cv::INTER_LINEAR);
        cv::Mat diff; cv::absdiff(I1, I2w, diff);
        cv::imshow("abs(I1 - warp(I2))", diff);
        cv::waitKey(0);
    }

    return 0;
}

File: Chapter13_Lesson2.java

Here, the “direct” example is KLT optical flow: it aligns image patches by minimizing photometric error, so it belongs to intensity-based tracking even though it outputs sparse point correspondences.

// Chapter13_Lesson2.java
/*
Autonomous Mobile Robots — Chapter 13, Lesson 2
Feature-Based vs Direct Methods (Java / OpenCV)

Notes:
- Requires OpenCV Java bindings (org.opencv.*) in your classpath.
- This demo shows:
  (1) Feature-based: ORB + Essential matrix + recoverPose
  (2) Direct (sparse): KLT optical flow (photometric patch alignment) + robust transform estimate
*/
import org.opencv.core.*;
import org.opencv.calib3d.Calib3d;
import org.opencv.features2d.*;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;
import org.opencv.video.Video;

import java.util.*;

public class Chapter13_Lesson2 {
    static {
        System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
    }

    static Mat loadGray(String path) {
        Mat img = Imgcodecs.imread(path, Imgcodecs.IMREAD_GRAYSCALE);
        if (img.empty()) throw new RuntimeException("Could not read image: " + path);
        return img;
    }

    static class PoseResult {
        Mat R;
        Mat tUnit;
        Mat inliersMask;
        MatOfPoint2f pts1;
        MatOfPoint2f pts2;
    }

    static PoseResult featureBasedPose(Mat I1, Mat I2, Mat K) {
        ORB orb = ORB.create(2000);

        MatOfKeyPoint k1 = new MatOfKeyPoint();
        MatOfKeyPoint k2 = new MatOfKeyPoint();
        Mat d1 = new Mat(), d2 = new Mat();

        orb.detectAndCompute(I1, new Mat(), k1, d1);
        orb.detectAndCompute(I2, new Mat(), k2, d2);

        if (d1.empty() || d2.empty()) throw new RuntimeException("Not enough descriptors.");

        DescriptorMatcher matcher = DescriptorMatcher.create(DescriptorMatcher.BRUTEFORCE_HAMMING);
        List<MatOfDMatch> knn = new ArrayList<>();
        matcher.knnMatch(d1, d2, knn, 2);

        List<DMatch> good = new ArrayList<>();
        for (MatOfDMatch pair : knn) {
            DMatch[] ms = pair.toArray();
            if (ms.length < 2) continue;
            if (ms[0].distance < 0.75 * ms[1].distance) good.add(ms[0]);
        }
        if (good.size() < 8) throw new RuntimeException("Not enough good matches.");

        KeyPoint[] kp1 = k1.toArray();
        KeyPoint[] kp2 = k2.toArray();

        List<Point> p1 = new ArrayList<>();
        List<Point> p2 = new ArrayList<>();
        for (DMatch m : good) {
            p1.add(kp1[m.queryIdx].pt);
            p2.add(kp2[m.trainIdx].pt);
        }

        MatOfPoint2f pts1 = new MatOfPoint2f();
        MatOfPoint2f pts2 = new MatOfPoint2f();
        pts1.fromList(p1);
        pts2.fromList(p2);

        Mat inliers = new Mat();
        Mat E = Calib3d.findEssentialMat(pts1, pts2, K, Calib3d.RANSAC, 0.999, 1.0, inliers);
        if (E.empty()) throw new RuntimeException("findEssentialMat failed.");

        Mat R = new Mat();
        Mat t = new Mat();
        Calib3d.recoverPose(E, pts1, pts2, K, R, t, inliers);

        double norm = Core.norm(t);
        Mat tUnit = (norm > 0) ? t.mul(new Scalar(1.0 / norm)) : t;

        PoseResult out = new PoseResult();
        out.R = R; out.tUnit = tUnit; out.inliersMask = inliers; out.pts1 = pts1; out.pts2 = pts2;
        return out;
    }

    static Point directSparseKLTTranslation(Mat I1, Mat I2) {
        Mat corners = new Mat();
        Imgproc.goodFeaturesToTrack(I1, corners, 400, 0.01, 8.0);

        MatOfPoint2f p0 = new MatOfPoint2f(corners);
        MatOfPoint2f p1 = new MatOfPoint2f();
        MatOfByte status = new MatOfByte();
        MatOfFloat err = new MatOfFloat();

        Video.calcOpticalFlowPyrLK(I1, I2, p0, p1, status, err);

        Point[] a = p0.toArray();
        Point[] b = p1.toArray();
        byte[] st = status.toArray();

        List<Double> dx = new ArrayList<>();
        List<Double> dy = new ArrayList<>();
        for (int i = 0; i < st.length; i++) {
            if (st[i] == 0) continue;
            dx.add(b[i].x - a[i].x);
            dy.add(b[i].y - a[i].y);
        }
        if (dx.size() < 20) return new Point(0, 0);

        Collections.sort(dx);
        Collections.sort(dy);
        double medx = dx.get(dx.size()/2);
        double medy = dy.get(dy.size()/2);

        return new Point(medx, medy);
    }

    public static void main(String[] args) {
        if (args.length < 2) {
            System.out.println("Usage: java Chapter13_Lesson2 <img1> <img2> [fx fy cx cy]");
            return;
        }
        String img1 = args[0];
        String img2 = args[1];
        double fx = (args.length >= 3) ? Double.parseDouble(args[2]) : 525.0;
        double fy = (args.length >= 4) ? Double.parseDouble(args[3]) : 525.0;
        double cx = (args.length >= 5) ? Double.parseDouble(args[4]) : 319.5;
        double cy = (args.length >= 6) ? Double.parseDouble(args[5]) : 239.5;

        Mat I1 = loadGray(img1);
        Mat I2 = loadGray(img2);

        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);

        System.out.println("=== Feature-based (ORB + Essential + recoverPose) ===");
        PoseResult pr = featureBasedPose(I1, I2, K);
        System.out.println("R:\n" + pr.R.dump());
        System.out.println("t (unit norm):\n" + pr.tUnit.dump() + " (monocular scale unknown)");

        System.out.println("\n=== Direct (sparse KLT photometric tracking) ===");
        Point uv = directSparseKLTTranslation(I1, I2);
        System.out.printf("Estimated pixel translation (median flow): u=%.3f, v=%.3f%n", uv.x, uv.y);
    }
}

File: Chapter13_Lesson2.m

MATLAB provides high-level, well-tested primitives for both approaches. Feature-based: ORB + essential matrix. Direct (sparse): KLT point tracking, which performs patch-based photometric alignment. In Simulink, these steps are typically wrapped inside a MATLAB Function block for rapid prototyping.

% Chapter13_Lesson2.m
% Autonomous Mobile Robots — Chapter 13, Lesson 2
% Feature-Based vs Direct Methods (MATLAB)
%
% Requirements:
%   - Computer Vision Toolbox (for ORB/KLT/essential matrix helpers)

clear; clc;

% ---- User inputs ----
img1Path = "frame1.png";   % reference (t-1)
img2Path = "frame2.png";   % current (t)

I1 = im2gray(imread(img1Path));
I2 = im2gray(imread(img2Path));

% Camera intrinsics (example defaults; replace with your calibration)
fx = 525; fy = 525; cx = 319.5; cy = 239.5;
intrinsics = cameraIntrinsics([fx fy], [cx cy], size(I1));

fprintf("=== Feature-based (ORB + Essential + relativeCameraPose) ===\n");
points1 = detectORBFeatures(I1);
points2 = detectORBFeatures(I2);

[feat1, valid1] = extractFeatures(I1, points1);
[feat2, valid2] = extractFeatures(I2, points2);

idxPairs = matchFeatures(feat1, feat2, "MaxRatio", 0.75, "Unique", true);
matched1 = valid1(idxPairs(:,1));
matched2 = valid2(idxPairs(:,2));

% Estimate essential matrix with RANSAC
[E, inlierIdx] = estimateEssentialMatrix(matched1, matched2, intrinsics, ...
    "Confidence", 99.9, "MaxNumTrials", 2000);

in1 = matched1(inlierIdx);
in2 = matched2(inlierIdx);

% Recover relative pose (R, t up to scale)
[orient, loc] = relativeCameraPose(E, intrinsics, in1, in2);
t_unit = loc(:) / (norm(loc) + 1e-12);

disp("R = "); disp(orient);
disp("t (unit norm, scale unknown) = "); disp(t_unit');

fprintf("\n=== Direct (sparse KLT / photometric tracking) ===\n");
tracker = vision.PointTracker("MaxBidirectionalError", 2.0);
p0 = detectMinEigenFeatures(I1, "MinQuality", 0.01);
p0 = p0.Location;

initialize(tracker, p0, I1);
[p1, valid] = tracker(I2);

p0v = p0(valid,:);
p1v = p1(valid,:);

% Robust translation estimate via median flow
flow = p1v - p0v;
uv = median(flow, 1);
fprintf("Estimated pixel translation (median flow): u=%.3f, v=%.3f\n", uv(1), uv(2));

% Optional: estimate a similarity transform (rotation+translation) in the image plane
if size(p0v,1) >= 20
    tform = estimateGeometricTransform2D(p0v, p1v, "similarity", ...
        "MaxNumTrials", 2000, "Confidence", 99.0);
    disp("Estimated 2D similarity transform (image plane):");
    disp(tform.A);
end

File: Chapter13_Lesson2.nb

The notebook below demonstrates image alignment in two modes: a feature-driven transform estimate, and an intensity-driven transform estimate. (Function availability depends on Mathematica version.)

(* Chapter13_Lesson2.nb *)
Notebook[{
  Cell["Autonomous Mobile Robots — Chapter 13, Lesson 2\nFeature-Based vs Direct Methods (Wolfram Language)", "Text"],
  Cell[BoxData@ToBoxes[
    Row[{
      "(* User inputs *)\n",
      "img1 = Import[\"frame1.png\"]; (* reference (t-1) *)\n",
      "img2 = Import[\"frame2.png\"]; (* current (t) *)\n\n",
      "g1 = ColorConvert[img1, \"Grayscale\"]; g2 = ColorConvert[img2, \"Grayscale\"]; \n\n",
      "(* Feature-based alignment (keypoints) *)\n",
      "(* Many WL versions support FindGeometricTransform with Method -> \"Feature\" *)\n",
      "tfFeature = Quiet@FindGeometricTransform[g2, g1, TransformationClass -> \"Similarity\", Method -> \"Feature\"];\n",
      "Print[\"Feature-based transform:\"]; Print[tfFeature];\n\n",
      "(* Direct (intensity-based) alignment *)\n",
      "tfIntensity = Quiet@FindGeometricTransform[g2, g1, TransformationClass -> \"Translation\", Method -> \"Intensity\"];\n",
      "Print[\"Direct (intensity) transform:\"]; Print[tfIntensity];\n\n",
      "(* Visual check: overlay after alignment *)\n",
      "g2a = ImageTransformation[g2, First[tfIntensity]];\n",
      "Show[ImageAdd[ImageAdjust[g1, {0,1}], Colorize[ImageAdjust[g2a, {0,1}], ColorFunction -> \"SolarColors\"]], ImageSize -> Large]\n"
    }]
  ], "Input"]
},
WindowSize -> {1000, 700},
WindowTitle -> "Chapter13 Lesson2"
]

Problem 1 (Direct Translation Update): Consider translation-only direct alignment: \( r_i(u,v)=I_t(x_i+u,y_i+v)-I_{t-1}(x_i,y_i) \): for pixels \( (x_i,y_i) \). Derive the Gauss–Newton update for \( (u,v) \) assuming a first-order Taylor approximation of \( I_t \).

Solution: Taylor expand around current \( (u_0,v_0) \): \( I_t(x_i+u_0+\delta u, y_i+v_0+\delta v) \approx I_t(\omega_i)+\nabla I_t(\omega_i)^\top[\delta u,\delta v]^\top \): where \( \omega_i=(x_i+u_0,y_i+v_0) \). Hence: \( r_i(u_0+\delta u,v_0+\delta v)\approx r_i(u_0,v_0)+[I_x(\omega_i)\;\; I_y(\omega_i)]\begin{bmatrix}\delta u\\\delta v\end{bmatrix} \): Stack all residuals:

\[ \mathbf{r} \approx \mathbf{r}_0 + \mathbf{J}\delta,\quad \mathbf{J}_i=\begin{bmatrix}I_x(\omega_i) & I_y(\omega_i)\end{bmatrix},\quad \delta=\begin{bmatrix}\delta u\\\delta v\end{bmatrix} \]

Minimizing \( \lVert \mathbf{r}_0 + \mathbf{J}\delta\rVert_2^2 \) yields:

\[ (\mathbf{J}^\top\mathbf{J})\delta = -\mathbf{J}^\top\mathbf{r}_0, \qquad \begin{bmatrix}u\\v\end{bmatrix} \leftarrow \begin{bmatrix}u\\v\end{bmatrix} + \delta \]

Problem 2 (Reprojection Normal Equations): For feature-based pose estimation with residuals \( \mathbf{r}_j(\mathbf{T})=\mathbf{u}'_j-\pi(\mathbf{K}(\mathbf{R}\mathbf{P}_j+\mathbf{t})) \): show that Gauss–Newton leads to normal equations \( (\mathbf{J}^\top\mathbf{J})\delta = -\mathbf{J}^\top\mathbf{r} \): after first-order linearization.

Solution: Linearize the stacked residual vector around current estimate: \( \mathbf{r}(\boldsymbol{\theta}_0+\delta)\approx \mathbf{r}(\boldsymbol{\theta}_0)+\mathbf{J}\delta \): where \( \boldsymbol{\theta} \) parameterizes the pose. Minimizing the squared norm gives the least-squares optimality condition:

\[ 0 = \frac{\partial}{\partial \delta} \left\lVert \mathbf{r}_0 + \mathbf{J}\delta \right\rVert_2^2 = 2\mathbf{J}^\top(\mathbf{r}_0+\mathbf{J}\delta) \Rightarrow (\mathbf{J}^\top\mathbf{J})\delta = -\mathbf{J}^\top\mathbf{r}_0 \]

Problem 3 (Affine Brightness Compensation): Suppose direct residual uses affine brightness: \( r_i = I_t(\omega_i) - (a I_{t-1}(p_i) + b) \): where \( a,b \) are unknown. Holding pose fixed, derive the least-squares solution for \( a,b \).

Solution: For fixed warp, define scalars \( y_i=I_t(\omega_i) \), \( x_i=I_{t-1}(p_i) \). Then \( r_i = y_i - (a x_i + b) \). Minimize:

\[ \min_{a,b}\sum_{i=1}^M (y_i - a x_i - b)^2 \]

This is linear regression with design matrix \( \mathbf{A}=\begin{bmatrix}x_1 & 1\\ \vdots & \vdots\\ x_M & 1\end{bmatrix} \) and vector \( \mathbf{y}=[y_1,\dots,y_M]^\top \). The normal equations give:

\[ \begin{bmatrix}a\\b\end{bmatrix} = (\mathbf{A}^\top\mathbf{A})^{-1}\mathbf{A}^\top\mathbf{y} \]

Problem 4 (Huber Robustification as Weighted LS): Using Huber loss \( \rho(r)=\tfrac{1}{2}r^2 \) for \( |r|\le\delta \) and \( \rho(r)=\delta(|r|-\tfrac{1}{2}\delta) \) otherwise, show that IRLS weight is \( w(r)=1 \) for small residuals and \( w(r)=\delta/|r| \) for large residuals.

Solution: For IRLS, weights are \( w(r)=\psi(r)/r \) with \( \psi=\rho'(r) \). For Huber: \( \psi(r)=r \) if \( |r|\le\delta \), and \( \psi(r)=\delta\,\operatorname{sign}(r) \) if \( |r|>\delta \). Therefore: \( w(r)=1 \) if \( |r|\le\delta \), and \( w(r)=\delta/|r| \) if \( |r|>\delta \).

Problem 5 (When Direct Fails): Consider the normal matrix for translation-only direct alignment: \( \mathbf{H}=\sum_i \nabla I(\omega_i)\nabla I(\omega_i)^\top \): show that if gradients are all collinear, \( \mathbf{H} \) is singular (not invertible), and interpret this geometrically.

Solution: If all gradients are collinear, there exists a unit vector \( \mathbf{g} \) and scalars \( \alpha_i \) such that \( \nabla I(\omega_i)=\alpha_i\mathbf{g} \): then

\[ \mathbf{H}=\sum_i \alpha_i^2 \mathbf{g}\mathbf{g}^\top = \left(\sum_i \alpha_i^2\right)\mathbf{g}\mathbf{g}^\top \]

The matrix \( \mathbf{g}\mathbf{g}^\top \) has rank 1 in 2D, so \( \mathbf{H} \) has rank 1 and is singular. Geometrically, intensity changes only along the gradient direction; motion orthogonal to that direction produces near-identical intensities, so translation is not observable in that direction (the classic aperture problem).

Feature-based methods transform images into sparse geometric measurements and estimate motion by minimizing reprojection error, typically with strong outlier rejection (RANSAC) and descriptor matching. Direct methods treat intensities as measurements and estimate motion by minimizing photometric error, relying on image gradients, warping, and robust weighting. Both reduce to the same Gauss–Newton/IRLS normal equations, differing mainly in how residuals and Jacobians are constructed. For AMRs, the best choice depends on texture, blur, lighting variability, compute constraints, and the amount of dynamic content.