MTP FY03 Year End Review – Oct 20-24, 2003 - 1 Visual Odometry Yang Cheng Machine Vision Group Section 348 ycheng@jpl.nasa.govycheng@jpl.nasa.gov Phone:

MTP FY03 Year End Review – Oct 20-24, 2003 - 1

Visual Odometry

Yang ChengMachine Vision Group

Section 348ycheng@jpl.nasa.gov Phone: 4-1857

12/16/2003

MTP FY03 Year End Review – Oct 20-24, 2003 - 2

Outline of this Talk

• Brief History• Algorithm• Software structure and interface• Software Features• Ground truth measurement• Some results• Future works

MTP FY03 Year End Review – Oct 20-24, 2003 - 3

Brief History

• H. Moravec’s PhD Thesis, “ Obstacle Avoidance and Navigation in the Real World by a Seeing Robot Rover, Stanford University, 1980

• Larry Matthies’ PhD Thesis, “Dynamic Stereo Vision”, Oct, 1989, CMU.

• A version of Visual Odometry in C was implemented in early 1990s in JPL.

• A C++ version of visual odometry was implemented by MTP Slope Navigation task led by Larry Matthies in 2001.

• The visual odometry has been ported to CLARAty and demonstrated onboard motion estimation on Rock 8 in 2002.

• The visual odometry has been used successfully on slip compensation by the slope navigation task.

• The visual odometry has been integrated officially to MER navigation software and demonstrated successfully in 2003.

• A few other versions of visual odometry were developed in academic and industry communities.

MTP FY03 Year End Review – Oct 20-24, 2003 - 4

Visual Odometry

Feature Selections

Feature Stereo Matching

Feature Gap Analysis

Feature Tracking

Rigidity Test

Least Medians SquareSchonemann Motion Estimation

Maximum Likelihood Motion Estimation

Visual Odometry Fusion

Input Images Input Motion

Output Motion

To use a (stereo) image sequence to track 3-D point features, or landmark, to estimate the motion of the vehicle.

MTP FY03 Year End Review – Oct 20-24, 2003 - 5

Feature (Landmark) Selection

• A landmark is a patch of image which must exhibit intensity variation that allows the landmark to be localized in subsequent image.

Input Image

LandmarksInterest Image

Forstner operator

MTP FY03 Year End Review – Oct 20-24, 2003 - 6

Feature Stereo Matching (Pyramid Searching)

Left Image

Right Image

MTP FY03 Year End Review – Oct 20-24, 2003 - 7

Feature Gap Analysis and Triangulation Error

Gap

Gap Analysis

Cameras

Error Vs Location Error Vs Correlation Peak

Error Vs Ray Gap

L C.

R.C.

L.C.

R.C.

Gaussian Error Model

MTP FY03 Year End Review – Oct 20-24, 2003 - 8

Feature Tracking

Left Image

Right Image

Feature pred

iction

Qp

Qc

Previous Frame

Current Frame

MTP FY03 Year End Review – Oct 20-24, 2003 - 9

Motion Estimation (Least-Squares Vs Maximum Likelihood)

• A closed form solution

• Rotation, R, with orthogonal constrain is estimated first

• Translation, T, is then estimated.

• Reflect the quality of the observations.

• It is fast.

• The resulting motion estimates can be substantially inferior.

• An nonlinear optimization solution

• Fully reflects the error model

• It is relative slow

• It needs an initial estimate.

• It is sensitive to outliers

• Its motion estimates in general is much superior than the least-squares estimation.

Least-squares Estimation Maximum Likelihood Estimation

ipici vTRQQ

ii

Tii

pici

eewTRq

