The topological map and grounding detections

Intelligent Systems for Robotics · Week 5

This week the graph gains

  • The map the robot navigates, as places and the edges between them
  • A path from detections to facts, so that room types come out of what the robot sees

Both live in the graph you started in week 4.

Week 1's square

start end 1 m 0.174 m

Arc turns, the bare motor-test floor, 4 m of travel. Schematic. The gap is to scale, and its direction is illustrative.

A route is longer than a square

  • The square drove 4 m and finished 0.174 m off
  • A route across the apartment is several times longer
  • 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.

No pose in a world frame

  • Every metric quantity in the map is relative to something the robot can perceive, such as a doorway or a piece of furniture
  • On arrival at a place, the robot perceives the place's anchor and measures its pose against it afresh
  • The odometric error from the edge it has just driven no longer matters

Re-anchoring bounds drift to one edge.

Route search pays for it. With no common frame, it has no straight-line distance to the goal.

Wormholes in a virtual maze

  • Participants walked a virtual hedge maze with two wormholes, which rotated and teleported them
  • A labelled-graph model fitted them decisively better than a Euclidean map model

The authors conclude that knowledge of navigation space is a labelled graph. Its local metric information is approximate, inconsistent, and not in a common coordinate system.

Warren, Rothman, Schnapp & Ericson (2017), Cognition 166, 152–163.

Read it as a lens

  • The flat claim that people hold no metric map is contested
  • Map-like and graph-like representations both exist, and can operate together or apart
  • Grid cells are a possible neural substrate for a Euclidean coordinate system. People's spatial memories violate Euclidean rules.

The defensible claim is narrower. People navigate well without a globally consistent metric map.

The robot's map depends on neither claim. Its reason is week 1's odometry.

Warren et al. (2017), Cognition 166. Peer, Brunec, Newcombe & Epstein (2021), Trends in Cognitive Sciences 25(1), 37–54.

Places, anchors and frames

A place has an anchor

  • A place is a node of the map, somewhere the robot can recognise and stop
  • The base ontology has had isfr:Place since week 4
  • This week every place gains an anchor, through isfr:anchoredBy
  • The anchor is what the robot perceives at the place

There are two kinds of anchor, a gap and an object.

Gap anchors

  • 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
the gap, 0.8 m wide TIAGo wall no return in the opening returns

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.

Object anchors

  • isfr:ObjectAnchor is an object the detector names, such as a sink, a fridge or a dining table
  • isfr:anchorClass names the ontology class of its object
  • COCO includes sink, oven, refrigerator, dining table, couch, bed, bottle and cup

Why the anchor names an ontology class comes back under grounding.

What the detector cannot name

  • COCO has no door and no worktop
  • A doorway is therefore anchored by its gap
  • A worktop is anchored by the sink or the oven on it, with the offset carried by the approach
  • A room centre is never a place. Nothing at the centre of a room can be recognised.

Two conditions on a place's frame

Every place defines a local frame, and the hints between places are written in those frames. The frame must be

  1. observable from the anchor on arrival, so that the robot can measure its own pose in it
  2. independent of the direction the robot arrived from
x x x y turns with the arrival: two hints fixed to the opening: one

Each edge carries one hint, and the reverse is computed from it. A frame that turned with the arrival would need one hint per arrival.

A gap's frame

x y first region second region isfr:width
  • Origin at the midpoint of the opening, x along its normal
  • isfr:joins ( first second ) sets the sign of x
  • The approach is the origin, facing along x

An object's frame

  • A detection gives an object's position, and not its orientation
  • The origin is the approach pose, a stand-off distance from the object
  • The x axis lies along the line of sight to the object on arrival
sink approach x dining table one approach, one line of sight every approach, its own frame

A sink against a wall is reached from one side, so the line of sight is nearly the same whatever the arrival.

A dining table can be approached from any side. Hints leaving such a place are less accurate.

Two places from the example


: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 ] .
					

A place belongs to one region through isfr:inRoom. The hall door joins the hall to the kitchen corner, so it belongs to the hall.

Hints

An edge carries a hint


:arch_to_bed_place a isfr:Edge ;
    isfr:from :arch ; isfr:to :bed_place ;
    isfr:hint [ isfr:x 1.38 ; isfr:y 0.6 ; isfr:yaw -1.57 ] .
					
  • An edge is an individual, since it carries a hint and runtime state, and a plain triple can carry neither
  • The hint is the approximate pose of the target place's frame in the source place's frame
  • x ahead and y to the left in metres, yaw anticlockwise in radians. These are the keys robot.pose() returns.

