In this course we primarily model a ground AMR moving on a plane. Let the (planar) workspace be \( \mathcal{W} \subset \mathbb{R}^2 \) with coordinates \( \mathbf{p} = [x, y]^\top \). Obstacles occupy a closed set \( \mathcal{O} \subset \mathcal{W} \) and the free space is \( \mathcal{F} = \mathcal{W} \setminus \mathcal{O} \).

A binary occupancy function is a map \( o(\mathbf{p}) \) that indicates whether a point is traversable:

\[ o(\mathbf{p}) = \begin{cases} 1 & \text{if } \mathbf{p} \in \mathcal{O} \\ 0 & \text{if } \mathbf{p} \in \mathcal{F} \end{cases} \]

Practical AMR modeling also includes boundary conditions (walls), unknown regions, and dynamic obstacles. For now, as a modeling assumption for Chapter 1, we separate the environment into: (i) a static component (walls/fixtures), and (ii) a dynamic component (people, moving objects). Static structure is what map representations typically encode; dynamic structure is handled by a planner/reactive layer later.

The AMR pose in the plane is typically \( \mathbf{q} = [x, y, \theta]^\top \in SE(2) \). Collision checking, however, is easiest when we reduce the robot to a point by inflating obstacles in configuration space. Let the robot footprint be a compact set \( \mathcal{R} \subset \mathbb{R}^2 \) expressed in the robot frame, and let the translation be \( T_\mathbf{p}(\mathcal{R}) = \{ \mathbf{p} + \mathbf{r} : \mathbf{r} \in \mathcal{R} \} \).

For translation-only collision checking (common when using a conservative footprint such as a disc), define the configuration-space obstacle set:

\[ \mathcal{O}_\text{C} \triangleq \mathcal{O} \oplus (-\mathcal{R}) = \{ \mathbf{o} - \mathbf{r} : \mathbf{o} \in \mathcal{O},\ \mathbf{r} \in \mathcal{R} \} \]

Theorem (Point-robot reduction via Minkowski sum). For a translation \( \mathbf{p} \in \mathcal{W} \), \( T_\mathbf{p}(\mathcal{R}) \cap \mathcal{O} \neq \varnothing \) if and only if \( \mathbf{p} \in \mathcal{O}_\text{C} \).

Proof. (\(\Rightarrow\)) If \( T_\mathbf{p}(\mathcal{R}) \cap \mathcal{O} \neq \varnothing \), then there exist \( \mathbf{r} \in \mathcal{R} \) and \( \mathbf{o} \in \mathcal{O} \) such that \( \mathbf{p} + \mathbf{r} = \mathbf{o} \). Rearranging gives \( \mathbf{p} = \mathbf{o} - \mathbf{r} \in \mathcal{O} \oplus (-\mathcal{R}) = \mathcal{O}_\text{C} \). (\(\Leftarrow\)) If \( \mathbf{p} \in \mathcal{O}_\text{C} \), then \( \mathbf{p} = \mathbf{o} - \mathbf{r} \) for some \( \mathbf{o} \in \mathcal{O},\ \mathbf{r} \in \mathcal{R} \). Hence \( \mathbf{p} + \mathbf{r} = \mathbf{o} \in \mathcal{O} \), which implies \( T_\mathbf{p}(\mathcal{R}) \cap \mathcal{O} \neq \varnothing \). \(\blacksquare\)

A common conservative approximation is a disc robot with radius \( r \), i.e. \( \mathcal{R} = \{\mathbf{r}\in\mathbb{R}^2 : \|\mathbf{r}\|_2 \le r\} \). Then \( \mathcal{O}_\text{C} \) is an isotropic dilation of obstacles by distance \( r \).

flowchart TD
  A["Physical environment"] --> B["Choose representation family"]
  B --> C1["Metric: grids / \nmeshes / point sets"]
  B --> C2["Geometric: segments / \npolygons / primitives"]
  B --> C3["Continuous fields: \ndistance / \ncost / potential"]
  B --> C4["Topological: \ngraph of places + \nconnections"]
  C1 --> D["Operations: collision checks, \ndistances, costs"]
  C2 --> D
  C3 --> D
  C4 --> D
  D --> E["Trade-offs: accuracy vs memory vs runtime"]
        

A 2D occupancy grid discretizes \( \mathcal{W} \) into cells \( \{\mathcal{C}_i\}_{i \in \mathcal{I}} \) with resolution \( \Delta > 0 \) (e.g., meters per cell). Each cell stores an occupancy variable \( M_i \in \{0,1\} \) and often its probability \( p_i = \mathbb{P}(M_i=1) \) with \( 0 < p_i < 1 \). Interpreting a grid as a random field is a modeling assumption that will later support Bayesian mapping and SLAM.

A key numerical trick is the log-odds transform: \( \ell_i \triangleq \log\frac{p_i}{1-p_i} \). It maps probabilities in \( (0,1) \) to real numbers and turns multiplicative Bayes updates into additive updates.

Proposition (Log-odds Bayes update becomes additive). Let \( p^- = \mathbb{P}(M=1) \) be a prior and let an observation \( z \) have likelihoods \( p(z\mid M=1) \) and \( p(z\mid M=0) \). Then the posterior log-odds satisfy:

\[ \ell^+ = \ell^- + \log\frac{p(z\mid M=1)}{p(z\mid M=0)} \]

