Snapping Positioning Fixes onto the Routing Graph

A smoothed position fix is still the wrong shape for a router. The wayfinding engine navigates a graph of nodes and edges, but a fix is a free-floating (x, y, level) point that almost never lands exactly on an edge — it hovers a metre off a corridor centreline, or between two parallel hallways, or in the dead centre of an atrium the graph does not model. This guide, part of the Indoor Positioning & Fingerprinting reference, is about the binding step: taking that noisy point and resolving it to a specific location on the network — an edge, a position along it, and the correct floor — so the router always has a legal start node and the blue dot rides the corridor instead of floating through walls.

The Problem: A Free-Floating Fix the Router Cannot Use #

The output of your positioning layer — whether a raw radio estimate or a particle-filtered pose — is a point in continuous space. The router works in a discrete graph. Three things go wrong when you connect them naively.

First, the nearest edge by raw distance is often the wrong one. Compute the closest edge to a fix with brute force and, near a junction or between two parallel corridors, the geometrically nearest edge flips frame to frame as the fix jitters. The snapped location oscillates between two hallways that are a metre apart, and the router keeps recomputing a route that reverses every second.

Second, the closest edge can be on the wrong floor. In a building with a stairwell or an atrium void, an edge one floor up may be Euclideanly closer to a fix than the correct edge on the user’s own level, because the 2D projection ignores Z. Snap without a level guard and you teleport the user up a storey.

Third, not every fix belongs on the graph at all. A user standing in the middle of a large open atrium, or a fix thrown far out by a bad radio sample, has no meaningful nearest edge. Force it onto one anyway and you assert a position the user is not at; the honest answer is to reject the fix as off-graph and let the router hold the last good location.

Snapping done right solves all three: a spatial index to find candidate edges cheaply, a perpendicular projection with a level guard to pick the right one, hysteresis to stop the flip-flop across frames, and a distance ceiling to reject fixes that belong nowhere.

Prerequisites & Dependencies #

The binding step sits between the positioning layer and the router, so it assumes both ends already exist:

• A routing graph with geometry. A networkx graph whose edges carry a shapely LineString geometry and a level attribute, built from your parsed floor plan. Each edge is a walkable segment; nodes are junctions, doors, and level-transition points. This is the same network the wayfinding engine traverses.
• Filtered position fixes. A stream of (x, y, level) points, ideally already smoothed by the particle filter so the snapper is not fighting raw radio jitter as well as the projection geometry.
• A consistent metric frame. Both the graph and the fixes must live in the building’s local Cartesian system — metres, shared origin — so a distance in graph space equals a distance in fix space. The level convention (integer storeys, mezzanines as half-levels) must match on both sides.
• Libraries. shapely (2.x) for projection and distance, rtree (or shapely’s built-in STR-tree) for the spatial index, networkx for the graph. numpy for the small vector maths.

How It Works: Index, Project, Stabilise #

Snapping a fix is three operations, and skipping any one of them reintroduces one of the three failure modes above.

1. Index and query. An R-tree over every edge’s bounding box turns “which edges are near this fix?” from an O(E) scan into an O(log E) lookup that returns a short candidate list. Only those candidates get the expensive geometric test.
2. Project and guard. For each candidate edge on the correct level, compute the perpendicular projection of the fix onto the edge’s LineString and its distance. The winner is the nearest projection among edges that pass the level guard — never the nearest edge overall.
3. Stabilise across frames. Compare the new best edge to the one the previous frame snapped to. Only switch edges when the new candidate is closer by more than a hysteresis margin; otherwise stay on the current edge. This is the lightweight form of online map-matching — the same idea a hidden-Markov map-matcher formalises as transition probabilities, reduced to a stickiness rule that kills the parallel-corridor flip-flop.

Step-by-Step Implementation #

Three functions build the snapper: candidate lookup, projection with the level guard, and cross-frame stabilisation. Edges are networkx edges keyed by (u, v) with a geom LineString and a level int.

Step 1: Find candidate edges with an R-tree #

Testing a fix against every edge is wasteful and, on a large multi-floor building, too slow for a real-time stream. Build an R-tree over edge bounding boxes once, then query a small box around each fix to get only the handful of edges worth projecting onto.

import logging
from typing import Any

from rtree import index
from shapely.geometry import LineString

logger = logging.getLogger(__name__)


def build_edge_index(edges: dict[tuple[Any, Any], dict]) -> tuple[index.Index, list]:
    """Build an R-tree over edge bounding boxes; returns the index and an id->edge lookup."""
    idx = index.Index()
    lookup: list = []
    for i, (key, data) in enumerate(edges.items()):
        geom: LineString = data["geom"]
        idx.insert(i, geom.bounds)
        lookup.append((key, data))
    logger.info("indexed %d edges", len(lookup))
    return idx, lookup


def candidates(fix: tuple[float, float], idx: index.Index, lookup: list,
               radius: float = 6.0) -> list:
    """Return edges whose bounding box lies within `radius` metres of the fix."""
    x, y = fix
    hits = list(idx.intersection((x - radius, y - radius, x + radius, y + radius)))
    if not hits:
        logger.debug("no candidate edges within %.1fm of (%.2f, %.2f)", radius, x, y)
    return [lookup[i] for i in hits]

Step 2: Project onto the best edge with a level guard #

