Bundle Adjustment - University of Minnesotahspark/CSci5980/Lec17_Supp...v Bob Alice X 1 P bob P...

Bundle Adjustment

v

Bob Alice

X

bobP aliceP

u

pp

X

1

2

geom

2

2

3

2

3

u

P X P X

P X X

u

P

E

x y

Black: given variables Red: unknowns

Camera & Point Jacobian

v

Bob Alice

X

bobP aliceP

u

pp

X

1

2

geom

2

2

3

2

3

u

P X P X

P X X

u

P

E

x y


( )

p,X

u

v

w

f w


v

Bob Alice

X

bobP aliceP

u

pp

X

1

2

geom

2

2

3

2

3

u

P X P X

P X X

u

P

E

x y


( )

p,X

u

v

w

f w 2

2

( )

p pp,X

p p

p p

u ww u

u

w w

v u wv v

w

w

f


v

Bob Alice

X

bobP aliceP

u

pp

X

1

2

geom

2

2

3

2

3

u

P X P X

P X X

u

P

E

x y


( )

p,X

u

v

w

f w 2

2

( )

p pp,X

p p

p p

u ww u

u

w w

v u wv v

w

w

f

2

2

( )

X Xp,X

X X

X X

u ww u

u

w w

v u wv v

ww

f


Bob

1X

bobP

u

p

X


Camera & Point Jacobian ( ) ( )

p X

p ,X p ,XJ J J

p Xj i j i

ij ij ij

j i

f f

v

Bob Alice

1X

bobP aliceP

u

pp

X



p X

p ,X p ,XJ J J

p Xj i j i

ij ij ij

j i

f f

J1,bobpJ 2 70 1,bobXJ

1,aliceXJ1,alicepJ2 70

v

Bob Alice

1X

bobP aliceP

u

pp

X



p X

p ,X p ,XJ J J

p Xj i j i

ij ij ij

j i

f f

J1,bobpJ 2 70 1,bobXJ


2X

2,bobpJ 2 70 2,bobXJ


2 30

2 30

2 30

2 30

J =

Cam 1 Pt 1 Pt 4

# of unknowns: 3x7+4x3 # of projections: 3x4

Cam 2 Cam 3 Pt 2 Pt 3

Pt 1

Pt 2

Pt 3

Pt 4

Camera 1

Camera 2 Camera 3

J =

Cam 1 Pt 1 Pt 4

# of unknowns: 3x7+4x3 # of projections: 9 (not all points are visible from cameras)


Pt 1

Pt 2

Pt 3

Pt 4

Camera 1

Camera 2 Camera 3

J = pJXJ

Camera 3D point

p XJ J J

J J =T

p XJ J JT T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:

T T T

T T T- ( )

p p p X p p

X p X X X X

J J J J J epb X

XJ J J J J ef

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:

T T

T T

p p p X p

X p X X X

J J I J J ep

XJ J J J I eor

T

p

X

eA B p

B D X eor

T T T

T T T- ( )

p p p X p p

X p X X X X

J J J J J epb X

XJ J J J J ef

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:

or

or

1

d

D

dM

-1

1

-1

-1

d

D

dM

Inversion of block diagonal matrix can be efficiently computed.

T T T

T T T- ( )

p p p X p p

X p X X X X

J J J J J epb X

XJ J J J J ef

T T

T T

p p p X p

X p X X X

J J I J J ep

XJ J J J I e

T

p

X

eA B p

B D X e

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:

1

d

D

dM

-1

1

-1

-1

d

D

dM


T

p

X

eA B p

B D X e

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:

1

d

D

dM

-1

1

-1

-1

d

D

dM


T

p

X

eA B p

B D X e

-1 -1

T

- -

p

X

eA B pI BD I BD

B D X0 I 0 I e

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:

1

d

D

dM

-1

1

-1

-1

d

D

dM


T

p

X

eA B p

B D X e

-1 -1

T

- -

p

X

eA B pI BD I BD

B D X0 I 0 I e

-1-1 T

T

--

p X

X

e BD epA BD B 0

XB D e

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:

-1 T-A BD BNote: is Schur complement of D

1

d

D

dM

-1

1

-1

-1

d

D

dM


T

p

X

eA B p

B D X e

-1 -1

T

- -

p

X

eA B pI BD I BD

B D X0 I 0 I e

-1-1 T

T

--

p X

X

e BD epA BD B 0

XB D e

-1-1 T -1

-1 T

- -

-

p X

X

p A BD B e BD e

X D e B p

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:


1

d

D

dM

-1

1

-1

-1

d

D

dM


Small size matrix

T

p

X

eA B p

B D X e

-1 -1

T

- -

p

X

eA B pI BD I BD

B D X0 I 0 I e

-1-1 T -1

-1 T

- -

-

p X

X

p A BD B e BD e

X D e B p

-1-1 T

T

--

p X

X

e BD epA BD B 0

XB D e

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points

# of images if visible

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

XJ

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

XJ

d

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

XJ

d

( )b pf

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

XJ

d