Proof. Bayes’ rule gives \( p^+ \triangleq \mathbb{P}(M=1\mid z) = \frac{p(z\mid M=1)p^-}{p(z\mid M=1)p^- + p(z\mid M=0)(1-p^-)} \). Compute the posterior odds:

\[ \frac{p^+}{1-p^+} = \frac{p(z\mid M=1)p^-}{p(z\mid M=0)(1-p^-)} = \frac{p^-}{1-p^-} \cdot \frac{p(z\mid M=1)}{p(z\mid M=0)} \]

Taking logs yields the claim. \(\blacksquare\)

In practice, grid resolution \( \Delta \) creates approximation error: thin obstacles may disappear when smaller than a cell, and computing collision checks becomes conservative when using inflated obstacles. These are not “bugs”; they are modeling trade-offs.

A geometric map represents obstacle boundaries as primitives, e.g., line segments, arcs, polygons, or parametric curves. In 2D indoor AMR environments, a common model is a set of line segments \( \mathcal{L} = \{\ell_j\}_{j=1}^m \), where segment \( j \) has endpoints \( \mathbf{a}_j, \mathbf{b}_j \in \mathbb{R}^2 \).

Distance-to-map computations are often needed for scan matching and residual formation (covered later). For a point \( \mathbf{p} \) and an infinite line through \( \mathbf{a},\mathbf{b} \), define the unit normal: \( \mathbf{n} = \frac{1}{\|\mathbf{b}-\mathbf{a}\|_2}\begin{bmatrix}-(b_y-a_y)\\ b_x-a_x\end{bmatrix} \). The signed point-to-line distance is:

\[ d_\text{line}(\mathbf{p};\mathbf{a},\mathbf{b}) = \mathbf{n}^\top (\mathbf{p}-\mathbf{a}) \]

For a segment (finite), the closest point is the projection clamped to \( t \in [0,1] \):

\[ t^*(\mathbf{p}) = \operatorname{clip}\left( \frac{(\mathbf{p}-\mathbf{a})^\top (\mathbf{b}-\mathbf{a})}{\|\mathbf{b}-\mathbf{a}\|_2^2},\ 0,\ 1 \right),\quad \mathbf{p}^* = \mathbf{a} + t^*(\mathbf{p})(\mathbf{b}-\mathbf{a}) \]

and the segment distance is \( \|\mathbf{p}-\mathbf{p}^*\|_2 \). Geometric maps are compact (memory efficient) and can be precise, but require robust data association and are sensitive to clutter.

Continuous-field representations assign a scalar value to each point in the workspace (or grid cell). The most important example for AMR is the distance-to-obstacle function: \( d(\mathbf{p},\mathcal{O}) \).

\[ d(\mathbf{p},\mathcal{O}) \triangleq \inf_{\mathbf{o}\in\mathcal{O}} \|\mathbf{p}-\mathbf{o}\|_2 \]

The signed distance field (SDF) is a function \( \phi: \mathcal{W} \to \mathbb{R} \) such that \( \phi(\mathbf{p}) > 0 \) in free space and \( \phi(\mathbf{p}) < 0 \) inside obstacles. One canonical definition uses the boundary \( \partial\mathcal{O} \) and the indicator of membership:

\[ \phi(\mathbf{p}) = \begin{cases} +d(\mathbf{p},\partial\mathcal{O}) & \text{if } \mathbf{p} \in \mathcal{F} \\ -d(\mathbf{p},\partial\mathcal{O}) & \text{if } \mathbf{p} \in \mathcal{O} \end{cases} \]

The SDF enables fast collision queries: a disc robot of radius \( r \) is safe at \( \mathbf{p} \) if \( \phi(\mathbf{p}) \ge r \), which is precisely the inflation concept from Section 2.

Proposition (1-Lipschitz property). The distance function \( d(\cdot,\mathcal{O}) \) is 1-Lipschitz:

\[ |d(\mathbf{p},\mathcal{O}) - d(\mathbf{q},\mathcal{O})| \le \|\mathbf{p}-\mathbf{q}\|_2 \]

Proof. For any \( \mathbf{o} \in \mathcal{O} \), triangle inequality gives \( \|\mathbf{p}-\mathbf{o}\|_2 \le \|\mathbf{p}-\mathbf{q}\|_2 + \|\mathbf{q}-\mathbf{o}\|_2 \). Taking the infimum over \( \mathbf{o} \in \mathcal{O} \) yields \( d(\mathbf{p},\mathcal{O}) \le \|\mathbf{p}-\mathbf{q}\|_2 + d(\mathbf{q},\mathcal{O}) \). Swap \( \mathbf{p},\mathbf{q} \) to obtain the reverse inequality, then combine. \(\blacksquare\)

A topological map abstracts geometry into a graph \( G=(V,E) \) whose vertices represent places (rooms, intersections, key viewpoints) and whose edges represent traversable connections. Define a nonnegative edge-weight function \( w:E\to\mathbb{R}_{\ge 0} \) (e.g., expected traversal time).

A path \( \pi = (v_0, v_1, \dots, v_k) \) has cost \( J(\pi) = \sum_{i=0}^{k-1} w(v_i,v_{i+1}) \). The shortest-path problem is the discrete optimization:

