Introduction to
Computer Vision and Robotics:
Navigation, Localization and Mapping
Tomas Kulvicius
Poramate Manoonpong
Robot Navigation, Localization,
and Mapping
“I know where I am &
I am able to make a plan to reach destination”
Problems: Navigation, Localization,
Mapping
1) “How do I get there?”
 Navigation from A to B
A
B
Problems: Navigation, Localization,
Mapping
2) “Where am I?”
 Localization problem
Problems: Navigation, Localization,
Mapping
3) “Where have I been?”
 Mapping
Biological Inspiration
Biological Inspiration
MacFarland (1999) has identified three important strategies in navigation:
1) Pilotage: Navigation using familiar landmarks or features of some sort
(visual, olfactory , etc.). For example, some insects navigate towards
objects that are associated with their nest.
2) Compass orientation: Navigation using a particular compass
direction without using landmarks. For example, the small white butterfly
moves in the same direction regardless of wind direction day after day.
3) True navigation: The ability to navigate to a goal point without the use
of landmarks and regardless of the direction. For example, Pigeons seem
to use true navigation and can return to their loft no matter where they
are released.
Biological Inspiration
Compass orientation
True navigation
Biological Inspiration
• Visual sensing Landmarks (objects,
mountains, lakes)
• Olfactory cues  Indentify home, track
food source or home
Some Important
components
for navigation
• Sun compass  Orientation with respect
to the sun (requires internal clock)
• Magnetic compass Earth’s magnetic
field (polarity & gradients)
• Star compass Orientation with respect
to the stars at night (e.g., North Star or
Polaris)
• Dead reckoning  Record the distances
and directions travelled from the home point
• Memory  Memorize specific gradients or
landmarks near their home base
Biological Inspiration
• Visual sensing Landmarks (objects,
mountains, lakes)
• Olfactory cues  Indentify home, track
food source or home
• Sun compass  Orientation with respect
to the sun (requires internal clock)
• Magnetic compass Earth’s magnetic
field (polarity & gradients)
• Star compass Orientation with respect
to the stars at night (e.g., North Star or
Polaris)
• Dead reckoning  Record the distances
and directions travelled from the home point
The use of landmark by
the digger wasp (Tinbergen’s study, 1951)
Biological Inspiration
• Visual sensing Landmarks (objects,
mountains, lakes)
• Olfactory cues  Indentify home, track
food source or home
• Sun compass  Orientation with respect
to the sun (requires internal clock)
• Magnetic compass Earth’s magnetic
field (polarity & gradients)
• Star compass Orientation with respect
to the stars at night (e.g., North Star or
Polaris)
• Dead reckoning  Record the distances
and directions travelled from the home point
Biological Inspiration
• Visual sensing Landmarks (objects,
mountains, lakes)
• Olfactory cues  Indentify home, track
food source or home
• Sun compass  Orientation with respect
to the sun (requires internal clock)
• Magnetic compass Earth’s magnetic
field (polarity & gradients)
• Star compass Orientation with respect
to the stars at night (e.g., North Star or
Polaris)
• Dead reckoning  Record the distances
and directions travelled from the home point
Biological Inspiration
• Visual sensing Landmarks (objects,
mountains, lakes)
• Olfactory cues  Indentify home, track
food source or home
• Sun compass  Orientation with respect
to the sun (requires internal clock)
• Magnetic compass Earth’s magnetic
field (polarity & gradients)
• Star compass Orientation with respect
to the stars at night (e.g., North Star or
Polaris)
Odor based navigation in rats
• Dead reckoning  Record the distances
and directions travelled from the home point
Biological Inspiration
• Visual sensing Landmarks (objects,
mountains, lakes)
• Olfactory cues  Indentify home, track
food source or home
• Sun compass  Orientation with respect
to the sun (requires internal clock)
• Magnetic compass Earth’s magnetic
field (polarity & gradients)
• Star compass Orientation with respect
to the stars at night (e.g., North Star or
Polaris)
• Dead reckoning  Record the distances
and directions travelled from the home point
Odor based navigation in ants
(pheromone trails)
• Memory  Memorize specific gradients or
landmarks near their home base
Biological Inspiration
• Visual sensing Landmarks (objects,
mountains, lakes)
• Olfactory cues  Indentify home, track
food source or home
• Sun compass  Orientation with respect
to the sun (requires internal clock)
• Magnetic compass Earth’s magnetic
field (polarity & gradients)
• Star compass Orientation with respect
to the stars at night (e.g., North Star or
Polaris)
• Dead reckoning  Record the distances
and directions travelled from the home point
Sahara desert ant (Cataglyphis bicolor)
From Biological Inspiration to Robot
Implementation
Mobile Robots
NASA’s 'curious' rover
E-puck robot
AMOS robot
ARMAR III
E-puck Robot Setup
Sensors for Robot Navigation,
Mapping, Localization
Sensors for Robot Navigation,
Localization, Mapping
•
Computer vision enable a robot to see and recognize landmarks, orient
using sun sensors, star sensors.
Star sensor
Star sensor
Volpe, IROS 1999
Sun sensor
Sensors for Robot Navigation,
Localization, Mapping
Computer vision enable a robot to see and recognize landmarks, orient
using sun sensors, star sensors.
•
Olfactory sensors (gas or alcohol sensing) enable a robot to detect
chemical substance or trail and a specific odor source.
Sensor response to alcohol (70%)
•
2260
2240
2220
2200
2180
2160
2140
0
2
4
6
8
10
Sensors for Robot Navigation,
Localization, Mapping
•
Computer vision enable a robot to see and recognize landmarks, orient
using sun sensors, star sensors.
•
Olfactory sensors (gas sensing) enable a robot to detect chemical
substance or trail and a specific odor source.
•
Compass provide an indication of magnetic north. But unreliable when
interfering with magnet or metal.
Analog Dinsmore compass
Devantech Magnetic
Compass
Sensors for Robot Navigation,
Localization, Mapping
•
Computer vision enable a robot to see and recognize landmarks, orient
using sun sensors, star sensors.
•
Olfactory sensors (gas sensing) enable a robot to detect chemical
substance or trail and a specific odor source.
•
Compass provide an indication of magnetic north. But unreliable when
interfering by magnetic or metal.
•
Clock (on board)  essential in connection with a sun sensor.
Sensors for Robot Navigation,
Localization, Mapping
•
Wheel encoders or joint angle sensors  measure distance traveled and
change in orientation and are use for path integration (dead reckoning) .
•
Range finders  enable a robot to estimate its distance from objects in the
environment. (IR, Ultrasonic sensors at short distances, laser scanner in
longer distance).
•
Gyroscopes provide heading directions and to improve odometric
readings.
•
GPS enable outdoor robots to determine (within centimeters) their latitude
and longitude. Unfortunately, it does not work indoors as well as on Mars,
the moon, and other planets.
•
Etc. Wind, pressure, sound, heat sensors!!!
Navigation
“Moving from a starting point to a goal”
I. Outdoor Navigation
• Robot’s initial position at “START”
• Move to “GOAL”
• Straight-line path is “NOT possible” due to obstacles
• GOAL & Two landmarks (L1, L2) are visible on a clear day!
I. Outdoor Navigation
Three possible scenarios:
1) Clear day, visible goal, unknown
distance to goal:
“What sensors should we use?”
I. Outdoor Navigation
Three possible scenarios:
1)Clear day, visible goal, unknown
distance to goal:
-Sensors: Vision, Compass, and Sonar or
IR sensors.
-Strategy: “Wandering Standpoint”
(Puttkamer 2000)
I. Outdoor Navigation
Three possible scenarios:
1) Clear day, visible goal, unknown
distance to goal:
- Sensors: Vision, Compass, and Sonar
or IR sensors.
- Strategy: “Wandering Standpoint”
(Puttkamer 2000)
q
Try to reach the goal from start in
direct line, when encountering an
obstacle  randomly turn left/right
OR check small turning angle to
avoid an obstacle and then turn to
that direction Move around the
obstacle (boundary-following)
until the goal is clear or visible then
heading to the goal.
I. Outdoor Navigation
Three possible scenarios:
1)Clear day, visible goal, unknown
distance to goal:
-Sensors: Vision, Compass, and Sonar or
IR sensors.
-Strategy: “Wandering Standpoint”
•Vision to recognize the goal
•Keeping the goal image in the center of
a view finder
•Moving in the same compass direction
toward the goal
•If an obstacle is detected (the goal is
not visible), then move to left or right
(randomly) and travel around the
obstacle
•When the original compass direction
is detected and/or the goal is visible,
the robot can turn and change its
heading then cont. toward the goal
I. Outdoor Navigation
This results in “Path 1”
“However, this is a simple strategy but
not optimal one! & might lead to an endless loop
in case of extreme obstacle placements”
Finding the best path from start to goal is called
“path planning”, using e.g., wavefront,
machine learning algorithm! (RL)
Three possible scenarios:
1)Clear day, visible goal, unknown
distance to goal:
-Sensors: Vision, Compass, and Sonar or
IR sensors.
-Strategy: “Wandering Standpoint”
•Vision to recognize the goal
•Keeping the goal image in the center of
a view finder
•Moving in the same compass direction
toward the goal
•If an obstacle is detected (the goal is
not visible), then move to left or right
(randomly) and travel around the
obstacle
•When the original compass direction is
detected and/or the goal is visible, the
robot can turn and change its heading
then cont. toward the goal
I. Outdoor Navigation
Three possible scenarios:
2) Goal not visible from start location,
landmarks visible, goal visible from
landmarks:
- Sensors: Vision
- Strategy: “Navigation by landmarks”
q
• Move to visible landmarks
• From landmarks to the goal
 This results in “Path 2”
