Semantic Mode Evidence for Hybrid Smoothing

In HybridInference the prior on a motion mode is a number typed into a DecisionTreeFactor. A robot usually has more to go on than that: operator notes, map labels and log lines that say something about the ground it is driving over. This notebook turns such text into a unary DecisionTreeFactor on each step’s mode and lets the hybrid factor graph weigh it against the geometry.

The setup:

A wheeled robot drives 40 steps with wheel odometry and a noisy GPS fix every 4 steps.
Each odometry step has a binary mode $m_k \in \{\text{normal}, \text{slippery}\}$ . On slippery ground the wheels spin, so the robot moves less than odometry reports.
Each step also comes with a one-line note, such as “Sprinklers ran on the lawn crossing at dawn.”

Between GPS fixes the geometry can tell that the robot fell short, but not on which steps. The notes can say which. We compare four sources for the per-step factor $P(m_k)$ :

source	$P(m_k = \text{slippery})$
geometry	a fixed prior of 0.25 for every step
keywords	0.85 if the note matches a list of slippery words, else 0.2
language model	the probability a language model gives when asked whether the note describes slippery ground
oracle	0.9 or 0.1 from the true mode (an upper bound)

The language-model probabilities come from Jev, TypeSafe’s calibrated decision model, following Frank Dellaert’s use of it as a zero-shot source of factors for discrete Bayes nets. They are recorded below, so the notebook runs without an API key; set LIVE = True to query the model again.

import re

import numpy as np
import plotly.graph_objects as go
from plotly.subplots import make_subplots

from gtsam import (
    DCSAM,
    BetweenFactorPose2,
    DecisionTreeFactor,
    DiscreteValues,
    HybridNonlinearFactor,
    HybridNonlinearFactorGraph,
    HybridValues,
    LevenbergMarquardtOptimizer,
    NonlinearFactorGraph,
    Pose2,
    PriorFactorPose2,
    Values,
    VectorValues,
    noiseModel,
)
from gtsam.symbol_shorthand import M, X

Notes¶

Five kinds of note. Two of them are hard for keywords on purpose: slippery ground described without any slippery word, and ordinary ground described with one (“slip ring”, “dry ice”). The last slippery kind carries no information at all, so no reader can find those steps from the text.

NOTES = {
    'slip_explicit': [
        'Floor in aisle 3 was mopped ten minutes ago and is still wet.',
        'Forklift oil leak near dock door 2, cones placed around the puddle.',
        'Light rain started, the outdoor path to building C is wet.',
        'Ice patch reported on the ramp by the loading bay.',
        'Spilled cleaning fluid across the corridor outside the lab.',
        'Mud tracked in from the construction site covers the entrance.',
    ],
    'slip_implicit': [
        'Sprinklers ran on the lawn crossing at dawn.',
        'Cleaning crew just finished the lobby, yellow signs still out.',
        'A pallet of olive jars broke open in aisle 4 an hour ago.',
        'Route goes through the walk-in freezer doorway.',
        'Path crosses the pebble garden by the cafeteria.',
        'Heavy dew on the grass shortcut this morning.',
    ],
    'slip_silent': [
        'Routine patrol, nothing to report.',
        'Passing the break room.',
        'Continuing to waypoint 7.',
        'Battery at 64 percent, on schedule.',
        'No obstacles detected ahead.',
        'Door to room 214 is closed.',
    ],
    'norm_plain': [
        'Carpeted corridor B, quiet.',
        'Concrete floor in warehouse zone 2.',
        'Hallway near reception, lights on.',
        'Passing the printer room.',
        'Rubber mat at the office entrance.',
        'Asphalt path between buildings A and B, dry and sunny.',
    ],
    'norm_trap': [
        'Maintenance replaced the lidar slip ring yesterday.',
        'Passing the dry ice storage cabinet, door closed.',
        'Water cooler in this hallway was refilled.',
        'Rain is forecast for tomorrow afternoon.',
        'Sand-colored carpet runner in the lobby.',
        'Grease trap inspection scheduled for next week.',
    ],
}