\[ \pi^* \in \arg\min_\pi \sum_{i=0}^{k-1} w(v_i,v_{i+1}), \quad \text{subject to } (v_i,v_{i+1}) \in E \]

Hybrid representations combine layers: e.g., a topological graph for global route selection with local metric grids/cost fields for obstacle avoidance. This “layering” mirrors AMR architecture and will become explicit when we discuss navigation stacks later.

flowchart TD
  S["Start: what must the robot do?"] --> Q1["Need accurate collision margins?"]
  Q1 -->|yes| D1["Use SDF / distance field \n(grid or continuous)"]
  Q1 -->|no| Q2["Need compact representation \nfor large area?"]
  Q2 -->|yes| D2["Use topological graph \n(places + edges)"]
  Q2 -->|no| Q3["Need dense local \nobstacle detail?"]
  Q3 -->|yes| D3["Use occupancy grid / \ncost grid"]
  Q3 -->|no| D4["Use geometric primitives \n(lines / polygons)"]
  D1 --> R["Combine layers if needed"]
  D2 --> R
  D3 --> R
  D4 --> R
        

The following reference implementations build an occupancy grid, compute a signed distance field, and perform obstacle inflation for a disc robot. These utilities will be reused in later labs (local planning, collision checking, and map building).

7.1 Python (NumPy; SciPy optional)

File: Chapter1_Lesson3.py

"""
Chapter 1 — Lesson 3: Environment Representations for AMR
File: Chapter1_Lesson3.py

This script builds a simple 2D environment, rasterizes polygon obstacles into an
occupancy grid, computes a signed distance field (SDF), and performs
configuration-space inflation using a disc robot approximation.

Dependencies:
  - numpy
  - scipy (optional; used for Euclidean distance transform)
"""

import math
import numpy as np

try:
    from scipy.ndimage import distance_transform_edt as edt
    _HAS_SCIPY = True
except Exception:
    _HAS_SCIPY = False


def point_in_polygon(px: float, py: float, poly_xy):
    """
    Ray casting test for point inside a simple polygon.
    poly_xy: list of (x,y) vertices (closed or open).
    Returns True if inside, False if outside (boundary treated as inside).
    """
    inside = False
    n = len(poly_xy)
    for i in range(n):
        x1, y1 = poly_xy[i]
        x2, y2 = poly_xy[(i + 1) % n]
        # Check if edge intersects ray to +infty in x direction
        cond = ((y1 > py) != (y2 > py))
        if cond:
            x_int = x1 + (py - y1) * (x2 - x1) / (y2 - y1 + 1e-15)
            if px <= x_int:
                inside = not inside
    return inside


def rasterize_polygons(polys, x_min, x_max, y_min, y_max, resolution):
    """
    Rasterize polygons into an occupancy grid.
    Returns:
      occ: (H,W) uint8 with 1=occupied, 0=free
      xs, ys: coordinate arrays for cell centers
    """
    xs = np.arange(x_min + 0.5 * resolution, x_max, resolution)
    ys = np.arange(y_min + 0.5 * resolution, y_max, resolution)
    W = xs.size
    H = ys.size
    occ = np.zeros((H, W), dtype=np.uint8)

    for r, y in enumerate(ys):
        for c, x in enumerate(xs):
            for poly in polys:
                if point_in_polygon(float(x), float(y), poly):
                    occ[r, c] = 1
                    break
    return occ, xs, ys


def brushfire_manhattan(binary_occ):
    """
    Multi-source BFS distance to obstacles (Manhattan distance in grid cells).
    binary_occ: 1=occupied, 0=free.
    Returns dist: int32 array, dist=0 on occupied cells.
    """
    H, W = binary_occ.shape
    INF = 10**9
    dist = np.full((H, W), INF, dtype=np.int32)

    # Queue arrays for speed and reproducibility (no deque dependency)
    qx = np.empty(H * W, dtype=np.int32)
    qy = np.empty(H * W, dtype=np.int32)
    head = 0
    tail = 0

    for r in range(H):
        for c in range(W):
            if binary_occ[r, c] == 1:
                dist[r, c] = 0
                qx[tail] = r
                qy[tail] = c
                tail += 1

    nbrs = [(-1, 0), (1, 0), (0, -1), (0, 1)]
    while head < tail:
        r = int(qx[head])
        c = int(qy[head])
        head += 1
        d = int(dist[r, c])
        for dr, dc in nbrs:
            rr = r + dr
            cc = c + dc
            if 0 <= rr < H and 0 <= cc < W:
                if dist[rr, cc] > d + 1:
                    dist[rr, cc] = d + 1
                    qx[tail] = rr
                    qy[tail] = cc
                    tail += 1

    return dist


def signed_distance_field(occ, resolution):
    """
    Compute signed distance field (SDF) in meters:
      SDF > 0 in free space, SDF < 0 inside obstacles.
    If SciPy is available: Euclidean transform; else Manhattan transform.
    """
    occ_bool = (occ.astype(np.uint8) == 1)
    free_bool = ~occ_bool

    if _HAS_SCIPY:
        # Distance from free cells to nearest obstacle
        d_free_to_obs = edt(free_bool) * float(resolution)
        # Distance from obstacle cells to nearest free
        d_obs_to_free = edt(occ_bool) * float(resolution)
    else:
        d_free_to_obs = brushfire_manhattan(occ) * float(resolution)
        d_obs_to_free = brushfire_manhattan(1 - occ) * float(resolution)

    sdf = d_free_to_obs.copy().astype(np.float64)
    sdf[occ_bool] = -d_obs_to_free[occ_bool].astype(np.float64)
    return sdf


