1 Motion estimation from image and inertial measurements Dennis Strelow and Sanjiv Singh Carnegie Mellon University

1

Motion estimation from image and inertial measurements

Dennis Strelow and Sanjiv Singh

Carnegie Mellon University

2

Introduction (1)

micro air vehicle (MAV) navigation

AeroVironment Black Widow AeroVironment Microbat

3

Introduction (2)

mars rover navigation

Mars Exploration Rovers (MER) Hyperion

4

Introduction (3)

robotic search and rescue

RhexCenter for Robot-Assisted Search and Rescue, U. of South Florida

5

Introduction (4)

NASA ISS personal satellite assistant

6

Introduction (5)

Each of these requires:

six degree of freedom motion

in unknown environments

without GPS or other absolute positioning

over the long term

…and in many cases…

small, light, and cheap sensors

7

Introduction (6)

If we adopt a camera as our sensor, estimate…

the vehicle (i.e., sensor) motion

…and as a by-product:

the sparse structure of the environment

8

Introduction (7)

One paradigm for estimating the motion and sparse structure uses two steps:

tracking: track image features through the image sequence

estimation: find 6 DOF camera positions and 3D point positions consistent with the image features

9

Introduction (8)

10

Introduction (8)

Lucas-Kanade

11

Introduction (9)

12

Introduction (10)

13

Introduction (10)

bundle adjustment, a.k.a. nonlinear shape-from-motion

14

Introduction (11)

This example sequence is benign:

small number of images

large number of easily tracked image features

all points visible in all frames

…and the estimated motion and scene structure are still ambiguous

15

Introduction (12)

Sequences from navigation applications much harder:

long observations sequences

small number of features, which may be poorly tracked

each feature visible in a small section of the image stream

16

Introduction (13)

We have been working on both:

tracking

estimation

This talk:

mostly estimation

17

Introduction (14)

18

Introduction (15)

Image measurements only

often the only option

Image and inertial measurements

can disambiguate image-only estimates

more complex: requires more calibration, estimation of additional unknowns

19

Introduction (16)

Batch estimation:

uses all of the observations at once

all observations must be available before computation begins

Online estimation:

observations are incorporated as they arrive

suitable for long or “infinite” sequences

20

Introduction (17)

Conventional images:

images from standard cameras

model as pinhole projection with radial distortion

Omnidirectional images:

images that result from combining a conventional camera with a convex mirror

requires a more complex projection model

21

Introduction (18)

22

Introduction (19)

23

Outline

Background: image-only batch estimation

Image-and-inertial batch estimation

Online estimation

Experiments

Future work

24

Background: image-only batch estimation (1)

25


Tracking provides image location xij for each point j that appears in image i

26


Suppose we also have estimates of:

the camera rotation ρi and translation ti at time of each image

Three dimensional point positions Xj of each tracked point

