Tracking/Servoing - the Institute for Computer Graphics and Vision

Download Report

Transcript Tracking/Servoing - the Institute for Computer Graphics and Vision

Slide 1

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 2

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 3

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 4

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 5

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 6

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 7

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 8

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 9

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 10

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 11

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 12

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 13

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 14

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 15

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 16

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 17

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 18

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 19

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 20

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 21

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 22

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 23

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 24

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 25

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 26

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 27

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 28

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 29

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 30

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 31

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 32

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 33

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 34

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 35

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 36

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 37

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 38

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 39

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 40

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 41

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 42

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 43

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 44

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 45

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 46

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 47

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 48

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 49

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 50

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 51

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 52

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 53

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 54

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 55

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 56

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 57

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 58

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 59

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 60

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 61

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 62

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 63

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 64

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65


Slide 65

ROBOT VISION
Lesson 10: Object Tracking and
Visual Servoing
Matthias Rüther

Robot Vision SS 2005

Matthias Rüther

1

Contents
 Object Tracking
– Appearance based tracking
• Kalman filtering
• Condensation algorithm

– Model based tracking
• Model fitting and tracking

 Visual Servoing
– Principle
– Servoing Types

Robot Vision SS 2005

Matthias Rüther

2

Tracking

Tracking

Robot Vision SS 2005

Matthias Rüther

3

Definition of Tracking
 Tracking:
– Generate some conclusions about the motion of the scene,
objects, or the camera, given a sequence of images.
– Knowing this motion, predict where things are going to
project in the next image, so that we don’t have so much
work looking for them.

Robot Vision SS 2005

Matthias Rüther

4

Why Track?

Robot Vision SS 2005

Matthias Rüther

5

Tracking a Silhouette by Measuring Edge Positions
 Observations are positions of edges along normals to tracked
contour

Robot Vision SS 2005

Matthias Rüther

6

Why not Wait and Process the Set of Images as a Batch?
 E.g. in a car system, detecting and tracking pedestrians in real time
is important.
 Recursive methods require less computing

Robot Vision SS 2005

Matthias Rüther

7

Implicit Assumptions of Tracking
 Physical cameras do not move instantly from a viewpoint to
another.
 Objects do not teleport between places around the scene.
 Relative position between camera and scene changes
incrementally.
 We can model motion

Robot Vision SS 2005

Matthias Rüther

8

Related Fields
 Signal Detection and Estimation
 Radar technology

Robot Vision SS 2005

Matthias Rüther

9

The Problem: Signal Estimation
 We have a system with parameters
– Scene structure, camera motion, automatic zoom
– System state is unknown (“hidden”)

 We have measurements
– Components of stable “feature points” in the images.
– “Observations”, projections of the state.

 We want to recover the state components from the observations

Robot Vision SS 2005

Matthias Rüther

10

Necessary Models

Robot Vision SS 2005

Matthias Rüther

11

A Simple Example of Estimation by Least Square Method

Robot Vision SS 2005

Matthias Rüther

12

Recursive Least Square Estimation
 We don’t want to wait until all data
have been collected to get an
estimate of the depth.
 We don’t want to reprocess old
data when we make a new
measurement.
 Recursive method: data at step i
are obtained from data at step i-1

Robot Vision SS 2005

Matthias Rüther

13

Recursive Least Square Estimation 2

Robot Vision SS 2005

Matthias Rüther

14

Recursive Least Square Estimation 3

Robot Vision SS 2005

Matthias Rüther

15

Least Square Estimation of the State Vector of a Static System

Robot Vision SS 2005

Matthias Rüther

16

Least Square Estimation of the State Vector of a Static System 2

Robot Vision SS 2005

Matthias Rüther

17

Dynamic System

Robot Vision SS 2005

Matthias Rüther

18

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

19

