3D SLAM for Omni-directional Camera Yuttana Suttasupa Advisor: Asst.Prof. Attawith Sudsang.

3D SLAM for Omni-directional Camera

Yuttana Suttasupa

Advisor: Asst.Prof. Attawith Sudsang

Introduction Localization

Robot can estimate its location with respect to landmarks in an environment

Mapping Robot can reconstruct the position of landmarks that its

encounter in an environment

SLAM Robot build up a map and localize itself simultaneously

while traversing in an unknown environment

2

The Problem Propose SLAM method for a hand-held omni-

directional Camera Omni-directional camera move freely in an unknown

indoor environment without knowing of camera motion model

Using only bearing data from omni-images and no need any initialize information

Reconstruct 3D camera path and 3D environment map (landmark-based)

3

The Problem Input

a captured image sequence from an omni-directional camera

4

5

The Problem Output

a camera state - 3D position and direction an environment map - 3D landmark positions

Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem

Image Processing SLAM Features and reference frames management

Experimental Results Result Evaluation

6

Omni-directional Camera Our omni-directional camera

Two parabolic mirrors CCD camera with 640×480 pixel @ 29.97 Hz 360° horizontal field of view -5° to 65° vertical field of view

7

Omni-directional Camera Normal camera

Omni-directional camera

8

Omni camera Calibration Find a mapping function from 2D image to 3D

object Using Omnidirectional Camera Calibration Toolbox

(Scaramuzza et al., 2006)

9




10

EKF SLAM Using extended kalman filter to solve SLAM

problem Assume a robot position and a map probability

distributions as gaussian distributions

Predict a robot position and landmarks distributions using a robot motion model

Correct the distributions using an observation model

11

EKF SLAM The distribution representation

Initial state Assume a robot position distribution with some value at the initial

state

12

Tnkk mmcx 0state

covariance

mmmc

cmcck PP

PPP

robot

probability distribution

EKF SLAM Predict state

Using a robot motion model to predict a robot position

13