# P(slippery) returned by jev-1.13.0 for each note with the LIVE request below, recorded 2026-09-29.
RECORDED = {
    'Floor in aisle 3 was mopped ten minutes ago and is still wet.': 1.0,
    'Forklift oil leak near dock door 2, cones placed around the puddle.': 0.97,
    'Light rain started, the outdoor path to building C is wet.': 0.99,
    'Ice patch reported on the ramp by the loading bay.': 1.0,
    'Spilled cleaning fluid across the corridor outside the lab.': 1.0,
    'Mud tracked in from the construction site covers the entrance.': 1.0,
    'Sprinklers ran on the lawn crossing at dawn.': 0.97,
    'Cleaning crew just finished the lobby, yellow signs still out.': 0.99,
    'A pallet of olive jars broke open in aisle 4 an hour ago.': 0.91,
    'Route goes through the walk-in freezer doorway.': 0.58,
    'Path crosses the pebble garden by the cafeteria.': 0.97,
    'Heavy dew on the grass shortcut this morning.': 1.0,
    'Routine patrol, nothing to report.': 0.0,
    'Passing the break room.': 0.0,
    'Continuing to waypoint 7.': 0.0,
    'Battery at 64 percent, on schedule.': 0.0,
    'No obstacles detected ahead.': 0.0,
    'Door to room 214 is closed.': 0.0,
    'Carpeted corridor B, quiet.': 0.05,
    'Concrete floor in warehouse zone 2.': 0.0,
    'Hallway near reception, lights on.': 0.0,
    'Passing the printer room.': 0.0,
    'Rubber mat at the office entrance.': 0.11,
    'Asphalt path between buildings A and B, dry and sunny.': 0.0,
    'Maintenance replaced the lidar slip ring yesterday.': 0.0,
    'Passing the dry ice storage cabinet, door closed.': 0.01,
    'Water cooler in this hallway was refilled.': 0.09,
    'Rain is forecast for tomorrow afternoon.': 0.04,
    'Sand-colored carpet runner in the lobby.': 0.12,
    'Grease trap inspection scheduled for next week.': 0.1,
}

LIVE = False  # set to True to query Jev (needs `pip install typesafe-sdk` and TYPESAFE_API_KEY)

if LIVE:
    from typesafe_sdk import Choice, TypeSafeClient

    notes = [n for kind in NOTES.values() for n in kind]
    questions = {
        f"q{i}": Choice(
            instructions=f'Note for this path segment: "{n}" '
            "Which best describes the ground under the robot on this segment?",
            criteria={
                "normal": "Traction is normal, so wheel odometry is reliable.",
                "slippery": "The ground is slippery or loose, so the wheels may slip.",
            },
        )
        for i, n in enumerate(notes)
    }
    state = {
        "robot": "A wheeled delivery robot that estimates its motion from wheel odometry.",
        "task": "For each path segment, judge the ground traction from the operator or log note "
        "attached to that segment. Judge each note on its own.",
    }
    with TypeSafeClient() as client:
        answers = client.system_one(model="jev-1.13.0", state=state, questions=questions)
    RECORDED = {n: answers.answers[f"q{i}"].probabilities["slippery"] for i, n in enumerate(notes)}

Routes¶

Slippery zones of 2 to 4 steps start with probability 0.12 at each step, so about 30% of the steps are slippery. On a slippery step the robot covers only 30 to 70% of the commanded 1 m.

N_STEPS, GPS_EVERY = 40, 4
TIGHT = noiseModel.Diagonal.Sigmas([0.05, 0.05, 0.02])  # normal wheel odometry
LOOSE = noiseModel.Diagonal.Sigmas([0.6, 0.6, 0.15])  # slipping wheels
GPS = noiseModel.Diagonal.Sigmas([0.15, 0.15, 10.0])  # position only


