View
219
Download
3
Embed Size (px)
Citation preview
Stanford CS223B Computer Vision, Winter 2007
Lecture 12 Tracking Motion
Professors Sebastian Thrun and Jana Košecká
CAs: Vaibhav Vaish and David Stavens
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
The Tracking Problem
Can we estimate the position of the object? Can we estimate its velocity? Can we predict future positions?
Image 4Image 1 Image 2 Image 3
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
The Tracking Problem
Given Sequence of Images Find center of moving object Camera might be moving or stationary
We assume: We can find object in individual images.
The Problem: Track across multiple images.
Is a fundamental problem in computer vision
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Methods
Bayes Filter
Particle Filter
Uncented Kalman Filter
Kalman Filter
Extended Kalman Filter
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Further Reading…
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Example: Moving Object
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
SlowDown!
Example of Bayesian Inference
?
Environment priorp(staircase) = 0.1
Bayesian inferencep(staircase | image) p(image | staircasse) p(staircase) p(im | stair) p(stair) + p(im | no stair) p(no stair) = 0.7 0.1 / (0.7 0.1 + 0.2 0.9) = 0.28
Sensor modelp(image | staircase) = 0.7p(image | no staircase) = 0.2p(staircase)
= 0.28
Cost modelcost(fast walk | staircase) = $1,000cost(fast walk | no staircase) = $0cost(slow+sense) = $1
Decision TheoryE[cost(fast walk)] = $1,000 0.28 = $280E[cost(slow+sense)] = $1
=
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filter Definition
Environment state xt
Measurement zt
Can we calculate p(xt | z1, z2, …, zt) ?
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filters Illustrated
Localization problem
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filters: Essential Steps
Belief: Bel(xt)
Measurement update: Bel(xt) Bel(xt) p(zt|xt)
Time update: Bel(xt+1) Bel(xt) p(xt+1|ut,xt)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
x = statet = timez = observationu = action = constant
Bayes Filters
),|()( 00 tttt uzxpxBel KK=
),,,|(),,,,|( 011011 zzuxpzzuxzp ttttttt KK −−−−=
),,,|()|( 011 zzuxpxzp ttttt K−−=
1011011 ),,|(),,,|()|( −−−−−∫= tttttttt dxzuxpzuxxpxzp KKη
1111 )(),|()|( −−−−∫= ttttttt dxxBeluxxpxzp
),,,,|( 011 uzuzxp tttt K−−=
Markov
Bayes
Markov
1021111 ),,|(),|()|( −−−−−−∫= ttttttttt dxuuzxpuxxpxzp Kη
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filtersx = statet = timez = observationu = action
1111 )(),|()|()( −−−−∫= tttttttt dxxBeluxxpxzpxBel
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filters Illustrated
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Bayes Filters
Initial Estimate of State Iterate
– Receive measurement, update your belief (uncertainty shrinks)
– Predict, update your belief (uncertainty grows)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Methods
Bayes Filter
Particle Filter
Uncented Kalman Filter
Kalman Filter
Extended Kalman Filter
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filters: Basic Idea
)(xp
tx
)|()( ...1 tttt zxpXxp ≈∈ (equality for )∞↑n
set of n particles Xt
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filter Explained
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
11...11...111...1...1 ),|(),|()|(),|( −−−−−∫= tttttttttttt dxuzxpxuxpxzpuzxp
Basic Particle Filter Algorithm
),|()( ...1...1 ttttt uzxpXxp ≈∈
Initialization:
X0 n particles x0 [i] ~ p(x0)
particleFilters(Xt 1 ){
for i=1 to n xt
[i] ~ p(xt | xt 1
[i]) (prediction) wt
[i] = p(zt | xt
[i]) (importance weights) endfor for i=1 to n include xt
[i] in Xt with probability wt[i] (resampling)
}
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Weight samples: w = f / g
Importance Sampling
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filter
By Frank Dellaert
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Case Study: Track moving objects from Helicopter
1. Harris Corners
2. Optical Flow (with clustering)
3. Motion likelihood function
4. Particle Filter
5. Centroid Extraction
David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
More Particle Filter Tracking
David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Some Robotics Examples
Tracking Hands, People Mobile Robot localization People localization Car localization Mapping
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Examples Particle Filter
Siu Chi Chan McGill University
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Another Example
Mike Isard and Andrew Blake
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Tracking Fast moving Objects
K. Toyama, A.Blake
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filters (1)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filters (2)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particles = Robustness
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Particle Filters for Tracking Cars
With Anya Petrovskaya
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Tracking with KFs: Gaussians!
updateinitial estimate
x
y
x
y
prediction
x
y
measurement
x
y
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filters
),(~)( ΣμNxp
⎭⎬⎫
⎩⎨⎧ −Σ−−∝ − )()(
2
1exp)( 1 μμ xxxp T
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filters
prior
Measurementevidence
posterior
)()|()|( xpxzpzxp ∝
dxxpxxpxp )()|'()'( ∫=
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
A Quiz
prior
Measurementevidence
)()|()|( xpxzpzxp ∝
posterior?
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Gaussians
2
2)(
2
1
2
2
1)(
:),(~)(
σμ
σπ
σμ
−−
=x
exp
Nxp
-σ σ
μ
Univariate
)()(2
1
2/12/
1
)2(
1)(
:)(~)(
ìxÓìx
Óx
Óìx
−−− −
=t
ep
,Íp
dπ
μ
Multivariate
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
),(~),(~ 22
2
σμσμ
abaNYbaXY
NX+⇒
⎭⎬⎫
+=
Properties of Univariate Gaussians
⎟⎟⎠
⎞⎜⎜⎝
⎛
+++
+⋅⇒
⎭⎬⎫
−− 22
21
222
21
21
122
21
22
212222
2111 1
,~)()(),(~
),(~
σσμ
σσ
σμ
σσ
σ
σμ
σμNXpXp
NX
NX
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Measurement Update Derived( ) ( )
⎭⎬⎫
⎩⎨⎧ −
−−
−⋅=⋅⇒⎭⎬⎫
22
22
21
21
212222
2111
2
1
2
1expconst.)()(
),(~
),(~
σ
μ
σ
μ
σμ
σμ xxXpXp
NX
NX
( ) ( )) new(for 0
2
1
2
12
2
22
1
12
2
22
21
21 μ
σ
μ
σ
μ
σ
μ
σ
μ=
−+
−=
⎭⎬⎫
⎩⎨⎧ −
−−
−∂
∂ xxxx
x
( ) ( ) 0212
221 =−+− σμμσμμ
212
221
22
21 )( σμσμσσμ +=+
22
21
212
221
σσσμσμμ
++
=
( ) ( ) 22
212
2
22
21
21
2
2
2
1
2
1 −− +=⎭⎬⎫
⎩⎨⎧ −
−−
−∂
∂σσ
σ
μ
σ
μ xx
x
22
21
1−− +
=σσ
σ
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.
),(~),(~ TAABANY
BAXY
NXΣ+⇒
⎭⎬⎫
+=Σ
μμ
Properties Multivariate GaussiansEssentially the same as in the 1-D case, but with more general notation
( )112
1121
12112
12121
222
111 )(,)()(~)()(),(~
),(~ −−−−− Σ+ΣΣΣ+Σ+ΣΣ+Σ⋅⇒⎭⎬⎫
Σ
Σμμ
μ
μNXpXp
NX
NX
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Linear Kalman Filter
tttttt uBxAx ε++= −1
tttt xCz δ+=
Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation
with a measurement
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Components of a Kalman Filter
tε
Matrix (n n) that describes how the state evolves from t to t+1 without controls or noise.
tA
Matrix (n i) that describes how the control ut changes the state from t to t+1.
tB
Matrix (k n) that describes how to map the state xt to an observation zt.tC
tδ
Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Algorithm
1. Algorithm Kalman_filter( μt-1, Σt-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return μt, Σt
ttttt uBA += −1μμ
tTtttt RAA +Σ=Σ −1
tttt CKI Σ−=Σ )(
1)( −+ΣΣ= tTttt
Tttt QCCCK
)( tttttt CzK μμμ −+=
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Updates in 1D
old belief
measurement
belief
new belief
belief
mea
sure
men
t
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Updates in 1D
1)(with )(
)()( −+ΣΣ=
⎩⎨⎧
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKICzK
xbelμμμ
2,
2
2
22 with )1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
σσσ
σσμμμ
+=
⎩⎨⎧
−=−+=
=
old belief
new belief
mea
sure
men
t
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Updates in 1D
⎩⎨⎧
+Σ=Σ+=
=−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(μμ
⎩⎨⎧
+=+=
= −2
,2221)(
tactttt
tttttt a
ubaxbel
σσσ
μμ
old belief
new belief
mea
sure
men
t
new belief
newest belief
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Updates
belief
latest belief
belief
measurement
belief
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
The Prediction-Correction-Cycle
⎩⎨⎧
+Σ=Σ+=
=−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(μμ
⎩⎨⎧
+=+=
= −2
,2221)(
tactttt
tttttt a
ubaxbel
σσσ
μμ
Prediction
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
The Prediction-Correction-Cycle
1)(,)(
)()( −+ΣΣ=
⎩⎨⎧
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKICzK
xbelμμμ
2,
2
2
22 ,)1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
σσσ
σσμμμ
+=
⎩⎨⎧
−=−+=
=
Correction
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
The Prediction-Correction-Cycle
1)(,)(
)()( −+ΣΣ=
⎩⎨⎧
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKICzK
xbelμμμ
2,
2
2
22 ,)1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
σσσ
σσμμμ
+=
⎩⎨⎧
−=−+=
=
⎩⎨⎧
+Σ=Σ+=
=−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(μμ
⎩⎨⎧
+=+=
= −2
,2221)(
tactttt
tttttt a
ubaxbel
σσσ
μμ
Correction
Prediction
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Summary
Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)
Optimal for linear Gaussian systems!
Most robotics systems are nonlinear!
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Overview
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Let’s Apply KFs to Tracking Problem
Image 4Image 1 Image 2 Image 3
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter with 2-Dim Linear Model
Linear Change (Motion)
Linear Measurement Model
),0( QNDCxz ++=What is A, B, Q?
⎟⎟⎠
⎞⎜⎜⎝
⎛=
2
2
0
0
σ
σQ⎟⎟
⎠
⎞⎜⎜⎝
⎛=
0
0D⎟⎟
⎠
⎞⎜⎜⎝
⎛=
10
01C
⎟⎟⎠
⎞⎜⎜⎝
⎛=
0
0B⎟⎟
⎠
⎞⎜⎜⎝
⎛=
10
01A
),0(' RNBAxx ++=What is C, D, R?
⎟⎟⎠
⎞⎜⎜⎝
⎛=
2
2
0
0
ρ
ρR
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter Algorithm
1. Algorithm Kalman_filter( μt-1, Σt-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return μt, Σt
ttttt uBA += −1μμ
tTtttt RAA +Σ=Σ −1
1)( −+ΣΣ= tTttt
Tttt QCCCK
)( DCzK tttttt −−+= μμμtttt CKI Σ−=Σ )(
⎟⎟⎠
⎞⎜⎜⎝
⎛=
2
2
0
0
σ
σQ⎟⎟
⎠
⎞⎜⎜⎝
⎛=
0
0D⎟⎟
⎠
⎞⎜⎜⎝
⎛=
10
01C
⎟⎟⎠
⎞⎜⎜⎝
⎛=
0
0B⎟⎟
⎠
⎞⎜⎜⎝
⎛=
10
01A ⎟⎟
⎠
⎞⎜⎜⎝
⎛=
2
2
0
0
ρ
ρR
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter in Detail
Measurements Change Prediction Measurement Update
),0( RNxz +=
),0(' QNxx +=
( ))'('''''
'''1
111
μμμ −Σ+=
+Σ=Σ−
−−−
zR
R
μμ =+Σ=Σ '' Q
updateinitial position
x
y
x
y
prediction
x
y
measurement
x
y
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Can We Do Better?
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman, Better!
initial position prediction measurement
x&
x
x&
x
x&
x
next prediciton
x&
x
update
x&
x
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
We Can Estimate Velocity!
past measurements
prediction
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Kalman Filter For 2D Tracking
Linear Measurement model (now with 4 state variables)
Linear Change
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛
=
2
2
000
000
0000
0000
q
⎟⎟⎠
⎞⎜⎜⎝
⎛=
2
2
0
0
r
rR),0(
0010
0001RN
y
x
y
x
y
x
obs
obs +
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛
⎟⎟⎠
⎞⎜⎜⎝
⎛=⎟⎟
⎠
⎞⎜⎜⎝
⎛
&
&
),0(
1000
0100
010
001
'
'
'
'
QN
y
x
y
x
t
t
y
x
y
x
+
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛Δ
Δ
=
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛
&
&
&
&
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Putting It Together Again
Measurements
Change
Prediction
Measurement Update
),0(0010
0001RNxz +⎟⎟
⎠
⎞⎜⎜⎝
⎛=
rr
),0(
1000
0100
010
001
' QNxt
t
x +
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛Δ
Δ
=rr
( ))(''
'1
111
μμμ AzRA
ARAT
T
−Σ+=
+Σ=Σ−
−−−
μμ C
CQC T
=
+Σ=Σ
'
'
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Summary Kalman Filter Estimates state of a system
– Position– Velocity– Many other continuous state variables possible
KF maintains– Mean vector for the state– Covariance matrix of state uncertainty
Implements– Time update = prediction– Measurement update
Standard Kalman filter is linear-Gaussian– Linear system dynamics, linear sensor model– Additive Gaussian noise (independent)– Nonlinear extensions: extended KF, unscented KF: linearize
More info: – CS226– Probabilistic Robotics (Thrun/Burgard/Fox, MIT Press)
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Summary Kalman Filter Estimates state of a system
– Position– Velocity– Many other continuous state variables possible
KF maintains– Mean vector for the state– Covariance matrix of state uncertainty
Implements– Time update = prediction– Measurement update
Standard Kalman filter is linear-Gaussian– Linear system dynamics, linear sensor model– Additive Gaussian noise (independent)– Nonlinear extensions: extended KF, unscented KF: linearize
More info: – CS226– Probabilistic Robotics (Thrun/Burgard/Fox, MIT Press)
Particle
and discrete
set of particles(example states)
= predictive sampling= resampling, importance weights
fully nonlinear
easy to implement
Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007
Summary
The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters