Incremental Light Bundle Adjustment for Robotics Navigation · Navigation Todd Lupton and Salah...

1

Incremental Light Bundle Adjustment for Robotics Navigation

Vadim Indelman, Andrew Melim, Frank Dellaert

Robotics and Intelligent Machines (RIM) Center College of Computing

Georgia Institute of Technology

2

Introduction

 Robot Navigation: Recover the state of a moving robot over time through fusion of multiple sensors, including a monocular camera

Indelman et al., Incremental Light Bundle Adjustment for Robot Navigation

Left image courtesy of Georgia Tech Research Institute Right image courtesy of Chris Beall

Simultaneous Localization and Mapping (SLAM)

3

Vision-Aided Robot Navigation

 Fusion of monocular image measurements and IMU measurements –  Full joint pdf:

Image from: http://www.tnt.uni-hannover.de/project/motionestimation

: number of robot states : set of observed 3D points at state i N

: Robot state at time i (pose and velocity)

: j-th 3D point

: Image observation

xi

lj

bi : IMU Bias at time i

zIMUi,i�1 : IMU measurement

zV ISi,j

Mi


p(X,L,B|Z) /NQ

i

0

@p(xi|xi�1, bi, z

IMUi,i�1 )

Q

j 2 Mi

p(zV ISi,j |xi, lj)

1

A

4

Vision-Aided Robot Navigation

 Fusion of monocular image measurements and IMU measurements –  Full joint pdf:

Image from: http://www.tnt.uni-hannover.de/project/motionestimation

: number of robot states : set of observed 3D points at state i N

: Robot state at time i (pose and velocity)

: j-th 3D point

: Image observation

xi

lj

⇡ (xi, lj).

= Ki

⇥Ri ti

⇤lj

Projection of a 3D point into the image plane Mahalanobis squared distance kak2⌃

.= aT⌃�1a

 Assuming Gaussian distributions: –  MAP estimate is obtained by bi : IMU Bias at time i

zIMUi,i�1 : IMU measurement

zV ISi,j

Mi

X⇤, Y ⇤, B⇤


X⇤, L⇤, B⇤=

argmax

X,L,Bp(X,L,B|Z)

J(X,L,B).

=NP

i = 1

0

@||xi � pred(xi�1, bizIMUi,i�1 )||2PIMU +

P

j 2 M1

||zV ISi,j � ⇡(xi, lj)||2PV IS

1

A

p(X,L,B|Z) /NQ

i

0

@p(xi|xi�1, bi, z

IMUi,i�1 )

Q

j 2 Mi

p(zV ISi,j |xi, lj)

1

A

5

Factor Graph Representation Factor graph: a graphical representation of the joint pdf factorization

p (z

i,j

|xi

, l

j

) / exp

✓�1

2

kzi,j

� ⇡ (x

i

, l

j

)k2⌃

◆.

= f

proj

(x

i

, l

j

)

p (X|Z) /Y

s

fs (Xs)

 Full SLAM pdf:

p(xi|xi�1, bizIMUi,i�1 ) ⇥ exp(�1

2

||xi � pred(xi�1, bi, zIMUi,i�1 ||2�IMU )

.

= f

IMU(xi, xi�1, bi)

x1 x2 x3 x4 x5 x6

b2 b3 b4 b5b1

f IMU f IMU f IMU f IMU f IMU

f bias f bias f bias f bias

l1

f proj f proj

f proj

f bias(bk+1, bk)

.= exp

✓�1

2

��bk+1 � hb(bk)

��2�b

◆

The naïve IMU factor can add a significant number of

unnecessary variables!


[Kschischang et al. 2001 ToIT]

�.= {X,L,B}

p(X,L,B|Z) /NQ

i

0

@p(xi|xi�1, bi, z

IMUi,i�1 )

Q

j 2 Mi

p(zV ISi,j |xi, lj)

1

A

6

Incremental Light Bundle Adjustment (iLBA) for Robot Navigation

 Problems! –  3D structure is expensive to compute (and not necessary for navigation):

•  Algebraically eliminate 3D points using multi-view geometry constraints

•  Significantly reduce the number of variables for optimization

•  3D points can always be reconstructed (if required) based on optimized camera poses

–  High rate sensors introduce large number of variables: •  Utilize pre-integration of IMU to reduce the number of variables [Lupton et al., TRO 2012]

•  Incremental inference requires only partial re-calculation –  Update factorization rather than compute from scratch


7

xk

xl

xm

View l

Viewk

Viewm

3D point

qkql

qm

tk!ltl!m

Three-View Constraints

  Theorem: Algebraic elimination of a 3D point that is observed by 3 views k,l and m leads to:

Epipolar constraints

Scale consistency

 Necessary and sufficient conditions

: translation from view i to view j

: rotation from global frame to view i Ri

g2v (xk, xl, zk, zl).

= qk · (tk!l ⇥ ql) = 0

g2v (xl, xm, zl, zm).

= ql · (tl!m ⇥ qm) = 0

g3v (xk, xl, xm, zk, zl, zm).

= (ql ⇥ qk) · (qm ⇥ tl!m)� (qk ⇥ tk!l) · (qm ⇥ ql) = 0

ti!j

qi.= RT

i K�1i zi

  LBA cost function:

hi 2 {g2v, g3v}

JLBA (X).=

NhX

i

khi (X,Z)k2⌃i

[Indelman et al., TAES 2012]

  Third equation – relates between the magnitudes of and tl!m tk!l


V. Indelman, P. Gurfil, E. Rivlin, H. Rotstein, “Real-Time Vision-Aided Localization and Navigation Based on Three-View Geometry’’, IEEE Transactions on Aerospace and Electronic Systems, 2012

8

Vision Only : Light Bundle Adjustment (LBA)

 LBA cost function:

–  : i-th multi-view constraint •  Involves several views and the corresponding image observations

–  : An equivalent covariance

–  : Jacobian with respect to image observations

⌃i ⌃i = Ai⌃ATi

Ai

hi

Number of optimized variables: 6N + 3M 6N

JLBA (X).=

NhX

i

khi (X,Z)k2⌃i

 Multi-view constraints - Different formulations in literature –  Epipolar geometry, trifocal tensors, quadrifocal tensors etc.

–  Independent relations exist only between up to three cameras [Ma et al., 2004]

–  Here, three-view constraints formulation is used •  Originally developed for navigation aiding [Indelman et al., TAES 2012]


9

Pre-Integrated IMU Factors

x1 x2 x3 x4 x5 x6

b2 b3 b4 b5b1

f IMU f IMU f IMU f IMU f IMU

f bias f bias f bias f bias

x1 x6

b1

f Equiv

Equivalent IMU (non-linear) factor

 Pre-integrate IMU measurements and insert equivalent factors only when inserting new LBA factors into the graph

 Components of are expressed in body-frame, not navigation frame, which allows relinearization of the factor without repeated computation

 Significantly reduces graph size, and subsequently time for elimination.

f

Equiv (xj , xi, bi).

= exp

✓�1

2

��xj � h

Equiv (xi, bi,�xi!j)��2�

◆�xi!j

.=��pi!j ,�vi!j , R

ij

= �

�ZIMUi!j , bi

�,

�xi!j


Todd Lupton and Salah Sukkarieh, “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions ’’, IEEE Transactions on Robotics, 2012

[Lupton et al., TRO 2012]

10

Second Component - Incremental Inference

 Example:

Factorized Jacobian matrix

Linearization

Factorization Elimination order Linearization and elimination

 When adding new variables\factors, calculations can be reused –  Factorization can be updated (and not re-calculated from scratch)

[Kaess et al., 2012]

x1 x6

b1

f Equiv

2-view factor

f bias

x12

b2

2-view factor

3-view factor

f EquivA =

2

6666664

⇥ ⇥⇥ ⇥

⇥ ⇥ ⇥⇥ ⇥ ⇥

⇥ ⇥ ⇥⇥ ⇥

3

7777775

x1 x6 x12 b1 b2

Jacobian matrix

x1 x6

b1

x12

b2R =

2

66664

⇥ ⇥ ⇥ ⇥⇥ ⇥ ⇥

⇥ ⇥ ⇥⇥ ⇥

⇥

3

77775

x1 b1 x6 b12 x12

x1, b1, x6, b2, x12


b2

11

R =

2

666666664

⇥ ⇥ ⇥ ⇥⇥ ⇥ ⇥

⇥ ⇥ ⇥⇥ ⇥ ⇥

⇥ ⇥ ⇥⇥ ⇥

⇥

3

777777775

x1 b1 x6 b2 x12 b3 x20

Incremental Inference in iLBA (Cont.)

Linearization

 Example: New camera and factors are added

 What should be re-calculated?

–  Nodes in all paths that lead from the last-eliminated node to nodes involved in new factors

–  Efficiently calculated using Bayes tree [Kaess et al., 2012]

A =

2

6666666666664

⇥ ⇥⇥ ⇥

⇥ ⇥ ⇥⇥ ⇥ ⇥

⇥ ⇥ ⇥⇥ ⇥

⇥ ⇥ ⇥⇥ ⇥

⇥ ⇥

3

7777777777775

x1 x6 x12 b1 b2 x20 b3


12

iLBA for Robotics – Monte-Carlo Study

0 50 100 1500

0.05

0.1

6 q

[deg

]

0 50 100 1500

0.02

0.04

0.06

0.08

6 e

[deg

]

0 50 100 1500

0.2

0.4

6 s

[deg

]

Time [sec]

0 50 100 1500

5

10

15

X Ax

is [m

g]

0 50 100 1500

5

10

Y Ax

is [m

g]

0 50 100 1500

5

10

15

Z Ax

is [m

g]

Time [sec]

0 50 100 1500

5

10

Nor

th [m

]

0 50 100 1500

2

4

6

8

East

[m]

0 50 100 1500

1

2

3

Hei

ght [

m]

Time [sec]

m LBAm Full BASqrt. Cov. LBASqrt. Cov. Full BA


0

500

1000

1500

−800−600

−400−200

0200

400

−400

−200

0

200

North [m]East [m]

Hei

ght [

m]

TrueInertial

Position 1-sigma errors and sqrt. covariance

Acceleration bias 1-sigma errors and sqrt. covariance

Euler Angles 1-sigma errors and sqrt. covariance

13

0 100 200 300 400 500 600 7000

5

10

15

20

25

Scenario Time [sec]

Proc

essi

ng ti

me

[sec

]

LBA + IMUBA + IMU

iLBA for Robotics – Simulation Results

 Scenario: –  Single camera

–  IMU

Estimated trajectory

0 100 200 300 400 500 600 7000

5

10

15

20

Scenario Time [sec]

Posi

tion

erro

rs [m

]

LBA + IMUBA + IMU

Position estimation errors

Processing time

Scenario (GE for illustration)

−1000 −500 0 500 1000 1500 2000−1000

−800

−600

−400

−200

0

200

400

600

East [m]

Nor

th [m

]

LBA + IMU BA + IMU Ground Truth IMU only

650 750300400500

150 250

203040

120014001600−680−660−640


14

Conclusions

 Algebraic elimination of 3D points significantly reduces the size of the optimization problem and provides speed up to online robot navigation

 Use of pre-integration methods for high-frequency inertial measurements also reduces the size of the problem

 Accuracy is similar to full SLAM

 At least 2-3.5x speed up in computation time


 Code and datasets are available from the author’s website –  http://www.cc.gatech.edu/~vindelma

Incremental Light Bundle Adjustment for Robotics Navigation · Navigation Todd Lupton and Salah...

Documents

Transcript of Incremental Light Bundle Adjustment for Robotics Navigation · Navigation Todd Lupton and Salah...