def make_route(seed):
    rng = np.random.default_rng(seed)
    modes, kinds = [], []
    while len(modes) < N_STEPS:
        if rng.random() < 0.12:
            kind = rng.choice(["slip_explicit", "slip_implicit", "slip_silent"], p=[0.4, 0.4, 0.2])
            for _ in range(min(int(rng.integers(2, 5)), N_STEPS - len(modes))):
                modes.append(1)
                kinds.append(str(kind))
        else:
            modes.append(0)
            kinds.append("norm_trap" if rng.random() < 0.3 else "norm_plain")
    notes = [str(rng.choice(NOTES[k])) for k in kinds]
    truth, odometry, gps = [Pose2()], [], {}
    for k in range(1, N_STEPS + 1):
        command = Pose2(1.0, 0.0, float(rng.choice([0.0, 0.15, -0.15])))
        if modes[k - 1]:
            actual = Pose2(command.x() * rng.uniform(0.3, 0.7), rng.normal(0, 0.1),
                           command.theta() + rng.normal(0, 0.05))
        else:
            actual = Pose2(command.x() + rng.normal(0, 0.03), rng.normal(0, 0.03),
                           command.theta() + rng.normal(0, 0.01))
        truth.append(truth[-1].compose(actual))
        odometry.append(command)
        if k % GPS_EVERY == 0:
            p = truth[k]
            gps[k] = Pose2(p.x() + rng.normal(0, 0.15), p.y() + rng.normal(0, 0.15), 0.0)
    return dict(modes=modes, kinds=kinds, notes=notes, truth=truth, odometry=odometry, gps=gps)


routes = [make_route(seed) for seed in range(40)]
route = routes[0]
for k in range(12):
    print(f"step {k + 1:2d}  {'slippery' if route['modes'][k] else 'normal  '}  {route['notes'][k]}")

step  1  normal    Grease trap inspection scheduled for next week.
step  2  slippery  Spilled cleaning fluid across the corridor outside the lab.
step  3  slippery  Spilled cleaning fluid across the corridor outside the lab.
step  4  normal    Concrete floor in warehouse zone 2.
step  5  normal    Rubber mat at the office entrance.
step  6  normal    Carpeted corridor B, quiet.
step  7  slippery  Battery at 64 percent, on schedule.
step  8  slippery  Continuing to waypoint 7.
step  9  slippery  Door to room 214 is closed.
step 10  slippery  Passing the break room.
step 11  slippery  Heavy dew on the grass shortcut this morning.
step 12  slippery  Sprinklers ran on the lawn crossing at dawn.

Evidence¶

Language models answer confidently, often with probabilities of exactly 0 or 1. A factor value of 0 makes a mode impossible, so the geometry could never overrule it. We therefore clip the probabilities to $[\epsilon, 1-\epsilon]$ with a floor $\epsilon$ that is revisited at the end.

KEYWORDS = re.compile(
    r"\b(slip\w*|slick|wet|water|ice|icy|snow\w*|oil\w*|grease|greasy|mud\w*|mopped|mop|"
    r"spill\w*|puddle\w*|gravel|sand\w*|rain\w*|frost\w*|polish\w*|wax\w*|leak\w*|flood\w*|"
    r"damp|condensation|lubric\w*|slush)\b",
    re.IGNORECASE,
)


def evidence(route, source, floor=0.05):
    if source == "geometry":
        return [0.25] * N_STEPS
    if source == "keywords":
        return [0.85 if KEYWORDS.search(n) else 0.2 for n in route["notes"]]
    if source == "language model":
        return [min(max(RECORDED[n], floor), 1 - floor) for n in route["notes"]]
    if source == "oracle":
        return [0.9 if m else 0.1 for m in route["modes"]]
    raise ValueError(source)

Each odometry step is a HybridNonlinearFactor with a tight and a loose component, as in HybridInference. Each component carries its noise model’s negLogConstant(), the negative log of the Gaussian’s normalization constant. Without it the loose component would explain any motion at no extra cost. The evidence is a DecisionTreeFactor on the same mode, and the GPS fixes are position priors.

DCSAM alternates between the continuous and the discrete estimate, so it finds a local optimum, and its first discrete step is taken at the initial poses. We therefore run it from two starts: the poses that the evidence’s most likely modes give, and the poses with every step loose, which lets the GPS show where the robot fell short. We keep the solution with the lower graph error, i.e. the higher posterior.

