SFM Kalman V5b21 3D computer vision Structure from motion using Kalman filter

SFM Kalman V5b2 1

3D computer vision

Structure from motion

using Kalman filter

SFM Kalman V5b2 2

Aims

• To obtain structure and camera motion from an image sequence

• Utilize inter-picture dynamics of a sequence– Such as constant speed, acceleration of the

camera etc.

SFM Kalman V5b2 3

Tracking methods

• Aims

• Kalman filtering

• Condensation (or called particle filter)

SFM Kalman V5b2 4

Part 1

Introduction to Kalman filter (KF) and Extended Kalman filters (EKF)

SFM Kalman V5b2 5

Kalman filter introduction

• Based on• An Introduction to the Kalman

FilterSourceTechnical Report: TR95-041 Year of Publication: 1995 Authors Greg Welch Gary Bishop Publisher University of North Carolina at Chapel Hill Chapel Hill, NC, US (http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf)

SFM Kalman V5b2 6

The process : state= position, velocity, acceleration

•

,R)N(prob(v)tmeasuremenv

tmeasuremenH

tsmeasuremenzwhere

vHxz

QNprob(w)w

u

B

A

kx

wBuAxx

m

nm

mk

kkk

n

nk

nn

nn

nk

kkkk

0 noice,

relation & state

,

)2(

**********************************

),0(noice, n transitiostate

input

gaininput

matrix sitionstate_tran

is at time state

)1(

)1(size

)(size

)1(size

)1(size

)1(size

)(size

)(size

)1(size

11

SFM Kalman V5b2 7

Noise

•

noise of covariance thecalled is

if,0

if,

Similarly

and allfor ,0

so t,independen are, Because

noise process of covariance thecalled is

if,0

if,

) wikipeida(see valueexpected theis ] [

noise v

noise process

1)size(

1)nsize(

tmeasuremenR

ki

kiRvvE

ikvwE

ww

Q

ki

kiQwwE

E

tmeasuremen

w

k

kTik

Tik

ik

k

kTik

m

SFM Kalman V5b2 8

Kalman Computation: Using the current information (a priori) to estimate the predicted future information (posteriori)

• x is a state (e.g. position, velocity & acceleration, etc)

• z is measurement (image position [u,v]T etc)

• k is time step index

factor blendingor gain Kalman

)4()ˆ(ˆˆ

)3( input state, realcurrent theis where, (2), from

)prediction& between ( innovation called is )ˆ(

est.state) previous*-(current alman_gain K

est.state previousest.stateCurrent :equ.(3) of Meaning

)3()ˆ(ˆˆ

as estimate prioria torelated is estimate state Posteriori

covarianceerror state estimate posteriori

covarianceerror state estimate prioria

ˆ

prioria estimate,ˆwhere,ˆ

)(

mnsizek

kkkkkk

kkkk

kkk

kkkkk

Tkkk

Tkkk

kkk

-kkk

K

xHvHxKxx

xvHxz

zdifferencetmeasuremenxHz

Htmeasuremen

xHzKxx

eeEP

eeEP

xxe

xxe

kk

kk

kk

PP

xx

xx

covariance err. Previous| covariance err.Current

ˆ state est. Previous| ˆ state est.Current

state Previous| stateCurrent

SFM Kalman V5b2 9

How good is the prediction? We judge it by looking at the error covariance: the smaller Pk the better.

•

ˆ ˆ (5)

Put (4) into (5)

ˆ ˆ ˆ ˆ{[ ( )][ ( )] }

ˆ ˆ ˆ ˆ{[( ) ( ) ][( ) ( ) ] }

ˆ{[( )( ) ][(

T

k k k k k

Tk k k k k k k k k k k k k

Tk k k k k k k k k k k k k k k

k k k k k k

P E x x x x

P E x x K Hx v Hx x x K Hx v Hx

P E x x K Hx Hx K v x x K Hx Hx K v

P E I K H x x K v I

ˆ)( ) ] }

ˆ ˆ ˆ{( )( )( ) ( ) { () * ( ) * )}

( )

for and are constant, so ( ) , ( )

ˆ ˆ( ) {( )( ) }( ) { () *

Tk k k k k

T Tk k k k k k k k k k

T Tk k k k

k k k k

T Tk k k k k k k

K H x x K v

P E I K H x x x x I K H E function x x v

K E v v K

K H E K K E H H

P I K H E x x x x I K H E funtion

ˆ( ) * )}

( )

ˆ ˆ{( )( ) } , ( )

ˆ ˆ( ) and has no correlation, so { () * (( ) * )} 0

( ) ( ) (6)

k k k

T Tk k k k

T Tk k k k k k k K

k k k k k k

T Tk k k k k K k

x x v

