Graph SLAM
A robot walks, gets a little lost with every step, then fixes its whole map in one go. Watch how.
Walking with your eyes closed
The robot counts its steps. Every count is a tiny bit wrong, and the mistakes add up.
Nodes: a pin for every step
Each time the robot moves, it drops a pin. A pin is a guess: where am I, and which way am I facing?
One node of the graph. It holds the robot's guess for step i.
Bigger i: later in the trip.
Smaller i: earlier in the trip.
Distance from the start, sideways on the map.
Bigger x: further to the right.
Smaller x: further to the left.
Distance from the start, up the map.
Bigger y: further up.
Smaller y: further down.
The heading, as an angle.
Bigger θ: turned more to the left.
Smaller θ: turned more to the right.
Edges: a string between two pins
The odometer says what happened between two steps. Tie a string between the pins to hold that fact. If the pins drift away from what the sensor said, the string stretches and pulls them back.
How far the string is stretched. Zero means the pins agree with the sensor.
Bigger: the pins disagree with the sensor more.
Smaller: the string is more relaxed.
The measured move from pin i to pin j: how far, and how much it turned.
Bigger: the sensor says the robot moved further.
Smaller: the sensor says it barely moved.
The move from i to j that you get from where the two pins sit right now.
Bigger: the pins are further apart.
Smaller: the pins are closer together.
Where the move started. Move this pin and h changes.
Shifted: the whole string moves with it.
Held still: only pin j can ease the stretch.
Where the move ended. The optimiser slides this pin until e shrinks.
Further from the sensor's answer: e grows.
Closer to the sensor's answer: e shrinks.
How sure is each string?
Two strings can disagree. The pin goes nearer the string we trust more. That trust is called information, and it is the second part of every edge.
One number: how unhappy this string is.
Bigger: the string is stretched, or we trust it a lot.
Smaller: the string is relaxed, or we barely trust it.
The same error as before, written as a row so it can be multiplied.
Bigger error: the price grows with its square.
Smaller error: the price falls fast.
Stiffness. It weighs the error before it is added to the price.
Bigger: a stiff string. It wins the tug of war.
Smaller: a loose string. It gives way.
The stretch from the last page, multiplied by itself and by Ω.
Bigger: the price grows with its square.
Smaller: the price falls fast.
One trust for each thing a pin holds: x, y and the heading.
Bigger numbers: a sharper, more trusted sensor.
Smaller numbers: a noisy sensor.
How wrong the sensor tends to be sideways, in metres.
Bigger σ: a sloppier sensor, so 1/σ² is small and Ω is loose.
Smaller σ: a precise sensor, so Ω is stiff.
The same, for the other direction.
Bigger σ: loose in y.
Smaller σ: stiff in y.
How wrong the sensor tends to be about which way it faces, in radians.
Bigger σ: loose in heading.
Smaller σ: stiff in heading.
Loop closure: "I have been here!"
Look at the strings before the robot returns: all relaxed, and yet the map is wrong. Then the robot recognises the start. One new string joins the last pin to the first, and that string is very stretched. Every pin on the way gets pulled a little, and the lap snaps shut.
One price for the whole map
Every string charges a price for being stretched. Add the prices up and you get one number for the whole map. The best map is the one with the lowest total.
x is every pin at once. F says how unhappy all the strings are with it.
Bigger: a worse map. Strings are stretched.
Smaller: a better map. Strings are relaxed.
Go through every string, from pin i to pin j, and add its price.
More strings: more facts to respect, and a bigger total.
Fewer strings: less to respect, so the map is easier to fool.
The error from before, turned on its side so it can be multiplied.
Bigger stretch: the price grows with its square.
Smaller stretch: the price shrinks fast.
It decides how loudly this string complains.
Bigger: even a small stretch costs a lot.
Smaller: the string can stretch for free.
The gap between what the sensor said and what the pins say.
Bigger: the pins disagree more.
Smaller: the pins agree.
The star means the best one. This is the answer we are after.
Higher price at x*: the sensors disagree with each other.
Price near zero at x*: every string is happy.
Try every possible map. Keep the one with the lowest price.
Bigger search: more pins, more to try.
Smaller search: fewer pins, faster.
The same total as above. The optimiser slides pins to make it fall.
Going up: the last move was a bad one.
Going down: the map is getting better.
Rolling down to the best map
The price is a bowl, and the best map is at the bottom. The computer cannot see the whole bowl, only the bit under its feet. So it looks at the slope here, draws a simple curve that hugs the bowl, jumps to the bottom of that curve, and looks again.
The curve of the bowl. It says how fast the slope itself changes.
Bigger: a steep, narrow bowl, so take a short careful jump.
Smaller: a wide, flat bowl, so take a long jump.
How far to slide every pin, all at once, in this step.
Bigger: still far from the bottom.
Smaller: nearly there. Stop when it is tiny.
Which way is downhill, and how steep.
Bigger: far up the bowl wall.
Zero: you are at the bottom.
Add one piece for each string. Strings we trust make the bowl steeper.
More or stiffer strings: a steeper bowl.
Fewer or looser strings: a flatter bowl.
Each string adds its own piece to H, and its own piece to b.
More strings: more pieces to add.
Fewer strings: a smaller table.
The same table as J, flipped so it can carry the error back to the pins.
Bigger: the pins push the error strongly.
Smaller: the pins barely matter to this string.
The same stiffness as before. It weighs this string.
Bigger: this string counts for more.
Smaller: this string counts for less.
Nudge a pin a tiny bit and see how much the stretch changes. One row per string.
Bigger: this pin controls the string a lot.
Smaller: moving this pin hardly changes the string.
Every string pushes on the pins it is tied to, by its stretch.
Bigger: the strings are pulling hard.
Zero: the pulls cancel out.
Turns the stretch of a string into a push on the pins it joins.
Bigger: a stronger push.
Smaller: a weaker push.
A trusted string pushes harder.
Bigger: pushes harder.
Smaller: pushes softly.
The gap between what the sensor said and what the pins say.
Bigger: a bigger push.
Zero: no push.
The new map. Then we measure everything again and repeat.
More rounds: closer to the bottom.
Fewer rounds: a rougher map.
The map we have now.
Far from the best: expect a big jump.
Close to the best: expect a small jump.
Added to every pin.
Bigger: the pins move a lot.
Smaller: the pins barely move.
Watch the lap settle
The same jumps, on the whole lap. The price drops fast at first, then slows down. After a handful of steps the pins stop moving.
Why we pin the first dot
Every string only knows the gap between two pins. It has no idea where the pins are on the page. So the whole map can slide or spin and the price stays the same. To stop that, we nail the first pin down.
One more string. It ties the first pin to the place we say it belongs.
Bigger: the first pin has been dragged from its place.
Zero: the first pin is where we said it would be.
The pin the optimiser is allowed to move.
Further from x̄: a bigger price.
Closer to x̄: a smaller price.
A fixed choice, often the origin. Everything else is measured from here.
Moved: the whole map moves with it.
Kept: the map stays put.
A huge number, so the first pin cannot really move.
Bigger: nailed down. It will not budge.
Smaller: a loose nail, and the map can drift again.
It appears twice because the price is a square: the gap, times the gap.
Bigger gap: the price grows with its square.
Smaller gap: the price falls fast.
The same fixed place as before.
Moved: the whole map moves with it.
Kept: the map stays put.
How sure is each pin?
A good map says more than where things are. It also says how sure it is. Each pin wears a cloud: the place it might really be. The cloud grows with every step of counting, and shrinks when a loop closure ties the ends together.
How far from its guess a pin might really be. One cloud per pin.
Bigger: a bigger cloud, so the pin is less sure.
Smaller: a tight cloud, so the pin is sure.
The same table as before. It measures how firmly the strings hold each pin.
Bigger: more or stiffer strings hold the pin.
Smaller: few or loose strings hold the pin.
Held firmly means a small cloud. Held loosely means a big cloud.
Bigger H: the flip makes the cloud smaller.
Smaller H: the flip makes the cloud bigger.
When a string lies
Sometimes the robot is fooled. Two corridors look the same, so it ties a string between two places that are far apart. One liar can crush the whole map. The fix: a string that is stretched far too much is probably lying, so listen to it less and less.
It replaces the plain price e². For small stretches it is almost the same. For huge ones it hardly grows.
Bigger stretch: the price rises, but more and more slowly.
Smaller stretch: the same as the plain price.
Sets how big the price can be before it flattens.
Bigger: the price keeps rising for longer.
Smaller: it flattens quickly, so everything is cheap.
The log curve rises fast, then bends over. That is what stops a liar winning.
Bigger input: only a small rise in price.
Smaller input: close to a straight line.
Measured in how many "usual misses" it is off.
Bigger: the string is probably lying.
Smaller: the string is probably honest.
A stretch about this big is where we start to doubt a string.
Bigger: more patient. Even odd strings are believed.
Smaller: quick to doubt. Good strings may be ignored too.
The whole recipe
That is all of Graph SLAM. Go round this list again for every new step.
- Drop a pinfor every step the robot takes.
- Tie a stringfrom the last pin to the new one.
- Say how sureeach string is. A sure string is stiff.
- Spot an old placeand tie a string back to its pin.
- Add up the pricesof all the stretched strings.
- Roll downhilluntil the price stops falling.
- Doubt liarsand keep a cloud of doubt on every pin.
Want to see it on a real map? Open the mapping demo.