def inflate_obstacles_via_sdf(sdf, robot_radius_m):
    """
    Inflate obstacles for a disc robot (point-robot reduction):
      A cell is safe iff SDF >= robot_radius_m.
    Returns inflated occupancy (1=occupied after inflation).
    """
    inflated_occ = (sdf < float(robot_radius_m)).astype(np.uint8)
    return inflated_occ


def main():
    # Environment bounds and resolution
    x_min, x_max = 0.0, 10.0
    y_min, y_max = 0.0, 10.0
    resolution = 0.05  # 5 cm grid

    # Polygon obstacles (counterclockwise vertices)
    polys = [
        [(2.0, 2.0), (4.5, 2.0), (4.5, 3.5), (2.0, 3.5)],
        [(6.0, 6.0), (8.5, 6.2), (8.2, 8.0), (6.2, 8.3)],
        [(1.0, 7.0), (2.0, 6.0), (3.0, 7.0), (2.0, 8.0)],
    ]

    occ, xs, ys = rasterize_polygons(polys, x_min, x_max, y_min, y_max, resolution)
    sdf = signed_distance_field(occ, resolution)

    robot_radius = 0.30  # 30 cm disc approximation
    occ_infl = inflate_obstacles_via_sdf(sdf, robot_radius)

    # Save artifacts for downstream lessons (e.g., planning, localization)
    np.savetxt("occupancy_grid.csv", occ, fmt="%d", delimiter=",")
    np.savetxt("inflated_occupancy_grid.csv", occ_infl, fmt="%d", delimiter=",")
    np.savetxt("signed_distance_field.csv", sdf, fmt="%.6f", delimiter=",")

    # Minimal textual report
    free_ratio = float(np.mean(occ == 0))
    infl_free_ratio = float(np.mean(occ_infl == 0))
    print("SciPy available:", _HAS_SCIPY)
    print("Grid size (H,W):", occ.shape)
    print("Free ratio (original):", free_ratio)
    print("Free ratio (inflated):", infl_free_ratio)
    print("Files written: occupancy_grid.csv, inflated_occupancy_grid.csv, signed_distance_field.csv")


if __name__ == "__main__":
    main()

7.2 C++ (minimal dependencies; C-style arrays)

File: Chapter1_Lesson3.cpp

/*
Chapter 1 — Lesson 3: Environment Representations for AMR
File: Chapter1_Lesson3.cpp

A minimal C++ (mostly C-style) implementation of:
  1) Occupancy grid from axis-aligned rectangular obstacles
  2) Multi-source BFS (Brushfire) distance transform (Manhattan distance)
  3) Signed distance field (SDF) and disc-robot inflation

No template types are used (to avoid angle brackets in HTML embedding).
Compile (example):
  g++ -O2 -std=c++17 Chapter1_Lesson3.cpp -o Chapter1_Lesson3
*/

#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#include "string.h"

static int idx(int r, int c, int W) { return r * W + c; }

static void brushfire_manhattan(const unsigned char* occ, int H, int W, int* dist) {
    const int INF = 1000000000;
    int N = H * W;

    for (int i = 0; i < N; ++i) dist[i] = INF;

    int* q = (int*)malloc((size_t)N * sizeof(int));
    int head = 0, tail = 0;

    for (int r = 0; r < H; ++r) {
        for (int c = 0; c < W; ++c) {
            int k = idx(r, c, W);
            if (occ[k] == 1) {
                dist[k] = 0;
                q[tail++] = k;
            }
        }
    }

    while (head < tail) {
        int k = q[head++];
        int r = k / W;
        int c = k - r * W;
        int d = dist[k];

        // 4-neighborhood
        const int dr[4] = {-1, 1, 0, 0};
        const int dc[4] = {0, 0, -1, 1};

        for (int t = 0; t < 4; ++t) {
            int rr = r + dr[t];
            int cc = c + dc[t];
            if (rr >= 0 && rr < H && cc >= 0 && cc < W) {
                int kk = idx(rr, cc, W);
                if (dist[kk] > d + 1) {
                    dist[kk] = d + 1;
                    q[tail++] = kk;
                }
            }
        }
    }

    free(q);
}

static void fill_rect(unsigned char* occ, int H, int W,
                      int r0, int c0, int r1, int c1) {
    // Fill rectangle [r0,r1] x [c0,c1] inclusive, clamped
    if (r0 < 0) r0 = 0;
    if (c0 < 0) c0 = 0;
    if (r1 >= H) r1 = H - 1;
    if (c1 >= W) c1 = W - 1;
    for (int r = r0; r <= r1; ++r) {
        for (int c = c0; c <= c1; ++c) {
            occ[idx(r, c, W)] = 1;
        }
    }
}

int main() {
    // Grid parameters
    const int H = 160;        // rows
    const int W = 200;        // cols
    const double res = 0.05;  // meters per cell (5 cm)

    unsigned char* occ = (unsigned char*)malloc((size_t)H * (size_t)W);
    unsigned char* occ_inv = (unsigned char*)malloc((size_t)H * (size_t)W);
    int* d_to_obs = (int*)malloc((size_t)H * (size_t)W * sizeof(int));
    int* d_to_free = (int*)malloc((size_t)H * (size_t)W * sizeof(int));
    double* sdf = (double*)malloc((size_t)H * (size_t)W * sizeof(double));

    if (!occ || !occ_inv || !d_to_obs || !d_to_free || !sdf) {
        printf("Memory allocation failed.\n");
        return 1;
    }

    memset(occ, 0, (size_t)H * (size_t)W);

    // Rectangular obstacles in grid indices (row,col)
    fill_rect(occ, H, W, 30, 40, 55, 100);
    fill_rect(occ, H, W, 90, 120, 130, 180);
    fill_rect(occ, H, W, 110, 20, 140, 45);

    // Distances to obstacle for free cells
    brushfire_manhattan(occ, H, W, d_to_obs);

    // Distances to free space for obstacle cells (run on inverted grid)
    for (int i = 0; i < H * W; ++i) occ_inv[i] = (unsigned char)(1 - occ[i]);
    brushfire_manhattan(occ_inv, H, W, d_to_free);

    // Signed distance field in meters
    for (int i = 0; i < H * W; ++i) {
        if (occ[i] == 1) sdf[i] = -((double)d_to_free[i]) * res;
        else sdf[i] = ((double)d_to_obs[i]) * res;
    }

    // Inflate obstacles for disc robot radius
    const double robot_radius = 0.30;
    unsigned char* occ_infl = (unsigned char*)malloc((size_t)H * (size_t)W);
    if (!occ_infl) return 1;

    for (int i = 0; i < H * W; ++i) {
        occ_infl[i] = (unsigned char)(sdf[i] < robot_radius ? 1 : 0);
    }

    // Export CSV for inspection (optional)
    FILE* f = fopen("occupancy_grid_cpp.csv", "w");
    if (f) {
        for (int r = 0; r < H; ++r) {
            for (int c = 0; c < W; ++c) {
                fprintf(f, "%d%s", (int)occ[idx(r, c, W)], (c + 1 < W) ? "," : "");
            }
            fprintf(f, "\n");
        }
        fclose(f);
    }

    FILE* g = fopen("inflated_occupancy_grid_cpp.csv", "w");
    if (g) {
        for (int r = 0; r < H; ++r) {
            for (int c = 0; c < W; ++c) {
                fprintf(g, "%d%s", (int)occ_infl[idx(r, c, W)], (c + 1 < W) ? "," : "");
            }
            fprintf(g, "\n");
        }
        fclose(g);
    }

    // Small console summary
    int free0 = 0, free1 = 0;
    for (int i = 0; i < H * W; ++i) {
        if (occ[i] == 0) free0++;
        if (occ_infl[i] == 0) free1++;
    }
    printf("Grid: H=%d, W=%d, res=%.3f m\n", H, W, res);
    printf("Free ratio (original): %.4f\n", (double)free0 / (double)(H * W));
    printf("Free ratio (inflated): %.4f\n", (double)free1 / (double)(H * W));
    printf("Wrote: occupancy_grid_cpp.csv, inflated_occupancy_grid_cpp.csv\n");

    free(occ);
    free(occ_inv);
    free(d_to_obs);
    free(d_to_free);
    free(sdf);
    free(occ_infl);
    return 0;
}

7.3 Java (standard library only)

File: Chapter1_Lesson3.java

/*
Chapter 1 — Lesson 3: Environment Representations for AMR
File: Chapter1_Lesson3.java

Java implementation of:
  1) Occupancy grid (rectangles)
  2) Brushfire distance transform (Manhattan, multi-source BFS)
  3) Signed distance field (SDF) and disc-robot inflation

Compile:
  javac Chapter1_Lesson3.java
Run:
  java Chapter1_Lesson3
*/

import java.io.FileWriter;
import java.io.IOException;

public class Chapter1_Lesson3 {

    static int idx(int r, int c, int W) { return r * W + c; }

    static void fillRect(byte[] occ, int H, int W, int r0, int c0, int r1, int c1) {
        if (r0 < 0) r0 = 0;
        if (c0 < 0) c0 = 0;
        if (r1 >= H) r1 = H - 1;
        if (c1 >= W) c1 = W - 1;
        for (int r = r0; r <= r1; r++) {
            for (int c = c0; c <= c1; c++) {
                occ[idx(r, c, W)] = 1;
            }
        }
    }

    static void brushfireManhattan(byte[] occ, int H, int W, int[] dist) {
        final int INF = 1000000000;
        final int N = H * W;
        for (int i = 0; i < N; i++) dist[i] = INF;

        int[] q = new int[N];
        int head = 0, tail = 0;

        for (int r = 0; r < H; r++) {
            for (int c = 0; c < W; c++) {
                int k = idx(r, c, W);
                if (occ[k] == 1) {
                    dist[k] = 0;
                    q[tail++] = k;
                }
            }
        }

        int[] dr = new int[]{-1, 1, 0, 0};
        int[] dc = new int[]{0, 0, -1, 1};

        while (head < tail) {
            int k = q[head++];
            int r = k / W;
            int c = k - r * W;
            int d = dist[k];

            for (int t = 0; t < 4; t++) {
                int rr = r + dr[t];
                int cc = c + dc[t];
                if (rr >= 0 && rr < H && cc >= 0 && cc < W) {
                    int kk = idx(rr, cc, W);
                    if (dist[kk] > d + 1) {
                        dist[kk] = d + 1;
                        q[tail++] = kk;
                    }
                }
            }
        }
    }