def build_graph(route, p_slippery):
    graph = HybridNonlinearFactorGraph()
    graph.push_back(PriorFactorPose2(X(0), Pose2(), noiseModel.Isotropic.Sigma(3, 0.01)))
    for k in range(1, N_STEPS + 1):
        mode, z = (M(k), 2), route["odometry"][k - 1]
        graph.push_back(HybridNonlinearFactor(
            mode, [(BetweenFactorPose2(X(k - 1), X(k), z, n), n.negLogConstant()) for n in (TIGHT, LOOSE)]))
        p = p_slippery[k - 1]
        graph.push_back(DecisionTreeFactor(mode, f"{1 - p} {p}"))
        if k in route["gps"]:
            graph.push_back(PriorFactorPose2(X(k), route["gps"][k], GPS))
    return graph


def poses_for(route, slippery):
    """Optimized poses with each step's mode fixed, used as a starting point for DCSAM."""
    graph, values, pose = NonlinearFactorGraph(), Values(), Pose2()
    graph.push_back(PriorFactorPose2(X(0), Pose2(), noiseModel.Isotropic.Sigma(3, 0.01)))
    values.insert(X(0), pose)
    for k in range(1, N_STEPS + 1):
        z = route["odometry"][k - 1]
        graph.push_back(BetweenFactorPose2(X(k - 1), X(k), z, LOOSE if slippery[k - 1] else TIGHT))
        if k in route["gps"]:
            graph.push_back(PriorFactorPose2(X(k), route["gps"][k], GPS))
        pose = pose.compose(z)
        values.insert(X(k), pose)
    return LevenbergMarquardtOptimizer(graph, values).optimize()


def solve(route, p_slippery, iterations=5):
    graph = build_graph(route, p_slippery)
    best = None
    for start in ([p > 0.5 for p in p_slippery], [True] * N_STEPS):
        dcsam = DCSAM()
        dcsam.update(graph, HybridValues(VectorValues(), DiscreteValues(), poses_for(route, start)))
        for _ in range(iterations - 1):
            dcsam.update()
        result = dcsam.calculateEstimate()
        if best is None or graph.error(result) < graph.error(best):
            best = result
    modes = [int(best.atDiscrete(M(k))) for k in range(1, N_STEPS + 1)]
    poses = [best.nonlinear().atPose2(X(k)) for k in range(N_STEPS + 1)]
    return modes, poses

Results over 40 routes¶

SOURCES = ["geometry", "keywords", "language model", "oracle"]
results = {}
for source in SOURCES:
    rows = []
    for route in routes:
        modes, poses = solve(route, evidence(route, source))
        for k in range(N_STEPS):
            error = np.hypot(poses[k + 1].x() - route["truth"][k + 1].x(),
                             poses[k + 1].y() - route["truth"][k + 1].y())
            rows.append((route["kinds"][k], route["modes"][k], modes[k], error))
    results[source] = rows

print(f"{'source':15} {'mode accuracy':>14} {'position RMSE [m]':>18}")
for source, rows in results.items():
    accuracy = np.mean([truth == estimate for _, truth, estimate, _ in rows])
    rmse = np.sqrt(np.mean([error**2 for *_, error in rows]))
    print(f"{source:15} {accuracy:14.3f} {rmse:18.3f}")

print(f"\n{'mode accuracy by note':22}" + "".join(f"{s:>16}" for s in SOURCES))
for kind in NOTES:
    line = f"{kind:22}"
    for source in SOURCES:
        rows = [r for r in results[source] if r[0] == kind]
        line += f"{np.mean([truth == estimate for _, truth, estimate, _ in rows]):16.3f}"
    print(line)

source           mode accuracy  position RMSE [m]
geometry                 0.824              0.305
keywords                 0.765              0.399
language model           0.937              0.295
oracle                   0.999              0.162

mode accuracy by note         geometry        keywords  language model          oracle
slip_explicit                    0.689           0.985           1.000           1.000
slip_implicit                    0.643           0.298           0.994           0.994
slip_silent                      0.692           0.144           0.058           1.000
norm_plain                       0.880           0.975           0.999           1.000
norm_trap                        0.903           0.564           0.997           1.000

