From earlier lessons, a visual or visual–inertial pipeline estimates a state \( \mathbf{x}_k \) (pose, velocity, biases, map variables) by minimizing residuals formed from camera and IMU data. In practice, three “non-idealities” dominate AMR deployments:

• Scale: monocular vision alone constrains translation only up to an unknown positive scale. Even with VIO, scale couples with accelerometer bias and gravity alignment during initialization.
• Lighting: brightness constancy is violated by auto-exposure, gain changes, HDR tone mapping, and spatial effects (vignetting). Direct methods are especially sensitive unless photometrically calibrated.
• Motion blur: images become temporally integrated measurements; gradients shrink, descriptors degrade, and tracking Jacobians lose rank. Rough terrain and wheel-induced vibrations make blur common for AMR.

We will treat these as modeling mismatches. The main pattern is: replace an invalid assumption (metric scale known, brightness constant, “instantaneous” image) with an explicit latent model, then estimate or marginalize the nuisance parameters using robust losses (from Chapter 12) and IMU priors (from Lesson 3).

flowchart TD
  S["Start"] --> A["Acquire image frame and IMU window"]
  A --> D["Compute diagnostics: blur score, \nexposure change, parallax check"]
  D -->|blur high| B1["Mitigate blur: \nshorten exposure, \ndrop frame, \nIMU-guided deblur"]
  D -->|lighting change| B2["Mitigate lighting: \naffine gain/offset, \nphotometric calibration"]
  D -->|scale uncertain| B3["Mitigate scale: \nVIO initialization, \nwheel/stereo/range cue"]
  B1 --> E["Run VO/VIO tracking and update"]
  B2 --> E
  B3 --> E
  E --> K["Keyframe decision and robust outlier handling"]
  K --> O["Output pose and map update"]

For a calibrated pinhole camera with intrinsics \( \mathbf{K} \), a 3D point \( \mathbf{X} \in \mathbb{R}^3 \) observed at time \(k\) projects to pixel \( \mathbf{u}_k \) as

\[ \tilde{\mathbf{u} }_k \propto \mathbf{K}\left(\mathbf{R}_k\mathbf{X} + \mathbf{t}_k\right),\qquad \mathbf{u}_k = \pi(\tilde{\mathbf{u} }_k), \quad \pi([x\;y\;z]^T)=\left[\frac{x}{z}\;\;\frac{y}{z}\right]^T. \]

Define the camera-frame point \( \mathbf{Y}_k = \mathbf{R}_k\mathbf{X} + \mathbf{t}_k \). Monocular scale ambiguity follows from the fact that normalized coordinates depend only on ratios:

\[ \pi(s\mathbf{Y}_k) = \left[\frac{sY_{k,1} }{sY_{k,3} }\;\;\frac{sY_{k,2} }{sY_{k,3} }\right]^T = \left[\frac{Y_{k,1} }{Y_{k,3} }\;\;\frac{Y_{k,2} }{Y_{k,3} }\right]^T = \pi(\mathbf{Y}_k), \quad s > 0. \]

Consequence (two-view geometry): epipolar constraints depend on the essential matrix \( \mathbf{E}=[\mathbf{t}]_\times \mathbf{R} \), but \( \mathbf{t} \) is only identifiable up to a scale factor: \( \mathbf{E} \) is unchanged by replacing \( \mathbf{t} \) with \( s\mathbf{t} \).

\[ \mathbf{x}_2^T \mathbf{E}\mathbf{x}_1 = 0,\quad \mathbf{E} = [\mathbf{t}]_\times \mathbf{R},\quad [s\mathbf{t}]_\times \mathbf{R} = s([\mathbf{t}]_\times \mathbf{R}) \;\Rightarrow\; \mathbf{E} \text{ defined up to scale.} \]

AMR-specific failure mode: wheeled robots often experience long periods of near-planar motion with limited parallax, especially in corridors (forward motion with small yaw). This weakens triangulation, inflates depth uncertainty, and makes scale drift in monocular SLAM more likely (even if the instantaneous scale is arbitrary, drift corrupts map consistency).

A common monocular SLAM back-end therefore tracks similarity transforms (Sim(3)) between keyframes to correct accumulated scale drift. The “scale state” becomes a slowly varying latent variable that must be constrained by loop closures or additional sensors.

IMU provides metric acceleration (specific force) and angular velocity. A standard continuous-time model is:

