The topological map and grounding detections

Intelligent Systems for Robotics · Week 5

Syllabus · Home

Week 4 gave the robot a vocabulary, and a reasoner that draws conclusions from what you wrote in it. This week the knowledge graph starts to drive behaviour. It gains the map the robot navigates, and the compositions of skills that the planner will reason over from week 6. Both live in the same graph. It also gains a path from detections to facts, so that the room types you defined in week 4 come out of what the robot sees from places you chose.

Objectives

No global metric map

In week 1 you drove a closed square and asked whether you believed the pose the robot reported. On the bare motor-test floor, with arc turns, the square finished 0.174 m from its start after 4 m of travel. Wheel odometry with an inertial yaw is the only pose estimate isfr.robot offers, and the course gives you no ground-truth channel. A route across the apartment is several times longer than that square, and it passes through doorways a few tenths of a metre wider than the base. If drift grows in proportion to the distance driven, a whole route accumulates more of it than a doorway leaves on either side of the base.

The map you build this week therefore holds no pose in a world frame. Every metric quantity in it is relative to something the robot can perceive from where it stands, such as a doorway or a piece of furniture. The robot never needs to know where it is in the apartment. It needs to know where the next recognisable thing is relative to itself. Each time it arrives at a place and perceives that place's anchor, it measures its pose against the anchor afresh, and the odometric error from the edge it has just driven no longer matters. Re-anchoring bounds drift to one edge. That is the only scale at which week 1's evidence says odometry is usable.

The choice has a cost, which the section on route search meets. With no common frame, the search has no straight-line distance to the goal to guide it.

The design has a parallel in how people find their way. You walk to a colleague's office by recognising a door, a corridor junction and a stairwell, and by knowing roughly where the next one lies from where you stand. Warren, Rothman, Schnapp and Ericson (2017) tested whether people's knowledge of navigation space is a Euclidean map or a labelled graph. Participants walked a virtual hedge maze that held two wormholes, which rotated and teleported them. Their shortcuts were biased by the wormholes, with mean constant errors of 37 and 41 degrees where 45 were expected. A labelled-graph model fitted the shortcuts decisively better than a Euclidean map model. The authors conclude that knowledge of navigation space is a labelled graph, whose local metric information is approximate, inconsistent, and not in a common coordinate system.

Read this as a lens, in the way you read Endsley's levels in week 1. The flat claim that people hold no metric map is contested. Peer, Brunec, Newcombe and Epstein (2021) review the evidence and report that map-like and graph-like representations both exist, and that they can operate together or apart. They name grid cells as a possible neural substrate for a Euclidean coordinate system, and they give the evidence that people's spatial memories violate Euclidean rules. The defensible form of the claim is narrower. People navigate well without a globally consistent metric map, and grid cells may still carry a metric code. The robot's map does not depend on either claim. Its reason is the odometry you measured in week 1.

Places, anchors and frames

A place is a node of the map. It is somewhere the robot can recognise and stop, and the base ontology has had the class isfr:Place since week 4. This week every place gains an anchor, given by isfr:anchoredBy. The anchor is what the robot perceives at the place. Two kinds exist.

A gap anchor, isfr:GapAnchor, is an opening in the lidar scan, with an expected width in isfr:width. Doorways and corridor junctions are gap anchors. A doorway reads as a gap of its own width in TIAGo's lidar. In a probe run on 2026-09-11, a 0.8 m doorway read between 0.793 and 0.812 m from 1.0 to 3.0 m away. An object anchor, isfr:ObjectAnchor, is an object the detector names, such as a sink, a fridge or a dining table. It names the ontology class of its object with isfr:anchorClass. The section on grounding explains why the anchor names an ontology class.

the gap, 0.8 m wide TIAGo wall no return in the opening returns