Geometry alone finds about two thirds of the slippery steps of every kind from the shortfall between GPS fixes, but often blames the wrong steps within an interval. Keywords help on plainly worded notes and fail on the other two kinds by construction. The language model reads the implicit notes and ignores the traps, so it gets nearly every step with an informative note right. On the silent notes it says “normal” with confidence and the solver follows it, where geometry alone finds most of those slips. That is why its position error is only a little below that of geometry, while the oracle shows how much better the trajectory gets when every mode is right.

route = routes[1]
steps = np.arange(1, N_STEPS + 1)
fig = go.Figure()
for source in ["geometry", "keywords", "language model"]:
    _, poses = solve(route, evidence(route, source))
    error = [np.hypot(poses[k].x() - route["truth"][k].x(), poses[k].y() - route["truth"][k].y())
             for k in steps]
    fig.add_trace(go.Scatter(x=steps, y=error, mode="lines+markers", name=source,
                             line=dict(color={"geometry": "#636efa", "keywords": "#ef553b",
                                              "language model": "#00cc96"}[source])))
for k, slippery in enumerate(route["modes"]):
    if slippery:
        fig.add_vrect(x0=k + 0.5, x1=k + 1.5, fillcolor="lightgray", opacity=0.5, line_width=0,
                      layer="below")
for k in route["gps"]:
    fig.add_vline(x=k, line_dash="dot", line_color="green", line_width=1)
fig.update_layout(xaxis_title="step (gray: slippery, dotted: GPS fix)",
                  yaxis_title="position error [m]", height=380)
fig.show()

One route up close¶

The trajectories show where the error comes from. Dead reckoning trusts every odometry step, so it overshoots each slippery stretch. The graph pulls the trajectory back toward the GPS fixes, and the mode evidence decides which steps absorb the correction.

SHOW = ["geometry", "language model"]
COLORS = {"geometry": "#636efa", "keywords": "#ef553b", "language model": "#00cc96"}
estimates = {source: solve(route, evidence(route, source)) for source in SHOW}

dead_reckoning = [Pose2()]
for z in route["odometry"]:
    dead_reckoning.append(dead_reckoning[-1].compose(z))

fig = go.Figure()
for name, poses, style in [("ground truth", route["truth"], dict(color="black", width=3)),
                           ("dead reckoning", dead_reckoning, dict(color="gray", dash="dot"))]:
    fig.add_trace(go.Scatter(x=[p.x() for p in poses], y=[p.y() for p in poses], mode="lines",
                             name=name, line=style))
for source in SHOW:
    poses = estimates[source][1]
    fig.add_trace(go.Scatter(x=[p.x() for p in poses], y=[p.y() for p in poses], mode="lines+markers",
                             marker=dict(size=4), name=source, line=dict(color=COLORS[source])))
slips = [k + 1 for k, m in enumerate(route["modes"]) if m]
fig.add_trace(go.Scatter(x=[route["truth"][k].x() for k in slips], y=[route["truth"][k].y() for k in slips],
                         mode="markers", name="slippery step (truth)",
                         marker=dict(symbol="x", size=9, color="firebrick")))
fig.add_trace(go.Scatter(x=[p.x() for p in route["gps"].values()], y=[p.y() for p in route["gps"].values()],
                         mode="markers", name="GPS fix", marker=dict(symbol="square", size=8, color="green")))
fig.update_layout(xaxis_title="x [m]", yaxis_title="y [m]", yaxis_scaleanchor="x", height=420)
fig.show()

The mode probabilities involved are of two kinds. The evidence is the unary factor $P(m_k)$ that each source puts on a step. The posterior combines it with the geometry: given the estimated poses, the mode of step $k$ is coupled only to its own odometry factor and evidence, so discretePosterior on a graph with just those two factors gives $P(m_k \mid x_{k-1}, x_k)$ .

