Goal:  Fast  and  Robust  Velocity  Es5ma5on  

P1 P2

P3 P4

Our  Approach:  Alignment  Probability  

●  Spatial Distance ●  Color Distance (if available) ●  Probability of Occlusion

Annealed  Dynamic  Histograms  

Combining 3D Shape, Color, and Motion for Robust Anytime Tracking  

David  Held,  Jesse  Levinson,  Sebas5an  Thrun,  and  Silvio  Savarese  

p(xt | z1 . . . zt) = η p(zt | xt, zt−1)p(xt | z1 . . . zt−1)

Goal:  Fast  and  Robust  Velocity  Es5ma5on  

Combining 3D Shape, Color, and Motion for Robust Anytime Tracking  

David  Held,  Jesse  Levinson,  Sebas5an  Thrun,  and  Silvio  Savarese  

p(xt | z1 . . . zt) = η p(zt | xt, zt−1)p(xt | z1 . . . zt−1)Baseline:  Centroid  Kalman  Filter  

Local Search Poor Local Optimum!

t+1 t

Baseline:  ICP  

Annealed  Dynamic  Histograms  

Goal:  Fast  and  Robust  Velocity  Es5ma5on  

P1 P2

P3 P4

Our  Approach:  Alignment  Probability  

●  Spatial Distance ●  Color Distance (if available) ●  Probability of Occlusion

Combining 3D Shape, Color, and Motion for Robust Anytime Tracking  

David  Held,  Jesse  Levinson,  Sebas5an  Thrun,  and  Silvio  Savarese  

p(xt | z1 . . . zt) = η p(zt | xt, zt−1)p(xt | z1 . . . zt−1)

Annealed  Dynamic  Histograms  

Mo#va#on  Quickly and robustly estimate the speed of nearby objects

Laser  Data  

Camera  Images  System  

Laser  Data  

Camera  Images  System  

Previous  Work  (Teichman,  et  al)  

System  

Laser  Data  

Camera  Images  

This  Work  

Velocity Estimation

Previous  Work  (Teichman,  et  al)  

Velocity  Es#ma#on  

t

Velocity  Es#ma#on  

t+1 t

Velocity  Es#ma#on  

t+1 t

Velocity  Es#ma#on  

t+1 t

Velocity  Es#ma#on  

t+1 t

ICP  Baseline  

ICP  Baseline  

ICP  Baseline  

ICP  Baseline  

Local Search Poor Local Optimum!

ICP  Baseline  

Tracking  Probability  

Velocity  Es#ma#on  

t

Velocity  Es#ma#on  

t+1 t

Velocity  Es#ma#on  

t+1 t

Velocity  Es#ma#on  

t+1 t

Velocity  Es#ma#on  

t+1 t

Velocity  Es#ma#on  

t+1 t

Velocity  Es#ma#on  

t+1 t

Xt

Velocity  Es#ma#on  

t+1 t

Xt

Measurement Model

Motion Model

Tracking  Probability  

Measurement Model

Motion Model

Tracking  Probability  

Constant velocity Kalman filter

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

Measurement Model

Tracking  Probability  

Motion Model

k

Measurement Model

Tracking  Probability  

Motion Model

Sensor noise

Sensor resolution

k

Delta Color Value

Pro

babi

lity

Color  Probability  

Including  Color  

Including  Color  

η!

"

zi∈ztexp(−1

2(zi − z̄j)TΣ−1(zi − z̄j)) + k

#

Including  Color  

η!

"

zi∈ztexp(−1

2(zi − z̄j)TΣ−1(zi − z̄j)) + k

#

Including  Color  

Including  Color  

Including  Color  

Delta Color Value

Pro

babi

lity

Including  Color  

Delta Color Value

Pro

babi

lity

Including  Color  

Delta Color Value

Pro

babi

lity

Probabilis#c  Framework  

3D Shape Color

Tracking

Motion History

Tracking  Probability  P1 P2

P3 P4

Tracking  Probability  vy

vx

??

? ?

?

Tracking  Probability  vy

vx

Dynamic  Decomposi#on  vy

vx

Dynamic  Decomposi#on  vy

vx

Dynamic  Decomposi#on  vy

vx

Dynamic  Decomposi#on  vy

vx

Derived from minimizing KL-divergence between approximate distribution and true posterior

Annealing  

Inflate the measurement model

Annealing  

Inflate the measurement model

Annealing  

Inflate the measurement model

Algorithm  1.  For  each  hypothesis  

A.  Compute  the  probability  of  the  alignment  

Measurement Model

Motion Model

Algorithm  1.  For  each  hypothesis  

A.  Compute  the  probability  of  the  alignment  

B.  Finely  sample  high  probability  regions  

Measurement Model