    static void writeCsv(String filename, byte[] grid, int H, int W) throws IOException {
        FileWriter fw = new FileWriter(filename);
        for (int r = 0; r < H; r++) {
            for (int c = 0; c < W; c++) {
                fw.write(Integer.toString(grid[idx(r, c, W)]));
                if (c + 1 < W) fw.write(",");
            }
            fw.write("\n");
        }
        fw.close();
    }

    public static void main(String[] args) throws Exception {
        final int H = 160;
        final int W = 200;
        final double res = 0.05;

        byte[] occ = new byte[H * W];
        byte[] occInv = new byte[H * W];
        int[] dToObs = new int[H * W];
        int[] dToFree = new int[H * W];
        double[] sdf = new double[H * W];
        byte[] occInfl = new byte[H * W];

        // Obstacles
        fillRect(occ, H, W, 30, 40, 55, 100);
        fillRect(occ, H, W, 90, 120, 130, 180);
        fillRect(occ, H, W, 110, 20, 140, 45);

        // Distances
        brushfireManhattan(occ, H, W, dToObs);
        for (int i = 0; i < H * W; i++) occInv[i] = (byte)(1 - occ[i]);
        brushfireManhattan(occInv, H, W, dToFree);

        // SDF
        for (int i = 0; i < H * W; i++) {
            if (occ[i] == 1) sdf[i] = -((double)dToFree[i]) * res;
            else sdf[i] = ((double)dToObs[i]) * res;
        }

        // Inflation
        final double robotRadius = 0.30;
        for (int i = 0; i < H * W; i++) {
            occInfl[i] = (byte)(sdf[i] < robotRadius ? 1 : 0);
        }

        // CSV export
        writeCsv("occupancy_grid_java.csv", occ, H, W);
        writeCsv("inflated_occupancy_grid_java.csv", occInfl, H, W);

        // Summary
        int free0 = 0, free1 = 0;
        for (int i = 0; i < H * W; i++) {
            if (occ[i] == 0) free0++;
            if (occInfl[i] == 0) free1++;
        }

        System.out.println("Grid: H=" + H + ", W=" + W + ", res=" + res + " m");
        System.out.println("Free ratio (original): " + ((double)free0 / (double)(H * W)));
        System.out.println("Free ratio (inflated): " + ((double)free1 / (double)(H * W)));
        System.out.println("Wrote: occupancy_grid_java.csv, inflated_occupancy_grid_java.csv");
    }
}

7.4 MATLAB / Simulink (bwdist; programmatic model skeleton)

File: Chapter1_Lesson3.m

% Chapter 1 — Lesson 3: Environment Representations for AMR
% File: Chapter1_Lesson3.m
%
% This script demonstrates:
%   1) Raster occupancy grid using rectangle obstacles
%   2) Signed distance field via bwdist
%   3) Disc-robot inflation in configuration space
%   4) Programmatic creation of a small Simulink model skeleton
%
% Requirements:
%   - Image Processing Toolbox for bwdist

clear; clc;

% Grid
H = 160; W = 200;
res = 0.05; % meters per cell

occ = zeros(H, W, 'uint8');

% Rectangle obstacles [r0 c0 r1 c1] inclusive
rects = [
    30  40  55 100
    90 120 130 180
   110  20 140  45
];

for k = 1:size(rects,1)
    r0 = max(1, rects(k,1)); c0 = max(1, rects(k,2));
    r1 = min(H, rects(k,3)); c1 = min(W, rects(k,4));
    occ(r0:r1, c0:c1) = 1;
end

occ_bool = occ == 1;
free_bool = ~occ_bool;

% Distance to obstacles for free cells
d_free_to_obs = bwdist(occ_bool) * res;

% Distance to free for obstacle cells
d_obs_to_free = bwdist(free_bool) * res;

% Signed distance field (SDF): positive outside, negative inside
sdf = double(d_free_to_obs);
sdf(occ_bool) = -double(d_obs_to_free(occ_bool));

% Disc-robot inflation
robot_radius = 0.30;
occ_infl = uint8(sdf < robot_radius);

% Export CSV
writematrix(occ, 'occupancy_grid_matlab.csv');
writematrix(occ_infl, 'inflated_occupancy_grid_matlab.csv');
writematrix(sdf, 'signed_distance_field_matlab.csv');

fprintf('Grid: H=%d, W=%d, res=%.3f m\n', H, W, res);
fprintf('Free ratio (original): %.4f\n', mean(occ(:)==0));
fprintf('Free ratio (inflated): %.4f\n', mean(occ_infl(:)==0));
fprintf('Wrote: occupancy_grid_matlab.csv, inflated_occupancy_grid_matlab.csv, signed_distance_field_matlab.csv\n');

% Optional visualization (comment out if you do not want figures)
figure; imagesc(occ); axis image; title('Occupancy grid (1=occupied)'); colorbar;
figure; imagesc(sdf); axis image; title('Signed distance field (meters)'); colorbar;