K E v v K

E x x x x P E v v R

x x v E function x x v

P I K H P I K H K R K

kk

kk

kk

PP

xx

xx




SFM Kalman V5b2 10

How to make Pk small?Find optimal gain Kk to make Pk small

From http://www.ee.ic.ac.uk/hp/staff/dmb/matrix/calculus.html

( ) ( ) ; ( { }) { }

From http://en.wikipedia.org/wiki/Matrix_differentiation

( { }) / , must be sqaure

( { }) / 2 ,

T T T

T

T

dy d y dy d tr y tr dy

d tr AB dA B AB

d tr ACA dA AC

must be symmetricC

(6), and {} {}

( ) ( ) (6)

( )( ( ) )

( ) ( )

( )

(

T Tk k k k k K k

T Tk k k k k k K k

T T Tk k k k k k k k k k K k

T T T Tk k k k k k k k k k K k

k k k k k k

From tr trace

P I K H P I K H K R K

P P K HP I K H K R K

P P P K H K HP K HP K H K R K

P P P K H K HP K HP H K K R K

P P K HP P K

1

) ( ) (7)

Since trace of a matrix = trace of its transpose

{ } { } 2 { } { ( ) }

so

( { })2( ) 2 ( ) 0

( )( ) (

T T Tk k k k

T Tk k k k k k k k

T Tkk k k k

k

T Tk k k

H K HP H R K

tr P tr P tr K HP tr K HP H R K

d tr PHP K HP H R

dK

result

K P H HP H R

8)

This is called optimal gain.kK

SFM Kalman V5b2 11

Error covariance Pk for update estimate

•

1

1 1

Put (8) back to (7)

( )( ) (8)

[ ] ( ) (7)

( )

( )( ) [( )( ) ]

(

T Tk k k

T T Tk k k k k k k k k k

T T T Tk k k k k k k k k k

T T T T T Tk k k k k k k k

k

K P H HP H R

P P K HP P K H K HP H R K

P P K HP P H K K HP H R K

P P P H HP H R HP P H P H HP H R

P

1 1

1 1

1

1

)( ) ( )[( )( ) ] (9)

The last term of (9)

( )( ) ( )[( )( ) ]

( )[( )( ) ]

So (9) becomes

( )( )

T T T T T Tk k k k k

T T T T T Tk k k k k k

T T T Tk k k

T Tk k k k k

H HP H R HP H R P H HP H R

P H HP H R HP H R P H HP H R

P H P H HP H R

P P P H HP H R HP

1

1

1

[( )( ) ]

( )[( )( ) ]

( )( )

( ) (10)

T T T Tk k k

T T T Tk k k

T Tk k k k k

k k k

P H P H HP H R

P H P H HP H R

P P P H HP H R HP

P I K H P

Error covariance prediction

•

SFM Kalman V5b2

