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
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.
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 \).
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 \).
\( \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
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.
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
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
Pop 〈bed place〉, cost 0. It is not the goal, so expand it.
Frontier
Cost
bed place → arch
1.50
bed place → counter place
2.00
The goal is on the frontier, and the search does not stop.
Bed place to counter place, 2 of 3
Pop 〈bed place, arch〉, cost 1.50. Expand it. The hall door costs 1.50 + 2.50.
Frontier
Cost
bed place → counter place
2.00
bed place → arch → hall door
4.00
Bed place to counter place, 3 of 3
Pop 〈bed place, counter place〉, cost 2.00. It ends at the goal, so stop.
Frontier, left unexpanded
Cost
bed place → arch → hall door
4.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
Graph
Holds
Changes
Map
The base ontology and the files you authored, loaded by KnowledgeGraph.load(*paths)
Never during a run
Belief
What the robot has established, which is grounded detections and blocked edges
As the robot establishes facts
Inferred
The OWL 2 RL closure of the other two
On 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
Graph closed
Closure time
The example's flat
about 0.1 s
The micro-project's map
about 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
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.
The go-to policy drives round an obstacle
navigate-to blocks the edge and finds another route round it
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.