% -------------------------------------------------------------------------
% Simulink skeleton: create a model with a Constant block feeding a MATLAB
% Function block that computes a distance transform. This is a minimal
% starting point, not a complete navigation pipeline.
% -------------------------------------------------------------------------
try
    mdl = 'Chapter1_Lesson3_SDF_Model';
    if bdIsLoaded(mdl)
        close_system(mdl, 0);
    end
    new_system(mdl);
    open_system(mdl);

    add_block('simulink/Sources/Constant', [mdl '/OccupancyGrid'], ...
        'Value', 'occ', 'Position', [50 60 200 110]);

    add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/ComputeSDF'], ...
        'Position', [260 50 430 130]);

    add_block('simulink/Sinks/To Workspace', [mdl '/SDF_to_WS'], ...
        'VariableName', 'sdf_sim', 'Position', [500 60 650 110]);

    add_line(mdl, 'OccupancyGrid/1', 'ComputeSDF/1');
    add_line(mdl, 'ComputeSDF/1', 'SDF_to_WS/1');

    % Set MATLAB Function block code
    fcnPath = [mdl '/ComputeSDF'];
    set_param(fcnPath, 'Script', sprintf([ ...
        'function sdf_out = fcn(occ_in)\n' ...
        '%% occ_in: uint8 matrix, 1=occupied\n' ...
        'occ_bool = occ_in == 1;\n' ...
        'free_bool = ~occ_bool;\n' ...
        'd_free_to_obs = bwdist(occ_bool);\n' ...
        'd_obs_to_free = bwdist(free_bool);\n' ...
        'sdf_out = double(d_free_to_obs);\n' ...
        'sdf_out(occ_bool) = -double(d_obs_to_free(occ_bool));\n' ...
        'end\n' ...
    ]));

    save_system(mdl);
    fprintf('Created Simulink model: %s.slx\n', mdl);
catch ME
    fprintf('Simulink model creation skipped (reason: %s)\n', ME.message);
end

7.5 Wolfram Mathematica (Notebook expression)

File: Chapter1_Lesson3.nb

(*
Chapter 1 — Lesson 3: Environment Representations for AMR
File: Chapter1_Lesson3.nb

This is a Wolfram Language Notebook expression that can be saved as a .nb file.
It builds a binary occupancy grid, computes a distance transform, and forms a
signed distance field and inflated obstacle map.
*)

