In indoor AMR, “traversable” often reduces to collision-free 2D occupancy. Outdoors, terrain mechanics enters: a cell can be collision-free but unsafe due to high slope, large roughness, low friction, sinkage, or vegetation compliance. We therefore estimate:

• \( y_{ij} \): a discrete terrain label for cell \( (i,j) \) (e.g., asphalt, gravel, grass, mud, rock, water/ditch).
• \( p_{ij} \): a probability of safe traversal \( p_{ij} = \Pr(\text{safe} \mid \mathcal{D}_{ij}) \), conditioned on local data \( \mathcal{D}_{ij} \).
• \( c_{ij} \): a planning cost (used in costmaps / local scoring), derived from \( p_{ij} \).

flowchart TD
  S1["Sensors: LiDAR / Stereo / Radar"] --> P1["Preprocess: filtering, time sync"]
  S2["Proprioception: IMU / Odometry"] --> P1
  P1 --> M1["2.5D map: height mean + variance per cell"]
  M1 --> F1["Features: slope, roughness, step, texture, reflectance"]
  F1 --> C1["Terrain class posterior per cell"]
  F1 --> T1["Traversability probability per cell"]
  C1 --> FUS["Fuse: expected cost by class"]
  T1 --> FUS
  FUS --> CM["Cost layer: c_ij for planner"]
  CM --> NAV["Planner: choose path + speed, replan on updates"]
        

The core modeling choice is whether traversability is rule-based (thresholds on slope/roughness), probabilistic (uncertainty-aware constraints), or learned (a classifier/regressor trained from data). In practice, outdoor stacks often combine them: geometric rules handle safety-critical limits, while learning handles semantic surface types and traction.

We assume a 2.5D elevation map (introduced earlier in Chapter 9), where each grid cell stores a height estimate \( \mu_{ij} \) and an uncertainty (variance) \( \sigma_{ij}^2 \). For a cell-centered coordinate \( \mathbf{s}_{ij} = (x_{ij}, y_{ij}) \), the map represents the surface as \( z \approx h(x,y) \).

If a range sensor provides a height measurement \( z_k \) for the same cell with measurement noise variance \( \sigma_z^2 \), a simple Bayesian (Kalman-style) fusion update is:

\[ K_{ij} \leftarrow \frac{\sigma_{ij}^2}{\sigma_{ij}^2 + \sigma_z^2},\quad \mu_{ij} \leftarrow \mu_{ij} + K_{ij}(z_k - \mu_{ij}),\quad \sigma_{ij}^2 \leftarrow (1-K_{ij})\sigma_{ij}^2 \]

This update is optimal (minimum-variance unbiased) under linear-Gaussian assumptions. Even when the surface is not strictly planar, the variance is useful as a confidence indicator for downstream classification: low-confidence cells should be treated cautiously.

Traversability is often first approximated using local geometry computed from the elevation map. Let a local neighborhood around cell \( (i,j) \) contain points \( \{(x_n,y_n,z_n)\}_{n=1}^m \). We fit a plane \( z = a x + b y + c \) via least squares.

\[ \mathbf{z} = \begin{bmatrix} z_1 \\ \vdots \\ z_m \end{bmatrix},\quad \mathbf{A} = \begin{bmatrix} x_1 & y_1 & 1 \\ \vdots & \vdots & \vdots \\ x_m & y_m & 1 \end{bmatrix},\quad \hat{\boldsymbol{\theta} } = \begin{bmatrix} \hat a \\ \hat b \\ \hat c \end{bmatrix} = (\mathbf{A}^\top \mathbf{A})^{-1}\mathbf{A}^\top \mathbf{z} \]

The (local) slope angle is then \( \alpha_{ij} \):

\[ \alpha_{ij} = \arctan\!\left(\sqrt{\hat a^2 + \hat b^2}\right) \]

Roughness can be defined as the RMS of residuals to the fitted plane: \( \rho_{ij} \):

\[ \rho_{ij} = \sqrt{\frac{1}{m}\sum_{n=1}^m \left(z_n - (\hat a x_n + \hat b y_n + \hat c)\right)^2} \]

Step height (to capture curbs/rocks) can be approximated on the height patch: \( \delta_{ij} \):

\[ \delta_{ij} = \max_{n \in \mathcal{N}_{ij} } z_n - \min_{n \in \mathcal{N}_{ij} } z_n \]

Uncertainty propagation (key result). Assume the measurement model \( \mathbf{z} = \mathbf{A}\boldsymbol{\theta} + \boldsymbol{\varepsilon} \) with \( \boldsymbol{\varepsilon}\sim\mathcal{N}(\mathbf{0}, \sigma^2\mathbf{I}) \). Then the LS estimator covariance is:

\[ \operatorname{Cov}(\hat{\boldsymbol{\theta} }) = \sigma^2(\mathbf{A}^\top \mathbf{A})^{-1} \]

This provides confidence intervals for slope (via the delta method), enabling risk-aware decisions such as “avoid if \( \Pr(\alpha_{ij} > \alpha_{\max}) \) is large”.

Let \( \boldsymbol{\phi}_{ij} \in \mathbb{R}^d \) denote a feature vector for cell \( (i,j) \), e.g., \( \boldsymbol{\phi}_{ij} = [\alpha_{ij}, \rho_{ij}, \delta_{ij}, r_{ij}, t_{ij}, \dots]^\top \), where \( r_{ij} \) might be LiDAR intensity statistics and \( t_{ij} \) image texture cues. A multiclass softmax model estimates \( \Pr(y_{ij}=c \mid \boldsymbol{\phi}_{ij}) \):

\[ \Pr(y_{ij}=c \mid \boldsymbol{\phi}_{ij}) = \frac{\exp(\mathbf{w}_c^\top \boldsymbol{\phi}_{ij})}{\sum_{c'=1}^{C}\exp(\mathbf{w}_{c'}^\top \boldsymbol{\phi}_{ij})} \]

To stabilize classifications over time, apply a Bayes filter per cell. With measurement \( z_t \) (features at time \( t \)) and prior belief \( b_{t-1}(c) \), the posterior is:

\[ b_t(c) \propto p(z_t \mid c)\, b_{t-1}(c),\quad \sum_{c=1}^{C} b_t(c) = 1 \]

For numerical stability, maintain log-beliefs: \( \ell_t(c) \):

\[ \ell_t(c) = \ell_{t-1}(c) + \log p(z_t \mid c) - \log Z_t,\quad Z_t = \sum_{c=1}^{C} p(z_t \mid c)\exp(\ell_{t-1}(c)) \]

The output of classification is not merely a label, but a distribution that can be consumed by planners via expected cost, worst-case risk, or CVaR-style objectives.

A practical definition is: \( p_{ij} = \Pr(\text{safe} \mid \boldsymbol{\phi}_{ij}) \). A common parametric model is logistic regression:

\[ p_{ij} = \sigma(s_{ij}),\quad s_{ij} = b + \mathbf{w}^\top \boldsymbol{\phi}_{ij},\quad \sigma(u)=\frac{1}{1+\exp(-u)} \]

Safety constraints can also be imposed explicitly. If a feature \( x \) is modeled as Gaussian \( x \sim \mathcal{N}(\mu, \sigma^2) \), then:

\[ \Pr(x \le \tau) = \Phi\!\left(\frac{\tau-\mu}{\sigma}\right),\quad \Pr(x > \tau) = 1-\Phi\!\left(\frac{\tau-\mu}{\sigma}\right) \]