For each candidate on the fix’s own level, project the fix onto the edge’s LineString with shapely’s project/interpolate pair, measure the perpendicular distance, and keep the nearest. The level guard is not optional — it is checked before distance so a closer edge on the wrong floor can never win.

import logging
from dataclasses import dataclass
from typing import Any, Optional

from shapely.geometry import Point

logger = logging.getLogger(__name__)


@dataclass
class Snap:
    edge_key: tuple[Any, Any]
    point: tuple[float, float]
    offset: float          # distance along the edge from its start, metres
    distance: float        # perpendicular distance from fix to edge, metres


def project_to_best(fix: tuple[float, float], level: int, cands: list,
                    max_snap: float = 4.0) -> Optional[Snap]:
    """Project a fix onto the nearest candidate edge on the correct level, or None if off-graph."""
    p = Point(fix)
    best: Optional[Snap] = None
    for key, data in cands:
        if data["level"] != level:          # level guard: never snap across floors
            continue
        geom = data["geom"]
        offset = geom.project(p)
        proj = geom.interpolate(offset)
        d = p.distance(proj)
        if best is None or d < best.distance:
            best = Snap(key, (proj.x, proj.y), offset, d)
    if best is None or best.distance > max_snap:
        logger.info("fix (%.2f, %.2f) rejected as off-graph", fix[0], fix[1])
        return None
    return best

Step 3: Stabilise across frames with hysteresis #

A per-frame nearest-edge decision flip-flops between parallel corridors. Hysteresis makes the snap sticky: keep the previous edge unless a different one is closer by more than a margin. This is online map-matching in its lightest form — enough to stop oscillation without the machinery of a full hidden-Markov matcher.

import logging
from typing import Any, Optional

logger = logging.getLogger(__name__)


def stabilise(new: Snap, prev_edge: Optional[tuple[Any, Any]],
              prev_distance: float, margin: float = 0.75) -> Snap:
    """Keep the previous edge unless the new one is closer by more than `margin` metres."""
    if prev_edge is None or new.edge_key == prev_edge:
        return new
    try:
        improvement = prev_distance - new.distance
    except TypeError as exc:
        logger.error("bad distance comparison: %s", exc)
        raise
    if improvement < margin:
        logger.debug("held edge %s (improvement %.2fm < margin)", prev_edge, improvement)
        # Stay on the prior edge; the caller re-projects the fix onto it.
        return Snap(prev_edge, new.point, new.offset, new.distance)
    logger.info("switched %s -> %s (closer by %.2fm)", prev_edge, new.edge_key, improvement)
    return new

Edge Cases & Gotchas #

The tension is stability versus responsiveness: hysteresis and the level guard buy stability, but too much stickiness makes the snapper miss real turns. Tune the margin against a surveyed walk.

Validation Output #

Validate two invariants on every snap: the result lies on an edge (perpendicular distance within tolerance) and it is on the fix’s own level. Replay a logged trajectory and assert both:

snap = project_to_best((12.4, 8.1), level=2, cands=cands, max_snap=4.0)
assert snap is not None, "fix should snap on a mapped corridor"
assert snap.distance <= 4.0, f"snap distance {snap.distance:.2f}m exceeds max_snap"

# The snapped point must lie on the chosen edge's geometry (within float tolerance).
edge_geom = graph_edges[snap.edge_key]["geom"]
assert edge_geom.distance(Point(snap.point)) < 1e-6, "snapped point is not on the edge"

# And the chosen edge must be on the requested level.
assert graph_edges[snap.edge_key]["level"] == 2, "snapped across a floor boundary"
print(f"snapped to {snap.edge_key} at offset {snap.offset:.2f}m, {snap.distance:.2f}m off")

A fix that returns None is a passing result when the point is genuinely off-graph — the correct behaviour is to hold the previous location, not to fabricate a snap.

Performance & Scale Notes #

• Build the index once, query it forever. R-tree construction is O(E log E) and happens at map load; each per-fix query is O(log E + k) for k candidates. On a large multi-floor building this is the difference between a sub-millisecond snap and a linear scan that misses the real-time budget.
• Candidate radius is the throughput knob. A radius too small returns no candidates near junctions and forces spurious rejections; too large returns dozens of edges to project onto. Size it to your fix noise — roughly two to three times the positioning standard deviation.
• Projection is cheap; do less of it. shapely’s project/interpolate is fast, but you only ever call it on the R-tree candidate shortlist, never the full edge set. That is the whole point of step 1.
• Per-fix latency, not throughput, is what matters. Positioning runs at a few hertz per user; the snapper must return within a frame so guidance never stalls. Keep the hot path free of graph mutation and re-indexing.
• Level-aware indexing scales further. For very large campuses, maintain one R-tree per level so the query never even considers other floors — the level guard then becomes a free consequence of which index you query, echoing the level-mapping and z-axis logic the graph itself uses.

Frequently Asked Questions #

Related #

• Snapping Noisy Positions to the Indoor Routing Graph — the compact, copy-ready snapper with a full parameter table.
• Sensor Fusion & Particle Filters for Indoor Positioning — the source of the smoothed fixes this step consumes.
• WiFi RSSI Fingerprinting — the upstream estimator whose fixes ultimately reach the graph.
• Level Mapping & Z-Axis Logic — the floor convention the level guard depends on.
• Fallback Routing Architectures — what the router does when a fix is rejected and no clean start location exists.

This page is part of the Indoor Positioning & Fingerprinting reference.