Notebook[{
  Cell["Chapter 1 — Lesson 3: Environment Representations for AMR", "Title"],
  Cell["Occupancy grid, signed distance field, and disc-robot inflation", "Subtitle"],

  Cell[BoxData@ToBoxes@Module[
    {
      H = 160, W = 200, res = 0.05,
      occ, rects, sdf, robotRadius = 0.30,
      dFreeToObs, dObsToFree, occInfl
    },
    occ = ConstantArray[0, {H, W}];

    rects = {
      {30, 40, 55, 100},
      {90, 120, 130, 180},
      {110, 20, 140, 45}
    };

    Do[
      occ[[r0 ;; r1, c0 ;; c1]] = 1,
      { {r0, c0, r1, c1} , rects}
    ];

    dFreeToObs = DistanceTransform[1 - occ]; (* distance to occupied *)
    dObsToFree = DistanceTransform[occ];     (* distance to free *)
    sdf = res * N[dFreeToObs];
    sdf = MapThread[If[#1 == 1, -res * #2, #3] &, {occ, dObsToFree, sdf}, 2];

    occInfl = Boole[sdf < robotRadius];

    Print["Free ratio (original): ", N[Count[occ, 0, 2]/(H*W)]];
    Print["Free ratio (inflated): ", N[Count[occInfl, 0, 2]/(H*W)]];

    Grid[{
      {"Occupancy", ArrayPlot[occ, Frame -> True, PlotLabel -> "occ (1=occupied)"]},
      {"SDF (m)", ArrayPlot[sdf, Frame -> True, PlotLabel -> "signed distance (meters)"]},
      {"Inflated", ArrayPlot[occInfl, Frame -> True, PlotLabel -> "inflated occ (1=occupied)"]}
    }, Spacings -> {2, 2}]
  ], "Input"]
}]

7.6 Exercise scaffold (Python)

File: Chapter1_Lesson3_Ex1.py

"""
Chapter 1 — Lesson 3 (Exercise 1)
File: Chapter1_Lesson3_Ex1.py

Exercise: Implement a multi-source BFS distance transform for a binary occupancy grid.
  - Input: occ (H,W) uint8 where 1=occupied, 0=free
  - Output: dist (H,W) int32 where dist=0 at occupied cells, and each free cell
            stores the Manhattan distance (in grid steps) to the nearest obstacle.

Instructions:
  1) Fill in the function bfs_distance_transform.
  2) Verify correctness on the provided test case.
"""

import numpy as np


def bfs_distance_transform(occ: np.ndarray) -> np.ndarray:
    H, W = occ.shape
    INF = 10**9
    dist = np.full((H, W), INF, dtype=np.int32)

    # TODO: initialize queue with all occupied cells (dist=0)
    # TODO: pop cells and relax 4-neighbors
    # TODO: return dist

    return dist


def _toy_test():
    occ = np.zeros((5, 7), dtype=np.uint8)
    occ[2, 3] = 1  # single obstacle
    dist = bfs_distance_transform(occ)
    print("occ:\n", occ)
    print("dist:\n", dist)


if __name__ == "__main__":
    _toy_test()

Suggested robotics libraries to explore (not required here): Python: NumPy/SciPy, Open3D, ROS2 (rclpy); C++: ROS2 (rclcpp), PCL; Java: ROSJava; MATLAB: Robotics System Toolbox; Mathematica: built-in geometry and image processing.

Problem 1 (Disc robot inflation in \( \mathbb{R}^2 \)). Let \( \mathcal{R} = \{\mathbf{r}: \|\mathbf{r}\|_2 \le r\} \) be a disc footprint. Show that the inflated obstacle set \( \mathcal{O}_\text{C} = \mathcal{O} \oplus (-\mathcal{R}) \) equals the closed \( r \)-neighborhood of \( \mathcal{O} \):

\[ \mathcal{O}_\text{C} = \{\mathbf{p} \in \mathbb{R}^2 : d(\mathbf{p},\mathcal{O}) \le r\} \]

Solution. First, suppose \( \mathbf{p} \in \mathcal{O}_\text{C} \). Then \( \mathbf{p} = \mathbf{o} - \mathbf{r} \) for some \( \mathbf{o}\in\mathcal{O} \) and \( \|\mathbf{r}\|_2 \le r \). Hence \( \|\mathbf{p}-\mathbf{o}\|_2 = \|\mathbf{r}\|_2 \le r \), so \( d(\mathbf{p},\mathcal{O}) \le r \). Conversely, if \( d(\mathbf{p},\mathcal{O}) \le r \), then there exists \( \mathbf{o}\in\mathcal{O} \) such that \( \|\mathbf{p}-\mathbf{o}\|_2 \le r \). Let \( \mathbf{r} = \mathbf{o}-\mathbf{p} \); then \( \|\mathbf{r}\|_2 \le r \) and \( \mathbf{p} = \mathbf{o} - \mathbf{r} \in \mathcal{O} \oplus (-\mathcal{R}) \). \(\blacksquare\)

Problem 2 (Log-odds update). Starting with prior \( p^- \) and likelihoods \( p(z\mid M=1) \), \( p(z\mid M=0) \), derive the additive posterior log-odds update \( \ell^+ = \ell^- + \log\frac{p(z\mid1)}{p(z\mid0)} \).

Solution. This is exactly the derivation in Section 3: compute the posterior odds \( \frac{p^+}{1-p^+} \) from Bayes’ rule and take logs. The crucial algebraic step is cancelling the common normalizing denominator and isolating \( \frac{p(z\mid1)}{p(z\mid0)} \) as a multiplicative factor.

Problem 3 (Grid discretization bound). Consider a square grid with cell side length \( \Delta \). Suppose we classify a cell as occupied if its center lies in \( \mathcal{O} \). Show that the resulting occupied region \( \widehat{\mathcal{O}} \) differs from \( \mathcal{O} \) by at most \( \tfrac{\sqrt{2}}{2}\Delta \) in Hausdorff distance (worst case).

Solution. Any point inside a cell is within Euclidean distance at most the cell’s circumradius from its center. For a square of side \( \Delta \), the circumradius is \( (\sqrt{2}/2)\Delta \). Therefore, a misclassification can only occur for points within that distance from the true boundary \( \partial\mathcal{O} \) (since the center test can “shift” the effective boundary by at most one circumradius). This yields the stated Hausdorff bound.

Problem 4 (Distance function Lipschitz). Prove that \( d(\cdot,\mathcal{O}) \) is 1-Lipschitz: \( |d(\mathbf{p},\mathcal{O}) - d(\mathbf{q},\mathcal{O})| \le \|\mathbf{p}-\mathbf{q}\|_2 \).

Solution. Use triangle inequality as in Section 5. The key idea is: for any fixed obstacle point \( \mathbf{o} \), moving from \( \mathbf{q} \) to \( \mathbf{p} \) changes the distance-to-\( \mathbf{o} \) by at most \( \|\mathbf{p}-\mathbf{q}\|_2 \), and taking the infimum over \( \mathbf{o} \) preserves the inequality.

Problem 5 (Topological shortest path). Let \( V=\{A,B,C,D\} \) and edges with weights: \( w(A,B)=1 \), \( w(B,C)=2 \), \( w(A,C)=4 \), \( w(C,D)=1 \), \( w(B,D)=5 \). Compute the minimum-cost path from \( A \) to \( D \).

Solution. Enumerate plausible paths: \( A\to B\to D \) has cost \( 1+5=6 \), \( A\to C\to D \) has cost \( 4+1=5 \), \( A\to B\to C\to D \) has cost \( 1+2+1=4 \). Therefore the shortest path is \( A\to B\to C\to D \) with total cost \( 4 \).

We defined the workspace, obstacle set, and free space, and introduced configuration-space obstacle inflation via Minkowski sums, providing a proof that reduces collision checking to point membership in an inflated set. We then formalized three major environment representation families used in AMR: (i) discrete occupancy grids (random-field view, log-odds), (ii) geometric/feature maps, and (iii) continuous fields such as distance transforms and signed distance fields, along with key analytic properties (Lipschitz continuity). Finally, we introduced topological graphs and hybrid representations as a modeling abstraction for large-scale navigation.