\[ \dot{\mathbf{p} } = \mathbf{v},\qquad \dot{\mathbf{v} } = \mathbf{g} + \mathbf{R}\left(\mathbf{a}_m - \mathbf{b}_a - \mathbf{n}_a\right), \qquad \boldsymbol{\omega}_m = \boldsymbol{\omega} + \mathbf{b}_\omega + \mathbf{n}_\omega, \]

where \( \mathbf{p},\mathbf{v} \) are position/velocity, \( \mathbf{R} \) is attitude, \( \mathbf{g} \) is gravity, and \( \mathbf{b}_a,\mathbf{b}_\omega \) are biases. Vision provides geometric constraints that are up-to-scale. Model the unknown metric scale with a scalar \( s \) such that:

\[ \mathbf{p}_k = s\,\hat{\mathbf{p} }_k,\qquad \mathbf{v}_k = s\,\hat{\mathbf{v} }_k, \]

where \( \hat{\mathbf{p} }_k,\hat{\mathbf{v} }_k \) are the “visual” trajectory expressed in arbitrary units. Differentiating gives \( \dot{\mathbf{v} } = s\,\dot{\hat{\mathbf{v} } } \). Plugging into the IMU dynamics:

\[ s\,\dot{\hat{\mathbf{v} } } = \mathbf{g} + \mathbf{R}\left(\mathbf{a}_m - \mathbf{b}_a - \mathbf{n}_a\right). \]

Key coupling: during initialization, scale \( s \) is entangled with gravity direction and accelerometer bias. If the platform motion is “degenerate” (e.g., near-constant velocity with little excitation), multiple combinations of \( (s,\mathbf{g},\mathbf{b}_a) \) explain the data nearly equally well.

A practical (and provable) engineering rule is: VIO needs sufficiently rich excitation (changes in acceleration direction and angular rates) over an initialization window to make the scale and gravity observable with acceptable conditioning.

flowchart TD
  V["Vision: up-to-scale pose increments"] --> F["Estimator (sliding window / factor graph)"]
  I["IMU: omega_m and a_m (metric)"] --> F
  F --> S["Estimate: scale, gravity, biases, poses"]
  S --> C["Consistency checks: scale jump, bias magnitude, reprojection error"]
  C -->|pass| O["Continue VIO tracking"]
  C -->|fail| R["Re-initialize with richer motion / longer window"]

Takeaway for AMR: if an AMR starts from rest and moves smoothly with tiny accelerations, initialization is fragile. A deliberate “excitation pattern” (short forward–stop–turn sequence) often stabilizes scale and bias estimation.

Direct methods rely on brightness constancy between a reference image \( I_r \) and a current image \( I_c \): for a pixel \( \mathbf{u} \) warped by motion parameters \( \boldsymbol{\xi} \), the ideal model is \( I_r(\mathbf{u}) \approx I_c(w(\mathbf{u};\boldsymbol{\xi})) \). Under exposure and gain changes, a more robust affine photometric model is:

\[ I_c(\mathbf{u}') \approx a\,I_r(\mathbf{u}) + b,\qquad a > 0, \quad \mathbf{u}' = w(\mathbf{u};\boldsymbol{\xi}). \]

The per-pixel residual becomes: \( r(\mathbf{u}) \):

\[ r(\mathbf{u}) = I_c(w(\mathbf{u};\boldsymbol{\xi})) - \left(a\,I_r(\mathbf{u}) + b\right). \]

For a set of selected pixels \( \Omega \), the (robust) objective is:

\[ \min_{\boldsymbol{\xi},a,b}\;\sum_{\mathbf{u}\in\Omega}\rho\!\left(r(\mathbf{u})\right), \]

with \( \rho(\cdot) \) a robust loss (Huber/Cauchy), consistent with Chapter 12. Linearizing around the current estimate yields the Gauss–Newton normal equations:

\[ \left(\mathbf{J}^T \mathbf{W}\mathbf{J}\right)\Delta\boldsymbol{\theta} = -\mathbf{J}^T \mathbf{W}\mathbf{r}, \quad \boldsymbol{\theta} = [\boldsymbol{\xi}^T\;a\;b]^T. \]

The Jacobians are structurally important:

\[ \frac{\partial r}{\partial a} = -I_r(\mathbf{u}),\qquad \frac{\partial r}{\partial b} = -1,\qquad \frac{\partial r}{\partial \boldsymbol{\xi} } = \nabla I_c(\mathbf{u}')\;\frac{\partial w(\mathbf{u};\boldsymbol{\xi})}{\partial \boldsymbol{\xi} }. \]

Feature-based note: descriptors (ORB/SIFT-like) aim for partial illumination invariance, but drastic exposure steps still cause (i) fewer stable keypoints due to saturation, and (ii) systematic mismatch due to nonlinear camera response. In practice, applying photometric calibration (response curve + vignetting + exposure time) improves both direct and feature-based tracking when auto-exposure is active.

For exposure duration \( T \), a blurred image is (approximately) a temporal average of sharp irradiance under a time-varying warp \( w_t \) caused by camera motion during exposure:

\[ I_{\text{blur} }(\mathbf{u}) \approx \frac{1}{T}\int_{0}^{T} I_{\text{sharp} }(w_t(\mathbf{u}))\,dt. \]

Under small motion and locally constant depth, this often reduces to a convolution: \( I_{\text{blur} } \approx \mathbf{K}\ast I_{\text{sharp} } \) with a motion-dependent kernel \( \mathbf{K} \). Convolution attenuates high spatial frequencies, which are precisely what direct alignment and feature extraction rely on.

A useful analytical bound follows from the fact that spatial gradients commute with convolution:

\[ \nabla(I_{\text{blur} }) = \nabla(\mathbf{K}\ast I_{\text{sharp} }) = \mathbf{K}\ast \nabla(I_{\text{sharp} }). \]

If \( \mathbf{K} \) is normalized (typical blur PSF), then gradient energy decreases (smoothing):

\[ \|\nabla I_{\text{blur} }\|_2 \;<\; \|\nabla I_{\text{sharp} }\|_2 \quad \Rightarrow \quad \mathbf{H}=\mathbf{J}^T\mathbf{W}\mathbf{J} \text{ becomes ill-conditioned in direct tracking.} \]

IMU cue (engineering approximation): for dominant rotational blur, pixel smear length is proportional to angular rate and exposure:

\[ L_{\text{px} } \approx f\,\|\boldsymbol{\omega}\|\,T, \]

where \( f \) is focal length in pixels. Thus gyro data can (i) predict blur severity, (ii) trigger frame dropping, or (iii) guide blur-aware tracking/deblurring in specialized pipelines.

Fast blur diagnostic (often used in practice): variance of Laplacian (focus measure). If \( \Delta I \) is the Laplacian image, then \( s_{\text{blur} } \):

\[ s_{\text{blur} } = \operatorname{Var}\!\left(\Delta I\right), \quad s_{\text{blur} } \text{ small } \Rightarrow \text{ image is likely blurred / low-detail.} \]

Below are mitigations that directly match models already introduced in Chapter 13 (VO/VIO) and Chapter 12 (robust optimization):

6.1 Scale

• VIO initialization discipline: ensure non-degenerate motion; increase window length; enforce bias priors.
• Additional metric cues: wheel odometry (already in earlier chapters), stereo/range, or known-height constraints in indoor AMR.
• Robust scale consistency checks: flag large scale jumps between keyframes; reinitialize when violated.

6.2 Lighting

• Affine brightness compensation: estimate \( a,b \) per frame-pair (closed form if warp fixed).
• Photometric calibration: compensate exposure time, response nonlinearity, and vignetting where possible.
• Robust losses: treat saturated pixels and shadows as outliers using robust kernels (Chapter 12).

6.3 Motion blur

• Frame selection: skip updates when blur score is below threshold; rely temporarily on IMU propagation.
• Multi-scale alignment: coarse-to-fine pyramids partially tolerate blur (because blur is “less severe” at coarse scales).
• IMU-triggered exposure control: increase shutter speed when gyro magnitude rises (hardware-dependent, but conceptually simple).

The following minimal implementations demonstrate: (i) monocular scale ambiguity + a synthetic IMU “scale fix”, (ii) affine brightness fitting, and (iii) blur score via variance of Laplacian. For real systems, these components sit inside the front-end (tracking) and initialization logic.

Code: Chapter13_Lesson4.py

# Chapter13_Lesson4.py
# Visual/VIO SLAM robustness: (1) monocular scale ambiguity, (2) lighting change via affine photometric model,
# (3) motion blur metric via variance of Laplacian.
#
# Dependencies: numpy, opencv-python
#
# Run:
#   python Chapter13_Lesson4.py

import numpy as np
import cv2


def project_points(K, R, t, Xw):
    '''
    Pinhole projection with intrinsics K:
      u = pi( K [R|t] X )
    '''
    Xc = (R @ Xw.T + t.reshape(3, 1)).T  # Nx3
    x = Xc[:, :2] / Xc[:, 2:3]          # normalized
    u = (K[:2, :2] @ x.T + K[:2, 2:3]).T
    return u, Xc


def monocular_two_view_scale_demo(seed=0):
    np.random.seed(seed)

    fx = fy = 420.0
    cx, cy = 320.0, 240.0
    K = np.array([[fx, 0, cx],
                  [0, fy, cy],
                  [0, 0, 1.0]], dtype=float)

    # 3D points (world) in front of camera 1
    N = 200
    Xw = np.column_stack([
        np.random.uniform(-2.0, 2.0, N),
        np.random.uniform(-1.0, 1.0, N),
        np.random.uniform(4.0, 8.0, N),
    ])

    R1 = np.eye(3)
    t1 = np.zeros(3)

    # Pose 2: small yaw + forward translation (true metric, unknown to monocular)
    yaw = np.deg2rad(5.0)
    R2 = np.array([[np.cos(yaw), 0, np.sin(yaw)],
                   [0, 1, 0],
                   [-np.sin(yaw), 0, np.cos(yaw)]], dtype=float)
    t2_metric = np.array([0.20, 0.0, 0.80])  # meters

    u1, _ = project_points(K, R1, t1, Xw)
    u2, _ = project_points(K, R2, t2_metric, Xw)

    # Add pixel noise
    noise = 0.5
    u1n = u1 + np.random.normal(0, noise, u1.shape)
    u2n = u2 + np.random.normal(0, noise, u2.shape)

    E, _inl = cv2.findEssentialMat(u1n, u2n, K, method=cv2.RANSAC, prob=0.999, threshold=1.0)
    _, _R_est, t_est, _mask = cv2.recoverPose(E, u1n, u2n, K)

    t_unit = t_est.reshape(3)
    t_unit_norm = np.linalg.norm(t_unit)

    print("\n--- Monocular 2-view: translation scale ambiguity ---")
    print("Recovered translation direction (unit up to noise):", t_unit)
    print("||t_unit||:", t_unit_norm)
    print("True metric translation:", t2_metric, "||t_metric||:", np.linalg.norm(t2_metric))

    # Suppose an IMU-based integration provides metric delta-position over the same interval.
    # Here we fake it as ground truth + small noise.
    delta_p_imu = t2_metric + np.random.normal(0, 0.02, 3)

    scale_hat = np.linalg.norm(delta_p_imu) / (t_unit_norm + 1e-12)
    t_metric_hat = scale_hat * t_unit

    print("IMU delta_p (fake):", delta_p_imu)
    print("Estimated scale_hat:", scale_hat)
    print("Metric translation from scale_hat * t_unit:", t_metric_hat)


def affine_brightness_fit(I1, I2, mask=None):
    '''
    Solve min_{a,b} sum_p (a I1(p) + b - I2(p))^2.
    Returns (a,b).
    '''
    if mask is None:
        mask = np.ones(I1.shape, dtype=bool)
    x = I1[mask].reshape(-1, 1).astype(np.float64)
    y = I2[mask].reshape(-1, 1).astype(np.float64)
    A = np.hstack([x, np.ones_like(x)])
    theta, _, _, _ = np.linalg.lstsq(A, y, rcond=None)
    a, b = float(theta[0]), float(theta[1])
    return a, b


def blur_score_var_laplacian(gray):
    L = cv2.Laplacian(gray, cv2.CV_64F)
    return float(L.var())


def photometric_demo():
    print("\n--- Lighting change + blur demo (synthetic) ---")

    img = np.zeros((240, 320), dtype=np.uint8)
    cv2.putText(img, "AMR", (60, 140), cv2.FONT_HERSHEY_SIMPLEX, 2.0, 255, 3, cv2.LINE_AA)

    a_true, b_true = 1.2, -10.0
    img2 = np.clip(a_true * img.astype(np.float64) + b_true, 0, 255).astype(np.uint8)

    # Add blur (simulating motion blur / defocus)
    img2 = cv2.GaussianBlur(img2, (9, 9), 2.5)

    a_hat, b_hat = affine_brightness_fit(img, img2)
    print("True (a,b):", (a_true, b_true))
    print("Estimated (a,b):", (a_hat, b_hat))

    s1 = blur_score_var_laplacian(img)
    s2 = blur_score_var_laplacian(img2)
    print("Blur score var(Laplacian): sharp=", s1, " blurred=", s2)


def main():
    monocular_two_view_scale_demo(seed=0)
    photometric_demo()


if __name__ == "__main__":
    main()

Code: Chapter13_Lesson4.cpp

// Chapter13_Lesson4.cpp
// Robustness utilities: affine brightness fit and blur metric (variance of Laplacian).
// Dependencies: OpenCV
//
// Build (example):
//   g++ -std=c++17 Chapter13_Lesson4.cpp `pkg-config --cflags --libs opencv4` -o lesson4
//
// Run:
//   ./lesson4 img1.png img2.png

#include <opencv2/opencv.hpp>
#include <iostream>
#include <cmath>

static double varLaplacian(const cv::Mat& gray) {
    cv::Mat lap;
    cv::Laplacian(gray, lap, CV_64F);
    cv::Scalar mu, sigma;
    cv::meanStdDev(lap, mu, sigma);
    return sigma[0] * sigma[0];
}

static void affineBrightnessFit(const cv::Mat& I1, const cv::Mat& I2, double& a, double& b) {
    // Solve min_{a,b} || a*I1 + b - I2 ||^2 (least squares), grayscale images.
    CV_Assert(I1.size() == I2.size() && I1.type() == CV_8U && I2.type() == CV_8U);

    double Sx = 0.0, Sy = 0.0, Sxx = 0.0, Sxy = 0.0;
    const int N = I1.rows * I1.cols;

    for (int y = 0; y < I1.rows; ++y) {
        const uchar* p1 = I1.ptr<uchar>(y);
        const uchar* p2 = I2.ptr<uchar>(y);
        for (int x = 0; x < I1.cols; ++x) {
            const double X = static_cast<double>(p1[x]);
            const double Y = static_cast<double>(p2[x]);
            Sx += X;   Sy += Y;
            Sxx += X * X;
            Sxy += X * Y;
        }
    }

    // Normal equations:
    // [Sxx Sx] [a] = [Sxy]
    // [Sx  N ] [b]   [Sy ]
    const double det = Sxx * N - Sx * Sx;
    if (std::abs(det) < 1e-12) {
        a = 1.0; b = 0.0; return;
    }
    a = (Sxy * N - Sy * Sx) / det;
    b = (Sxx * Sy - Sx * Sxy) / det;
}

int main(int argc, char** argv) {
    if (argc < 3) {
        std::cerr << "Usage: " << argv[0] << " img1 img2\n";
        return 1;
    }

    cv::Mat img1 = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);
    cv::Mat img2 = cv::imread(argv[2], cv::IMREAD_GRAYSCALE);
    if (img1.empty() || img2.empty()) {
        std::cerr << "Could not read images.\n";
        return 1;
    }
    cv::resize(img2, img2, img1.size());

    double a = 1.0, b = 0.0;
    affineBrightnessFit(img1, img2, a, b);

    const double s1 = varLaplacian(img1);
    const double s2 = varLaplacian(img2);

    std::cout << "Estimated affine brightness: a=" << a << " b=" << b << "\n";
    std::cout << "Blur score var(Laplacian): img1=" << s1 << " img2=" << s2 << "\n";
    return 0;
}

Code: Chapter13_Lesson4.java

// Chapter13_Lesson4.java
// Robustness utilities: affine brightness fit and blur metric (variance of Laplacian).
// Dependencies: OpenCV Java bindings.
//
// Run (conceptual):
//   javac -cp .:opencv-xxx.jar Chapter13_Lesson4.java
//   java  -cp .:opencv-xxx.jar -Djava.library.path=<path_to_native_libs> Chapter13_Lesson4 img1.png img2.png

import org.opencv.core.*;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;

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

    static double varLaplacian(Mat gray) {
        Mat lap = new Mat();
        Imgproc.Laplacian(gray, lap, CvType.CV_64F);
        MatOfDouble mean = new MatOfDouble();
        MatOfDouble std  = new MatOfDouble();
        Core.meanStdDev(lap, mean, std);
        double s = std.toArray()[0];
        return s * s;
    }

    static double[] affineBrightnessFit(Mat I1, Mat I2) {
        // Solve min_{a,b} || a*I1 + b - I2 ||^2 via normal equations (grayscale).
        if (I1.type() != CvType.CV_8U) I1.convertTo(I1, CvType.CV_8U);
        if (I2.type() != CvType.CV_8U) I2.convertTo(I2, CvType.CV_8U);

        double Sx = 0, Sy = 0, Sxx = 0, Sxy = 0;
        int rows = I1.rows(), cols = I1.cols();
        int N = rows * cols;

        byte[] buf1 = new byte[cols];
        byte[] buf2 = new byte[cols];

        for (int y = 0; y < rows; y++) {
            I1.get(y, 0, buf1);
            I2.get(y, 0, buf2);
            for (int x = 0; x < cols; x++) {
                double X = (buf1[x] & 0xFF);
                double Y = (buf2[x] & 0xFF);
                Sx += X; Sy += Y;
                Sxx += X * X;
                Sxy += X * Y;
            }
        }

        double det = Sxx * N - Sx * Sx;
        double a = 1.0, b = 0.0;
        if (Math.abs(det) >= 1e-12) {
            a = (Sxy * N - Sy * Sx) / det;
            b = (Sxx * Sy - Sx * Sxy) / det;
        }
        return new double[]{a, b};
    }

    public static void main(String[] args) {
        if (args.length < 2) {
            System.out.println("Usage: java Chapter13_Lesson4 img1 img2");
            return;
        }

        Mat img1 = Imgcodecs.imread(args[0], Imgcodecs.IMREAD_GRAYSCALE);
        Mat img2 = Imgcodecs.imread(args[1], Imgcodecs.IMREAD_GRAYSCALE);
        if (img1.empty() || img2.empty()) {
            System.out.println("Could not read images.");
            return;
        }
        Imgproc.resize(img2, img2, img1.size());

        double[] ab = affineBrightnessFit(img1, img2);
        double s1 = varLaplacian(img1);
        double s2 = varLaplacian(img2);

        System.out.println("Estimated affine brightness: a=" + ab[0] + " b=" + ab[1]);
        System.out.println("Blur score var(Laplacian): img1=" + s1 + " img2=" + s2);
    }
}

Code: Chapter13_Lesson4.m

% Chapter13_Lesson4.m
% Visual/VIO SLAM robustness demo:
%  (1) monocular scale ambiguity via essential matrix (synthetic),
%  (2) affine brightness fit (gain/offset),
%  (3) blur score via Laplacian variance.
%
% Requires: Computer Vision Toolbox for estimateEssentialMatrix / relativeCameraPose.

function Chapter13_Lesson4()

% --- 1) Scale ambiguity demo (synthetic) ---
rng(0);
fx = 420; fy = 420; cx = 320; cy = 240;
K = [fx 0 cx; 0 fy cy; 0 0 1];

