Monte Carlo Hidden Markov Models - People | MIT...

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

Kalman Filter Tracking

Particle Filter Tracking

Mixture of KF / PF (Unscented PF)

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

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 =

),,,|(),,,,|( 011011 zzuxpzzuxzp ttttttt −−−−= η

),,,|()|( 011 zzuxpxzp ttttt −−= η

1011011 ),,|(),,,|()|( −−−−−∫= tttttttt dxzuxpzuxxpxzp η

1111 )(),|()|( −−−−∫= ttttttt dxxBeluxxpxzpη

),,,,|( 011 uzuzxp tttt −−=

Markov

1021111 ),,|(),|()|( −−−−−−∫= ttttttttt dxuuzxpuxxpxzp η

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

)|()( ...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

1. Harris Corner Extraction

2. Optical Flow + Motion Detection

3. Motion Likelihood Function

4. Particle Filters

5. Extract Centroid

More Particle Filter Tracking

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: IllustrationWith: Wolfram Burgard, Dieter Fox, Frank Dellaert

),|()( ...1...1 ttttt uzxpXxp ≈∈

Particle Filters (1)

Particle Filters (2)

Particles = Robustness

Tracking People from Moving Platform

• robot location (particles)• people location (particles)• laser measurements (wall) With Michael Montemerlo

Tracking People from Moving Platform

With Michael Montemerlo

• robot location (particles)• people location (particles)• laser measurements (wall)

Particle Filters for Tracking Cars

With Anya Petrovskaya

Mapping Environments (SFM)

With Dirk Haehnel

Overview

Tracking with KFs: Gaussians!

updateinitial estimte

prediction

measurement

Kalman Filters

),(~)( ΣµNxp

−Σ−−∝ − )()(

1exp)( 1 µµ xxxp T

Kalman Filters

Measurementevidence

posterior

)()|()|( xpxzpzxp ∝

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

A Quiz

Measurementevidence

)()|()|( xpxzpzxp ∝

posterior?

Gaussians

:),(~)(

−−=

-σ σ

µUnivariate

:)(~)(

μxΣμx

−−− −

Multivariate

),(~),(~ 22

σµσµabaNY

NX+⇒

Properties of Univariate Gaussians

⋅⇒

−− 22

212222

2111 1

,~)()(),(~

σσµ

σσσµ

σσσ

σµσµ

NXpXpNX

Measurement Update Derived( ) ( )

−−−−⋅=⋅⇒

212222

1expconst.)()(

σµσµ xx

XpXpNX

( ) ( )) new(for 0

σµ =−+−=

−−−−

∂∂ xxxx

( ) ( ) 0212

221 =−+− σµµσµµ

21 )( σµσµσσµ +=+

σσσµσµµ

( ) ( ) 22

1 −− +=

−−−−

∂∂ σσ

σµ xx

1−− +

We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.

),(~),(~ TAABANY

NXΣ+⇒

Properties Multivariate GaussiansEssentially the same as in the 1-D case, but with more general notation

( )112

111 )(,)()(~)()(),(~

),(~ −−−−− Σ+ΣΣΣ+Σ+ΣΣ+Σ⋅⇒

µµµµ

NXpXpNX

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

Matrix (n × n) that describes how the state evolves from t to t+1 without controls or noise.

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

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

1)(with )(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

ttttttt QCCCK

CzKxbel

µµµ

22 with )1(

tttttt K

zKxbel

σσσ

σσµµµ

−=−+=

old belief

new belief

+Σ=Σ+=

tTtttt

tttttt RAA

uBAxbel

1)(µµ

= −2

,2221)(

tactttt

tttttt a

ubaxbel

σσσµµ

old belief

new belief

newest belief

Kalman Filter Updates

belief

latest belief

belief

measurement

belief

The Prediction-Correction-Cycle

+Σ=Σ+=

tTtttt

tttttt RAA

uBAxbel

1)(µµ

= −2

,2221)(

tactttt

tttttt a

ubaxbel σσσ

Prediction

1)(,)(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

ttttttt QCCCK

CzKxbel

µµµ

22 ,)1(

tttttt K

zKxbel

σσσ

σσµµµ

−=−+=

Correction

1)(,)(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

ttttttt QCCCK

CzKxbel

µµµ

22 ,)1(

tttttt K

zKxbel

σσσ

σσµµµ

−=−+=

+Σ=Σ+=

tTtttt

tttttt RAA

uBAxbel

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?

),0(' RNBAxx ++=What is C, D, 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µµt

Ttttt RAA +Σ=Σ −1

1)( −+ΣΣ= tTttt

Tttt QCCCK

)( DCzK tttttt −−+= µµµtttt CKI Σ−=Σ )(

Kalman Filter in Detail

Measurements Change Prediction Measurement Update

),0( RNxz +=

),0(' QNxx +=

( ))'('''''

µµµ −Σ+=+Σ=Σ

−−−

µµ =+Σ=Σ '' Q

updateinitial position

prediction

measurement

Can We Do Better?

Kalman, Better!

initial position prediction measurement

update

We Can Estimate Velocity!

past measurements

prediction

Kalman Filter For 2D Tracking

Linear Measurement model (now with 4 state variables)

Linear Change

rR),0(

0001RN

Putting It Together Again

Measurements

Change

Prediction

Measurement Update

),0(0010

0001RNxz +

' QNxt

( ))(''

µµµ AzRA

−Σ+=+Σ=Σ

−−−

µµ C

=+Σ=Σ

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

Monte Carlo Hidden Markov Models - People | MIT...

Documents

Sebastian Thrun and Jana Kosecha CS223B Computer Vision, Winter 2007 Stanford CS223B Computer Vision, Winter 2007 Lecture 4 Camera Calibration Professors

LQR-RRT*: Optimal Sampling-Based Motion Planning with ...people.csail.mit.edu/rplatt/papers/icra2012_perez_submitted.pdf · the LQR control policy to perform the node extensions

Zach Pincus CS223b Progress Report 0. Abstract 1. Introduction …robots.stanford.edu/cs223b04/inter2/P20.pdf · 2004-02-12 · Zach Pincus CS223b Progress Report 0. Abstract Modern

Stanford CS223B Computer Vision, Winter 2005 Lecture 3 Advanced Features

Correlation of Boolean Functions - MIT CSAILpeople.csail.mit.edu/.../Coded_Aperture/Correlation_Boolean_Functio… · of Boolean Functions Desired Cryptographic Properties of Boolean

Introduction to OpenCV - Stanford Artificial …ai.stanford.edu/~dstavens/cs223b/stavens_opencv_optical...Introduction to OpenCV David Stavens Stanford Artificial Intelligence Lab

Historical, Observed, and Modeled Wildfire Severity in Montane …public.gettysburg.edu/~rplatt/Sherriff 2014 PlosOne... · 2014-09-24 · Historical, Observed, and Modeled Wildfire

Camera Calibration Sebastian Thrun, Gary Bradski, Daniel Russakoff Stanford CS223B Computer Vision (with material from

Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens

Stanford CS223B Computer Vision, Winter 2005 Final Project Presentations + Papers Sebastian Thrun, Stanford Rick Szeliski, Microsoft Hendrik Dahlkamp and

CS223B Homework 1 Results

LEARNING AND GENERALIZING CONTROL-BASED ...people.csail.mit.edu/rplatt/papers/platt_thesis.pdfLEARNING AND GENERALIZING CONTROL-BASED GRASPING AND MANIPULATION SKILLS A Thesis Presented

Lecture6 Clustering and Seg p2 Cs223b

Image Stitching on a Camera Cell Phone T. Brow, S. Elayaperumal, A. Rajgarhia CS223B Winter 2008

Stereo Sebastian Thrun, Gary Bradski, Daniel Russakoff Stanford CS223B Computer Vision (with slides by James Rehg and

Streaming Variational Bayes - MIT CSAILpeople.csail.mit.edu/tbroderick/files/broderick_2014_streaming_variational.pdfOverview • Big Data inference generally non-Bayesian • Why

Sebastian Thrun & Jana Kosecka CS223B Computer Vision, Winter 2007 Stanford CS223B Computer Vision, Winter 2007 Lecture 2b Software for Computer Vision

Motion estimation Introduction to Computer Vision CS223B, Winter 2005 Richard Szeliski

CS223b, Jana Kosecka Rigid Body Motion and Image Formation

Helicopter Tracking Project Final Reportrobots.stanford.edu/cs223b04/project_reports/P1.pdf1 CS223B Project Final Report – March 12, 2004 Autonomous Helicopter Tracking and Localization