The detector limits the choice. It returns COCO class names, and COCO includes sink, oven, refrigerator, dining table, couch, bed, bottle and cup. It has no door and no worktop. A doorway is therefore anchored by its gap, and a worktop by the sink or the oven on it, with the offset carried by the approach. A room centre is never a place, since nothing at the centre of a room can be recognised.

Every place defines a local frame, and the hints between places are written in those frames. A frame has to meet two conditions. It must be observable from the anchor on arrival, so that the robot can measure its own pose in it. It must also be independent of the direction the robot arrived from. Each edge carries one hint, and the reverse hint is computed from it. If a place's frame turned with the direction of arrival, the hint of every edge leaving that place would depend on which edge the robot came in by, and one hint per edge could not be right for every arrival.

x x x y turns with the arrival: two hints fixed to the opening: one

A gap meets both conditions. Its frame has its origin at the midpoint of the opening and its x axis along the normal to the opening. The gap's authored isfr:joins pair of regions fixes the sign, and the axis points from the first region into the second. Both ends of the opening are visible in the lidar scan, so the frame is observable. The robot knows which region it came from, so it can resolve the sign. The approach of a gap place is the frame's origin, facing along x, so the robot stops in the opening. A gap place belongs to the first region of its pair.

x y first region second region isfr:width

An object meets only the first condition. A detection gives an object's position and not its orientation. An object place's frame therefore has its origin at the approach pose, a stand-off distance from the object, and its x axis along the line of sight to the object on arrival. For an object the robot can reach from one side only, such as a sink against a wall, the line of sight is nearly the same whatever direction the robot came from. For a dining table, which it can approach from any side, the line of sight changes with the arrival, and hints leaving such a place are less accurate. The design tolerates this. Hints are coarse by construction, and once the next anchor is in view the robot steers by what it sees.

sink approach x dining table one approach, one line of sight every approach, its own frame

Here are two places from this week's example, a small flat with a hall and two corners:

:hall_door a isfr:Place ;
    rdfs:label "hall door" ;
    isfr:inRoom :hall ;
    isfr:anchoredBy [ a isfr:GapAnchor ; isfr:width 1.0 ;
                      isfr:joins ( :hall :kitchen_corner ) ] .

:counter_place a isfr:Place ;
    rdfs:label "counter place" ;
    isfr:inRoom :kitchen_corner ;
    isfr:anchoredBy [ a isfr:ObjectAnchor ; isfr:anchorClass isfr:KitchenSink ] .

Regions group places. isfr:Region has the subclasses isfr:Room and isfr:Corridor, and a place belongs to one region through isfr:inRoom, as in week 4. The hall door is a gap from the hall into the kitchen corner, so it belongs to the hall. The counter place belongs to the room its sink stands in.

You author the map this week. The same graph could be built by exploration, with the robot recording anchors and hints as it drives. The course authors it to save time, and building it by exploration is outside its scope.

Hints

An edge joins two places. It is an individual of class isfr:Edge, with isfr:from and isfr:to, because it carries a hint and runtime state and a plain triple can carry neither. The hint, isfr:hint, is the approximate pose of the target place's frame in the source place's frame. It has isfr:x and isfr:y in metres, x ahead and y to the left, and isfr:yaw in radians, anticlockwise. These are the keys robot.pose() returns. A hint such as 2 m ahead, 0.5 m to the left and turned a quarter turn left is enough. It does two jobs. It gives the robot a goal to drive towards until the target's anchor is in view, and it tells the robot which of two similar openings is the intended one.

A hint is an element of SE(2), the group of rigid motions in the plane. Write a hint as $h = (x, y, \theta)$, with $\theta$ the yaw. Let $a$ be the pose of frame $B$ in frame $A$, and $b$ the pose of frame $C$ in frame $B$. The pose of $C$ in $A$ is their composition:

$$a \oplus b = \begin{pmatrix} x_a + x_b \cos\theta_a - y_b \sin\theta_a \\ y_a + x_b \sin\theta_a + y_b \cos\theta_a \\ \theta_a + \theta_b \end{pmatrix}$$