N = 200;
Xw = [ (rand(N,1)*4-2), (rand(N,1)*2-1), (rand(N,1)*4+4) ]; % z in [4,8]

R1 = eye(3); t1 = [0;0;0];

yaw = deg2rad(5);
R2 = [cos(yaw) 0 sin(yaw); 0 1 0; -sin(yaw) 0 cos(yaw)];
t2_metric = [0.20; 0.0; 0.80];

u1 = projectPoints(K,R1,t1,Xw);
u2 = projectPoints(K,R2,t2_metric,Xw);

noise = 0.5;
u1n = u1 + noise*randn(size(u1));
u2n = u2 + noise*randn(size(u2));

[E,inliers] = estimateEssentialMatrix(u1n,u2n,K,K,'Confidence',99.9,'MaxNumTrials',2000);
[orient, loc] = relativeCameraPose(E, K, u1n(inliers,:), u2n(inliers,:));

t_unit = loc(:); % camera2 location in cam1 coordinates (up to scale)
t_unit_norm = norm(t_unit);

fprintf('\n--- Monocular 2-view: translation scale ambiguity ---\n');
fprintf('Recovered translation direction (up to scale): [%g %g %g], ||t||=%g\n', t_unit, t_unit_norm);
fprintf('True metric translation: [%g %g %g], ||t||=%g\n', t2_metric, norm(t2_metric));