A coarse hint is enough

2 m ahead, 0.5 m to the left, and turned a quarter turn left.

  • It gives the robot a goal to drive towards until the target's anchor is in view
  • It tells the robot which of two similar openings is the intended one

Composition in SE(2)

A hint is \( h = (x, y, \theta) \), with \( \theta \) the yaw. Let \( a \) be the pose of frame \( B \) in \( A \), and \( b \) the pose of \( C \) in \( B \).

\[ 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 \). The yaws add.
  • Composition is associative, and it is not commutative

Inversion

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

The inverse of a hint is the pose of the source's frame in the target's frame.

A hint composed with its inverse gives the identity, \( h \oplus (\ominus h) = (0, 0, 0) \).

A worked inversion

The edge from the arch to the bed place: \( h = (1.38, 0.60, -1.57) \), so \( \cos\theta \approx 0 \) and \( \sin\theta \approx -1 \).

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

\( \ominus h \approx (0.60, -1.38, 1.57) \). Negating alone reads the arch 1.38 m behind, true in the arch's own axes; the quarter turn makes it 1.38 m to the bed place's right.

Author one direction

  • The reverse of an edge 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 to trust
  • edges() raises an error, naming both edges, when a pair of places is joined in both directions

arch -> bed place: x 1.38 m, y 0.60 m, yaw -1.57 rad
bed place -> arch: x 0.60 m, y -1.38 m, yaw 1.57 rad (derived)
					

Using a hint while driving

Odometry gives \( p_0 \) on leaving the source place and \( p_t \) now.

  • \( \Delta = (\ominus p_0) \oplus p_t \) is the robot's pose in the source's frame
  • \( g = (\ominus \Delta) \oplus h \) is the target in the robot's own frame
source place target place robot now Δ g h

Only the change in odometry enters, so drift accumulates over this one edge.

When the anchor appears, or does not

  • Once the target's anchor is in view, its perceived pose replaces \( g \)
  • go-to is the learned skill that drives one edge. If the anchor does not appear within the hint's distance and a margin, it is designed to end in failure, with the reason that the expected landmark was not seen.

That is monitoring in week 1's sense, at the lowest layer of the stack.

A patchwork map

  • The Spatial Semantic Hierarchy has four levels, control, causal, topological and metrical
  • The topological level introduces places, paths and regions
  • Local frames, linked by topological connections, form a patchwork map. They merge into one global frame only when enough information and computation are available.
hall door counter place arch bed place

Your map is a patchwork map, with a frame at every place and a hint on every edge. The course never merges the frames.

Kuipers (2000), The Spatial Semantic Hierarchy, Artificial Intelligence 119(1–2), 191–233.

Route search

A route is found when it is needed

  • A route is a sequence of edges. Its length is not fixed, so it is found by search and never written into the graph.
  • route(kg, start, goal) 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} \).

Uniform-cost search

hall door counter place arch bed place 2.00 2.50 1.50 2.00, the hatch

Keep a frontier of paths ordered by cost. Always expand the cheapest. Stop when the cheapest path on the frontier ends at the goal.

Poole & Mackworth, section 3.5.4, where it is called lowest-cost-first search. Costs in metres, rounded.

Bed place to counter place, 1 of 3

hall door counter place arch bed place 2.00 2.50 1.50 2.00

Pop ⟨bed place⟩, cost 0. It is not the goal, so expand it.

FrontierCost
bed place → arch1.50
bed place → counter place2.00

The goal is on the frontier, and the search does not stop.

Bed place to counter place, 2 of 3

hall door counter place arch bed place 2.00 2.50 1.50 2.00

Pop ⟨bed place, arch⟩, cost 1.50. Expand it. The hall door costs 1.50 + 2.50.

FrontierCost
bed place → counter place2.00
bed place → arch → hall door4.00

Bed place to counter place, 3 of 3

hall door counter place arch bed place 2.00 2.50 1.50 2.00

Pop ⟨bed place, counter place⟩, cost 2.00. It ends at the goal, so stop.

Frontier, left unexpandedCost
bed place → arch → hall door4.00

The route is the hatch, one edge of 2.0 m.

A* needs a heuristic

  • A* orders the frontier by cost so far plus \( H(n) \), an estimate of the cost from \( n \) to the goal
  • \( H \) is admissible when it never exceeds the cost of a lowest-cost path from a node to a goal
  • Then A* returns an optimal solution when one exists, 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

Poole & Mackworth, section 3.6, and Proposition 3.1 in section 3.6.1.