The translation of $b$ is rotated into $A$'s orientation and added to the translation of $a$, and the yaws add. Composition is associative, and it is not commutative. The inverse of a hint is the pose of the source's frame in the target's frame:

$$\ominus h = \begin{pmatrix} -x \cos\theta - y \sin\theta \\ x \sin\theta - y \cos\theta \\ -\theta \end{pmatrix}$$

Composing a hint with its inverse gives the identity, $h \oplus (\ominus h) = (0, 0, 0)$. Expand the first row to check it. isfr/knowledge/topology.py implements the two operations as compose and invert, and wraps each yaw into $(-\pi, \pi]$.

Take an edge from the example. The edge from the arch to the bed place has the hint $(1.38, 0.60, -1.57)$. With $\theta = -1.57$, $\cos\theta \approx 0$ and $\sin\theta \approx -1$, so the inverse is $(0.60, -1.38, 1.57)$. Seen from the bed place, the arch is 0.60 m ahead and 1.38 m to the right, and its frame is turned a quarter turn anticlockwise from the bed place's.

x y arch bed place 1. the hint (1.38, 0.60) x y arch bed place 2. negate (−1.38, −0.60), the arch's axes x y bed place arch 3. rotate by −θ (0.60, −1.38), the bed place's

You author each edge in one direction only. The reverse is derived by inversion, so the two directions cannot disagree. Authoring both would give two hints for one pair of places, each with its own error, and nothing would say which one to trust. edges() raises an error, naming both edges, when a pair of places is joined in both directions.

Composition is also how the robot uses a hint while it drives. Let $p_0$ be the odometric pose at which the robot left the source place, and $p_t$ its odometric pose now. The robot leaves from the source's approach pose, which is the origin of the source place's frame, facing along its x axis. The change $\Delta = (\ominus p_0) \oplus p_t$ is therefore the robot's pose in the source place's frame, and the target, in the robot's own frame, is

$$g = (\ominus \Delta) \oplus h$$

source place target place robot now Δ g h

Only the change in odometry enters, so the origin of the odometry frame cancels and drift accumulates over this one edge. Once the target's anchor is in view, the perceived pose of the anchor replaces $g$. If the anchor does not appear within the hint's distance and a margin, go-to, the learned skill that drives one edge, is designed to end in failure with the reason that the expected landmark was not seen. That is monitoring in week 1's sense, noticing that what the robot expected has stopped being true, at the lowest layer of the stack.

Kuipers (2000) describes the Spatial Semantic Hierarchy in four levels: control, causal, topological and metrical. Its topological level introduces places, paths and regions by abduction from the states and actions of the causal level. Local geometric frames, linked by topological connections, form what Kuipers calls a patchwork map, and they are merged into a single global frame only when enough information and computation are available. Your map is a patchwork map in that sense, with a frame at every place and a hint on every edge. The course never takes the last step of merging the frames.

hall door counter place arch bed place

Route search

A route is a sequence of edges from one place to another. Its length is not fixed, so a route is found by search when it is needed, and it is never written into the graph. The provided skill navigate-to is designed to run the search and then drive the route with go-to, one edge at a time. This week you call the search directly, as route(kg, start, goal), which returns the edges in order, or None when no route exists.

Each edge costs the Euclidean length of its hint:

$$c(e) = \sqrt{x_h^2 + y_h^2}$$

Here $x_h$ and $y_h$ are the translation of the edge's hint. That cost is as coarse as the hint, and the route found is the cheapest under those costs. The search is uniform-cost search. Poole and Mackworth, section 3.5.4, describe it under the name lowest-cost-first search. It keeps a frontier of paths ordered by cost, always expands the cheapest, and stops when the cheapest path on the frontier ends at the goal. It is the simplest method guaranteed to find a minimum-cost path.

