Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens 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 Image 1 Image 2 Image 3 Image 4 Can we estimate the position of the object? Can we estimate its velocity? Can we predict future positions? 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 Kalman Filter Uncented Kalman Filter Particle 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 Kalman Filter Tracking Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Particle Filter Tracking Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Mixture of KF / PF (Unscented PF) 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 Example of Bayesian Inference p(staircase) Slow = 0.28 Down! Cost Sensor model model cost(fast p(imagewalk | staircase) | staircase) = 0.7= $1,000 cost(fast p(imagewalk | no staircase) | no staircase) = 0.2= $0 cost(slow+sense) = $1 Environment prior p(staircase) = 0.1 Decision BayesianTheory inference p(staircase walk)] | image) = $1,000 • 0.28 = $280 E[cost(fast p(image | staircasse) p(staircase) E[cost(slow+sense)] = $1 = 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 ? 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 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 = state t = time z = observation u = action η = constant Bayes Filters Bel ( xt ) = = p( xt | z0t , u0t ) p( xt | zt , ut − 1 , zt − 1 , , u0 ) Bayes = η p( zt | xt , ut − 1 , zt − 1 , , z0 ) p( xt | ut − 1 , zt − 1 , , z0 ) Markov = η p( zt | xt ) p( xt | ut − 1 , zt − 1 , , z0 ) = η p( zt | xt ) ∫ p( xt | xt − 1 , ut − 1 , , z0 ) p( xt − 1 | ut − 1 , , z0 ) dxt − 1 Markov = η p ( zt | xt ) ∫ p( x t | xt −1 , ut −1 ) p ( xt −1 | zt −1 , ut − 2 , u0 ) dxt −1 = η p( zt | xt ) ∫ p( xt | xt − 1 , ut − 1 ) Bel ( xt − 1 ) dxt − 1 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Bayes Filters x t z u = state = time = observation = action Bel ( xt ) = η p( zt | xt ) ∫ p( xt | xt − 1 , ut − 1 ) Bel ( xt − 1 ) dxt − 1 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 Kalman Filter Uncented Kalman Filter Particle 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 p(x ) xt set of n particles Xt p( xt ∈ X t ) ≈ p( xt | z1...t ) Sebastian Thrun and Jana Košecká (equality for n ↑ ∞ ) CS223B Computer Vision, Winter 2007 Particle Filter Explained Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Basic Particle Filter Algorithm 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) } p( xt | z1...t , u1...t ) = η p( zt | xt ) ∫ p( xt | ut , xt − 1 ) p( xt − 1 | z1...t − 1 , u1...t − 1 ) dxt − 1 p( xt ∈ X t ) ≈ p( xt | z1...t , u1...t ) Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Importance Sampling Weight samples: w = f / g 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. 2. 3. 4. 5. Harris Corners Optical Flow (with clustering) Motion likelihood function Particle Filter Centroid Extraction David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 1. Harris Corner Extraction David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 2. Optical Flow + Motion Detection David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 3. Motion Likelihood Function David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 4. Particle Filters David Stavens, Andrew Lookingbill, David Lieb, CS223b Winter 2004 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 5. Extract Centroid 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 Sebastian Thrun and Jana Košecká K. Toyama, A.Blake CS223B Computer Vision, Winter 2007 Particle Filters: Illustration With: Wolfram Burgard, Dieter Fox, Frank Dellaert p( xt ∈ X t ) ≈ p( xt | z1...t , u1...t ) 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 Tracking People from Moving Platform • robot location (particles) • people location (particles) • laser measurements (wall) Sebastian Thrun and Jana Košecká With Michael Montemerlo CS223B Computer Vision, Winter 2007 Tracking People from Moving Platform • robot location (particles) • people location (particles) • laser measurements (wall) Sebastian Thrun and Jana Košecká With Michael Montemerlo CS223B Computer Vision, Winter 2007 Particle Filters for Tracking Cars With Anya Petrovskaya Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Mapping Environments (SFM) With Dirk Haehnel 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! initial estimte prediction y y x Sebastian Thrun and Jana Košecká measurement update y x y x CS223B Computer Vision, Winter 2007 x Kalman Filters p( x ) ~ N ( µ , Σ ) 1 T −1 p( x ) ∝ exp − ( x − µ ) Σ ( x − µ ) 2 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Kalman Filters p( x | z ) ∝ p( z | x ) p( x ) posterior Measurement evidence prior p( x ' ) = ∫ p( x ' | x ) p( x ) dx Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 A Quiz p( x | z ) ∝ p( z | x ) p( x ) Measurement evidence prior posterior? Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Gaussians Univariate µ p( x) ~ N ( µ , σ 2 ) : p( x) = 1 e 2π σ − 1 ( x −µ )2 2 σ2 σ -σ Multivariate p (x) ~ Ν (μ,Σ) : µ p ( x) = 1 (2π ) d /2 Σ 1/ 2 e 1 − ( x −μ ) t Σ −1 ( x −μ ) 2 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Properties of Univariate Gaussians X ~ N ( µ , σ 2 ) 2 2 ⇒ Y ~ N ( a µ + b , a σ ) Y = aX + b 2 2 σ 22 X 1 ~ N ( µ1 , σ 1 ) σ 1 1 ⇒ p( X 1 ) ⋅ p( X 2 ) ~ N 2 µ1 + 2 µ2 , 2 2 −2 −2 2 σ1 + σ 2 σ1 + σ 2 X 2 ~ N ( µ 2 , σ 2 ) σ1 + σ 2 Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Measurement Update Derived 2 1 ( x − µ1 ) 2 1 ( x − µ 2 ) 2 X 1 ~ N ( µ1 , σ 1 ) ⇒ p( X 1 ) ⋅ p ( X 2 ) = const. ⋅ exp− − 2 2 2 2 2 σ σ X 2 ~ N ( µ 2 , σ 2 ) 1 2 2 2 ∂ 1 ( x − µ1 ) 1 ( x − µ2 ) − − = 2 2 ∂x 2 σ 1 2 σ2 x − µ1 x − µ 2 + 2 2 σ1 σ2 ( µ − µ1 )σ 2 2 + ( µ − µ 2 )σ 12 = 0 µ (σ 1 + σ 2 ) = µ1 σ 2 + µ 2σ 1 2 µ ∂2 ∂x 2 2 2 µ σ + µ 2σ 1 = 1 22 2 σ1 + σ 2 2 = 0 (for new µ ) 2 2 1 ( x − µ1 ) 2 1 ( x − µ 2 ) 2 −2 −2 − = σ + σ − 1 2 2 2 2 2 σ σ 1 2 σ = Sebastian Thrun and Jana Košecká 1 −2 −2 σ1 +σ 2 CS223B Computer Vision, Winter 2007 Properties Multivariate Gaussians Essentially the same as in the 1-D case, but with more general notation X ~ N ( µ , Σ) T ⇒ Y ~ N ( A µ + B , A Σ A ) Y = AX + B X 1 ~ N ( µ1 , Σ1 ) −1 −1 −1 −1 −1 ⇒ p ( X 1 ) ⋅ p ( X 2 ) ~ N (Σ1 + Σ 2 ) Σ 2 µ1 + (Σ1 + Σ 2 ) Σ1µ 2 , (Σ1 + Σ 2 ) X 2 ~ N ( µ 2 , Σ 2 ) ( We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations. Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 ) Linear Kalman Filter Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation xt = At xt −1 + Bt ut + ε t with a measurement zt = Ct xt + δ t Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Components of a Kalman Filter At Matrix (n × n) that describes how the state evolves from t to t+1 without controls or noise. Bt Matrix (n × i) that describes how the control ut changes the state from t to t+1. Ct Matrix (k × n) that describes how to map the state xt to an observation zt. εt Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively. δt 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. µ t = At µt −1 + Bt ut 4. Σ t = At Σ t −1 AtT + Rt 5. Correction: 6. 7. K t = Σ t CtT (Ct Σ t CtT + Qt ) −1 µ t = µ t + K t ( z t − Ct µ t ) 8. Σ t = ( I − K t Ct ) Σ t 9. Return µt, Σt Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Kalman Filter Updates in 1D measurement belief belief measu remen t new belief old belief Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Kalman Filter Updates in 1D µ t = µ t + K t ( zt − µ t ) bel ( xt ) = 2 2 σ = ( 1 − K ) σ t t t µ t = µ t + K t ( z t − Ct µ t ) bel ( xt ) = Σ t = ( I − K t Ct ) Σ t σ t2 with K t = 2 2 σ t + σ obs ,t with K t = Σ t CtT (Ct Σ t CtT + Qt ) −1 measu remen t new belief old belief Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Kalman Filter Updates in 1D µt = at µt −1 + bt ut bel ( xt ) = 2 2 2 2 σ = a σ + σ t t act ,t t µt = At µt −1 + Bt ut bel ( xt ) = T Σ = A Σ A t t t −1 t + Rt remen t new belief new belief measu newest belief old belief Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Kalman Filter Updates belief latest belief measurement belief Sebastian Thrun and Jana Košecká belief CS223B Computer Vision, Winter 2007 The Prediction-Correction-Cycle Prediction µt =at µt −1 +bt ut bel ( xt ) = 2 2 2 2 σt =at σt +σact ,t µ = At µt −1 +Bt ut bel ( xt ) = t T Σt = At Σt −1 At +Rt Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 The Prediction-Correction-Cycle µ = µ + K t ( zt − µt ) σt2 bel ( xt ) = t 2 t , K = t 2 2 σt2 +σobs σt = (1 − K t )σt ,t µ = µt + K t ( zt −Ct µt ) bel ( xt ) = t , K t = Σt CtT (Ct Σt CtT +Qt ) −1 Σ = ( I − K C ) Σ t t t t Correction Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 The Prediction-Correction-Cycle Prediction µ = µ + K t ( zt − µt ) σt2 bel ( xt ) = t 2 t , K = t 2 2 σt2 +σobs σt = (1 − K t )σt ,t µt =at µt −1 +bt ut bel ( xt ) = 2 2 2 2 σt =at σt +σact ,t µ = µt + K t ( zt −Ct µt ) bel ( xt ) = t , K t = Σt CtT (Ct Σt CtT +Qt ) −1 Σ = ( I − K C ) Σ t t t t µ = At µt −1 +Bt ut bel ( xt ) = t T Σt = At Σt −1 At +Rt Correction 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 1 Image 2 Sebastian Thrun and Jana Košecká Image 3 Image 4 CS223B Computer Vision, Winter 2007 Kalman Filter with 2-Dim Linear Model Linear Change (Motion) x' = Ax + B + N (0, R ) 1 0 A = 0 1 t is a h W 0 B = 0 R? , D C, ρ2 R = 0 0 2 ρ Linear Measurement Model z = Cx + D + N (0, Q) 1 0 C = 0 1 Sebastian Thrun and Jana Košecká s i t a Wh ? Q , A, B σ 2 0 0 D = Q = 2 0 0 σ CS223B Computer Vision, Winter 2007 Kalman Filter Algorithm 1. Algorithm Kalman_filter( µt-1, Σt-1, ut, zt): 2. Prediction: 4. µ t = At µt −1 + Bt ut Σ t = At Σ t −1 AtT + Rt 5. Correction: 3. 6. 7. 8. 9. 1 0 A = 0 1 K t = Σ t CtT (Ct Σ t CtT + Qt ) −1 µ t = µ t + K t ( z t − Ct µ t − D ) Σ t = ( I − K t Ct ) Σ t 1 0 C = Return µt, Σt 0 1 Sebastian Thrun and Jana Košecká 0 B = 0 ρ2 R = 0 0 2 ρ σ 2 0 0 D = Q = 2 0 0 σ CS223B Computer Vision, Winter 2007 Kalman Filter in Detail Measurements z = x + N (0, R ) x ' = x + N (0, Q ) Change Σ' = Σ+Q µ' = µ Prediction −1 −1 − 1 Measurement Update Σ ' ' = ( Σ ' + R ) µ ' ' = µ '+ Σ ' ' R − 1 ( z − µ ' ) initial position prediction y measurement y x Sebastian Thrun and Jana Košecká update y x y x CS223B Computer Vision, Winter 2007 x Can We Do Better? Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Kalman, Better! initial position prediction x measurement x update x x x x Sebastian Thrun and Jana Košecká x ne xt x pr e dic it on x x CS223B Computer Vision, Winter 2007 We Can Estimate Velocity! prediction past measurements Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007 Kalman Filter For 2D Tracking Linear Measurement model (now with 4 state variables) xobs 1 0 0 0 = yobs 0 1 0 0 x y x + N (0, R ) y r2 R = 0 0 2 r Linear Change x' 1 y' 0 x ' = 0 y ' 0 0 ∆t 1 0 0 0 1 0 0 ∆t 0 1 x y x + N (0, Q) y Sebastian Thrun and Jana Košecká 0 0 Q= 0 0 0 0 0 0 0 q2 0 0 0 0 0 2 q CS223B Computer Vision, Winter 2007 Putting It Together Again Change Prediction 1 0 0 0 z = x + N (0, R ) 0 1 0 0 Measurements 1 0 x' = 0 0 Σ ' = Σ + CQC µ ' = Cµ Measurement Update Sebastian Thrun and Jana Košecká 0 ∆t 0 1 0 ∆t x + N (0, Q ) 0 1 0 0 0 1 T Σ ' = ( Σ + A R A) −1 T −1 −1 µ ' = µ + Σ ' AT R − 1 ( z − Aµ ) 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 Particle Summary Kalman Filter Estimates state of a system – Position and discrete – Velocity – Many other continuous state variables possible KF maintains – Mean vector for the state – Covariance matrix of state uncertainty set of particles (example states) Implements – Time update = prediction = predictive sampling – Measurement update = resampling, importance weights Standard Kalman filter is linear-Gaussian – Linear system dynamics, linear sensor model fully nonlinear – Additive Gaussian noise (independent) – Nonlinear extensions: extended KF, unscented KF: linearize More info: easy t o – CS226 – Probabilistic Robotics (Thrun/Burgard/Fox, MIT Press) Sebastian Thrun and Jana Košecká implem ent CS223B Computer Vision, Winter 2007 Summary The Tracking Problem Bayes Filters Particle Filters Kalman Filters Using Kalman Filters Sebastian Thrun and Jana Košecká CS223B Computer Vision, Winter 2007