Motion Model

Algorithm  1.  For  each  hypothesis  

A.  Compute  the  probability  of  the  alignment  

B.  Finely  sample  high  probability  regions  

C.  Go  to  step  1  to  compute  the  probability  of  new  hypotheses  

Measurement Model

Motion Model

Annealing  

More time More accurate

Any#me  Tracker  

0 0.5 1 1.5 2 2.50

0.1

0.2

0.3

0.4

0.5

0.6

Mean runtime (ms)

RM

S er

ror (

m/s

)

Any#me  Tracker  

Choose runtime based on: Total runtime requirements Importance of tracked object ...

0 0.5 1 1.5 2 2.50

0.1

0.2

0.3

0.4

0.5

0.6

Mean runtime (ms)

RM

S er

ror (

m/s

)

Comparisons  

0 0.5 1 1.5 2 2.50

0.1

0.2

0.3

0.4

0.5

0.6

Mean runtime (ms)

RM

S er

ror (

m/s

)

0 0.5 1 1.5 2 2.50

0.1

0.2

0.3

0.4

0.5

0.6

Mean runtime (ms)

RMS

erro

r (m

/s)

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)

Kalman FilterICPKalman ICP with Centroid InitKalman ICP with Kalman InitAnnealed Dynamic Histograms

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2

RM

S er

ror (

m/s

)

Mean runtime (ms)

Kalman FilterICPKalman ICP with Centroid InitKalman ICP with Kalman InitAnnealed Dynamic Histograms

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)

Comparisons  

0 0.5 1 1.5 2 2.50

0.1

0.2

0.3

0.4

0.5

0.6

Mean runtime (ms)

RM

S er

ror (

m/s

)

0 0.5 1 1.5 2 2.50

0.1

0.2

0.3

0.4

0.5

0.6

Mean runtime (ms)

RMS

erro

r (m

/s)

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)

Kalman FilterICPKalman ICP with Centroid InitKalman ICP with Kalman InitAnnealed Dynamic Histograms

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2

RM

S er

ror (

m/s

)

Mean runtime (ms)

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2

RM

S er

ror (

m/s

)

Mean runtime (ms)

Kalman FilterICPKalman ICP with Centroid InitKalman ICP with Kalman InitAnnealed Dynamic Histograms

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)

Comparisons  

0 0.5 1 1.5 2 2.50

0.1

0.2

0.3

0.4

0.5

0.6

Mean runtime (ms)

RM

S er

ror (

m/s

)

0 0.5 1 1.5 2 2.50

0.1

0.2

0.3

0.4

0.5

0.6

Mean runtime (ms)

RMS

erro

r (m

/s)

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)

Kalman FilterICPKalman ICP with Centroid InitKalman ICP with Kalman InitAnnealed Dynamic Histograms

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2

RM

S er

ror (

m/s

)

Mean runtime (ms)

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

1.2R

MS

erro

r (m

/s)

Mean runtime (ms)

Kalman Filter

Kalman Filter

ADH Tracker (Ours)

Models  

Quan#ta#ve  Evalua#on  2  

People Bikes Cars0

0.1

0.2

0.3

0.4

Crispness

People Bikes Cars0

5

10

Crisp

ness

Kalman FilterKalman ICPADH Tracker (Ours)

Sampling  Strategies  

0 5 10 15 20 250

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8R

MS

erro

r (m

/s)

Mean runtime (ms)

ADH Tracker (Ours)Dense samplingDense sampling with motion predictionTop cell sampling

Advantages  over  Radar  

Conclusions  3D Shape Color

Tracking

Motion History

●  Robust to Occlusions, Viewpoint Changes

Conclusions  3D Shape Color

Tracking

Motion History

●  Robust to Occlusions, Viewpoint Changes

●  Runs in Real-time ●  Robust to Initialization Errors

Delta Color Value

Pro

babi

lity

Color  Probability  

Error  vs  Number  of  Points  

0 200 400 600 8000

0.5

1

1.5

Number of points

RMS

erro

r (m

/s)

Kalman FilterADH Tracker (Ours)

Error  vs  Distance  

0 20 40 6000.20.40.60.8

11.21.41.61.8

2

Distance to tracked vehicle (m)

RMS

erro

r (m

/s)

Kalman FilterADH Tracker (Ours)

Error  vs  Number  of  Frames  

0 100 200 3000

0.5

1

1.5

2

Number of frames tracked

RM

S er

ror (

m/s

)

Error  vs  Number  of  Frames  

0 5 10 15 20 25 300

0.5

1

1.5

2

Number of frames tracked

RM

S er

ror (

m/s

)

ADH TrackerADH Tracker without motion model

No color No motion model No 3D shape0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

RM

S er

ror (

m/s

)