12

)prediction covarianceerror (

so ,constant is Since

)prediction covarianceerror (or

,

,0,0n,correlatio no have and Since

Since

,0 of value)(expected mean The

ˆ

, Because

earlier described as definitionby , ˆ also , ˆ

1

111

1

1

1

1

111

1

1

1

111

QAAPP

A

QAPAP

QAPAwwEAeeEAP

wwAeeAEwweAeAEP

eAwweAew

wweAwweAeAeAEP

weAweAEP

weAweAEeeEP

w

weAe

xAwxAe

wxAx

xxexxe

Tkk

Tkkkk

Tkkk

Tkk

Tk

Tkkkk

Tkk

Tk

Tkkk

Tkk

Tkkkkk

Tkkk

Tkkkkk

Tkk

Tkkk

Tkkk

Tkkkkk

Tk

Tkkkkkk

Tkkkkkk

T

kkk

k

kkkk

kkkkkk

kkkk

kkkkkk

SFM Kalman V5b2 13

Kalman filter (KF) recursive functions

•

kkk

kkkkk

Tk

Tkk

Tkk

kk

PHKIP

xHzKxx

RHHPHPK

QAAPP

xAx

)( Covariance Update

)ˆ(ˆˆ EstimateUpdate

))(( gain Kalman

equations Update


)prediction state(ˆˆ

statenext intoproject :equations Prediction

11

1

1

11

kk

kk

kk

PP

xx

xx




SFM Kalman V5b2 14

Kalman Filter (KF) Iteration flow

kkk

kkkkk

Tk

Tkk

PHKIP

xHzKxx

RHHPHPK

)( Covariance Update

)ˆ(ˆˆ EstimateUpdate

))(( gain Kalman

equations Update

11

1


)prediction state(ˆˆ


1

11

QAAPP

xAxT

kk

kk

11 andˆ

states Initial

kk Px

Extended Kalman filter EKF

• For non linear problems

SFM Kalman V5b2 15

SFM Kalman V5b2 16

DefinitionsNote: measurement to state is non-linear

•

ˆ state vectorw

length focal,functiont measuremen

)( Example

where,)(

function linear -non a ofresult theis t Measuremen

noiset measuremen

noise, statestate, structure

-- Transition State

t1

t

fh

z

y

z

xfXh

noisevvXh

h(X)

XX

v

X

t

C

C

C

C

t

tttt

t

tt

t

SFM Kalman V5b2 17

EKF Iteration flow

h()h

PhKIP

xhzKxx

RhPhhPK

kkk

kkkkk

Tk

Tkk

functiont measuremen theof jacobian theis

Covariance Update

0,ˆˆˆ EstimateUpdate

gain Kalman

equations UpdateEKF

1

1

kPx andˆ

states Initial

k

•


solinear is , of jacobian theis where,

functionlinear a is , ˆ)ˆ(ˆ ),prediction state(ˆˆ


1

11

11111

QAAPP

AFffFQFPFP

xAxfxxAx

Tkk

Tkkkk

kkkkk

Ref: http://en.wikipedia.org/wiki/Extended_Kalman_filter

SFM Kalman V5b2 18

Part IITwo-pass Kalman filter for structure

and pose estimationDescription

Major reference [Yu, June 2005]

SFM Kalman V5b2 19

Two-pass Kalman filter for structure and pose estimation

• Structure and pose updating

Feature extractionand trackingusing KLT

ModelInitialization

Set i=2

Step 1Pose estimationfor the ith frame

Step 2Structure updatingusing N EKFs withthe ith image as the

measurement

The final3D model

The i-th frame is not the last frame.Increment i by 1.

The last frame is reached

SFM Kalman V5b2 20

Model initializations

• f=focal length (found by camera calibration)• Zinit=initial guess of z direction of the model points

(e.g. Zinit=1 meter from the camera)

scoordinateobjectTO

iOi

Oiguess

TCi

Ci

Ci

Ci

Ci

Ci

Ci

Ci

initi

i

TCi

Ci

Ci

Tii

zyxzyx

mizyx

y

x

z

fv

u

izyx

vu

_

1: allfor guessed be can Zinit)(,,

equation projection theuse

s.coordinatecamera in feature D-3a is

measured position pixel

SFM Kalman V5b2 21

Kalman tracking

Step 1: Kalman pose tracking

SFM Kalman V5b2 22

Kalman filtering pose trackingAssume model is known (by

guessing)•

Xc

Yc

Zc

Xo

Yo

Zo

Oo

Oc

u

v

C (image center)

Model

XiO = [xi

O yiO zi

O]’

Tc

Object coordinateframe

Camera coordinateframecoordintes (world)object at point model D3

coordintescamera at point model D3

,)(

Oi

Ci

COi

Ci

X

X

whereTTRXX

SFM Kalman V5b2 23

Kalman pose filterStates• State transition

ˆ state vectorw

dtTTT

diag

w

wherewAw

dtdTTTTT

whereTTTTTTw

w

sss

tt

ttt

T

tt

where,10

1,,

10

1

noisen transitiostate vector,state

,ˆˆ

resp. axes zy, x,aroundrotation are,,

etc; resp. axes zy, x,alongon translatiare ,,

,,,,,,,,,,,, ˆ

as defined is state

)1212(

'

)112('

)112(1)1212()112(

321

11321

332211332211)112(1,

SFM Kalman V5b2 24

Kalman pose filter measurement

• measurement

)1(,...,...,)(_

_

)(

noiset measuremen,length focal

1

1

1

1

''

'

T

Cn

Cn

Cn

Cn

Ci

Ci

Ci

Ci

C

C

C

C

tt

tttt

t

z

y

z

x

z

y

z

x

z

y

z

xfwg

vimage

uimage

vwg

vf

Kalman for pose estimationIMMKalmanPoseGen.m

•

SFM Kalman V5b2

25

}

)7(

)6(

)5()ˆ(ˆˆ

--equations -Update-

)4(

)3(ˆˆ{

)1(min ationsfor K_iter follwoing tehloop :frame image eachfor

)2(10

1,

10

1,

10

1,

10

1,

10

1,

10

1

Gains Kalman features, ofnumber

resp. axes zy,x, around rotation are,,

etc; resp. axes zy,x, along on translatiare ,,

,,,,,,,,,,, variablesstateˆ

1

22)22('

)212()1212(1,)122()212()1212(1,)212(

)1212(1,)122()212()1212(1,)1212(,

)12()2(1,)2('

)212()112(1,)112(,

)1212('

)1212()1212(1,1)1212()1212(1,

)112(1,1)1212()112(1,

)212(

321

11321

332211332211)112(1,

NNNNtNTwttNwN

TwttN

ttNwNtttt

NNtttNtNcurrentttnexttt

tT

tttt

previousttcurrenttt

N

T

tt

RgPggPK

PgKPP

wgKww

QAPAP

wAw

TsTsTsTsTsTsdiagA

KN

dtdTTTTT

TTTTTTw

‧‧

Implementation

•

SFM Kalman V5b2 26

by KLT ede.g.obtain ts,measuremen

:

:

:

:

)1(,...,...,)(

_

_

:

:

_

_

)(

noiset measuremen,length focal

12

2

2

1

1

'

1

1

1

1

1

1

''

'

NN

N

t

T

Cn

Cn

Cn

Cn

Ci

Ci

Ci

Ci

C

C

C

C

tt

N

N

tttt

t

v

u

v

u

v

u

z

y

z

x

z

y

z

x

z

y

z

xfwg

vimage

uimage

vimage

uimage

vwg

vf

Implementation details, equation 2,3

•

SFM Kalman V5b2

27

resp. axes zy,x, around rotation are,,

etc; resp. axes zy,x, along on translatiare ,,

)3(

1

1

1

1

1

1

1

1

1

1

1

1

)3(ˆˆ

,,,,,,,,,,, ˆ

10

1,

10

1,

10

1,

10

1,

10

1,

10

1

321

11321

1,13

3

2

2

1

1

3

3

2

2

1

1

1,3

3

2

2

1

1

3

3

2

2

1

1

)112(1,1)1212()112(1,

332211332211)112(1,

1212

dtdTTTTT

T

T

T

T

T

T

Ts

Ts

Ts

Ts

Ts

Ts

T

T

T

T

T

T

wAw

TTTTTTw

dtTs

TsTsTsTsTsTsdiagA

tttt

previousttcurrenttt

T

tt

State transition matrix A12x12

• Typical

• A12x12

• Ts=1 used in the

program

SFM Kalman V5b2 28

1

1

1

1

1

1

1

1

1

1

1

1

1212

Ts

Ts

Ts

Ts

Ts

Ts

A

Initialize pose_covar P1,1 • %Initialize state vector covariance for the pose filter

• pose_covar=P1,1 = diag([1 1 1 1 1 1 0.001 0.0001 0.1 0.01 0.01 0.001]*1000);

SFM Kalman V5b2 29

1212

11

1.0

1

1.0

1

10

1

1000

1000

1000

1000

1000

1000

.

P ,

Covariance of state noise= Q12x12 , • Qp = diag([10 10 10 10 10 10 0.0001 0.0001 0.1 0.01 0.001 0.0001]);

SFM Kalman V5b230

1212

1212

0001.0

001.0

01.0

1.0

0001.0

0001.0

10

10

10

10

10

10

Q

Covariance of measurement noise=R12x12• Rc = diag(repmat([2 2], 1, n_features));

• N=number of features

SFM Kalman V5b231

NN

NNR

22

22

2

2

2

2

:

:

:

:

2

2

2

2

Motion and projection

•

SFM Kalman V5b2

32

312

213

3333231

2232221

312

123

3333231

1131211

,...,1

3

2

1

333231

232221

131211

3

2

1

333231

232221

131211

3211

onsmanipulati someAfter

'

',

'

'

.length focal of camera aby captured

are projection image The

'

'

'

].,position D-3 new in the is ],point model ,motion After