Then the reprojections are:))(( iji tXR

27


28


29


So, minimize:

with respect to all the ρi, ti, Xj

“Bundle adjustment”: use iterative nonlinear minimization on this error

)),)(((,

image ijijji

i xtXRDE

30


Bundle adjustment pros:

any projection model can be used

some missing points can be tolerated

can be extended

Bundle adjustment cons:

requires some initial estimate

slow

31


Batch estimation cons:

all observations must be in hand before estimation begins

Image-only estimation cons:

sensitivity to mistracking

cannot recover global scale

32

Outline



Online estimation

Experiments

Future work

33

Image-and-inertial batch estimation (1)

34


Image and inertial measurements are highly complementary

35


Image and inertial measurements are highly complementary

Inertial measurements can:

establish the global scale

provide robustness if:• too few features• features infinitely far away• features in an “accidental” configuration

36


Image measurements can:

reduce inertial integration drift

separate effects of:• rotation• gravity• acceleration• biasin inertial readings

37


Gyro measurements:

ω’, ω: measured and actual angular velocity

bω: gyro bias

n: gaussian noise

nb '

38


Accelerometer measurements:

a’, a: measured and actual acceleration

g: gravity vector

ba: accelerometer bias

n: gaussian noise

nbgaRa' aT )((

39


Minimizes a combined error:

inertialimagecombined EEE

40


Image term Eimage is the same as before

41


Inertial error term Einertial is:

1

1n,translatio

1

1velocity,

1

1rotation,

ntranslatiovelocityrotationinertial

f

ii

f

ii

f

ii

E

E

E

EEEE

42



1

1n,translatio

1

1velocity,

1

1rotation,


f

ii

f

ii

f

ii

E

E

E

EEEE

43



1

1n,translatio

1

1velocity,

1

1rotation,


f

ii

f

ii

f

ii

E

E

E

EEEE

44


)),...,,(,( 11n,translatio iiitii tItDE

timeτi-1 (time of image i - 1)

ti-1 = t(τi-1)

ti = t(τi)

I(τi-1, τi, …, ti-1)

τi (time of image i)

tran

slat

ion

45


timeτ0

tran

slat

ion

τ1 τ2 τ5 τ3 τ4 τf-3 τf-2 τf-1

1

1n,translationtranslatio

f

iiEE

46


1

1n,translatio

1

1velocity,

1

1rotation,


f

ii

f

ii

f

ii

E

E

E

EEEE

47


1

1n,translatio

1

1velocity,

1

1rotation,


f

ii

f

ii

f

ii

E

E

E

EEEE

48


It(τi-1, τi ,…, ti-1) depends on:

τi-1, τi (known)

all inertial measurements for times τi-1< τ < τi (known)

ρi-1, ti-1

camera linear velocities: vi

g

bω, ba

49


The combined error function (image-and-inertial) is then minimized with respect to:

ρi, ti

Xj

50


The combined error function (image and inertial) is then minimized with respect to:

ρi, ti

Xj

vi

g

bω, ba

51


52


accurate motions even when image-only and inertial-only motions are poor

53


In addition:

recovers good gravity values, even with poor initialization

global scale often recovered with less than 5% error

54

Outline



Online estimation

image measurements only

image and inertial measurements

Experiments

Future work

55

Online algorithms (1)

56


Batch estimation:

uses all of the observations at once

all observations must be available before computation begins

Online estimation:

observations are incorporated as they arrive

suitable for long or “infinite” sequences

57


Our online algorithms are iterated extended Kalman filters (IEKFs)

Some components of the IEKF when applied to vehicle motion:

state estimate distribution gaussian: mean and covariance

motion model

measurement model

58


initializationprior distribution on unknowns

timestamped observations

time propagationprior distribution on unknowns

timestamp kinit + 1

measurement updateprior distribution on unknowns

observation kinit + 1

prior distribution on unknowns


timestamp i


observation i

1, …, kinit

59

Online estimation (5): image-only

For the image-only online algorithm, the unknowns (state) are:

ρ(τ), t(τ)

currently visible point positions Xj

60

Online estimation (6): image-only, cont.


timestamped observations1, …, kinit

apply the batch image-only algorithm to observations 1, …, kinit

61




timestamp i

assume that ρ, t are perturbed by gaussian noise

assume that Xj are unchanged

62


n

tXR

tXR

x

x

z

pp

))()((

))()((

1

0

1

0



observation i

To update given an image measurement z, use the reprojection equation:

63


For points that become visible after online operation has begun…

…adapt Smith, Self, and Cheeseman’s method for SLAM

64

Online estimation (10): image-and-inertial


timestamped observations


timestamp kinit + 1


observation kinit + 1



timestamp i


observation i

1, …, kinit

65

Online estimation (11): image-and-inertial, cont.

Augmented state includes:

ρ(τ), t(τ)


66



ρ(τ), t(τ)


v(τ)

g

bω, ba

67



ρ(τ), t(τ)


v(τ)

g

bω, ba

camera system angular velocity: ω(τ)

world system linear acceleration: a(τ)

68



timestamped observations1, …, kinit

apply the batch image-and-inertial algorithm

69


assume that ω, a are perturbed by gaussian noise

assume that Xj, g, bω, ba are unchanged



timestamp i

70


nbgaR

b

az

aT

)()('

'



observation i

use the inertial sensor model we’ve seen:

71

Outline


Image and inertial batch estimation

Online estimation

Experiments

Hyperion

CMU crane

Future work

72

Experiments (1): Hyperion

73

Experiments (2): Hyperion, cont.

74


75


76


77


78


79

Experiments (7): CMU crane

Crane capable of translating a platform…

…through x, y, z…

…through a workspace of about 10 x 10 x 5 m

80

Experiments (8): CMU crane, cont.

y tr

ansl

atio

n (m

eter

s)

x translation (meters)3.00.0-3.0

-3.0

0.0

3.0

(x, y) translation ground truth

81

Experiments (9): CMU crane, cont.z

(m)

time

3.5

4.5

z translation ground truth

No change in rotation

82


83


Hard sequence:

• Each image contains an average of 56.0 points

• Each point appears in an average of 62.3 images (4.4% of sequence)

• Image-and-inertial online algorithm applied

• 40 images used in batch initialization

84


85


Estimated z camera translations

86


6 DOF errors, after scaled rigid alignment:

Rotation: 0.14 radians average

Translation: 31.5 cm average (0.9% of distance traveled)

Global scale error: -3.4%

87

Outline


Image and inertial batch estimation

Online estimation

Experiments

Future work

88

Future work (1)

For long sequences:

no single external point is always visible

Estimated positions will drift due to…

random and gross observation errors

modeling errors

suboptimality in online estimation

89

Future work (2)

90

Future work (3)

91

Future work (4)

Need to incorporate some advances from the SLAM community to deal with this issue…

…in particular, reacquiring revisited features

92

Future work (5)

Two issues:

(1) recognizing revisited features

(2) exploiting revisited features in the estimation

Lowe’s SIFT features a good candidate for (1)

93

94

Thanks!

Pointers to:

these slides

related work by others

related publications by our group

feature tracking movies

VRML models

at:http://www.cs.cmu.edu/~dstrelow/uw

http://www.cs.cmu.edu/~dstrelow/uw

Documents

1 Motion estimation from image and inertial measurements Dennis Strelow and Sanjiv Singh Carnegie Mellon University