This map offers none

  • The usual heuristic, straight-line distance to the goal, needs both positions in one frame. This map has no common frame.
  • Composing hints along a path needs a path first. Two paths give two estimates, since the hints do not agree.
  • Neither estimate is guaranteed to stay below the true cost

The one heuristic known to be admissible here is \( H(n) = 0 \). A* with a zero heuristic is uniform-cost search.

Routes stay outside PDDL

  • 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 domain holds navigate-to, with the precondition (reachable ?from ?to)
  • The reachable facts are computed from the graph when the problem is generated

Week 6's planner never sees a route.

What was written, seen and concluded

One dataset, three named graphs

GraphHoldsChanges
MapThe base ontology and the files you authored, loaded by KnowledgeGraph.load(*paths)Never during a run
BeliefWhat the robot has established, which is grounded detections and blocked edgesAs the robot establishes facts
InferredThe OWL 2 RL closure of the other twoOn close(), when the belief graph changes

KnowledgeGraph, in isfr/knowledge/graph.py, wraps one rdflib dataset.

Written, seen, concluded

  • asserted() returns the map and belief graphs together. union() returns all three.
  • 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
Map Belief Inferred close() asserted() union(), all three
Graph closedClosure time
The example's flatabout 0.1 s
The micro-project's mapabout 0.3 s

Measured on the instructor's laptop, Apple M4 Max.

Blocking an edge


:hatch isfr:blocked true .
					
  • block(edge) writes this one triple to the belief graph. The map graph does not change.
  • The derived reverse shares the authored edge's IRI, so the edge closes in both directions, and the route search skips it
  • It stays blocked for the rest of the run. Letting it expire would need a timing policy, and nothing in the course teaches one.

The reroute


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

A detection

  • 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.

From a COCO name to a class

  • 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
  • bottle becomes an isfr:Bottle, and sink an isfr:Sink
  • A detection whose COCO name no class carries is left ungrounded. ground returns it in a separate list.

One COCO class, two kinds of sink


isfr:Sink a owl:Class ; rdfs:subClassOf isfr:Furniture ; isfr:cocoClass "sink" .
isfr:KitchenSink a owl:Class ; rdfs:subClassOf isfr:Sink .
isfr:BathroomSink a owl:Class ; rdfs:subClassOf isfr:Sink .
					
  • The detector cannot tell the two apart, so the COCO name sits on the parent
  • A sink grounded from its COCO name alone is an isfr:Sink

The kitchen axiom needs an isfr:KitchenSink. It would never fire, and no kitchen would ever be inferred from a detection.

The anchor class settles it


:counter_place isfr:anchoredBy [ a isfr:ObjectAnchor ;
                                 isfr:anchorClass isfr:KitchenSink ] .
					
  • A detection takes the anchor class when that class, or a superclass, carries its COCO name, and it is the nearest detection of that name
  • 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. The room type is still inferred.

At the 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

Which rooms have a type beyond Room?
  before reasoning: 0 rows; after: 1 rows
  + kitchen corner | Kitchen
					

The bottle takes the most general class for its COCO name. The sink takes the anchor's class, and the reasoner makes the kitchen corner a kitchen.

Data association, in 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.

  • Poses are relative to the place they were seen from
  • Two places share no frame in which to compare them, only a hint too coarse to tell one bottle from two
  • The same bottle seen from two places therefore becomes two individuals
place A place B bottle_1 bottle_2 no shared frame to compare them in 0.3 m 0.3 m

The stub detector

  • StubDetector reads the apartment from data/apartment/world.json and reports what the head camera would see from a pose
  • A bottle, a cup or a wine glass within 1.0 m. Anything else within 4.0 m.
  • A field of view 1.0 rad wide, assumed until the apartment world reports the camera's. Never through a wall.

The world coordinates the stub reads stay in its input. None reaches a detection or the graph.

An open world, again

  • 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

Its answer depends on where you stood the robot.

Repair, and the micro-project

Repair, in week 8

A failure is repaired at the lowest layer able to repair it.

  1. The go-to policy drives round an obstacle
  2. navigate-to blocks the edge and finds another route round it
  3. The task planner replans when a skill ends in failure

The micro-project

  • Map the kitchen and the two living rooms of the apartment, in 8 to 12 places that include four the README names
  • Compute each hint from approach poses you read off the floor plan
  • Ground what the stub detector sees at each place, and write the two queries

In exercises/05-topological-map/. Nothing in it needs Webots.

Then the check script

  • Stands the stub detector at each of your places, grounds what it sees and closes the graph
  • 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.

Next week

  • 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, 3.6 and 6.1, and start the micro-project with the map.


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