stanford cs223b computer vision, winter 2007 lecture 12 tracking motion professors sebastian thrun...

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


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


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


Methods

Bayes Filter

Particle Filter

Uncented Kalman Filter

Kalman Filter

Extended Kalman Filter


Further Reading…


Example: Moving Object


Overview



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

=


Bayes Filter Definition

Environment state xt

Measurement zt

Can we calculate p(xt | z1, z2, …, zt) ?


Bayes Filters Illustrated

Localization problem


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)


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η


Bayes Filtersx = statet = timez = observationu = action

1111 )(),|()|()( −−−−∫= tttttttt dxxBeluxxpxzpxBel


Bayes Filters Illustrated


Bayes Filters

Initial Estimate of State Iterate

– Receive measurement, update your belief (uncertainty shrinks)

– Predict, update your belief (uncertainty grows)


Methods

Bayes Filter

Particle Filter

Uncented Kalman Filter

Kalman Filter

Extended Kalman Filter


Overview



Particle Filters: Basic Idea

)(xp

tx

)|()( ...1 tttt zxpXxp ≈∈ (equality for )∞↑n

set of n particles Xt


Particle Filter Explained


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)

}


Weight samples: w = f / g

Importance Sampling


Particle Filter

By Frank Dellaert


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


More Particle Filter Tracking

David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004


Some Robotics Examples

Tracking Hands, People Mobile Robot localization People localization Car localization Mapping


Examples Particle Filter

Siu Chi Chan McGill University


Another Example

Mike Isard and Andrew Blake


Tracking Fast moving Objects

K. Toyama, A.Blake


Particle Filters (1)


Particle Filters (2)


Particles = Robustness


Particle Filters for Tracking Cars

With Anya Petrovskaya


Overview



Tracking with KFs: Gaussians!

updateinitial estimate

x

y

x

y

prediction

x

y

measurement

x

y


Kalman Filters

),(~)( ΣμNxp

⎭⎬⎫

⎩⎨⎧ −Σ−−∝ − )()(

2

1exp)( 1 μμ xxxp T


Kalman Filters

prior

Measurementevidence

posterior

)()|()|( xpxzpzxp ∝

dxxpxxpxp )()|'()'( ∫=


A Quiz

prior

Measurementevidence

)()|()|( xpxzpzxp ∝

posterior?


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


),(~),(~ 22

2

σμσμ

abaNYbaXY

NX+⇒

⎭⎬⎫

+=

Properties of Univariate Gaussians

⎟⎟⎠

⎞⎜⎜⎝

⎛

+++

+⋅⇒

⎭⎬⎫

−− 22

21

222

21

21

122

21

22

212222

2111 1

,~)()(),(~

),(~

σσμ

σσ

σμ

σσ

σ

σμ

σμNXpXp

NX

NX


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−− +

=σσ

σ


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


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


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.


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 μμμ −+=


Kalman Filter Updates in 1D

old belief

measurement

belief

new belief

belief

mea

sure

men

t



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



⎩⎨⎧

+Σ=Σ+=

=−

−

tTtttt

tttttt RAA

uBAxbel

1

1)(μμ

⎩⎨⎧

+=+=

= −2

,2221)(

tactttt

tttttt a

ubaxbel

σσσ

μμ

old belief

new belief

mea

sure

men

t

new belief

newest belief


Kalman Filter Updates

belief

latest belief

belief

measurement

belief


The Prediction-Correction-Cycle

⎩⎨⎧

+Σ=Σ+=

=−

−

tTtttt

tttttt RAA

uBAxbel

1

1)(μμ

⎩⎨⎧

+=+=

= −2

,2221)(

tactttt

tttttt a

ubaxbel

σσσ

μμ

Prediction



1)(,)(

)()( −+ΣΣ=

⎩⎨⎧

Σ−=Σ−+=

= tTttt

Tttt

tttt

ttttttt QCCCK

CKICzK

xbelμμμ

2,

2

2

22 ,)1(

)()(

tobst

tt

ttt

tttttt K

K

zKxbel

σσσ

σσμμμ

+=

⎩⎨⎧

−=−+=

=

Correction



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


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!


Overview



Let’s Apply KFs to Tracking Problem

Image 4Image 1 Image 2 Image 3


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


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


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


Can We Do Better?


Kalman, Better!

initial position prediction measurement

x&

x

x&

x

x&

x

next prediciton

x&

x

update

x&

x


We Can Estimate Velocity!

past measurements

prediction


Kalman Filter For 2D Tracking

Linear Measurement model (now with 4 state variables)

Linear Change

⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛

=

2

2

000

000

0000

0000

q

qQ

⎟⎟⎠

⎞⎜⎜⎝

⎛=

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

+

⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛

⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛Δ

Δ

=

⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛

&

&

&

&


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

=

+Σ=Σ

'

'


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)


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


Summary


stanford cs223b computer vision, winter 2007 lecture 12 tracking motion professors sebastian thrun...

Documents