A planner rarely consumes probabilities directly; it consumes costs. A standard conversion is the negative log: \( c_{ij} \):

\[ c_{ij} = -\log(p_{ij}+\varepsilon) \]

This is powerful because independent safety factors become additive. If \( p_{ij} = \prod_{k=1}^{K} p_{ij}^{(k)} \), then \( -\log p_{ij} = \sum_k -\log p_{ij}^{(k)} \), giving a clean decomposition across slope/roughness/traction terms.

Exteroception captures geometry and appearance; proprioception captures interaction. For wheeled robots, slip ratio is a common indicator: \( \kappa \):

\[ \kappa = \frac{\omega R - v}{\max(|v|, \epsilon_v)} \]

where \( \omega \) is wheel angular speed, \( R \) wheel radius, and \( v \) forward speed estimated from state estimation (IMU+odometry+GPS). Persistent \( |\kappa| \) indicates low traction or sinkage.

A practical self-supervised scheme is: (i) predict terrain class from camera/LiDAR features, (ii) use measured slip as a weak label for “low traction” vs “high traction”, (iii) update the classifier online (with safeguards to avoid drift). This closes the loop between perception and locomotion without requiring manual labeling for every environment.

Outdoor navigation often benefits from coupling traversability with speed control. A simple rule is to bound speed by expected slip and terrain roughness: \( v_{\max}(\mathbf{s}) \):

\[ v_{\max}(\mathbf{s}_{ij}) = \min\!\left(v_{\text{robot} }, \frac{a_{\text{lat,max} } }{\max(\tan(\alpha_{ij}), \epsilon)} \right) \]

where \( a_{\text{lat,max} } \) is a lateral acceleration limit (stability/rollover margin) and \( \alpha_{ij} \) slope. This is a simplified heuristic; advanced models also incorporate friction and curvature.

flowchart TD
  A["New sensor data"] --> B["Update elevation + features"]
  B --> C["Update class posterior + p_trav"]
  C --> D["Convert to cost + risk masks"]
  D --> E["Planner evaluates candidates"]
  E --> F["Choose path + speed"]
  F --> G["Execute; measure slip + failures"]
  G --> H["Optional: adjust model weights"]
  H --> A
        

Note the feedback loop: when execution reveals unexpected slip or sinkage, the traversability model should become more conservative in similar cells (within reasonable bounds to avoid instability).

The following reference implementations build a simple height map, compute geometric features (slope/roughness/step), and produce a traversability probability map. They are intentionally minimal to emphasize the math and data flow.

Chapter18_Lesson2.py

# Chapter18_Lesson2.py
# Traversability + terrain classification (toy implementation)
#
# This script demonstrates:
#   1) 2.5D elevation mapping (mean height per cell)
#   2) geometric features: slope, roughness, step height
#   3) a simple probabilistic traversability score via logistic model
#
# Output: a CSV map with per-cell features and p_trav.
#
# Notes:
# - In real robots, you would feed point clouds from LiDAR/stereo via ROS2
#   (e.g., sensor_msgs/PointCloud2) and publish the cost layer to Nav2.
# - Here we keep it self-contained, no ROS dependency.

from __future__ import annotations

import argparse
import math
import os
from dataclasses import dataclass
from typing import Tuple

import numpy as np


@dataclass
class GridSpec:
    x_min: float
    x_max: float
    y_min: float
    y_max: float
    resolution: float

    @property
    def shape(self) -> Tuple[int, int]:
        nx = int(math.ceil((self.x_max - self.x_min) / self.resolution))
        ny = int(math.ceil((self.y_max - self.y_min) / self.resolution))
        return ny, nx  # (rows, cols)

    def world_to_cell(self, x: np.ndarray, y: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
        ix = np.floor((x - self.x_min) / self.resolution).astype(int)
        iy = np.floor((y - self.y_min) / self.resolution).astype(int)
        return iy, ix

    def cell_center(self, iy: int, ix: int) -> Tuple[float, float]:
        xc = self.x_min + (ix + 0.5) * self.resolution
        yc = self.y_min + (iy + 0.5) * self.resolution
        return xc, yc


def sigmoid(s: np.ndarray) -> np.ndarray:
    return 1.0 / (1.0 + np.exp(-s))


def synthetic_point_cloud(n: int = 200_000, seed: int = 0) -> np.ndarray:
    """
    Create a synthetic outdoor surface: slope + bumps + ditch.
    Returns Nx3 array [x,y,z].
    """
    rng = np.random.default_rng(seed)
    x = rng.uniform(-10.0, 10.0, size=n)
    y = rng.uniform(-10.0, 10.0, size=n)

    # base plane (gentle slope)
    z = 0.05 * x + 0.02 * y

    # add bumps (rocks)
    bumps = [(-3.0, 2.0, 0.35, 1.2), (4.0, -1.5, 0.25, 0.9), (1.0, 5.0, 0.30, 1.0)]
    for cx, cy, amp, sig in bumps:
        z += amp * np.exp(-((x - cx) ** 2 + (y - cy) ** 2) / (2.0 * sig**2))

    # add ditch (negative obstacle)
    ditch = (x > -2.0) & (x < 2.0) & (y > -6.0) & (y < -3.0)
    z[ditch] -= 0.6

    # sensor noise
    z += rng.normal(0.0, 0.02, size=n)

    return np.c_[x, y, z]


def points_to_height_map(points_xyz: np.ndarray, grid: GridSpec) -> Tuple[np.ndarray, np.ndarray]:
    """
    Mean height per cell (NaN for empty). Also returns count per cell.
    """
    ny, nx = grid.shape
    H_sum = np.zeros((ny, nx), dtype=float)
    C = np.zeros((ny, nx), dtype=int)

    x, y, z = points_xyz[:, 0], points_xyz[:, 1], points_xyz[:, 2]
    iy, ix = grid.world_to_cell(x, y)

    valid = (ix >= 0) & (ix < nx) & (iy >= 0) & (iy < ny)
    iy = iy[valid]
    ix = ix[valid]
    z = z[valid]

    # accumulate
    np.add.at(H_sum, (iy, ix), z)
    np.add.at(C, (iy, ix), 1)

    H = np.full((ny, nx), np.nan, dtype=float)
    mask = C > 0
    H[mask] = H_sum[mask] / C[mask]
    return H, C


def fill_nan_nearest(H: np.ndarray, max_iters: int = 6) -> np.ndarray:
    """
    Simple hole filling: iteratively replace NaNs by mean of 8-neighbors.
    """
    out = H.copy()
    ny, nx = out.shape
    for _ in range(max_iters):
        nan_mask = np.isnan(out)
        if not nan_mask.any():
            break
        out_new = out.copy()
        for iy in range(ny):
            for ix in range(nx):
                if not nan_mask[iy, ix]:
                    continue
                y0, y1 = max(0, iy - 1), min(ny - 1, iy + 1)
                x0, x1 = max(0, ix - 1), min(nx - 1, ix + 1)
                patch = out[y0 : y1 + 1, x0 : x1 + 1]
                vals = patch[~np.isnan(patch)]
                if vals.size > 0:
                    out_new[iy, ix] = float(vals.mean())
        out = out_new
    return out


def local_features(H: np.ndarray, grid: GridSpec) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    Compute:
      slope_rad: atan(norm(grad h))
      rough: local std of heights in 3x3
      step: local max-min in 3x3
    """
    Hf = fill_nan_nearest(H, max_iters=6)

    # gradient (central differences)
    # numpy.gradient returns [d/dy, d/dx]
    dhdy, dhdx = np.gradient(Hf, grid.resolution, grid.resolution)
    slope_rad = np.arctan(np.sqrt(dhdx**2 + dhdy**2))

    ny, nx = H.shape
    rough = np.zeros((ny, nx), dtype=float)
    step = np.zeros((ny, nx), dtype=float)

    for iy in range(ny):
        for ix in range(nx):
            y0, y1 = max(0, iy - 1), min(ny - 1, iy + 1)
            x0, x1 = max(0, ix - 1), min(nx - 1, ix + 1)
            patch = Hf[y0 : y1 + 1, x0 : x1 + 1]
            rough[iy, ix] = float(np.std(patch))
            step[iy, ix] = float(np.max(patch) - np.min(patch))

    return slope_rad, rough, step


def traversability_probability(
    slope_rad: np.ndarray,
    rough: np.ndarray,
    step: np.ndarray,
    C: np.ndarray,
) -> np.ndarray:
    """
    Simple logistic model:
        p = sigmoid(b + w1*slope + w2*rough + w3*step)
    Weights are illustrative; in practice, you calibrate them to your robot
    and terrain with a dataset (labels from human driving success or slip).
    """
    # illustrative weights (tune/calibrate)
    b = 3.0
    w1 = -6.0     # slope_rad
    w2 = -40.0    # rough (meters)
    w3 = -25.0    # step (meters)

    s = b + w1 * slope_rad + w2 * rough + w3 * step
    p = sigmoid(s)
    p = np.where(C > 0, p, np.nan)
    return p


def write_csv(out_path: str, grid: GridSpec, p: np.ndarray, slope_rad: np.ndarray, rough: np.ndarray, step: np.ndarray, C: np.ndarray) -> None:
    ny, nx = grid.shape
    slope_deg = slope_rad * 180.0 / np.pi

    with open(out_path, "w", encoding="utf-8") as f:
        f.write("x,y,p_trav,slope_deg,rough_m,step_m,count\n")
        for iy in range(ny):
            for ix in range(nx):
                if C[iy, ix] == 0:
                    continue
                xc, yc = grid.cell_center(iy, ix)
                f.write(
                    f"{xc:.6f},{yc:.6f},{p[iy,ix]:.8f},{slope_deg[iy,ix]:.4f},{rough[iy,ix]:.6f},{step[iy,ix]:.6f},{C[iy,ix]}\n"
                )


def main() -> None:
    parser = argparse.ArgumentParser()
    parser.add_argument("--points_csv", type=str, default="", help="CSV with columns x,y,z (no header required). If empty, use synthetic data.")
    parser.add_argument("--resolution", type=float, default=0.25)
    parser.add_argument("--out_csv", type=str, default="traversability_map.csv")
    args = parser.parse_args()

    if args.points_csv and os.path.isfile(args.points_csv):
        pts = np.loadtxt(args.points_csv, delimiter=",")
        if pts.ndim == 1:
            pts = pts.reshape(1, -1)
        pts = pts[:, :3]
    else:
        pts = synthetic_point_cloud()

    x_min, x_max = float(np.min(pts[:, 0])), float(np.max(pts[:, 0]))
    y_min, y_max = float(np.min(pts[:, 1])), float(np.max(pts[:, 1]))

    grid = GridSpec(x_min=x_min, x_max=x_max, y_min=y_min, y_max=y_max, resolution=args.resolution)

    H, C = points_to_height_map(pts, grid)
    slope_rad, rough, step = local_features(H, grid)
    p = traversability_probability(slope_rad, rough, step, C)

    write_csv(args.out_csv, grid, p, slope_rad, rough, step, C)
    print(f"Exported: {args.out_csv}")


if __name__ == "__main__":
    main()
      

Chapter18_Lesson2.cpp

// Chapter18_Lesson2.cpp
// Traversability + terrain classification (toy implementation, C++17)
//
// Build:
//   g++ -O2 -std=c++17 Chapter18_Lesson2.cpp -o Chapter18_Lesson2
//
// Run:
//   ./Chapter18_Lesson2 --points_csv points.csv --resolution 0.25 --out_csv traversability_map.csv
//   (If --points_csv omitted, generates synthetic point cloud)
//
// CSV input columns: x,y,z (no header required).
//
// Notes:
// - For production: integrate with ROS2 rclcpp + sensor_msgs/PointCloud2,
//   and publish a costmap layer to Nav2.
// - Here we keep it dependency-free (no Eigen/PCL) to focus on math/pipeline.

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <fstream>
#include <iostream>
#include <random>
#include <sstream>
#include <string>
#include <tuple>
#include <vector>

struct GridSpec {
  double x_min, x_max, y_min, y_max, res;
  int nx() const { return static_cast<int>(std::ceil((x_max - x_min) / res)); }
  int ny() const { return static_cast<int>(std::ceil((y_max - y_min) / res)); }
};

static inline double sigmoid(double s) { return 1.0 / (1.0 + std::exp(-s)); }

std::vector<std::array<double,3>> synthetic_point_cloud(std::size_t n=200000, unsigned seed=0) {
  std::mt19937 rng(seed);
  std::uniform_real_distribution<double> uni(-10.0, 10.0);
  std::normal_distribution<double> noise(0.0, 0.02);

  std::vector<std::array<double,3>> pts;
  pts.reserve(n);

  struct Bump { double cx, cy, amp, sig; };
  std::vector<Bump> bumps = {
    {-3.0,  2.0, 0.35, 1.2},
    { 4.0, -1.5, 0.25, 0.9},
    { 1.0,  5.0, 0.30, 1.0}
  };

  for (std::size_t i=0;i<n;i++) {
    double x = uni(rng);
    double y = uni(rng);
    double z = 0.05*x + 0.02*y;

    for (const auto& b : bumps) {
      double dx = x - b.cx;
      double dy = y - b.cy;
      z += b.amp * std::exp(-(dx*dx + dy*dy) / (2.0*b.sig*b.sig));
    }

    bool ditch = (x > -2.0 && x < 2.0 && y > -6.0 && y < -3.0);
    if (ditch) z -= 0.6;

    z += noise(rng);
    pts.push_back({x,y,z});
  }
  return pts;
}

std::vector<std::array<double,3>> read_xyz_csv(const std::string& path) {
  std::ifstream in(path);
  if (!in) throw std::runtime_error("Cannot open file: " + path);

  std::vector<std::array<double,3>> pts;
  std::string line;
  while (std::getline(in, line)) {
    if (line.empty()) continue;
    std::stringstream ss(line);
    std::string tok;
    std::vector<double> cols;
    while (std::getline(ss, tok, ',')) {
      try {
        cols.push_back(std::stod(tok));
      } catch (...) {
        cols.clear(); // allow header
        break;
      }
    }
    if (cols.size() >= 3) pts.push_back({cols[0], cols[1], cols[2]});
  }
  if (pts.empty()) throw std::runtime_error("No valid points in: " + path);
  return pts;
}

void points_to_height_map(
    const std::vector<std::array<double,3>>& pts,
    const GridSpec& g,
    std::vector<double>& H,      // mean height (NaN for empty)
    std::vector<int>& C          // count per cell
) {
  const int Nx = g.nx();
  const int Ny = g.ny();
  H.assign(Nx*Ny, 0.0);
  C.assign(Nx*Ny, 0);

  std::vector<double> Hsum(Nx*Ny, 0.0);

  for (const auto& p : pts) {
    double x = p[0], y = p[1], z = p[2];
    int ix = static_cast<int>(std::floor((x - g.x_min) / g.res));
    int iy = static_cast<int>(std::floor((y - g.y_min) / g.res));
    if (ix < 0 || ix >= Nx || iy < 0 || iy >= Ny) continue;
    int k = iy*Nx + ix;
    Hsum[k] += z;
    C[k] += 1;
  }

  H.resize(Nx*Ny);
  for (int k=0;k<Nx*Ny;k++) {
    if (C[k] > 0) H[k] = Hsum[k] / static_cast<double>(C[k]);
    else H[k] = std::numeric_limits<double>::quiet_NaN();
  }
}

void fill_nan(std::vector<double>& H, int Nx, int Ny, int iters=6) {
  for (int it=0; it<iters; it++) {
    bool any = false;
    std::vector<double> out = H;
    for (int y=0;y<Ny;y++) {
      for (int x=0;x<Nx;x++) {
        int k = y*Nx + x;
        if (!std::isnan(H[k])) continue;
        any = true;
        double s = 0.0;
        int cnt = 0;
        for (int yy=std::max(0,y-1); yy<=std::min(Ny-1,y+1); yy++) {
          for (int xx=std::max(0,x-1); xx<=std::min(Nx-1,x+1); xx++) {
            int kk = yy*Nx + xx;
            if (std::isnan(H[kk])) continue;
            s += H[kk];
            cnt++;
          }
        }
        if (cnt > 0) out[k] = s / static_cast<double>(cnt);
      }
    }
    H.swap(out);
    if (!any) break;
  }
}

void compute_features(
    const std::vector<double>& H_in,
    const std::vector<int>& C,
    const GridSpec& g,
    std::vector<double>& slope_rad,
    std::vector<double>& rough,
    std::vector<double>& step
) {
  const int Nx = g.nx();
  const int Ny = g.ny();

  std::vector<double> H = H_in;
  fill_nan(H, Nx, Ny, 6);

  slope_rad.assign(Nx*Ny, std::numeric_limits<double>::quiet_NaN());
  rough.assign(Nx*Ny, std::numeric_limits<double>::quiet_NaN());
  step.assign(Nx*Ny, std::numeric_limits<double>::quiet_NaN());

  // slope via central differences (clamped boundaries)
  for (int y=0;y<Ny;y++) {
    for (int x=0;x<Nx;x++) {
      int xm1 = std::max(0, x-1), xp1 = std::min(Nx-1, x+1);
      int ym1 = std::max(0, y-1), yp1 = std::min(Ny-1, y+1);
      double hx1 = H[y*Nx + xp1], hx0 = H[y*Nx + xm1];
      double hy1 = H[yp1*Nx + x], hy0 = H[ym1*Nx + x];
      double dhdx = (hx1 - hx0) / (2.0 * g.res);
      double dhdy = (hy1 - hy0) / (2.0 * g.res);
      double gn = std::sqrt(dhdx*dhdx + dhdy*dhdy);
      slope_rad[y*Nx + x] = std::atan(gn);
    }
  }

  // roughness (std) and step (max-min) in 3x3 patch
  for (int y=0;y<Ny;y++) {
    for (int x=0;x<Nx;x++) {
      double s = 0.0, s2 = 0.0;
      double mn = std::numeric_limits<double>::infinity();
      double mx = -std::numeric_limits<double>::infinity();
      int cnt = 0;
      for (int yy=std::max(0,y-1); yy<=std::min(Ny-1,y+1); yy++) {
        for (int xx=std::max(0,x-1); xx<=std::min(Nx-1,x+1); xx++) {
          double v = H[yy*Nx + xx];
          s += v; s2 += v*v;
          mn = std::min(mn, v);
          mx = std::max(mx, v);
          cnt++;
        }
      }
      double mu = s / static_cast<double>(cnt);
      double var = std::max(0.0, s2 / static_cast<double>(cnt) - mu*mu);
      rough[y*Nx + x] = std::sqrt(var);
      step[y*Nx + x] = mx - mn;
    }
  }

  // invalidate empty cells
  for (int k=0;k<Nx*Ny;k++) {
    if (C[k] == 0) {
      slope_rad[k] = std::numeric_limits<double>::quiet_NaN();
      rough[k] = std::numeric_limits<double>::quiet_NaN();
      step[k] = std::numeric_limits<double>::quiet_NaN();
    }
  }
}

void write_csv(
    const std::string& out_path,
    const GridSpec& g,
    const std::vector<int>& C,
    const std::vector<double>& slope,
    const std::vector<double>& rough,
    const std::vector<double>& step
) {
  const int Nx = g.nx();
  const int Ny = g.ny();

  // illustrative weights (calibrate for your robot)
  const double b  = 3.0;
  const double w1 = -6.0;   // slope_rad
  const double w2 = -40.0;  // rough
  const double w3 = -25.0;  // step

  std::ofstream out(out_path);
  out << "x,y,p_trav,slope_deg,rough_m,step_m,count\n";

  for (int iy=0; iy<Ny; iy++) {
    double yc = g.y_min + (iy + 0.5) * g.res;
    for (int ix=0; ix<Nx; ix++) {
      int k = iy*Nx + ix;
      if (C[k] == 0) continue;
      double s = b + w1*slope[k] + w2*rough[k] + w3*step[k];
      double p = sigmoid(s);
      double slope_deg = slope[k] * 180.0 / M_PI;
      double xc = g.x_min + (ix + 0.5) * g.res;
      out << xc << "," << yc << "," << p << "," << slope_deg << "," << rough[k] << "," << step[k] << "," << C[k] << "\n";
    }
  }
}

int main(int argc, char** argv) {
  std::string points_csv = "";
  std::string out_csv = "traversability_map.csv";
  double res = 0.25;

  for (int i=1; i<argc; i++) {
    std::string a = argv[i];
    if (a == "--points_csv" && i+1<argc) points_csv = argv[++i];
    else if (a == "--out_csv" && i+1<argc) out_csv = argv[++i];
    else if (a == "--resolution" && i+1<argc) res = std::stod(argv[++i]);
    else if (a == "--help") {
      std::cout << "Usage: ./Chapter18_Lesson2 [--points_csv points.csv] --resolution 0.25 --out_csv traversability_map.csv\n";
      return 0;
    }
  }

  std::vector<std::array<double,3>> pts;
  if (!points_csv.empty()) {
    pts = read_xyz_csv(points_csv);
  } else {
    pts = synthetic_point_cloud(200000, 0);
  }

  double x_min = pts[0][0], x_max = pts[0][0];
  double y_min = pts[0][1], y_max = pts[0][1];
  for (const auto& p : pts) {
    x_min = std::min(x_min, p[0]); x_max = std::max(x_max, p[0]);
    y_min = std::min(y_min, p[1]); y_max = std::max(y_max, p[1]);
  }

  GridSpec g{x_min, x_max, y_min, y_max, res};
  const int Nx = g.nx();
  const int Ny = g.ny();

  std::vector<double> H;
  std::vector<int> C;
  points_to_height_map(pts, g, H, C);

  std::vector<double> slope, rough, step;
  compute_features(H, C, g, slope, rough, step);

  write_csv(out_csv, g, C, slope, rough, step);

  std::cout << "Exported: " << out_csv << "\n";
  std::cout << "Grid: " << Ny << " x " << Nx << " (resolution=" << res << ")\n";
  return 0;
}
      

Chapter18_Lesson2.java

// Chapter18_Lesson2.java
// Traversability + terrain classification (toy implementation, no external libs).
//
// Compile: javac Chapter18_Lesson2.java
// Run:     java Chapter18_Lesson2 --points_csv points.csv --resolution 0.25 --out_csv traversability_map.csv
//
// CSV input columns: x,y,z (no header required).
// Output CSV columns: x,y,p_trav,slope_deg,rough_m,step_m,count
//
// Note: In production, Java teams often integrate with ROS2 via rcljava and
// represent maps as nav_msgs/OccupancyGrid or custom layers.

import java.io.*;
import java.util.*;

public class Chapter18_Lesson2 {

  static class GridSpec {
    double xMin, xMax, yMin, yMax, res;
    int width()  { return (int)Math.ceil((xMax - xMin) / res); }
    int height() { return (int)Math.ceil((yMax - yMin) / res); }
  }

  static double sigmoid(double s) { return 1.0 / (1.0 + Math.exp(-s)); }

  static class Points {
    double[] x, y, z;
    Points(double[] x, double[] y, double[] z) { this.x=x; this.y=y; this.z=z; }
    int n() { return x.length; }
  }

  static Points readXYZ(String path) throws IOException {
    ArrayList<Double> xs = new ArrayList<>();
    ArrayList<Double> ys = new ArrayList<>();
    ArrayList<Double> zs = new ArrayList<>();
    try (BufferedReader br = new BufferedReader(new FileReader(path))) {
      String line;
      while ((line = br.readLine()) != null) {
        if (line.trim().isEmpty()) continue;
        String[] parts = line.split(",");
        if (parts.length < 3) continue;
        try {
          xs.add(Double.parseDouble(parts[0].trim()));
          ys.add(Double.parseDouble(parts[1].trim()));
          zs.add(Double.parseDouble(parts[2].trim()));
        } catch (NumberFormatException ex) {
          // allow header
        }
      }
    }
    if (xs.isEmpty()) throw new IllegalArgumentException("No valid points in " + path);
    double[] x = new double[xs.size()], y = new double[ys.size()], z = new double[zs.size()];
    for (int i=0;i<xs.size();i++) { x[i]=xs.get(i); y[i]=ys.get(i); z[i]=zs.get(i); }
    return new Points(x,y,z);
  }

  static void pointsToHeightMap(Points p, GridSpec g, double[] H, int[] C) {
    int Nx = g.width(), Ny = g.height();
    Arrays.fill(H, 0.0);
    Arrays.fill(C, 0);

    for (int i=0;i<p.n();i++) {
      int ix = (int)Math.floor((p.x[i] - g.xMin) / g.res);
      int iy = (int)Math.floor((p.y[i] - g.yMin) / g.res);
      if (ix < 0 || ix >= Nx || iy < 0 || iy >= Ny) continue;
      int k = iy * Nx + ix;
      H[k] += p.z[i];
      C[k] += 1;
    }
    for (int k=0;k<H.length;k++) {
      if (C[k] > 0) H[k] /= (double)C[k];
      else H[k] = Double.NaN;
    }
  }

  static void computeFeatures(
      double[] HIn, int[] C, GridSpec g,
      double[] slopeRad, double[] rough, double[] step
  ) {
    int Nx = g.width(), Ny = g.height();
    double[] H = Arrays.copyOf(HIn, HIn.length);
    // fill holes
    for (int it=0; it<6; it++) {
      boolean any = false;
      double[] out = Arrays.copyOf(H, H.length);
      for (int y=0;y<Ny;y++) for (int x=0;x<Nx;x++) {
        int k = y*Nx + x;
        if (!Double.isNaN(H[k])) continue;
        any = true;
        double s = 0.0;
        int cnt = 0;
        for (int yy=Math.max(0,y-1); yy<=Math.min(Ny-1,y+1); yy++) {
          for (int xx=Math.max(0,x-1); xx<=Math.min(Nx-1,x+1); xx++) {
            int kk = yy*Nx + xx;
            if (Double.isNaN(H[kk])) continue;
            s += H[kk];
            cnt++;
          }
        }
        if (cnt > 0) out[k] = s / (double)cnt;
      }
      H = out;
      if (!any) break;
    }

    // slope via central differences
    for (int y=0;y<Ny;y++) for (int x=0;x<Nx;x++) {
      int xm1 = Math.max(0, x-1), xp1 = Math.min(Nx-1, x+1);
      int ym1 = Math.max(0, y-1), yp1 = Math.min(Ny-1, y+1);
      double hx1 = H[y*Nx + xp1], hx0 = H[y*Nx + xm1];
      double hy1 = H[yp1*Nx + x], hy0 = H[ym1*Nx + x];
      double dhdx = (hx1 - hx0) / (2.0 * g.res);
      double dhdy = (hy1 - hy0) / (2.0 * g.res);
      double gn = Math.sqrt(dhdx*dhdx + dhdy*dhdy);
      slopeRad[y*Nx + x] = Math.atan(gn);
    }

    // roughness/std and step/max-min in 3x3 neighborhood
    for (int y=0;y<Ny;y++) for (int x=0;x<Nx;x++) {
      double s = 0.0, s2 = 0.0;
      double mn = Double.POSITIVE_INFINITY, mx = Double.NEGATIVE_INFINITY;
      int cnt = 0;
      for (int yy=Math.max(0,y-1); yy<=Math.min(Ny-1,y+1); yy++) {
        for (int xx=Math.max(0,x-1); xx<=Math.min(Nx-1,x+1); xx++) {
          double v = H[yy*Nx + xx];
          s += v;
          s2 += v*v;
          if (v < mn) mn = v;
          if (v > mx) mx = v;
          cnt++;
        }
      }
      double mu = s / (double)cnt;
      double var = Math.max(0.0, s2 / (double)cnt - mu*mu);
      rough[y*Nx + x] = Math.sqrt(var);
      step[y*Nx + x] = mx - mn;
    }

    // invalidate empty cells
    for (int k=0;k<C.length;k++) {
      if (C[k] == 0) {
        slopeRad[k] = Double.NaN;
        rough[k] = Double.NaN;
        step[k] = Double.NaN;
      }
    }
  }

  static void writeCSV(String outPath, GridSpec g, int[] C, double[] slope, double[] rough, double[] step) throws IOException {
    // illustrative weights (calibrate for your robot)
    double b = 3.0;
    double w1 = -6.0, w2 = -40.0, w3 = -25.0;

    int Nx = g.width(), Ny = g.height();
    try (PrintWriter pw = new PrintWriter(new FileWriter(outPath))) {
      pw.println("x,y,p_trav,slope_deg,rough_m,step_m,count");
      for (int iy=0; iy<Ny; iy++) {
        double yc = g.yMin + (iy + 0.5) * g.res;
        for (int ix=0; ix<Nx; ix++) {
          int k = iy*Nx + ix;
          if (C[k] == 0) continue;
          double s = b + w1*slope[k] + w2*rough[k] + w3*step[k];
          double p = sigmoid(s);
          double slopeDeg = slope[k] * 180.0 / Math.PI;
          double xc = g.xMin + (ix + 0.5) * g.res;
          pw.printf(Locale.US, "%.6f,%.6f,%.8f,%.4f,%.6f,%.6f,%d%n",
                    xc, yc, p, slopeDeg, rough[k], step[k], C[k]);
        }
      }
    }
  }

  public static void main(String[] args) throws Exception {
    String pointsCSV = "";
    String outCSV = "traversability_map.csv";
    double res = 0.25;

    for (int i=0;i<args.length;i++) {
      if (args[i].equals("--points_csv") && i+1<args.length) pointsCSV = args[++i];
      else if (args[i].equals("--out_csv") && i+1<args.length) outCSV = args[++i];
      else if (args[i].equals("--resolution") && i+1<args.length) res = Double.parseDouble(args[++i]);
      else if (args[i].equals("--help")) {
        System.out.println("java Chapter18_Lesson2 --points_csv points.csv --resolution 0.25 --out_csv traversability_map.csv");
        return;
      }
    }

    if (pointsCSV.isEmpty()) throw new IllegalArgumentException("--points_csv required for this Java demo.");

    Points p = readXYZ(pointsCSV);
    double xMin = Arrays.stream(p.x).min().getAsDouble();
    double xMax = Arrays.stream(p.x).max().getAsDouble();
    double yMin = Arrays.stream(p.y).min().getAsDouble();
    double yMax = Arrays.stream(p.y).max().getAsDouble();

    GridSpec g = new GridSpec();
    g.xMin = xMin; g.xMax = xMax; g.yMin = yMin; g.yMax = yMax; g.res = res;

    int Nx = g.width(), Ny = g.height();
    double[] H = new double[Nx*Ny];
    int[] C = new int[Nx*Ny];
    pointsToHeightMap(p, g, H, C);

    double[] slope = new double[Nx*Ny];
    double[] rough = new double[Nx*Ny];
    double[] step = new double[Nx*Ny];
    computeFeatures(H, C, g, slope, rough, step);

    writeCSV(outCSV, g, C, slope, rough, step);
    System.out.println("Exported: " + outCSV);
  }
}
      

Chapter18_Lesson2.m

% Chapter18_Lesson2.m
% Traversability + terrain classification (toy implementation) + Simulink model stub.
%
% Usage (MATLAB):
%   % Input CSV columns: x,y,z
%   Chapter18_Lesson2('points.csv', 0.25, 'traversability_map.csv');
%
% Or generate a synthetic point cloud:
%   Chapter18_Lesson2('', 0.25, 'traversability_map.csv');
%
% This is an educational script (no ROS required). For real robots, you can
% integrate with ROS Toolbox (ros2subscriber / ros2publisher) and Nav2-style
% costmap layers.

function Chapter18_Lesson2(points_csv, resolution, out_csv)

if nargin < 1, points_csv = ''; end
if nargin < 2, resolution = 0.25; end
if nargin < 3, out_csv = 'traversability_map.csv'; end

if ~isempty(points_csv)
    P = readmatrix(points_csv);
    P = P(:,1:3);
else
    P = synthetic_point_cloud(200000, 0);
end

x_min = min(P(:,1)); x_max = max(P(:,1));
y_min = min(P(:,2)); y_max = max(P(:,2));

Nx = ceil((x_max - x_min) / resolution);
Ny = ceil((y_max - y_min) / resolution);

H_sum = zeros(Ny, Nx);
C = zeros(Ny, Nx);

ix = floor((P(:,1) - x_min) / resolution) + 1;
iy = floor((P(:,2) - y_min) / resolution) + 1;

valid = ix >= 1 & ix <= Nx & iy >= 1 & iy <= Ny;
ix = ix(valid); iy = iy(valid); z = P(valid,3);

for i = 1:numel(z)
    H_sum(iy(i), ix(i)) = H_sum(iy(i), ix(i)) + z(i);
    C(iy(i), ix(i)) = C(iy(i), ix(i)) + 1;
end

H = nan(Ny, Nx);
mask = C > 0;
H(mask) = H_sum(mask) ./ C(mask);

Hf = fillmissing(H, 'nearest');

% Gradient-based slope
[dhdy, dhdx] = gradient(Hf, resolution, resolution);
slope_rad = atan(sqrt(dhdx.^2 + dhdy.^2));
slope_deg = slope_rad * 180/pi;

% Roughness: local std in 3x3 window
rough = local_std(Hf, 1);

% Step height: local max-min in 3x3
step = local_step(Hf, 1);

% Simple calibrated model (illustrative weights; learn for your robot)
b  = 3.0;
w1 = -6.0;   % slope_rad
w2 = -40.0;  % rough [m]
w3 = -25.0;  % step [m]
p_trav = 1 ./ (1 + exp(-(b + w1*slope_rad + w2*rough + w3*step)));
p_trav(~mask) = NaN;

% Export per-cell CSV
fid = fopen(out_csv, 'w');
fprintf(fid, 'x,y,p_trav,slope_deg,rough_m,step_m,count\n');
for iy0 = 1:Ny
    yc = y_min + (iy0 - 0.5) * resolution;
    for ix0 = 1:Nx
        if ~mask(iy0, ix0), continue; end
        xc = x_min + (ix0 - 0.5) * resolution;
        fprintf(fid, '%.6f,%.6f,%.8f,%.4f,%.6f,%.6f,%d\n', ...
            xc, yc, p_trav(iy0,ix0), slope_deg(iy0,ix0), rough(iy0,ix0), step(iy0,ix0), C(iy0,ix0));
    end
end
fclose(fid);

disp(['Exported: ', out_csv]);

% Optional: create a Simulink model that implements the logistic traversability block
% (requires Simulink). Uncomment to generate:
% create_traversability_simulink_model();

end

function P = synthetic_point_cloud(n, seed)
rng(seed);
x = -10 + 20*rand(n,1);
y = -10 + 20*rand(n,1);

z = 0.05*x + 0.02*y;
% bumps
bumps = [-3 2 0.35 1.2; 4 -1.5 0.25 0.9; 1 5 0.30 1.0];
for i=1:size(bumps,1)
    cx=bumps(i,1); cy=bumps(i,2); a=bumps(i,3); s=bumps(i,4);
    z = z + a * exp(-((x-cx).^2 + (y-cy).^2)/(2*s^2));
end
% ditch
ditch = x > -2 & x < 2 & y > -6 & y < -3;
z(ditch) = z(ditch) - 0.6;

z = z + 0.02*randn(n,1);
P = [x y z];
end

function S = local_std(A, k)
[Ny, Nx] = size(A);
S = zeros(Ny, Nx);
for y=1:Ny
    ys = max(1,y-k):min(Ny,y+k);
    for x=1:Nx
        xs = max(1,x-k):min(Nx,x+k);
        patch = A(ys, xs);
        S(y,x) = std(patch(:), 0);
    end
end
end

function D = local_step(A, k)
[Ny, Nx] = size(A);
D = zeros(Ny, Nx);
for y=1:Ny
    ys = max(1,y-k):min(Ny,y+k);
    for x=1:Nx
        xs = max(1,x-k):min(Nx,x+k);
        patch = A(ys, xs);
        D(y,x) = max(patch(:)) - min(patch(:));
    end
end
end

function create_traversability_simulink_model()
% Programmatically create a small Simulink model:
% Inputs: slope_rad, rough, step
% Output: p_trav = sigmoid(b + w1*slope + w2*rough + w3*step)
%
% This is intended as a teaching aid: you can connect this block to a
% perception pipeline and feed signals from ROS Toolbox or logged data.

model = 'Chapter18_Lesson2_Simulink';
new_system(model);
open_system(model);

add_block('simulink/Sources/In1', [model '/slope_rad'], 'Position', [30 30 60 50]);
add_block('simulink/Sources/In1', [model '/rough'],     'Position', [30 80 60 100]);
add_block('simulink/Sources/In1', [model '/step'],      'Position', [30 130 60 150]);

add_block('simulink/User-Defined Functions/MATLAB Function', [model '/Traversability'], ...
    'Position', [160 55 320 145]);
set_param([model '/Traversability'], 'Script', ...
    sprintf(['function p = f(slope_rad, rough, step)\n' ...
             '%% logistic traversability\n' ...
             'b=3.0; w1=-6.0; w2=-40.0; w3=-25.0;\n' ...
             's = b + w1*slope_rad + w2*rough + w3*step;\n' ...
             'p = 1./(1+exp(-s));\n' ...
             'end\n']));

add_block('simulink/Sinks/Out1', [model '/p_trav'], 'Position', [380 85 410 105]);

add_line(model, 'slope_rad/1', 'Traversability/1');
add_line(model, 'rough/1',     'Traversability/2');
add_line(model, 'step/1',      'Traversability/3');
add_line(model, 'Traversability/1', 'p_trav/1');

save_system(model);
disp(['Created Simulink model: ', model]);

end
      

Chapter18_Lesson2.nb

(* Chapter18_Lesson2.nb
   Wolfram Mathematica Notebook (plain-text) *)

Notebook[{
  Cell["Chapter 18 - Lesson 2: Traversability and Terrain Classification", "Title"],
  Cell["This notebook is a minimal, self-contained demonstration of grid-based elevation mapping and traversability scoring.", "Text"],

  Cell["1) Load / generate point cloud", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    ClearAll["Global`*"];
    SyntheticPointCloud[n_Integer: 200000, seed_Integer: 0] := Module[{x, y, z, bumps, ditch},
      SeedRandom[seed];
      x = RandomReal[{-10, 10}, n];
      y = RandomReal[{-10, 10}, n];
      z = 0.05 x + 0.02 y;
      bumps = { {-3, 2, 0.35, 1.2}, {4, -1.5, 0.25, 0.9}, {1, 5, 0.30, 1.0} };
      Do[
        z = z + a Exp[-((x - cx)^2 + (y - cy)^2)/(2 s^2)],
        { {cx, cy, a, s}, bumps}
      ];
      ditch = Boole[(-2 < x < 2) && (-6 < y < -3)] /. Thread[{x, y} -> Transpose[{x, y}]];
      z = z - 0.6 ditch;
      z = z + RandomVariate[NormalDistribution[0, 0.02], n];
      Transpose[{x, y, z}]
    ];

    pts = SyntheticPointCloud[200000, 0];
  ], "Input"],

  Cell["2) Build a 2.5D height map (mean height per grid cell)", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    GridSpec[xmin_, xmax_, ymin_, ymax_, res_] := <|
      "xmin" -> xmin, "xmax" -> xmax, "ymin" -> ymin, "ymax" -> ymax, "res" -> res,
      "Nx" -> Ceiling[(xmax - xmin)/res], "Ny" -> Ceiling[(ymax - ymin)/res]
    |>;

    spec = GridSpec[-10, 10, -10, 10, 0.25];

    ToIndex[{x_, y_}, s_] := Module[{ix, iy},
      ix = Floor[(x - s["xmin"])/s["res"]] + 1;
      iy = Floor[(y - s["ymin"])/s["res"]] + 1;
      {ix, iy}
    ];

    Hsum = ConstantArray[0., {spec["Ny"], spec["Nx"]}];
    Cnt  = ConstantArray[0,  {spec["Ny"], spec["Nx"]}];

    Do[
      With[{x = pts[[i, 1]], y = pts[[i, 2]], z = pts[[i, 3]]},
        With[{ij = ToIndex[{x, y}, spec]},
          If[1 <= ij[[1]] <= spec["Nx"] && 1 <= ij[[2]] <= spec["Ny"],
            Hsum[[ij[[2]], ij[[1]]]] += z;
            Cnt[[ij[[2]], ij[[1]]]] += 1;
          ]
        ]
      ],
      {i, Length[pts]}
    ];

    H = MapThread[If[#2 > 0, #1/#2, Indeterminate] &, {Hsum, Cnt}, 2];
  ], "Input"],

  Cell["3) Features: slope, roughness, step height", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    (* Fill Indeterminate holes by nearest-neighbor style smoothing *)
    FillHoles[A_, iters_: 5] := Nest[
      Function[M,
        MapIndexed[
          Function[{v, idx},
            If[v === Indeterminate,
              Module[{y = idx[[1]], x = idx[[2]], ys, xs, patch, vals},
                ys = Range[Max[1, y - 1], Min[Length[M], y + 1]];
                xs = Range[Max[1, x - 1], Min[Length[M[[1]]], x + 1]];
                patch = Flatten[M[[ys, xs]]];
                vals = Select[patch, # =!= Indeterminate &];
                If[Length[vals] > 0, Mean[vals], Indeterminate]
              ],
              v
            ]
          ],
          M,
          {2}
        ]
      ],
      A,
      iters
    ];

    Hf = FillHoles[H, 6];

    (* Gradient approximations *)
    dhdx = ListCorrelate[{ {-1, 0, 1} }, Hf, {2, 2}, 0] / (2 spec["res"]);
    dhdy = ListCorrelate[{ {-1}, {0}, {1} }, Hf, {2, 2}, 0] / (2 spec["res"]);
    slopeRad = ArcTan[Sqrt[dhdx^2 + dhdy^2]];

    (* Roughness and step in 3x3 neighborhoods *)
    NeighborhoodPatch[M_, {y_, x_}] := Flatten[M[[Range[Max[1, y - 1], Min[spec["Ny"], y + 1]],
                                                   Range[Max[1, x - 1], Min[spec["Nx"], x + 1]]]]];

    rough = Table[StandardDeviation[NeighborhoodPatch[Hf, {y, x}]], {y, spec["Ny"]}, {x, spec["Nx"]}];
    step  = Table[Max[NeighborhoodPatch[Hf, {y, x}]] - Min[NeighborhoodPatch[Hf, {y, x}]], {y, spec["Ny"]}, {x, spec["Nx"]}];
  ], "Input"],

  Cell["4) Traversability probability (logistic model)", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    b = 3.0; w1 = -6.0; w2 = -40.0; w3 = -25.0;
    Sigmoid[s_] := 1/(1 + Exp[-s]);
    pTrav = MapThread[Sigmoid[b + w1 #1 + w2 #2 + w3 #3] &, {slopeRad, rough, step}, 2];

    (* Mask out empty cells *)
    pTravMasked = MapThread[If[#2 > 0, #1, Indeterminate] &, {pTrav, Cnt}, 2];
  ], "Input"],

  Cell["5) Export (x,y,p_trav,slope_deg,rough,step,count) to CSV", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    CellCenter[{ix_, iy_}] := {
      spec["xmin"] + (ix - 0.5) spec["res"],
      spec["ymin"] + (iy - 0.5) spec["res"]
    };

    rows = Reap[
      Do[
        If[Cnt[[iy, ix]] > 0,
          With[{xy = CellCenter[{ix, iy}]},
            Sow[{xy[[1]], xy[[2]],
                 pTravMasked[[iy, ix]],
                 (180./Pi) slopeRad[[iy, ix]],
                 rough[[iy, ix]],
                 step[[iy, ix]],
                 Cnt[[iy, ix]]}]
          ]
        ],
        {iy, 1, spec["Ny"]}, {ix, 1, spec["Nx"]}
      ]
    ][[2, 1]];

    Export["traversability_map.csv",
      Prepend[rows, {"x","y","p_trav","slope_deg","rough_m","step_m","count"}]
    ];
  ], "Input"],

  Cell["Done.", "Text"]
},
WindowSize-> {980, 720},
WindowMargins-> { {Automatic, 30}, {Automatic, 30} },
StyleDefinitions->"Default.nb"
]
      

Library notes (practical integration):

• Python: NumPy for grids; scikit-learn for quick calibration; ROS2 via rclpy; map publication via nav_msgs/OccupancyGrid or custom traversability layers.
• C++: ROS2 via rclcpp; point clouds via PCL; linear algebra via Eigen; costmaps via Nav2 costmap plugins.
• Java: ROS2 via rcljava; matrix ops via EJML; useful for Android/edge deployments.
• MATLAB/Simulink: ROS Toolbox for ROS2; point clouds via Computer Vision Toolbox; Simulink for rapid prototyping of the traversability scoring block.
• Wolfram Mathematica: symbolic derivations, uncertainty propagation, and quick validation of estimators.

Problem 1 (Plane-fit estimator and covariance): In a neighborhood, measurements satisfy \( \mathbf{z} = \mathbf{A}\boldsymbol{\theta} + \boldsymbol{\varepsilon} \) with \( \boldsymbol{\varepsilon}\sim\mathcal{N}(\mathbf{0}, \sigma^2\mathbf{I}) \). (a) Derive the least-squares estimate \( \hat{\boldsymbol{\theta} } \). (b) Show that \( \operatorname{Cov}(\hat{\boldsymbol{\theta} })=\sigma^2(\mathbf{A}^\top\mathbf{A})^{-1} \).

Solution:

(a) The LS problem minimizes \( J(\boldsymbol{\theta})=\|\mathbf{z}-\mathbf{A}\boldsymbol{\theta}\|_2^2 \). Setting the gradient to zero: \( \nabla J = -2\mathbf{A}^\top(\mathbf{z}-\mathbf{A}\boldsymbol{\theta})=\mathbf{0} \), yielding normal equations \( \mathbf{A}^\top\mathbf{A}\hat{\boldsymbol{\theta} }=\mathbf{A}^\top\mathbf{z} \). If \( \mathbf{A}^\top\mathbf{A} \) is invertible, \( \hat{\boldsymbol{\theta} }=(\mathbf{A}^\top\mathbf{A})^{-1}\mathbf{A}^\top\mathbf{z} \).

(b) Substitute the model: \( \hat{\boldsymbol{\theta} } = (\mathbf{A}^\top\mathbf{A})^{-1}\mathbf{A}^\top(\mathbf{A}\boldsymbol{\theta}+\boldsymbol{\varepsilon}) = \boldsymbol{\theta} + (\mathbf{A}^\top\mathbf{A})^{-1}\mathbf{A}^\top\boldsymbol{\varepsilon} \). Therefore:

\[ \operatorname{Cov}(\hat{\boldsymbol{\theta} }) = (\mathbf{A}^\top\mathbf{A})^{-1}\mathbf{A}^\top \operatorname{Cov}(\boldsymbol{\varepsilon}) \mathbf{A}(\mathbf{A}^\top\mathbf{A})^{-1} = (\mathbf{A}^\top\mathbf{A})^{-1}\mathbf{A}^\top (\sigma^2\mathbf{I}) \mathbf{A}(\mathbf{A}^\top\mathbf{A})^{-1} = \sigma^2(\mathbf{A}^\top\mathbf{A})^{-1} \]

Problem 2 (Negative-log cost additivity): Suppose safety decomposes into independent factors \( p = \prod_{k=1}^{K} p^{(k)} \), with all \( p^{(k)} \in (0,1] \). Show that defining \( c=-\log p \) yields \( c=\sum_k c^{(k)} \) where \( c^{(k)}=-\log p^{(k)} \).

Solution:

\[ c = -\log\!\left(\prod_{k=1}^{K} p^{(k)}\right) = -\sum_{k=1}^{K}\log(p^{(k)}) = \sum_{k=1}^{K} \left(-\log p^{(k)}\right) \]

Problem 3 (Chance constraint for slope): Let the slope estimate be uncertain: \( \alpha \sim \mathcal{N}(\mu_\alpha, \sigma_\alpha^2) \). Derive an equivalent deterministic constraint ensuring \( \Pr(\alpha \le \alpha_{\max}) \ge 1-\delta \).

Solution: Since \( \Pr(\alpha \le \alpha_{\max}) = \Phi((\alpha_{\max}-\mu_\alpha)/\sigma_\alpha) \), the condition is:

\[ \Phi\!\left(\frac{\alpha_{\max}-\mu_\alpha}{\sigma_\alpha}\right) \ge 1-\delta \;\;\Longleftrightarrow\;\; \frac{\alpha_{\max}-\mu_\alpha}{\sigma_\alpha} \ge z_{1-\delta} \;\;\Longleftrightarrow\;\; \mu_\alpha + z_{1-\delta}\sigma_\alpha \le \alpha_{\max} \]

where \( z_{1-\delta} \) is the \( (1-\delta) \)-quantile of the standard normal. This is a classic risk buffer: add a margin proportional to uncertainty.

Problem 4 (Bayes filtering on terrain classes): Consider a single cell with classes \( c\in\{1,\dots,C\} \). Show that if you store unnormalized log-beliefs \( \ell_t(c)=\log \tilde b_t(c) \), the Bayes update can be implemented as \( \ell_t(c)=\ell_{t-1}(c)+\log p(z_t\mid c) \) plus a normalization constant.

Solution:

Bayes update: \( b_t(c) = \frac{p(z_t\mid c)\,b_{t-1}(c)}{\sum_{c'}p(z_t\mid c')\,b_{t-1}(c')} \). Define unnormalized \( \tilde b_t(c)=p(z_t\mid c)\,b_{t-1}(c) \). Taking logs: \( \log \tilde b_t(c)=\log b_{t-1}(c)+\log p(z_t\mid c) \). Normalization is a subtractive constant \( \log Z_t = \log \sum_{c'}\tilde b_t(c') \) applied after exponentiating or via log-sum-exp.

Problem 5 (Design a conservative traversability rule): Suppose you use three features: slope \( \alpha \), roughness \( \rho \), and step \( \delta \). Give a conservative rule that declares a cell traversable only if each chance constraint holds with confidence \( 1-\delta_0 \).

Solution:

If each feature is modeled as Gaussian, require: \( \Pr(\alpha \le \alpha_{\max}) \ge 1-\delta_0 \), \( \Pr(\rho \le \rho_{\max}) \ge 1-\delta_0 \), \( \Pr(\delta \le \delta_{\max}) \ge 1-\delta_0 \). Using the quantile form from Problem 3, the deterministic constraints are:

\[ \mu_\alpha + z_{1-\delta_0}\sigma_\alpha \le \alpha_{\max},\quad \mu_\rho + z_{1-\delta_0}\sigma_\rho \le \rho_{\max},\quad \mu_\delta + z_{1-\delta_0}\sigma_\delta \le \delta_{\max} \]

By the union bound, \( \Pr(\text{all constraints hold}) \ge 1-3\delta_0 \), so you can set \( \delta_0=\delta/3 \) to achieve overall risk level \( \delta \).

We formalized outdoor traversability as a probabilistic inference problem over terrain geometry and interaction: (i) build an elevation map with uncertainty, (ii) extract geometric features (slope/roughness/step) with confidence, (iii) infer terrain classes and a traversability probability, and (iv) convert the result into an additive cost layer for planners. The next lessons will use these tools to handle long-range drift and environment changes that dominate field deployments.