% Fake IMU metric delta-position:
delta_p_imu = t2_metric + 0.02*randn(3,1);
scale_hat = norm(delta_p_imu)/(t_unit_norm+1e-12);
t_metric_hat = scale_hat*t_unit;
fprintf('Estimated scale_hat=%g, metric t_hat=[%g %g %g]\n', scale_hat, t_metric_hat);

% --- 2) Photometric affine fit ---
img1 = zeros(240,320,'uint8');
img1 = insertText(img1,[60 110],'AMR','FontSize',72,'BoxOpacity',0,'TextColor','white');
img1 = rgb2gray(img1);

a_true = 1.2; b_true = -10;
img2 = uint8(min(max(a_true*double(img1)+b_true,0),255));
img2 = imgaussfilt(img2,2.5);

[a_hat, b_hat] = affineBrightnessFit(img1,img2);
fprintf('\n--- Lighting change fit ---\n');
fprintf('True (a,b)=(%g,%g), estimated=(%g,%g)\n', a_true,b_true,a_hat,b_hat);

% --- 3) Blur score ---
s1 = varLaplacian(img1);
s2 = varLaplacian(img2);
fprintf('\n--- Blur score ---\n');
fprintf('Blur score var(Laplacian): sharp=%g, blurred=%g\n', s1, s2);