def mode_posterior(route, p_slippery, poses):
    values = Values()
    for k, pose in enumerate(poses):
        values.insert(X(k), pose)
    posterior = []
    for k in range(1, N_STEPS + 1):
        mode, z = (M(k), 2), route["odometry"][k - 1]
        step = HybridNonlinearFactorGraph()
        step.push_back(HybridNonlinearFactor(
            mode, [(BetweenFactorPose2(X(k - 1), X(k), z, n), n.negLogConstant()) for n in (TIGHT, LOOSE)]))
        p = p_slippery[k - 1]
        step.push_back(DecisionTreeFactor(mode, f"{1 - p} {p}"))
        slippery = DiscreteValues()
        slippery[M(k)] = 1
        posterior.append(step.discretePosterior(values)(slippery))
    return posterior


fig = make_subplots(rows=2, cols=1, shared_xaxes=True, vertical_spacing=0.08,
                    subplot_titles=["evidence P(slippery)", "posterior P(slippery | estimated poses)"])
for source in SHOW:
    p = evidence(route, source)
    color = COLORS[source]
    fig.add_trace(go.Scatter(x=steps, y=p, mode="lines+markers", name=source, legendgroup=source,
                             line=dict(color=color, shape="hvh")), row=1, col=1)
    fig.add_trace(go.Scatter(x=steps, y=mode_posterior(route, p, estimates[source][1]),
                             mode="lines+markers", name=source, legendgroup=source, showlegend=False,
                             line=dict(color=color, shape="hvh")), row=2, col=1)
for k, slippery in enumerate(route["modes"]):
    if slippery:
        for row in (1, 2):
            fig.add_vrect(x0=k + 0.5, x1=k + 1.5, fillcolor="lightgray", opacity=0.5, line_width=0,
                          layer="below", row=row, col=1)
fig.update_yaxes(range=[-0.05, 1.05])
fig.update_layout(xaxis2_title="step (gray: slippery)", height=520)
fig.show()

How hard to floor the language model¶

A higher floor keeps the text evidence from overruling the geometry when the note is uninformative. Here it helps little. At a floor of 0.4, in about half of the routes where silent slips are missed, marking them slippery would lower the graph error, so the misses come partly from DCSAM settling in a local optimum and not only from the evidence.

print(f"{'floor':>6} {'mode accuracy':>14} {'silent-slip accuracy':>21} {'position RMSE [m]':>18}")
for floor in [0.05, 0.1, 0.2, 0.3, 0.4]:
    correct, silent, squared = [], [], []
    for route in routes:
        modes, poses = solve(route, evidence(route, "language model", floor))
        for k in range(N_STEPS):
            correct.append(modes[k] == route["modes"][k])
            if route["kinds"][k] == "slip_silent":
                silent.append(modes[k] == route["modes"][k])
            squared.append((poses[k + 1].x() - route["truth"][k + 1].x())**2
                           + (poses[k + 1].y() - route["truth"][k + 1].y())**2)
    print(f"{floor:6.2f} {np.mean(correct):14.3f} {np.mean(silent):21.3f} {np.sqrt(np.mean(squared)):18.3f}")

 floor  mode accuracy  silent-slip accuracy  position RMSE [m]
  0.05          0.937                 0.058              0.295

  0.10          0.937                 0.058              0.295

  0.20          0.937                 0.058              0.295

  0.30          0.937                 0.106              0.290

  0.40          0.934                 0.125              0.284

Remarks¶

The notes here are synthetic. What carries over is the pattern: a text source turned into a floored unary factor per discrete mode, fused with geometry by the hybrid graph.
The wording of the request matters on borderline notes. With a shorter task description (“judge the ground traction from the note attached to it”), “Grease trap inspection scheduled for next week” came back 0.58 slippery instead of 0.10. Repeating an identical request changed probabilities by at most 0.02.
Ask the language model to compare described options (“normal” or “slippery” for this note). Numbers it would have to estimate, such as noise levels or how often slipping happens, belong in the model or should be learned from data.
DCSAM is a local method, and the start matters as much as the evidence. Started only from the all-loose poses, the language model reaches a mode accuracy of 0.86 instead of 0.94. Trying more than one start and comparing graph errors is cheap.