Estimation when System Model is Nonlinear (Extended Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

20

Tracking Steps

Robot Vision SS 2005

Matthias Rüther

21

Recursive Least Square Estimation for a Dynamic System (Kalman Filter)

Robot Vision SS 2005

Matthias Rüther

22

Tracking as a Probabilistic Inference Problem
 Find distributions for state vector ai and for measurement vector xi.
Then we are able to compute the expectations âi and x^i.
 • Simplifying assumptions (same as for HMM)

Robot Vision SS 2005

Matthias Rüther

23

Tracking as Inference

Robot Vision SS 2005

Matthias Rüther

24

Model based tracking

Robot Vision SS 2005

Matthias Rüther

25

MODEL-BASED 3-D TRACKING
IDEA: if motion is caused by known 3-D object, we can
track 3-D motion parameters, not just individual features!
ADVANTAGES:
- low dimensionality (3 rotations, 3 translations
independent of number of features tracked)
- mutually constrained motion instead of
independently moving points

LIMITATIONS:
- 6 params only with rigid objects!
Not articulated, not deformable.
- assumes 3-D model known a priori

Robot Vision SS 2005

Matthias Rüther

26

Example Algorithm
[Wunsch,Hirzinger IEEE RA 1997]
SKETCH OF ALGORITHM:
0. Initialize 3-D pose R0, t0 (rot, transl)
1. Extract features from image It
2. Match img features with features of 3-D model
positioned at Rt-1, tt-1
3. Evaluate global error metric in 3-D space
(notice, not in image space)
4. Estimate Rt, tt aligning img and model features
5. Next frame and go to 1.

Robot Vision SS 2005

Matthias Rüther

27

Some Details
FEATURES: for instance using image edges with orient.
 and offset d (and sx, sy camera scale factors), then
n  ( s x cos   s y sin  , 0 )

is the normal of the 3-D plane through the img edge.
3-D plane through img edge
q
p

Corresponding model edge

ERROR METRIC: in 3-D space for efficiency (no backprojection): orthogonality of n and model edge
E  [ n R ( p  t )]  [ n R ( q  t )]
T

Robot Vision SS 2005

2

T

Matthias Rüther

2

28

Some Details
MINIMISATION: using, say, 3 types of features:
{
wjE j 
R ,t 
f1

min



j

f 2

wjE j

j





f3

wjE j }

j

Trick 1: Approximating R with differential rotations:
 0

Rx  x  dz

  dy

 dz
0
dx

dy 

 dx x  x  [ ] x

0 

All E terms can be linearized, a linear system obtained
from the quadratic minimization, and a solution
computed in closed form: e.g., for edges,
  n k n Tk

T
  v k n k

Robot Vision SS 2005

n
v

T
T
v k   t    (n k p k )n k 

  
T 
T
v
(
n
p
)
v

k
k 
k
k
k 
    


k

Matthias Rüther

29

Some Details
... where v Tk  n Tk p k
The resulting linear system
A [t  ] = b
is (trick 2) applied iteratively at each time instant to reduce
errors; a few iterations should suffice for small frame-toframe displacements.
NOTICE ASSUMPTIONS MADE:
- rigid object
- model known a priori
- small frame-to-frame displacements
- img-model feature correspondences known
(if small displacements, by min distance)

Robot Vision SS 2005

Matthias Rüther

30

Problems with Tracking
 Initial detection
– If it is too slow we will never catch up
– If it is fast, why not do detection at every frame?

 Even if raw detection can be done in real time, tracking saves
processing cycles compared to raw detection.

 The CPU has other things to do.
 Detection is needed again if you lose tracking

 Most vision tracking prototypes use initial detection done by hand

Robot Vision SS 2005

Matthias Rüther

31

Visual Servoing
 Vision System operates in a closed control loop.
 Better Accuracy than „Look and Move“ systems

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

32

Visual Servoing
 Example: Maintaining relative Object Position

Figures from P. Wunsch and G. Hirzinger. Real-Time Visual Tracking of 3-D Objects with Dynamic Handling of Occlusion
Robot Vision SS 2005

Matthias Rüther

33

Visual Servoing
 Camera Configurations:

End-Effector Mounted

Fixed

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

34

Visual Servoing
 Servoing Architectures

Figures from S.Hutchinson: A Tutorial on Visual Servo Control
Robot Vision SS 2005

Matthias Rüther

35

Visual Servoing
 Position-based and Image Based control
– Position based:
•
•
•
•
•

Alignment in target coordinate system
The 3D structure of the target is rconstructed
The end-effector is tracked
Sensitive to calibration errors
Sensitive to reconstruction errors

End-effector

target

– Image based:
•
•
•
•
•
•

Alignment in image coordinates
No explicit reconstruction necessary
Insensitive to calibration errors
Only special problems solvable
Depends on initial pose
Depends on selected features

Robot Vision SS 2005

Matthias Rüther

Image of end
effector

Image of target

36

Visual Servoing
 EOL and ECL control

– EOL: endpoint open-loop; only the target is observed by the camera

–

ECL: endpoint closed-loop; target as well as end-effector are observed by
the camera

EOL
Robot Vision SS 2005

ECL
Matthias Rüther

37

Visual Servoing
 Position Based Algorithm:
1. Estimation of relative pose
2. Computation of error between current pose and target pose
3. Movement of robot

 Example: point alignment

p1
p2

Robot Vision SS 2005

Matthias Rüther

38

Visual Servoing
 Position based point alignment
p1m

p2m
d

 Goal: bring e to 0 by moving p1
e = |p2m – p1m|
u = k*(p2m – p1m)

 pxm is subject to the following measurement errors: sensor position, sensor
calibration, sensor measurement error
 pxm is independent of the following errors: end effector position, target
position
Robot Vision SS 2005

Matthias Rüther

39

Visual Servoing
 Image based point alignment

p1

u1
d1

 Goal: bring e to 0 by moving p1
e = |u1m – v1m| + |u2m – v2m|

v1

p2

u2

v2
d2

c1

c2

 uxm, vxm is subject only to sensor measurement error

 uxm, vxm is independent of the following measurement errors: sensor
position, end effector position, sensor calibration, target position

Robot Vision SS 2005

Matthias Rüther

40

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

41

Visual Servoing
 Example Laparoscopy

Figures from A.Krupa: Autonomous 3-D Positioning of Surgical Instruments in Robotized Laparoscopic Surgery Using Visual Servoing
Robot Vision SS 2005

Matthias Rüther

42

Tracking using CONDENSATION

CONditional DENSity PropagATION

M. Isard and A. Blake, CONDENSATION – Conditional density
propagation for visual tracking, Int. J. Computer Vision 29(1), 1998,
pp. 4-28.

Robot Vision SS 2005

Matthias Rüther

43

Goal
 Model-based visual tracking in dense clutter at near video frame
rates

Robot Vision SS 2005

Matthias Rüther

44

Example

Robot Vision SS 2005

Matthias Rüther

45

Approach
 Probabilistic framework for tracking objects such as curves in clutter
using an iterative sampling algorithm.
 Model motion and shape of target
 Top-down approach
 Simulation instead of analytic solution

Robot Vision SS 2005

Matthias Rüther

46

Probabilistic Framework
 Object dynamics form a temporal Markov chain

 Observations, zt , are independent (mutually and w.r.t process)

 Use Bayes’ rule

Robot Vision SS 2005

Matthias Rüther

47

Notation
X

State vector, e.g., curve’s position and orientation

Z

Measurement vector, e.g., image edge locations

p(X)

Prior probability of state vector; summarizes prior
domain knowledge, e.g., by independent
measurements

p(Z)

Probability of measuring Z; fixed for any given image

p(Z | X) Probability of measuring Z given that the state is X;
compares image to expectation based on state
p(X | Z) Probability of X given that measurement Z has
occurred; called state posterior

Robot Vision SS 2005

Matthias Rüther

48

Tracking as Estimation
 Compute state posterior, p(X|Z), and select next state to be the one
that maximizes this (Maximum a Posteriori (MAP) estimate)
 Measurements are complex and noisy, so posterior cannot be
evaluated in closed form

 Particle filter (iterative sampling) idea:
– Stochastically approximate the state posterior with a set of N
weighted particles, (s, ), where s is a sample state and  is its
weight

 Use Bayes’ rule to compute p(X|Z)

Robot Vision SS 2005

Matthias Rüther

49

Factored Sampling
 Generate a set of samples that approximates the posterior p(X|Z)
 Sample set s={s(1), …, s(N)} generated from p(X); each sample has a
weight (“probability”)

Robot Vision SS 2005

Matthias Rüther

50

Factored Sampling

N=15

• CONDENSATION for one image
Robot Vision SS 2005

Matthias Rüther

51

Estimating Target State

Mean of weighted state
samples

State samples

Robot Vision SS 2005

Matthias Rüther

52

Bayes’ Rule
This is what you may
know a priori, or what
you can predict

This is what you can
evaluate

p( X | Z ) 

p(Z | X ) p( X )
p(Z )

This is what you want.
Knowing p(X|Z) will tell us
what is the most likely state
X.

Robot Vision SS 2005

This is a constant for a
given image

Matthias Rüther

53

CONDENSATION Algorithm
1. Select: Randomly select N particles from {st-1(n)} based on weights
t-1(n); same particle may be picked multiple times (factored
sampling)
2. Predict: Move particles according to deterministic dynamics (drift),
then perturb individually (diffuse)
3. Measure: Get a likelihood for each new sample by comparing it
with the image’s local appearance, i.e., based on p(zt|xt); then
update weight accordingly to obtain {(st(n), t(n))}

Robot Vision SS 2005

Matthias Rüther

54

CONDENSATION Scheme

Robot Vision SS 2005

Matthias Rüther

55

Notes on Updating
 Enforcing plausibility: Particles
that represent impossible
configurations are discarded
 Diffusion modeled with a
Gaussian

 Likelihood function: Convert
“goodness of prediction” score to
pseudo-probability
– More markings closer to
predicted markings -> higher
likelihood

Robot Vision SS 2005

Matthias Rüther

56

State Posterior

Robot Vision SS 2005

Matthias Rüther

57

State Posterior Animation

Robot Vision SS 2005

Matthias Rüther

58

Object Motion Model
 For video tracking we need a way to propagate probability
densities, so we need a “motion model” such as
Xt+1 = A Xt + B Wt where W is a noise term and A and B are state
transition matrices that can be learned from training sequences

 The state, X, of an object, e.g., a B-spline curve, can be
represented as a point in a 6D state space of possible 2D affine
transformations of the object

Robot Vision SS 2005

Matthias Rüther

59

Evaluating p(Z | X)

M

p ( z | x )  qp ( z | clutter ) 



p ( z | x,  m ) p ( m )

m 1

where m = {true measurement is zm} for m = 1,…,M, and q = 1 mp(m) is the probability that the target is not visible

m

Robot Vision SS 2005


 xm  z m




Matthias Rüther

2

if x m  z m  



otherwise

60

Dancing Example

Robot Vision SS 2005

Matthias Rüther

61

Hand Example

Robot Vision SS 2005

Matthias Rüther

62

Pointing Hand Example

Robot Vision SS 2005

Matthias Rüther

63

3D Model-based Example
 3D state space: image position + angle
 Polyhedral model of object

Robot Vision SS 2005

Matthias Rüther

64

Advantages of Particle Filtering
 Nonlinear dynamics, measurement model easily incorporated
 Copes with lots of false positives
 Multi-modal posterior okay (unlike Kalman filter)
 Multiple samples provides multiple hypotheses
 Fast and simple to implement

Robot Vision SS 2005

Matthias Rüther

65