end


function u = projectPoints(K,R,t,Xw)
Xc = (R*Xw' + t);
x = Xc(1:2,:)./Xc(3,:);
u = (K(1:2,1:2)*x + K(1:2,3)).';
end

function [a,b] = affineBrightnessFit(I1,I2)
x = double(I1(:));
y = double(I2(:));
A = [x, ones(size(x))];
theta = A\y;
a = theta(1); b = theta(2);
end

function s = varLaplacian(gray)
L = imfilter(double(gray), [0 1 0; 1 -4 1; 0 1 0], 'replicate');
s = var(L(:));
end

Code: Chapter13_Lesson4.nb

(* Chapter13_Lesson4.nb *)
(* Wolfram Language notebook-style script (saved with .nb extension). *)

(* --------------------------------------------------------------- *)
(* 1) Monocular scale ambiguity: pinhole projection invariance      *)
(* --------------------------------------------------------------- *)

(* In normalized coordinates, a 3D point maps as:
      x = (Y1/Y3, Y2/Y3) where Y = R X + t.
   If we scale the scene and translation by s>0:
      Y' = R (s X) + s t = s (R X + t) = s Y
   then x' = (s Y1)/(s Y3) = Y1/Y3, so the image is unchanged.
*)

ClearAll[s, Y1, Y2, Y3];
Assuming[s > 0 && Y3 != 0,
  Simplify[
    (s {Y1, Y2, Y3})/(s {Y1, Y2, Y3}[[3]]) - ({Y1, Y2, Y3})/({Y1, Y2, Y3}[[3]])
  ]
]