( )b pf

-1

1

-1

-1

d

D

dM

d = d I

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

XJ

d

( )b pf

T

T- ( )

p p

X X

J eb X

J ef

-1

1

-1

-1

d

D

dM

d = d I

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

XJ

d

( )b pf

T

T- ( )

p p

X X

J eb X

J ef

T T

T T T

p p p X

X p X X

J J I J JA B

B D J J J J I

-1

1

-1

-1

d

D

dM

d = d I

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

XJ

d

( )b pf

T

T- ( )

p p

X X

J eb X

J ef

-1-1 T -1

-1 T

- -

-

p X

X

p A BD B e BD e

X D e B p

T T

T T T

p p p X

X p X X

J J I J JA B

B D J J J J I

-1

1

-1

-1

d

D

dM

d = d I

1D Camera Bundle Adjustment

Camera pose

2D points

nPoints = 5; nCameras = 3; X = rand(nPoints, 2) + 2; c = rand(nCameras,2); for i = 1 : nCameras m = X-ones(nPoints,1)*c(i,:); theta = atan2(m(:,2), m(:,1)); theta = mean(theta); C(i).c = c(i,:)'; theta = theta; C(i).R = [sin(theta) -cos(theta); cos(theta) sin(theta)]; u = C(i).R*[eye(2) -C(i).c] * [X'; ones(1,nPoints)]; C(i).m = u(1,:)./u(2,:); C(i).c = c(i,:)'+0.3*randn(2,1); theta = theta + 0.3*randn(1,1); C(i).R = [sin(theta) -cos(theta); cos(theta) sin(theta)]; end X = X + 0.3*randn(size(X)); [C X] = BA(C, X);

BundleAdjustment1D.m

c

X

Data generation


Camera pose

2D points



Data generation

Ground truth projection

c

X


Camera pose

2D points



Data generation


c

X

Add noise


Camera pose

2D points



Data generation


c

X

Add noise

1D bundle adjustment


Camera pose

2D points