I. Outdoor Navigation
Three possible scenarios:
3) Goal and landmarks not visible,
direction to the goal known:
-Sensors: GPS or Compass & Wheel
Encoders (obtain current position!)
-Strategy: “Dead reckoning”
q
Dead Reckoning
• Basically this is “keeping track of your distance
and your turns”, then adding them all together
(“path integration”) to figure out your “total
displacement”
Dead Reckoning
Goal
Don’t see the goal from starting point!!!
Start
Recording distance and heading direction for “path integration“
Dead Reckoning
Goal
Start
X=0, Y = 0, q = E
X=5, Y = 0 …….
X=10, Y = 0, q = N
Recording distance and heading direction for “path integration“
Dead Reckoning
Goal
…
X=10, Y = 2.5
Start
X=0, Y = 0, q = E
X=5, Y = 0 …….
X=10, Y = 0, q = N
Recording distance and heading direction for “path integration“
Dead Reckoning
Goal
X_final=10, Y_final = 5,
q_final = N
…
X=10, Y = 2.5
Start
X=0, Y = 0, q = E
X=5, Y = 0 …….
X=10, Y = 0, q = N
Recording distance and heading direction for “path integration“
Dead Reckoning
Goal
X_final=10, Y_final = 5,
q_final = N
…
“Return to start”
X=10, Y = 2.5
Start
X=0, Y = 0, q = E
X=5, Y = 0 …….
X=10, Y = 0, q = N
Recording distance and heading direction for “path integration“
Dead Reckoning
Goal
X_final=10, Y_final = 5,
q_final = N
Q
…
X=10, Y = 2.5
Start
Q
X=0, Y = 0, q = E
X=5, Y = 0 …….
X=10, Y = 0, q = N
Recording distance and heading direction for “path integration“
Dead Reckoning
D = sqrt (X_final ^2 + Y_final ^2)
Goal
Q= arcsin(Y_final/D)
X_final=10, Y_final = 5,
Go home:
q_final = N
Distance = D
Q
Heading = 90+ Q
…
X=10, Y = 2.5
Start
Q
X=0, Y = 0, q = E
X=5, Y = 0 …….
X=10, Y = 0, q = N
Recording distance and heading direction for “path integration“
Dead Reckoning
• Control inputs to the robot: linear
velocity V(t), and rotational velocity
(w (t))
• Starting position (x0, y0) and
orientation (q0)
q
(x0, y0, q0)
• The current robot pose (x, y, q)
can be computed as:
Dead reckoning & path integration:
navigation of desert ants (Cataglyphis)
Don’t know where is the food!! 
Do random exploration to search for
the food!
Problems: Dead reckoning & path
integration for navigation
Problems: Dead reckoning & path
integration for navigation
Leg lengths were
normal during the
outbound journey
but manipulated
during the
homebound run
Wittlinger et al. 2006, Science,312 (5782): 1965-1967
Leg lengths were
manipulated during
the outbound
journey & the
homebound run
Problems: Dead reckoning & path
integration for navigation
Problems: Dead reckoning & path
integration for navigation
• Wheels slip
• Terrain change
• Inaccurate sensors (proprioceptive sensors)
• Robot is not exactly symmetrical
Problems: Dead reckoning & path
integration for navigation
• Wheels slip
• Terrain change
• Inaccurate sensors (proprioceptive sensors)
• Robot is not exactly symmetrical
The distance traveled and
orientation will deviate randomly from
the estimated values
Problems: Dead reckoning & path
integration for navigation
• Wheels slip
• Terrain change
• Inaccurate sensors (proprioceptive sensors)
• Robot is not exactly symmetrical
The distance traveled and
orientation will deviate randomly from
the estimated values
Improve by:
Using additional (exteroceptive) sensors, e.g., Vision, Gyroscope, GPS.
Combining local (propriocep) and global (exterocep) strategies
Using statistic estimation techniques (Probabilistic Localization, Kalman filter)
II. Odor based navigation
Laying and sensing odor markings
Robots exploring an unknown
environment lay trails indicating the
route back to their starting positions
A path finder robot ‘A’ lays a trail for
load carrying robots ‘B’ and ‘C’
(Russell, 1995)
Odor tracking robot
Robot tracking an ethanol vapor (Ishida et al, 2002)
Olfactory coordinated area coverage
Larinova et al, 2006
Path finding based on self-marking
navigation
(Robot)
Kulvicius et al., 2008
Path finding based on self-marking
navigation
Odor following
Reactive
control
Sabaliauskas, 2009
Path finding based on self-marking
navigation
Kulvicius et al., 2008
Path finding based on self-marking
navigation
Robot setup
Sabaliauskas, 2009
Path finding based on self-marking
navigation
5th Run
10th Run
Sabaliauskas, 2009
Path finding based on self-marking
navigation
Statistics
140
120
Time (s)
100
1.5m
n=9
80
60
1.5m
40
20
0
0
5
10
Number of runs
15
Sabaliauskas, 2009
IV. Maze Navigation
Wall-following: Touching (Left/Right)
Right wall following
Left wall following
Wall-following: Left/Rightmost path
strategy
“A robot uses the left/right-handed rule If a
robot comes to an intersection with several
open sides, it follows the left/rightmost path”
Leftmost
Rightmost
Left/Rightmost path strategy
Check state
Move
Left/Rightmost path strategy
Leftmost path
Rightmost path
Problem for Wall-following
Recursive exploration
“This leads to Full maze exploration requires
us to generate an internal representation of
the maze and to maintain a bit-field of
marking whether a particular square has
already been visited”
Recursive exploration
Algorithm:
1) Explore the whole maze starting at the start square
& visit all reachable square to obtain the map of the area.
E.g., move FrontLeftRight & every visited square or location  mark!
2) Compute the shortest distance from the start
square to any other square (or “GOAL”) using
a “wavefront” algorithm.
3) Allow the user to enter the coordinate of a goal:
Then determine the shortest driving path by reversing the path in the
the “wavefront” array from the destination to the start square
The Wavefront Planner: Setup
Goal
Starting
The Wavefront Planner: Setup
Goal
Starting
The Wavefront in Action (1)
• Starting with the goal, set all adjacent cells with “0” to the current cell + 1
– 4-Point Connectivity or 8-Point Connectivity?
– Your Choice. We’ll use 8-Point Connectivity in our example
Starting
The Wavefront in Action (2)
• Now repeat with the modified cells
– This will be repeated until no 0’s are neighboring to cells with values >= 2
0’s will only remain when regions are unreachable
Starting
The Wavefront in Action (3)
• Repeat again….
Starting
The Wavefront in Action (4)
• And again….
Starting
The Wavefront in Action (5)
• And again until….
Starting
The Wavefront in Action (DONE!)
• You’re done
– Remember, 0’s should only remain if unreachable regions exist
Starting
The Wavefront, Now Navigation!
• To find the shortest path, according to your metric, simply always move
toward a cell with a lower number
– The numbers generated by the Wavefront planner are roughly proportional to
their distance from the goal
Starting
The Wavefront, Now Navigation!
• To find the shortest path, according to your metric, simply always move
toward a cell with a lower number
– The numbers generated by the Wavefront planner are roughly proportional to
their distance from the goal
Localization
“Where am I? My position with respect to a reference frame”
Proprioceptive sensors (Encoders)
Dead reckoning
Deviation
Deviation
Proprioceptive sensors (Encoders)
Dead reckoning
Deviation
Deviation
GPS (Outdoor Localization)
• Knows latitude, longitude, altitude
– Can derive velocities, heading
• Provides “Direct observation of state”
– State: [x, y, q] ~ [longitude, latitude, heading]
GPS (Outdoor Localization)
• Knows latitude, longitude, altitude
– Can derive velocities, heading
• Provides “Direct observation of state”
– State: [x, y, q] ~ [longitude, latitude, heading]
Agrawal, M. & Konolige, K. 2006
Sun compass & vision(Outdoor Localization)
Panoramic visual system, polarized-light sensors, ambient-light sensors.
Sahabot 2
D. Lambrinos et. al., Robotics and Autonomous systems, 2000
Beacon measurements (Indoor
Localization)
Freq 1
Freq 2
Freq 3
Sonar signals
Mapping
“Creating models of the environment they
traverse using sensor data”
Sensors for the task:
• Internal sensors Encoders, Compass (Dead reckoning)
• External sensors Computer vision, sonar, IR, GPS, and laser range finders
The information obtained from the sensors can be transmitted to external
receivers or stored on board for later analysis “indoor mapping”
Outdoor mapping is more difficult since the external environment is not
conveniently arranged in orthogonal corridors like those found in interior
settings.
Two common approaches to mapping are:
• Topological mapping
• Grid-based or metric mapping
Topological mapping
• It relies on landmarks (e.g., door, hallway
intersections, T-junctions for indoor).
• It involves the creation of a map where the
location of landmarks is essential and not
distance between them.
• It is represented by graph in which each
node is a landmark and adjacent nodes are
connected by edges.
Grid-based or metric mapping
• We cover the environment to be mapped
with an evenly space grid.
• The robot does not have complete and
accurate priori knowledge concerning the
presence of obstacles.
• Each cell in the grid stores the probability
p(x,y) that cell c(x,y) is occupied. This value
represents the robot’s belief that it can or
cannot move to the center of the cell.
p(x,y)
• This grid-based map is also called “occupancy
map”.
Grid superimposed on the map
Example of Map Generation
Mapping algorithm
Mobile robot setup
Example of Map Generation
StepA: The robot starts with a completely
unknown occupancy grid and an empty
corresponding configuration space (i.e. no
obstacles). The first step is to do a 360° scan
around the robot.
If a range sensor returns a value larger than a
threshold, then a preliminary obstacle is entered
in the cell at the measured distance.
All cells between this obstacle and the current
robot position are marked as preliminary empty.
The same is entered for all cells in line of a
measurement that does not locate an obstacle;
all other cells remain “unknown”. Only final
obstacle states are entered into the configuration
space, therefore space A is still empty.
Preliminary free
Preliminary obstacle
Unknown
Free
Example of Map Generation
StepB: The robot drives to the closest obstacle
in order to examine it closer. The robot
performs a wall-following behavior around the
obstacle and, while doing so, updates both grid
and space.
Now at close range, preliminary obstacle states
have been changed to final obstacle states and
their precise location has been entered into
configuration space B.
Example of Map Generation
StepC: The robot has completely surrounded
one object by performing the wall-following
algorithm. The robot is now close again to its
starting position.
Since there are no preliminary cells left around
this rectangular obstacle, the algorithm
terminates the obstacle-following behavior and
looks for the nearest preliminary obstacle.
Example of Map Generation
StepD: The whole environment has been
explored by the robot, and all preliminary states
have been eliminated by a subsequent
obstacle-following routine around the
rectangular obstacle on the right-hand side.
The final occupancy grid and the final
configuration space are matched.
Other simulation results
Real robot experimental results
Filters
Removing noise from a signal
Filters
Filters
Low Pass Filters
I.) Moving average (FIR filter):
Filtered Sensory signal
Sensory signal
Tuning parameters:
Low Pass Filters
Low Pass Filters
II.) Exponential moving average (IIR filter):
Sensory signal
Filtered Sensory signal
Tuning parameters:
Low Pass Filters
0.5
0.125
0.03125
Kalman Filter (Estimator)
•A set of mathematical equations to estimate the state of a process:
Controlled process:
Measurement:
Process noise:
A – relates previuos state xk-1 to current
state xk
B – relates optional control input u to
state x
H – relates state x to measurement z
Measurement noise:
R – process noise covariance
Q – measurement noise covariance
Kalman Filter (Estimator)
•Procedure:
Kalman Filter (Estimator)
•Procedure:
Kalman Filter (simplified version)
•We had:
•If we drop state matrices and control input out we get:
Kalman Filter (Estimator)
3
actual volt.
measured volt.
EMA filter
MA filter (n=30)
Kalman filter
voltage
2.5
2
1.5
1
1
0.9
0.5
eEMA
eKalman
Kalman gain (K)
0.7
0.6
0.0523
0
0
eMA
0.8
0.0418
0.0194
0.5
0.4
0.3
50
100
time
150
200
0.2
0.1
0
0
50
100
150
200
Conclusions
• Problems: Navigation (How to go there?), Mapping (Where have I been?), Localization (Where am I?)
• Biological Inspiration
Pilotage: Navigation using familiar landmarks
 Compass orientation: Navigation using a particular compass direction NOT using landmarks.
 True navigation: Navigation to a goal point NOT using landmarks and regardless of the direction.
Components: Visual sensing, Olfactory cues, Compass, Dead reckoning, Memory
• Navigation
 Sensors for Robot Navigation, Mapping, Localization
 Simplified Outdoor Navigation: Wandering standpoint, Landmarks, Dead reckoning & Path
integration
 Odor based navigation: Laying and sensing odor markings, Odor tracking robot, Olfactory
coordinated area coverage, Path finding based on self-marking navigation
 Maze navigation: Wall following, Recursive exploration + Wavefront
• Localization: Local & Global sensors, Low pass filters (High freq noise), Kalman filter (Gaussian noise)
• Mapping: Topological mapping, Grid-based or metric mapping