(* --------------------------------------------------------------- *)
(* 2) Affine brightness model: closed-form least squares            *)
(* --------------------------------------------------------------- *)

(* Given samples (x_i, y_i), minimize Sum_i (a x_i + b - y_i)^2.
   The normal equations yield:
     [Sum x_i^2  Sum x_i] [a] = [Sum x_i y_i]
     [Sum x_i    N     ] [b]   [Sum y_i    ]
*)

ClearAll[x, y, n];
Sx  = Sum[x[i], {i, 1, n}];
Sy  = Sum[y[i], {i, 1, n}];
Sxx = Sum[x[i]^2, {i, 1, n}];
Sxy = Sum[x[i] y[i], {i, 1, n}];

theta = LinearSolve[{ {Sxx, Sx}, {Sx, n} }, {Sxy, Sy}];
theta // Simplify

(* --------------------------------------------------------------- *)
(* 3) Motion blur: frequency-domain interpretation                 *)
(* --------------------------------------------------------------- *)

(* If I_blur = K * I, then Fourier: F{I_blur}(w) = H(w) F{I}(w).
   For gradients: F{dI/dx}(w) = (j wx) F{I}(w), so blur attenuates high frequencies
   when |H(w)| is small at large |w|, reducing gradient energy and thus alignment information.
*)