function [C X] = BA(C, X) lambda = 0.5; nIters = 100; xp = []; for i = 1 : length(C) theta=atan2(C(i).R(2,2), C(i).R(2,1)); xp = [xp; C(i).c; theta]; end xx = []; for i = 1 : size(X,1) xx = [xx; X(i,:)']; end

c

X


for j = 1 : nIters Jp = []; Jx = []; D_inv = []; err = []; for iPoint = 1 : size(X,1) X1 = xx(2*(iPoint-1)+1:2*iPoint); d = zeros(2,2); for iC = 1 : length(C) c = xp(3*(iC-1)+1:3*(iC-1)+2); theta = xp(3*(iC-1)+3); R = [sin(theta) -cos(theta); cos(theta) sin(theta)]; df_dc = JacobianC1D(R, c, X1); df_dR = JacobianR1D(R, c, X1)*JacobianQ1D(theta); df_dx = JacobianX1D(R, c, X1); j1 = zeros(1,3*length(C)); j1(:,3*(iC-1)+1:3*iC) = [df_dc df_dR]; j2 = zeros(1,2*size(X,1)); j2(:,2*(iPoint-1)+1:2*iPoint) = df_dx; Jp = [Jp; j1]; Jx = [Jx; j2]; d = d + df_dx'*df_dx; u = R * [eye(2) -c] * [X1;1]; u = u/u(2); u1 = C(iC).m; e = [u1(iPoint) - u(1)]; err = [err; e]; end d = d + lambda*eye(2); D_inv = blkdiag(D_inv, inv(d)); end ep = Jp' * err; ex = Jx' * err; A = Jp'*Jp + lambda*eye(3*length(C)); B = Jp'*Jx; delta_p = inv(A-B*D_inv*B') * (ep-B*D_inv*ex); delta_x = D_inv * (ex-B'*delta_p); xp = xp + delta_p; xx = xx + delta_x; end


for iC = 1 : length(C) C(iC).c = xp(3*(iC-1)+1:3*(iC-1)+2); theta = xp(3*(iC-1)+3); C(iC).R = [sin(theta) -cos(theta); cos(theta) sin(theta)]; end for iX = 1 : size(X,1) X(iX,:) = xx(2*(iX-1)+1:2*iX); end


Model update



Model update

mean_delta_X = 0.0265 mean_delta_p = 0.2648

Why camera and point error do not converge to zero while reprojection error converges?



Model update

mean_delta_X = 0.0265 mean_delta_p = 0.2648

Why camera and point error do not converge to zero while reprojection error converges?

Because the bundle adjustment is still up to scale and orietation.

Data Capture

• Enough baseline between images • Share common 3D structure • Multiple planes

Reference image

• An image in the middle may be a good choice of reference image because you can find many matches across all images.

Reference Image Selection

Matching Ref. with other images

Reference image

i j

,f ju,f i

u

,f iu ,f j

u1u = K u

Indicator of no match

In addition to Mx and My, you may want to have an indicator matrix for visibility.

Camera Pose Estimation

Choose an initial pair of images to compute camera pose • Have enough number of matches •

1i 2i

Camera Pose Estimation

1i 2i

Find Next Best Image

1i 2i

Reconstructed image index

i

Find the image that has maximum number of 3D-2D matches

u

X

3D-2D correspondence: u X

= P K R t (3)R SOwhere

11

12

13

14x x x x1 1 1 1 1 1 1

21y y y y1 1 1 1 1 1 1

22

23x x x x

24y y y y

31

32

33

34

1

1

1

1

m m m m m m m

m m m m m m m

p

p

p

pX Y Z -u X -u Y -u Z -u


p



p

p

p

p

0

0

0

0

x 11 12 13 14

31 32 33 34

+ +

+ +

p X p Y p Z pu

p X p Y p Z py 21 22 23 24

31 32 33 34

+ +

+ +

p X p Y p Z pu

p X p Y p Z p

2mx12

A x 0

Camera Registration

u

X

If K is given,

= P K R t (3)R SOwhere

1 2 3

K R t p p p p

-11 2 3= R K p p p

11

-1 T1 2 3 22

33

K p p p U V

d

d

d

11 d

TR UV : SVD cleanup

-14

11

=K p

t d

: Translation and scale recovery

Camera Registration

Camera Registration

Camera Registration

Reprojection error:

X

bobP

uu

p


X CK -R

u

v

w

where

2 2

geom

Ev

wx y

u

w

-KC

R

u

v

w

=

Cp

q

3+4 parameters

R

q R q

u u

v v

w w

p C q

u u u

v v v

w w w

( )

pf

u

w

v

w

2

2

( )

p pp

p p

p p

u ww u

u

w w

v u wv v

w

w

f

Camera Registration

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff

Camera Registration

Camera Registration

Triangulation of Unreconstructed Points

1i 2i

i

This is a point that is not reconstructed yet while it matches with reconstructed images.

v

Bob Alice

X

bobP aliceP

u

Baseline

General camera pose

1 bob=1 1

u XP

Two 3D vectors are parallel.

bob1 1

u XP 0

bob1 1

u XP 0 : Knowns

: Unknowns

alice1

vP

mikeP

mike1

wP

Point Triangulation

v

Bob Alice

X

bobP aliceP

u

2

bob bob bob

bob bob bob

geom

alice alice alice

alice alice alice

/

/

/

/

u w x

v w yE

u w x

v w y


2

2

( )

X XX

X X

X X

u ww u

u

w w

v u wv v

ww

f

-1

T T( ) ( ) ( )( )

x x xx b x

x x x

f f ff

Nonlinear Point Triangulation

Nonlinear Point Triangulation

-1

T T( ) ( ) ( )( )

x x xx b x

x x x

f f ff

2

2

( )

X XX

X X

X X

u ww u

u

w w

v u wv v

ww

f

Damping factor (Levenberg-Marquardt algorithm)

-1

T T( ) ( ) ( )( )

x x xx I b x

x x x

f f ff

v

Bob Alice

X

bobP aliceP

u

pp

X

1

2

geom

2

2

3

2

3

u

P X P X

P X X

u

P

E

x y


( )

p,X

u

v

w

f w 2

2

( )

p pp,X

p p

p p

u ww u

u

w w

v u wv v

w

w

f

2

2

( )

X Xp,X

X X

X X

u ww u

u

w w

v u wv v

ww

f

Bundle Adjustment

J =

Cam 1 Pt 1 Pt 4

# of unknowns: 3x7+4x3 # of projections: 9 (not all points are visible from cameras)


Pt 1

Pt 2

Pt 3

Pt 4

Camera 1

Camera 2 Camera 3

Visibility Reasoning

T T

T

TT T

p p p X

X p X X

J J J J A BJ J

B DJ J J J

J J =T

T T - ( )

pJ J J b X

Xf

Normal equation:


1

d

D

dM

-1

1

-1

-1

d

D

dM


T

p

X

eA B p

B D X e

-1 -1

T

- -

p

X

eA B pI BD I BD

B D X0 I 0 I e

-1-1 T

T

--

p X

X

e BD epA BD B 0

XB D e

-1-1 T -1

-1 T

- -

-

p X

X

p A BD B e BD e

X D e B p

Jacobian Sparsity

-1

T T( ) ( ) ( )( )

p p pp I b p

p p p

f f ff p XJ J J

-1

1

-1

-1

d

D

dM# of points


pJ

XJ

d

( )b pf

T

T- ( )

p p

X X

J eb X

J ef

-1-1 T -1

-1 T

- -

-

p X

X

p A BD B e BD e

X D e B p

T T

T T T

p p p X

X p X X

J J I J JA B

B D J J J J I

-1

1

-1

-1

d

D

dM

d = d I

Bundle Adjustment - University of Minnesotahspark/CSci5980/Lec17_Supp...v Bob Alice X 1 P bob P...

Documents

Transcript of Bundle Adjustment - University of Minnesotahspark/CSci5980/Lec17_Supp...v Bob Alice X 1 P bob P...