kkkk Wuxfx ),( 1

kTkkkk QFPFP

1

Predicted state

Predicted estimate covariance

robot

EKF SLAM Correction state

Using an observation model to update a robot position and landmark positions

14

),ˆ(,, ikkiki mxhzy

kkkk PHKIP )(

Observation model

Updated estimate covariance

kikki vmxhz ),(,

kkkk yKxx ˆˆ

Innovation residual

Updated state estimate

robot landmarkmeasurement

kTkkkk RHPHS

1 kTkkk SHPK

Innovation covariance

Optimal Kalman gain

adjustment




15

Introduction to Problem Feature detection Problem

How a computer recognizes objects from an image

Feature association Problem How can we find feature

relations between two images

16

Introduction to Problem Observability Problem

A camera given only a bearing-only data How can we estimate a high dimensional state with low

dimensional measurements

17

Landmark

Camera

How far is it?




18

Solution to Problem The proposed algorithm includes 3 steps

Image Processing Detect features Find feature associations Calculate feature measurements

SLAM Apply measurement data to SLAM

Features and reference frames management Add and remove features from SLAM state Add and remove reference frames from SLAM state

19

Solution to Problem System Coordinate

World Frame Camera Frame Reference Frames

20

landmark

World Frame

Camera Frame

Reference Frame

Cy Wy

1Ry 2Ry

Solution to Problem SLAM State

Camera state – represent camera frame Reference frame states – represent reference frames Landmark states – represent landmark positions

21

)ˆ( cx)ˆ( rx

)ˆ( yx




22

Image Processing Input

an image from an omni-directional camera

Old SLAM state

Output Feature measurement Feature association

23

Image Processing Feature detection (for new features)

Using point features Finds corners in the image using harris corner detector

24

Image Processing Feature associations

Describe which landmark that the feature in current image is associated to

Find the relation between a current image and old features in an old image

Using optical flow to track features

Using template matching to refine a feature position

25

Image Processing Feature associations – features tracking

Tracking features from a previous image to get a current features position

Using pyramid Lucas-Kanade optical flow

26

Image Processing Feature associations – feature positions refinement

Track features using optical flow may cause a feature drift

Using pyramid template matching to correct a feature position

27current image with a drifted feature

patch

feature

search region

result after refinement using template matching

Image Processing Feature associations – feature position refinements

Select patch from a reference image Patch rotation and scale may not match Transform function may need to apply to patch

28

Reference image

Current image

Not match

Match

Image Processing Feature associations – feature position refinements

Find transform function by project 3D patch creating from a current image to a reference image

29Current image Reference image

Image sphere

3D patch

Image Processing Find transform function

Project every patch pixel may lead to a computational cost problem

Use perspective transform as a transform function instead

Need 4 project points to calculate a perspective transform function

30Real distortion Perspective distortion

Image Processing Feature associations – example

31

Image Processing Feature measurements

Using feature points in omni-image to be a measurement data

Feature points must be converted into bearing-only measurement in the form of yaw and pitch angles

32

)/arctan(

)/arctan( 22

xy

yxz

rr

rrr

z

y

x

landmark

)(

)(

Ray (r)




33

Using EKF SLAM to estimate Camera state, Reference frame states and Landmark states

Prediction Determine how a camera move Find state transition model (camera motion model)

Correction How to measurement a landmark Find observation model

34

Simultaneous localization and mapping (SLAM)


Input Measurement data from

omni-image

Output Estimated SLAM state

Camera state Reference frame states Landmark states

35

Prediction Determine how a camera move But the camera motion is unpredictable Assume that a camera can move freely in any direction

with some limit velocity

36


Before predict After predict

Correction Using a bunch of measurement (include current

measurements data and old measurements data at reference frame) to update SLAM state

37


landmark

Current cameraReference frame

Reference frame

Correction

38


Tui

ui

Cii zzz )2()1(

)/arctan(

)/arctan(

xy

yyxxzXi yy

yyyyyz

Measurement data for landmark i

Observation model for each measurement

y'

landmark

is a landmark position in X coordinatey

Correction step can separate in 2 parts Camera and reference frames Correction Landmarks Correction

39


Camera and reference frames Correction Assume that the measurement data can measurement

landmark positions accurately The correction affects only a camera state and

reference frame states

40


Before correction After correction

Landmarks Correction Assume that the camera state is accurate The correction affects only landmark states

41


Before correction After correction




42

Features and reference frames management

Remove features Feature points is out of image bound The landmark position is not accurate enough

43

the feature of this landmark is out of bound


Add features Add new features

using harris corner detector to detect a new feature Add new features when we have a new reference frame

Add old features Consider that the old landmark may be appear in the omni

image again

44


Add new features Add new landmarks to SLAM state Estimate a landmark position by assume a large

variance for a range data

45


Add old features project an old landmark to

the current image

check if a feature available in the image using template matching

46

landmark

Image sphere

feature


Add reference frame When no suitable reference frames for feature tracking When landmark number is below some threshold

Select a current camera state as a new reference frame

47




48

Experimental Results

49


50


51


52


53


54




55

Result Evaluation Localization Evaluation

2D localization evaluation 3D localization evaluation

Mapping Evaluation

56

Result Evaluation 2D Localization Evaluation

Using wiimote as a bird eye view camera Detect IR point on the omni camera while traversing in

2D plane by a mobile robot

57

IR point


58


59


60

1 12 23 34 45 56 67 78 89 1001111221331441551661771881992102212322432542652762872983093203313420

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

0.5

error

average


Using wiimote attach with an omni-directional camera to localize the 3D camera position related to reference IR board

61


62


63


64

1 15 29 43 57 71 85 99 1131271411551691831972112252392532672812953093233373513653793934074214354494630

0.05

0.1

0.15

0.2

0.25

0.3

0.35

error

average

Result Evaluation Mapping Evaluation

Compare the mapping result with known structure environment

65

Result Evaluation Mapping Evaluation

66

Conclusion Summary

Our algorithm can localize camera position and build up a map in 3D using only a omni-camera image

Omni-directional camera move freely in an unknown indoor environment without knowing of camera motion model

Evaluation Result shows the correspondence of the localization and mapping outcome with the ground truth

67

Thank you

StatisticLocation Conf. room Corridor Stairway

Max feature count 38 36 28

Min feature count 14 13 10

Avg. Feature count 23.9279 20.4866 13.7657

Max landmark count 43 65 43

Min landmark count 25 14 10

Avg. Landmark count 33.1654 45.0207 19.7684

Max time per frame (ms) 1019.73 1424.87 703.188

Min time per frame (ms) 34.9818 19.657 15.1698

Avg. time per frame (ms) 160.925 479.858 172.845

Frame count 804 1157 747

69

Visual SLAM for 3D Large-Scale Seabed Acquisition Employing Underwater Vehicles

70

Featureless Vehicle-Based Visual SLAM with a Consumer Camera

71

Scan-SLAM: Combining EKF-SLAM and ScanCorrelation

72

The Problem Input

a captured image sequence from an omni-directional camera

Output a camera state

3D position and direction an environment map

3D landmark positions

73

Omni camera Calibration

74

Omni camera Calibration

75

)(fv

u

z

y

x

P 22 vu

44

33

2210)( aaaaaf

yc

xc

v

u

e

dc

v

u

1

ycv

xcu

ce

d

v

u

det/det/

det/det/1dec det

Image Processing Feature detection (for new feature)

Using point features Finds corners in the image using harris corner detector

76

Prediction Determine how a camera move But the camera motion is unpredictable Assume that a camera can move freely in any direction

with some limit velocity

77


TWCk

WCkkckc qtxfx 111,, )ˆ(ˆ

kkckc QPP

1,,

Predicted State

Predicted estimate covariance

Correction

78


Tui

ui

Cii zzz )2()1(

Tiuiiuiiciirci yrhyrhyxhyxxh )ˆ,ˆ()ˆ,ˆ()ˆ,ˆ()ˆ,ˆ,ˆ( )2()1(

)/arctan(

)/arctan(),(

xy

yyxxzi yy

yyyyyyxh

)(yTyyyy xT

zyx

yxMyTx )()( 1

while is transform function which transform a landmark position (y) from world coordinate to reference coordinate (x)

Measurement data for landmark i

Observation model for landmark i

)(yTx

y'

)(

)(

Camera and reference frames Correction Assume that the measurement data can measurement

landmark positions accurately The correction affects only a camera state and

reference frame states

79


)ˆ,ˆ,ˆ(ˆ irciii yxxhv RHPHHPHS T

yyTcc

1 SHPK Tc

vKxx ˆˆˆ

PKHIP c )(

Innovation or measurement residual

Innovation (or residual) covariance

Optimal Kalman gain

Update state estimate

Update estimate covariance

is Jacobian Matrix of function h at

is Jacobian Matrix of function h atcH

yHcx̂

yx̂

Landmarks Correction Assume that the camera state is accurate The correction affects only landmark states

80


)ˆ,ˆ,ˆ(ˆ irciii yxxhv RHPHHPHS T

yyTcc

1 SHPK Ty

vKxx ˆˆˆ

PKHIP y )(

Innovation or measurement residual

Innovation (or residual) covariance

Optimal Kalman gain

Update state estimate

Update estimate covariance

is Jacobian Matrix of function h at

is Jacobian Matrix of function h atcH

yHcx̂

yx̂

Solution to Problem SLAM State

Camera state – represent camera frame Reference frame states – represent reference frames Landmark states – represent landmark positions

81

y

r

c

x

x

x

x

ˆ

ˆ

ˆ

ˆ

yyrycy

yrrrcr

ycrccc

xxxxxx

xxxxxx

xxxxxx

PPP

PPP

PPP

P

TWCWCc qtx ˆ

Tr rrx 21 ˆˆˆ TWRi

WRii qtr ˆ

Ty yyx 21 ˆˆˆ

)ˆ( cx)ˆ( rx

)ˆ( yx


Select reference frame for feature tracking

82

xy

z

x

y

z

cyry

camera frame

reference frame

)cos( rc yy

25.0 r

c

y

y


Select reference frame for update SLAM state

83

xy

z

x

y

z

cyry

camera frame

reference frame

0 rc yy

84

Feature ray

pitch

yaw

landmark

x

y

z

camera)(

)(

3D SLAM for Omni-directional Camera Yuttana Suttasupa Advisor: Asst.Prof. Attawith Sudsang.

Documents

Transcript of 3D SLAM for Omni-directional Camera Yuttana Suttasupa Advisor: Asst.Prof. Attawith Sudsang.