Problem 1 (Monocular scale invariance): Let \( \mathbf{Y}=\mathbf{R}\mathbf{X}+\mathbf{t} \) and \( \pi(\mathbf{Y})=[Y_1/Y_3\;\;Y_2/Y_3]^T \). Prove that for any \( s > 0 \), \( \pi(s\mathbf{Y})=\pi(\mathbf{Y}) \).

Solution: Substitute: \( \pi(s\mathbf{Y})=[(sY_1)/(sY_3)\;\;(sY_2)/(sY_3)]^T=[Y_1/Y_3\;\;Y_2/Y_3]^T=\pi(\mathbf{Y}) \). Hence monocular image measurements cannot determine the absolute scale of \( \mathbf{Y} \).

Problem 2 (Essential matrix scale): Show that replacing \( \mathbf{t} \) with \( s\mathbf{t} \) leaves the epipolar constraint unchanged.

Solution: Since \( [s\mathbf{t}]_\times = s[\mathbf{t}]_\times \), we get \( \mathbf{E}' = [s\mathbf{t}]_\times \mathbf{R} = s\left([\mathbf{t}]_\times \mathbf{R}\right) = s\mathbf{E} \). The epipolar constraint is \( \mathbf{x}_2^T \mathbf{E}\mathbf{x}_1 = 0 \); multiplying \( \mathbf{E} \) by any nonzero scalar does not change the set of solutions to this equation. Therefore, monocular two-view geometry identifies translation only up to an unknown positive scale (i.e., direction only).

Problem 3 (Closed-form affine brightness): Given paired intensities \( \{(x_i,y_i)\}_{i=1}^N \), derive the normal equations for \( \min_{a,b}\sum_i (a x_i + b - y_i)^2 \).

Solution: Set partial derivatives to zero:

\[ \frac{\partial}{\partial a}\sum_i (a x_i + b - y_i)^2 = 2\sum_i x_i(a x_i + b - y_i)=0, \]

\[ \frac{\partial}{\partial b}\sum_i (a x_i + b - y_i)^2 = 2\sum_i (a x_i + b - y_i)=0. \]

Rearranging yields:

\[ \begin{bmatrix} \sum_i x_i^2 & \sum_i x_i \\ \sum_i x_i & N \end{bmatrix} \begin{bmatrix} a \\ b \end{bmatrix} = \begin{bmatrix} \sum_i x_i y_i \\ \sum_i y_i \end{bmatrix}. \]

Problem 4 (Blur reduces gradient energy): Let \( I_{\text{blur} }=\mathbf{K}\ast I \) where \( \mathbf{K} \) is a normalized blur kernel. Prove \( \|\nabla I_{\text{blur} }\|_2 \;<\; \|\nabla I\|_2 \) for typical low-pass kernels.

Solution: We have \( \nabla I_{\text{blur} }=\mathbf{K}\ast \nabla I \). Convolution by a normalized low-pass kernel is a smoothing operator whose Fourier magnitude response satisfies \( |H(\omega)| \;<\; 1 \) for high frequencies. Since gradients emphasize high frequencies, Parseval’s identity implies energy decreases: \( \|\nabla I_{\text{blur} }\|_2^2=\int |H(\omega)|^2\,|\mathcal{F}\{\nabla I\}(\omega)|^2 d\omega \), hence it is smaller than the sharp case when \( |H(\omega)| \) is not identically 1.

Problem 5 (IMU-based blur predictor): Using \( L_{\text{px} } \approx f\|\boldsymbol{\omega}\|T \), compute the predicted blur length for \( f=500 \) px, \( \|\boldsymbol{\omega}\|=2 \) rad/s, \( T=8 \) ms.

Solution: Convert \( T=8 \) ms to seconds: \( T=0.008 \) s. Then \( L_{\text{px} } \approx 500 \times 2 \times 0.008 = 8 \) pixels. This is typically enough to degrade keypoints and direct alignment unless mitigated (shorter exposure, frame skipping, or blur-aware tracking).

We formalized three robustness bottlenecks for visual and visual–inertial SLAM on AMR: (i) monocular scale ambiguity (and scale drift) due to projective invariance, (ii) lighting change as a photometric model mismatch handled by affine gain/offset (and full photometric calibration), and (iii) motion blur as temporal integration that reduces gradient information and harms tracking. The unifying remedy is explicit modeling + robust estimation + sensor-aided checks (IMU/wheels) that prevent catastrophic updates.