s.expression hesimplify t todropped is index The

matrix rotational theis

on vector, translati theis , Ar time

TZYX

TZYXf

Z

Yf

TZrYrXr

TZrYrXrfv

TZYX

TZYXf

Z

Xf

TZrYrXr

TZrYrXrfu

Z

Yf

Z

Xf

v

uq

f

q

T

T

T

Z

Y

X

rrr

rrr

rrr

T

T

T

Z

Y

X

R

Z

Y

X

Z'[X',Y'Z [X,YR,T

t

rrr

rrr

rrr

R

TTTTtt

iii

iiiRi

Ri

iii

iiii

iii

iiiRi

Ri

iii

iiii

i

i

i

i

i

ii

Ni

i

i

i

i

i

i

i

i

i

T

T

•

SFM Kalman V5b2

330 and

0 all practice In

columns. 12 hasrow Eachrows. 2 totalrows, 2 generates feature Each

122

::

::

:

:

1

1

1

Rsince

),,,,,,,,( Given

321321

321321

2

2

1

2

1

2

2

2

1

2

1

2

3

1

3

1

2

1

2

1

1

1

1

1

3

1

3

1

2

1

2

1

1

1

1

1

3

1

3

1

2

1

2

1

1

1

1

1

3

1

3

1

2

1

2

1

1

1

1

1

)122(

12

13

23

333231

232221

131211

312

213

3333231

2232221

312

123

3333231

1131211

321321

vvv

T

v

T

v

T

v

uuu

T

u

T

u

T

u

N

N

T

v

T

v

T

vT

u

T

u

T

u

vvvvvv

T

v

T

v

T

v

T

v

T

v

T

v

uuuuuu

T

u

T

u

T

u

T

u

T

u

T

u

g

rrr

rrr

rrr

TZYX

TZYXf

Z

Yf

TZrYrXr

TZrYrXrfv

TZYX

TZYXf

Z

Xf

TZrYrXr

TZrYrXrfu

ZYXTTTgu

iii

iii

iiiiiiiiiiii

iiiiiiiiiiii

Nw

iii

iiiRi

Ri

iii

iiii

iii

iiiRi

Ri

iii

iiii

iiiui

Pose Jacobian Matrix)122( Nwg

SFM Kalman V5b2 34

Yu-recursive Kalman [Yu, June 2005] algorithm

Matlab for pose Jacobian matrices (full) • %oct 2014• %************************************************************************• function TestJacobian_pose_full• % Try to solve the differentiate equations without simplification• clc,clear;• disp('TestJacobian');• • syms a b c; % a= pitch ( around x axis), b=yaw(around y), c=roll (around z) respectively• syms f X Y Z T1 T2 T3;• F = [• %u• f*((cos(b)*cos(c)*X - cos(b)*sin(c)*Y + sin(b)*Z+ T1)...• /((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X ...• + (cos(a)*sin(b)*sin(c)+ sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3));• %v• ((sin(a)*sin(b)*cos(c)+ cos(a)*sin(c))*X ...• + (-sin(a)*sin(b)*sin(c)+ cos(a)*cos(c))*Y - sin(a)*sin(b)*Z + T2)...• /((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X + (cos(a)*sin(b)*sin(c)...• + sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3)]• • V = [a,b,c,T1,T2,T3];• Fjaco = jacobian(F,V);• disp('Fjaco =');• disp(Fjaco);• size(Fjaco) % Fjaco is 2x6• %Fjaco(1,1)• %************************

SFM Kalman V5b2

35

Yu-recursive Kalman [Yu, June 2005] algorithm Matlab for pose Jacobian matrices (approximation)

• % TestJacobian_pose_approx• %function TestJacobian_pose_approx• %'==test jacobian for chang,wong ieee_mm 2 pass lowe=================='• %use twist (small) angles approximation. • clear, clc;• • syms R dR M TT XYZ ZZ x y z f u v a1 a2 a3 t1 t2 t3 aa1 aa2 aa3 tt1 tt2 tt3• R=[1 -aa3 aa2; aa3 1 -aa1; -aa2 aa1 1];• dR=[1 -a3 a2; a3 1 -a1; -a2 a1 1];• M=[x;y;z];• TT=[tt1;tt2;tt3];• dt=[t1;t2;t3]• XYZ=dR*R*M+TT+dt; % R is a matrix multiplication transform• u=f*XYZ(1)/XYZ(3);• v=f*XYZ(2)/XYZ(3);• ja=jacobian([u ;v],[tt1 t1 tt2 t2 tt3 t3 aa1 a1 aa2 a2 aa3 a3])• size(ja) %size of ja is 2x12• ja(1,1),ja(1,2),ja(1,3),ja(1,4),ja(1,5),ja(1,6),ja(1,7),ja(1,8),ja(1,9),ja(1,10),ja(1,11),ja(1,12)

SFM Kalman V5b2 36

Kalman tracking

Step2: Kalman structure tracking

SFM Kalman V5b2 37

Kalman filtering structure trackingAssume pose is known

•

Xc

Yc

Zc

Xo

Yo

Zo

Oo

Oc

u

v

C (image center)

Model

XiO = [xi

O yiO zi

O]’

Tc

Object coordinateframe

Camera coordinateframecoordintes (world)object at point model D3

coordintescamera at point model D3

,)(

Oi

Ci

COi

Ci

X

X

whereTTRXX

SFM Kalman V5b2 38

Kalman structure filterStates

• Structure State transition ˆ state vectorw

}

length focal,functiont measuremen

)(

)(

tMeasuremen

noiset measuremen

noise, statestate, structure

-- Transition State{

N to1dexfeature_inFor

t1

t

fh

z

y

z

xfXh

vXh

XX

v

X

t

C

C

C

C

t

ttt

tt

t

SFM Kalman V5b2 39

Extended Kalman filter iteration for the model structureThe process is an identify matrix (no change of structure against time)The procedure is for each features:

}

)()(

)(

)ˆ(ˆˆ

--equations -Update-

modelfor Jacobiangain, Kalman

covarianceerror ,covariance noise where

,

ˆˆ

states structurefor equations Prediction

N{ to1dexfeature_inFor

1

)22()21()11(1,)12()21()11(1,)21(

)11(1,1)12()21()11(1,1)11(,

)12(1,'

)21()11(1,1)11(,

1,1)13(1,

)11(1,1)11(1,

tTXttX

TXtt

ttXtttt

tttttttt

X

t

ttttt

tttt

RhhhW

hW

XhWXX

hW

Q

Q

XX

SFM Kalman V5b2 40

Yu-recursive Kalman [Yu, June 2005] algorithm Matlab for structure Jacobian matrices (full)

• % Solve the problem without simplification• % Since is the yaw, pitch, roll of the rotation respectively. The R can be represent by as:• % • % Substitute R into (1), we have the following program (in matlab):• %************************************************************************• function TestJacobian• % Try to solve the differentiate equations without simplification• clc,clear;• disp('TestJacobian');• • syms a b c; %yaw( around x axis), pitch(around y), roll(aroudn z) respectively• syms f X Y Z T1 T2 T3;• F = [• %u• f*((cos(b)*cos(c)*X - cos(b)*sin(c)*Y + sin(b)*Z+ T1)...• /...• ((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X ...• + (cos(a)*sin(b)*sin(c)+ sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3));• %v• ((sin(a)*sin(b)*cos(c)+ cos(a)*sin(c))*X ...• + (-sin(a)*sin(b)*sin(c)+ cos(a)*cos(c))*Y - sin(a)*sin(b)*Z + T2)...• /...• ((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X + (cos(a)*sin(b)*sin(c)...• + sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3)]• • V = [X,Y,Z];• Fjaco = jacobian(F,V);• %************************

SFM Kalman V5b2 41

Structure Jacobian• function lpf = KalmanStructJacobSP(RT_solution, a, f, u, v, initz)

• b = 1/f;• tx = RT_solution(1);• ty = RT_solution(2);• tzxb = RT_solution(3);• wx = RT_solution(4);• wy = RT_solution(5);• wz = RT_solution(6);

• temp1 = 1 - b*sin(wy)*(u + a*b*u) + b*cos(wy)*sin(wx)*(v + a*b*v) + b*cos(wy)*cos(wx)*(a - initz) + tzxb;

• • lpf = zeros(2, 1);

• lpf(1) = (cos(wz)*cos(wy)*b*u + (cos(wz)*sin(wy)*sin(wx) - sin(wz)*cos(wx))*b*v + cos(wz)*sin(wy)*cos(wx) + sin(wz)*sin(wx))/(temp1) - ...

• (cos(wz)*cos(wy)*(u + a*b*u) + (cos(wz)*sin(wy)*sin(wx) - sin(wz)*cos(wx))*(v + a*b*v) + ...• (cos(wz)*sin(wy)*cos(wx) + sin(wz)*sin(wx))*(a - initz) + tx)*(((-b)^2)*sin(wy)*u +

(b^2)*cos(wy)*sin(wx)*v + b*cos(wy)*cos(wx)) /(temp1^2);

• lpf(2) = (sin(wz)*cos(wy)*b*u + (sin(wz)*sin(wy)*sin(wx) + cos(wz)*cos(wx))*b*v + sin(wz)*sin(wy)*cos(wx) - cos(wz)*sin(wx))/(temp1) - ...

• (sin(wz)*cos(wy)*(u + a*b*u) + (sin(wz)*sin(wy)*sin(wx) + cos(wz)*cos(wx))*(v + a*b*v) + ...• (sin(wz)*sin(wy)*cos(wx) - cos(wz)*sin(wx))*(a - initz) + ty)*(((-b)^2)*sin(wy)*u +

(b^2)*cos(wy)*sin(wx)*v + b*cos(wy)*cos(wx)) /(temp1^2);

• return

SFM Kalman V5b2 42

Yu-recursive Kalman [Yu, June 2005] algorithm Matlab for structure Jacobian matrices

(approximate))• '====test jacobian for structure================='• %use twist (small) angles approximation. • clear, clc;• • syms R dR M TT XYZ ZZ x y z f u v a1 a2 a3 t1 t2 t3 aa1 aa2 aa3

tt1 tt2 tt3• R=[1 -aa3 aa2; aa3 1 -aa1; -aa2 aa1 1];• dR=[1 -a3 a2; a3 1 -a1; -a2 a1 1];• M=[x;y;z];• TT=[tt1;tt2;tt3];• dt=[t1;t2;t3]• XYZ=dR*R*M+TT+dt; % R is a matrix multiplication transform• u=f*XYZ(1)/XYZ(3);• v=f*XYZ(2)/XYZ(3);• • ja=jacobian([u ;v],[x y z])• •

SFM Kalman V5b2 43

Part BInteracting Multiple Model Method

(IMM) structure and pose estimation

Major reference [Yu Aug.2005]

SFM Kalman V5b2 44

Example: Interacting Multiple Model Method (IMM) for (IMM-KSFM)

• 3 dynamic models =3 parallel Kalman structures– General, Rotation, Translation

• Use IMM to find the suitable dynamic model at time t• Each Kalman structure is similar to IV-KSFR• Reference

1) Ying Kin Yu, Kin Hong Wong and Michael Ming Yuen Chang, "Merging Artificial Objects with Marker-less Video Sequences Based on the Interacting Multiple Model Method", IEEE Transactions on Multimedia, Volume 8, Issue 3, June 2006 Page(s):521 - 528.

SFM Kalman V5b2 45

Steps

SFM Kalman V5b2 46

Pose

SFM Kalman V5b2 47

IMM switching

SFM Kalman V5b2 48

Part CUSE Trifocal tensor (TRI-KSFM)

structure and pose estimation

Major reference [Yu Feb.2005]

SFM Kalman V5b2 49

Example : USE Trifocal tensor (TRI-KSFM)

• Always use first second (I1, I2) frames as reference• Use It as the third frame to form a trifocal tensor at time t• If only pose output required, use one Kalman filter• If structure is needed use N Kalman filters for N features,

similar to IV-KSFM.• Reference

1) Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang, "Recursive 3D Model Reconstruction Based on Kalman Filtering", IEEE Transactions on Systems, Man and Cybernetics B, Vol.35, No.3, June 2005.

SFM Kalman V5b2 50

TRI-KSFM flow diagram

SFM Kalman V5b2 51

The flow of the recursive algorithm with the structure recovery

extension.

SFM Kalman V5b2 52

Condensation

1) Reference1) Kin-Hong Wong and Michael Ming-Yuen Chang, "Pose tracking for

virtual walk-through environment construction", Proceedings of the Recent Development in Theories and Numerics, Editor: Y.C. Hon , World Scientific Publishing Co. Pte. Ltd. 2003. ISBN 981-238-266-2

SFM Kalman V5b2 53

Conclusion

• Studied various techniques for model and pose tracking in 3D computer vision.

SFM Kalman V5b2 54

References on Kalman filter1. [Azarbayejani 1995] A.Azarbayejani and A.P.Pentland, “Recursive estimation of motion, structure, and focal length”, IEEE Trans. on

PAMI, vol. 17, no 6, June 1995.2. [Azarbayejani 1993] A. Azarbayejani, T. Starner, B. Horowitz, A. Pentland,“, Visually Controlled Graphics“, IEEE Transactions on

Pattern Analysis and Machine Intelligence, vol.15, no. 6, pages 602-605,1993.3. [Yu, June 2005] Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang, "Recursive 3D Model Reconstruction Based on

Kalman Filtering", IEEE Transactions on Systems, Man and Cybernetics B, Vol.35, No.3, June 2005. 4. Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang, "A Fast and Robust Simultaneous Pose Tracking and Structure

Recovery Algorithm for Augmented Reality Applications", International Conference on Image Processing (ICPR04), Singapore, Oct. 2004.

5. Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang,"A Fast Recursive 3D Model Reconstruction Algorithm for Multimedia Applications", International Conference on Pattern Recognition ICPR2004, Cambridge, August 2004.

6. Ying Kin Yu, Kin Hong Wong and Michael Ming Yuen Chang, "Merging Artificial Objects with Marker-less Video Sequences Based on the Interacting Multiple Model Method", IEEE Transactions on Multimedia, Volume 8, Issue 3, June 2006 Page(s):521 - 528.

7. Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang, "Recursive 3D Model Reconstruction Based on Kalman Filtering", IEEE Transactions on Systems, Man and Cybernetics B, Vol.35, No.3, June 2005.

8. Kin-Hong Wong and Michael Ming-Yuen Chang, "Pose tracking for virtual walk-through environment construction", Proceedings of the Recent Development in Theories and Numerics, Editor: Y.C. Hon , World Scientific Publishing Co. Pte. Ltd. 2003. ISBN 981

9. Greg Welch and Gary Bishop, "An Introduction to the Kalman Filter, "TR 95-041 Department of Computer Science University of North Carolina at Chapel Hill Chapel Hill, NC 27599-3175 www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf (excellent tutorial)

10. T.J.Broida, S.Chandrasiiekhar and R.Chellappa, “Recursive 3-D motion estimation from monocular image sequence”, IEEE Trans. on Aerospace and Electronic Systems, vol 26, no. 4, July 1990. (The first paper of Kalman on pose estimation)

11. http://mathworld.wolfram.com/Quaternion.html12. http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm13. A.W. Rihaczek, “Recursive methods for the estimation of rotation quaternions”, IEEE transactions on aerospace and electronic systems,

Vol. 32, No. 2, April 1996.14. [Yu, Feb 2005]Ying Kin Yu, Kin Hong Wong, Michael Ming Yuen Chang and Siu Hang Or, "Recursive Camera Motion Estimation

with Trifocal Tensor:, Submitted to a journal.

AppendixSome math background

• Mean

• Variance/ standard deviation

• Covariance

• Covariance matrix

SFM Kalman V5b2 55

Mean, variance (var) and standard_deviation (std)

• x =• 2.5000• 0.5000• 2.2000• 1.9000• 3.1000• 2.3000• 2.0000• 1.0000• 1.5000• 1.1000• mean_x = 1.8100• var_x = 0.6166• std_x = 0.7852

)var()(

)1(

1)var(

1

2

1

1

xxstd

xn

x

xn

xmean

n

ii

n

ii

SFM Kalman V5b2 56

%matlab codex=[2.5 0.5 2.2 1.9 3.1 2.3 2 1 1.5 1.1]' mean_x=mean(x)var_x=var(x)std_x=std(x)%%%%%%%%Answer:mean_x = 1.8100var_x = 0.6166std_x = 0.7852

x

sample

N or N-1 as denominator??see

http://stackoverflow.com/questions/3256798/why-does-matlab-native-function-cov-covariance-matrix-computation-use-a-differe

• “n-1 is the correct denominator to use in computation of variance. It is what's known as Bessel's correction” (http://en.wikipedia.org/wiki/Bessel%27s_correction) Simply put, 1/(n-1) produces a more accurate expected estimate of the variance than 1/n

SFM Kalman V5b2 57

Class exercise 1

By computer (Matlab)• x=[1 3 5 10 12]'

• mean(x)

• var(x)

• std(x)

• Mean(x)

• = 6.2000

• Variance(x)= 21.7000

• Stand deviation = 4.6583

By and

• x=[1 3 5 10 12]'

• mean=

• Variance=

• Standard deviation=

SFM Kalman V5b2

%class exercise1x=[1 3 5 10 12]'

mean(x)var(x)std(x)

58

)var()(

)1(

1)var(

1

2

1

1

xxstd

xn

x

xn

xmean

n

ii

n

ii

Answer1:

By computer (Matlab)• x=[1 3 5 10 12]'

• mean(x)

• var(x)

• std(x)

• Mean(x)

• = 6.2000

• Variance(x)= 21.7000

• Stand deviation = 4.6583

By hand

• x=[1 3 5 10 12]'

• mean=(1+3+5+10+12)/5

• =6.2

• Variance=((1-6.2)^2+(3-6.2)^2+(5-6.2)^2+(10-6.2)^2+(12-6.2)^2)/(5-1)=21.7

• Standard deviation= sqrt(21.7)= 4.6583

SFM Kalman V5b2

)var()(

)1(

1)var(

1

2

1

1

xxstd

xn

x

xn

xmean

n

ii

n

ii

59

What do mean, variance, standard deviation tell you

• Mean: the average value of a set of numbers

• Variance: the sum of the square of the difference between the numbers and the mean.

• Stand deviation : the square root of the mean , a measurement of how the numbers are deviated from the mean. Give you intuition than the Variance.

SFM Kalman V5b2 60

• See

• http://www.biostat.jhsph.edu/~fdominic/teaching/bio655/extras/matrixnotes2.pdf

SFM Kalman V5b2 61

Variance/Covariance matrix (or called Covariance matrix) of a vector of random variables

variances

theare digonal theandmatrix symmteric is

..covcov

:.::

cov..cov

cov..cov

is _matrixofcovariance

between variancecov

of variance

]',,,[

mean zero with variablesrandom ofa vector is

2n21

22i12

1122i

2i

4321

nn

n

n

j iij

i

X

j, i and YY

y

yyyyY

Y

Variance -covariance matrix

• Videos

• Co-variance matrix and correlation– https://www.youtube.com/watch?

v=ZfJW3ol2FbA

• Co-variance matrix– https://www.youtube.com/watch?

v=locZabK4Als

SFM Kalman V5b2 62

Particle filter

• Video– https://www.youtube.com/watch?v=O-

lAJVra1PU– With matlab code:

• https://www.youtube.com/watch?v=HZvF8KlFoWk

SFM Kalman V5b2 63

Documents

SFM Kalman V5b21 3D computer vision Structure from motion using Kalman filter