TRQQe

),(

MTP FY03 Year End Review – Oct 20-24, 2003 - 10

Least-squares Estimation

ii

Tii

pici

eewTRq

TRQQe

),(

jTi

i jiii

Tiij

Tjiii

jTii

Ti

rrmrrleewmlTRq

jijirrrr

3

1

3

0,

)1(}{),,,(

}3,2,1{,01

Merit Function:

Orthogonal constrains:

Solutions:

][1

1

21

21

21

RQQw

TUVR

USVEQQw

AEQQwA

QQQQww

T

TTTcipii

picii

MTP FY03 Year End Review – Oct 20-24, 2003 - 11

Maximum Likelihood Estimation

Merit Function: iTiWeeM

W = covariance matrix of the feature i

Solutions: To linearize the merit function and determine the three attitude and three translation iteratively. Page 150 of Larry Matthies’ thesis

MTP FY03 Year End Review – Oct 20-24, 2003 - 12

Visual Odometry Interface

VOMotionStart( leftCam, rightCam, ParameterFile, leftImage, rightImage, leftDisp, InitialMotion)

VOMotion(leftImage, rightImage, leftDisp, InitialMotion, *estMotion)

Camera models: CAHV, CAHVOR, CAHVORE

Motion file: Position[3], attitude [3], covarence[6][6]

leftDisp: the disparity image generated by stereo processing.

Parameter File contains 48 parameters

MTP FY03 Year End Review – Oct 20-24, 2003 - 13

Some VO ParametersVO_MAX_NUM_VO_FEATURES 600 features

VO_MIN_NUM_VO_FEATURES 8 iteration

VO_VO_MAX_PIXEL_OFFSET 1 pixel

VO_MAX_VO_ITERATIONS 50 iteration

VO_VO_CORR_WINDOW_ROWS 9 pixel

VO_VO_CORR_WINDOW_COLS 9 pixel

VO_VO_TRACK_WINDOW_SIZE 50 pixel

VO_VO_SELECT_WINDOW_SIZE 9 pixel

VO_VO_NUM_IMAGE_PAIRS 4 images

VO_VO_IMAGE_ROWS 640 pixel

VO_VO_IMAGE_COLS 480 pixel

VO_SCHONEMANN_ITERATIONS 50 iteration

VO_VO_MIN_DIST_FEATURE 0.5 meter

VO_VO_MAX_DIST_FEATURE 20.0 meter

VO_VO_AFFINE_MATCH_FLAG 0 Boolean

VO_MAX_DELTA 0.000006

VO_DEFAULT_VO_MIN_CORRELATION 0.8 correlation

MTP FY03 Year End Review – Oct 20-24, 2003 - 14

Arroyo Data Collection

1. About 8 meters of image (20 cm step) sequence were collected at JPL arroyo in March, 2002.2. Onboard IMU, wheel odometry and other data were collected.3. Ground truth data (position and attitude) were collectedby totalstation.

MTP FY03 Year End Review – Oct 20-24, 2003 - 15

Semiautomatic Rover Position and Attitude Measurement

Pitch, Roll, Heading error < 0.5 degree;Position error < 3mm.

1

2

3

Three points are measured at each stop.

The position and attitude can be determined.

Total Station & Prism

MTP FY03 Year End Review – Oct 20-24, 2003 - 16

Motion Estimation (X)

0

1

2

3

4

5

6

7

8

9

0 5 10 15 20 25 30 35 40 45 50

Image Step

X (

m)

VO (Rear)

WO

VO ( front)

GT

MTP FY03 Year End Review – Oct 20-24, 2003 - 17

Motion Estimation (Y)

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

0 5 10 15 20 25 30 35 40 45 50

Image Step

Y (

m)

VO (rear)

VO (front)

GT

WO

MTP FY03 Year End Review – Oct 20-24, 2003 - 18

Motion Estimation (Z)

-0.7

-0.6

-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0 5 10 15 20 25 30 35 40 45 50

Image Step

Z(m

)

VO(rear)

WO

VO( front)

GT

MTP FY03 Year End Review – Oct 20-24, 2003 - 19

Heading Estimation

-15

-10

-5

0

5

10

15

20

0 5 10 15 20 25 30 35 40 45 50

Image Step

Hea

din

g (

deg

)

VO(Rear)

VO (Front)

GT

MTP FY03 Year End Review – Oct 20-24, 2003 - 20

Roll Estimation

-5

0

5

10

15

20

25

0 5 10 15 20 25 30 35 40 45 50

Image Step

Rx(

Deg

) Rx(Rear)

Rx(Front)

Rx(GT)

MTP FY03 Year End Review – Oct 20-24, 2003 - 21

-5

0

5

10

15

20

0 5 10 15 20 25 30 35 40 45 50

Image Step

RY

(deg

) VO(Rear)

VO(Front)

GT

Pitch Estimation

MTP FY03 Year End Review – Oct 20-24, 2003 - 22

VO Fusion (front and rear Has Camera )

-0.02

-0.015

-0.01

-0.005

0

0.005

0.01

0.015

0.02

0.025

0.03

0 5 10 15 20 25 30 35 40 45 50

Rear Rear + front

Absolute Error (x)

MTP FY03 Year End Review – Oct 20-24, 2003 - 23

VO Fusion (front and rear Has Camera )

Image Step

-0.02

-0.01

0

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

0 5 10 15 20 25 30 35 40 45 50

Ab

solu

te E

rro

r (m

)

Rear Rear + front

Absolute Error (Y)

MTP FY03 Year End Review – Oct 20-24, 2003 - 24

VO Fusion (front and rear Has Camera )

Comparison Between Single VO and Fusioned VO

-0.14

-0.12

-0.1

-0.08

-0.06

-0.04

-0.02

0

0 5 10 15 20 25 30 35 40 45 50

Image Step

Ab

so

lute

Err

or

(m)

Rear Rear + front

Absolute Error (Z)

MTP FY03 Year End Review – Oct 20-24, 2003 - 25

Absolute Error (Heading)

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

0 5 10 15 20 25 30 35 40 45 50

Image Step

He

ad

ing

Err

or

Rear Rear + front

MTP FY03 Year End Review – Oct 20-24, 2003 - 26

Absolute Error (Pitch)

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

0 5 10 15 20 25 30 35 40 45 50

Image Step

Pit

ch

Ab

olu

te E

rro

r (d

eg

)

Rear Rear + front

MTP FY03 Year End Review – Oct 20-24, 2003 - 27

Absolute Error (Roll)

Roll Error

-1.5

-1

-0.5

0

0.5

1

1.5

0 5 10 15 20 25 30 35 40 45 50

Image Step

Ab

olu

te E

rro

r (d

eg

)

Rear Rear + front

MTP FY03 Year End Review – Oct 20-24, 2003 - 28

Heading Estimation

0 4

7 10

MTP FY03 Year End Review – Oct 20-24, 2003 - 29

0

5

10

15

20

25

30

35

1 2 3 4 5 6 7 8 9

Steps

He

ad

ing

(de

g)

VO(Rear)

VO(Front)

GT

Heading Estimation (1)

MTP FY03 Year End Review – Oct 20-24, 2003 - 30

05

1015

20253035

1 2 3 4 5 6 7 8 9

Image Step

Hea

din

g (

deg

)

Small Angle Steering Large Angle Steering

Heading Estimation (2)

MTP FY03 Year End Review – Oct 20-24, 2003 - 31

Field Test

Location• Johnson Valley, Mojave Desert, CA• Sandy slopes of up to 20-25° slopes

Logistics• 4 days – 4 people

– 1.5 days of setup and break down

– 2.5 days of experimentation

Motivation• Mars Yard is too small and

has no slopes– The size is mostly a factor for

visual odometry which looks far beyond traverse distance

MTP FY03 Year End Review – Oct 20-24, 2003 - 32

Sample of images

MTP FY03 Year End Review – Oct 20-24, 2003 - 33

Field Test Results

Visual Odometry vs. Ground Truth

0 5 10 15 20 25

-4

-2

0

2

x (meters)

y (m

ete

rs)

ground truthvisual odometry

area expanded (and rotated) in next slide

• Error (0.37 m) is less than 1.5% of distance traveled (29 m)

• Ground truth data collected with a Leica Total Station and four rover mounted prisms

MTP FY03 Year End Review – Oct 20-24, 2003 - 34

Field Test Results

Slip Compensation/Path Following Results

carrot heading

visual odometry pose

kinematics pose

desired path

0 2 4 6 8 10

0

0.5

1

1.5

2

x (meters)

y (m

eter

s)

expanded below

5 5.5 6 6.5 7 7.5 81.2

1.4

1.6

1.8

2

2.2

x (meters)

y (m

eter

s)

• There is a noticeable bias between the visual odometry pose and the kinematics pose in the y direction of many estimates; this is due to the downhill slippage of the rover; this bias is being compensated for in the slip compensation algorithm

MTP FY03 Year End Review – Oct 20-24, 2003 - 35

MER VO Test (Rough)

MTP FY03 Year End Review – Oct 20-24, 2003 - 36

MER Test

MTP FY03 Year End Review – Oct 20-24, 2003 - 37

Target Approach

(a)

Target

(b)

Designated Target

Target Tracking

time = t2(avoiding an obstacle)

time = t1

1st Frame 37th Frame after 10 m

MTP FY03 Year End Review – Oct 20-24, 2003 - 38

Integrated 2D/3D Tracker

Not tested onboard in the integrated system

Stereo ProcessStereo Process

IMU

Hazard Cameras

NavigatorNavigator

R8 LocomotorR8 Locomotor

Motion Cmd

Navigator

DepthMap

Harris Multiple Feature Extractor

Harris Multiple Feature Extractor

Rover Pose Estimate

Visual Odometry

Visual OdometryNon-flat Surface

Filter

Non-flat Surface Filter

Rover Pose Estimate + uncertainty?

Wheel odo

Compute MastPointing Angle

Compute MastPointing Angle

Tilt

Sub windowStereo Map

Sub windowStereo Map

Pan & Tilt Angles

Large Uncert-ainty?

Large Uncert-ainty?

DesignateTarget (DT)(r,c) in right image

No

DT(r,c) to DT (x,y,z)

Mast Cameras

Yes

Output: DT(r,c) at t0+ Δt

Mast ImagesDepth Disparity

Expected2D location

Pan-TiltController

Pan-TiltController

Mast KinematicsMast KinematicsDesignated Point Visual Tracking• Track once w/ 4 mm camera• Seed & track again w/ 16 mm camera

Pointing Vector

1st Tier 2D location estimate

1st Tier 2D location estimate

Adaptive View-Based Matching

Adaptive View-Based Matching

Normalized Cross Correlation

Normalized Cross Correlation

Surface Normals Grow Feature Win

Surface Normals Grow Feature Win

Single Pt Stereo

Single Pt Stereo

DesignatedTarget

Template

2nd Tier 2D location estimate

Affine TrackerAffine Tracker

MTP FY03 Year End Review – Oct 20-24, 2003 - 39

Tracking Results over Rough Terrain

Tracking Video

View from 4 mm camera

View from 16 mm camera

MTP FY03 Year End Review – Oct 20-24, 2003 - 40

Ground Truth Data Collection System

• Automatically tracks the position of 1 prism and finds the 3 other prisms when rover stops

• Simplifies and speeds the collection of ground truth data in field tests

• Locates rover frame in world frame and the initial rover frame

• +/- 2mm position accuracy

• +/- 0.3º orientation accuracy

MTP FY03 Year End Review – Oct 20-24, 2003 - 41

Future Works

• A real-time Visual Odometry• Data Fusion with other sensors (IMU …) to achieve better

estimation• Visual Odometry Applications