A* would usually expand fewer paths. It orders the frontier by the cost so far plus a heuristic estimate $H(n)$ of the cost from node $n$ to the goal. Poole and Mackworth, section 3.6, call a heuristic admissible when it never exceeds the cost of a lowest-cost path from a node to a goal. Their Proposition 3.1, in section 3.6.1, shows that, when a solution exists, A* with an admissible heuristic returns an optimal one if the branching factor is bounded and every arc cost exceeds some positive constant. A finite map whose edges all have positive length meets both conditions. On a metric map the straight-line distance to the goal is the usual admissible heuristic. It needs both positions in one frame, and this map has no common frame. You could compose hints along some path to estimate where the goal lies. That needs a path first, and two paths would give two different estimates, since the hints are approximate and do not agree with one another. Neither estimate is guaranteed to stay below the true cost. The one heuristic known to be admissible here is $H(n) = 0$, and A* with a zero heuristic is uniform-cost search.

The maps in this course have tens of places, so the search is cheap. reachable(kg) returns every ordered pair of places joined by a route, each place with itself included. Week 6's planner never sees a route. Its domain holds navigate-to, with the precondition (reachable ?from ?to), and the reachable facts are computed from the graph when the problem is generated. Route finding stays outside PDDL.

What was written, seen and concluded

The knowledge graph is now one rdflib dataset that holds three named graphs. KnowledgeGraph, in isfr/knowledge/graph.py, wraps it.

asserted() returns the map and belief graphs together, and union() returns all three. Keeping them apart lets you inspect separately what was written, what was seen and what was concluded. As in week 4, a query on asserted() shows what the reasoner had to work with, and the same query on union() shows its answers. The closure takes about 0.1 s for the example's flat and about 0.3 s for the micro-project's map, measured on the instructor's laptop (Apple M4 Max).

Map Belief Inferred close() asserted() union(), all three

When the robot finds an edge impassable, block(edge) writes one triple to the belief graph:

:hatch isfr:blocked true .

The map graph does not change. The derived reverse of an edge shares the authored edge's IRI, so blocking an edge closes it in both directions, and the route search skips it. A blocked edge stays blocked for the rest of the run. Letting it expire would need a timing policy, and nothing in the course teaches one. In the example, the hatch joins the bed place to the counter place directly:

route: bed place -> counter place
after blocking the hatch: bed place -> arch -> hall door -> counter place

The hatch is one edge of 2.0 m. The way round, through the arch and the hall door, is 6.0 m. After block, the same call to route returns the way round.

Grounding

Perception is tooling in this course, and grounding what it returns is a course topic. A detection is a COCO class name, a confidence and a position in the robot's base frame, x ahead and y to the left, in metres. ground(kg, place, detections), in isfr/perception/grounding.py, turns the detections made at a place into individuals in the belief graph. isfr:objectAt puts each individual at the place, with a pose relative to the place's frame. It never receives a pose in a world frame.

The detector names COCO classes, and the graph holds ontology classes. Week 4's annotation isfr:cocoClass joins the two. A detection takes the most general class that carries its COCO name, so a bottle becomes an isfr:Bottle and a sink an isfr:Sink. A detection whose COCO name no class carries is left ungrounded, and ground returns it in a separate list.

The sink from week 4 returns here. The base ontology splits isfr:Sink into isfr:KitchenSink and isfr:BathroomSink, and keeps the COCO name on the parent, because the detector cannot tell the two apart. A sink grounded from its COCO name alone is therefore an isfr:Sink. The kitchen axiom needs an isfr:KitchenSink, so it would never fire, and no kitchen would ever be inferred from a detection.

The anchor class settles it. When you anchor a place on a kitchen sink, you write isfr:anchorClass isfr:KitchenSink. A sink detected at that place is explained by the anchor, and it takes the anchor's class. The rule is that a detection takes the anchor class when that class, or a superclass of it, carries the detection's COCO name, and the detection is the nearest of that name to the robot. At a place anchored on a bathroom sink, the same detection becomes an isfr:BathroomSink, and no kitchen follows. The fact you author is about the fixture, as in week 4, where you asserted which sink was which. The room type is still inferred, and you never write that a room is a kitchen. The example shows the anchor at work at its counter place:

at the counter place the stub sees:
  bottle: 0.80 m ahead, 0.30 m left
  sink: 1.00 m ahead, -0.30 m left
grounded as:
  counter_place-bottle-1: Bottle
  counter_place-sink-1: KitchenSink

Data association is one rule. A detection of the same class, at the same place, within 0.3 m of an existing individual's pose, is that individual. Any other detection is a new individual. The rule is kept to one line because data association is a perception problem, and perception is tooling here. Its cost follows from the map. Poses are relative to the place they were seen from, and two places share no frame in which their poses could be compared, only a hint too coarse to tell one bottle from two. The same bottle seen from two places therefore becomes two individuals. The 0.3 m radius is a proposal. The stub detector has no noise, and YOLO's depth error has not been measured.

place A place B bottle_1 bottle_2 no shared frame to compare them in 0.3 m 0.3 m

This week every detection comes from StubDetector, in isfr/perception/stub.py. It reads the apartment from data/apartment/world.json and reports what the head camera would see from a pose. It sees a bottle, a cup or a wine glass within 1.0 m, and anything else within 4.0 m, inside a field of view 1.0 rad wide, and never through a wall. The 1.0 m follows a probe of YOLO on Webots models. A beer bottle read as bottle with confidence 0.746 from 0.6 m with the head tilted down, and from 1.4 m no bottle was labelled at a confidence of 0.25 or more. The field of view is assumed until the apartment world reports the camera's. The world coordinates the stub reads stay in its input, and none reaches a detection or the graph.

The reasoner works under the open-world assumption, as in week 4. When no sink was seen from any of a room's places, the room's type is unknown. The robot may not have looked from the right place, or the sink may lie outside the field of view or more than 4.0 m away. A query for rooms that are not kitchens, written with FILTER NOT EXISTS, reads absence as falsehood, and its answer depends on where you stood the robot.

Repair, and the micro-project

Week 8 shows a failure repaired at the lowest layer able to repair it, at one of three levels. The go-to policy drives round an obstacle, navigate-to blocks the edge and finds another route round it, and the task planner replans when a skill ends in failure.

The micro-project builds this week's part of the fetch system. You map the kitchen and the two living rooms of the apartment, in 8 to 12 places that include four the README names, and compute each hint from approach poses you read off the floor plan. Then the check script stands the stub detector at each of your places, grounds what it sees, closes the graph, and runs two queries you write, one for room types and one for the drinks the robot has seen. If you anchored a place on the kitchen sink and stood the robot where it can see it, the kitchen comes out a kitchen. Nothing in it needs Webots.

Week 6 plans over the skills, with the reachable facts computed from a map like yours. Before then, run the example, read Poole and Mackworth, sections 3.5 and 3.6, and start the micro-project with the map.

Slides

Deck for this week

Example

Run from the repository root:

PYTHONPATH=. python3 examples/05-topological-map/main.py

It loads a small flat of four places and four edges, prints every edge with its hint and marks the derived reverses, finds a route, blocks the hatch and finds the route again. It then stands the stub detector at two places, grounds what it sees, and asks which rooms have a type before and after reasoning. Nothing in it needs Webots. Read flat.ttl next to the base ontology's week 5 terms, and compare each authored edge with its derived reverse.

Micro-project

This week's exercise is exercises/05-topological-map/. Its README.md states the task and ends in a Self Check list you can verify unaided. The instructor's map of the whole apartment is published in data/reference/week-05/ after the deadline.

The micro-project runs from the inputs it provides. It does not need any earlier week's output.

Reading

Further, optional, and not examined:

More background, optional: further reading.


Intelligent Systems for Robotics · course materials