real-time simultaneous localization and mapping with a single camera (mono slam) 2005. 9. 26 young...

Real-Time Simultaneous Localization and Mapping with a

Single Camera(Mono SLAM)

2005. 9. 26Young Ki Baik

Computer Vision Lab.

Seoul National University

Real-Time SLAM with a Single Camera

Computer Vision Lab. SNU

Contents

References

Kalman filter and SLAM

Mono-SLAM

Simulation Demo

Conclusion



References

Real-Time Simultaneous Localisation and Mapping with a Single Camera

Andrew J. Davison (ICCV 2003)

A Solution to the Simultaneous Localization and Map Building (SLAM) problem

Gamini Dissanayake. Et. Al. (IEEE Trans. Robotics and Automation 2001)

An Introduction to the Kalman Filter

G. Welch and G. Bishop (SIGGRAPH 2001)

Site for Quaternion

http://www.euclideanspace.com/maths/geometry/rotations



Kalman filter

What is a Kalman filter?Mathematical power tool

Optimal recursive data processing algorithm

Noise effect minimization

ApplicationsTracking (head, hands etc.)

Lip motion from video sequences of speakers

Fitting spline

Navigation

Lot’s of computer vision problem



Kalman filter

Example

Real location

Location with error

Measurement error

Localizing error (Processing error)

Refined location

Robot

Landmark

Movement noise

Sensor noise

How can we

obtain optimal

pose of robot and

landmark

simultaneously?

Kalman filter



Kalman filter

Example (Simple Gaussian form)

Assumption

All error form Gaussian noise

Estimated value

Measurement value

2, eex 2, eexN

2, mmxN 2, mmx



Example (Simple Gaussian form)

Optimal variance

Optimal value

Kalman filter

2,xN

mme

ee

me

m xxx

22

2

22

2

222

111

me

emme

ee xxxx

22

2

iKxx e Kalman gain

Innovation



SLAM

SLAM

Simultaneously Localization and map building system

EKF(Extended Kalman filter)-based framework

If we have the solution to the SLAM problem…

Allow robots to operate in an environment without a priori knowledge of a map

Open up a vast range of potential

application for autonomous vehicles

and robot

Research over the last decade has

shown that SLAM is indeed possible



SLAM

Kalman filter and SLAM problem

Extended Kalman filter form for SLAM

Prediction

Observation

Update

kSJkPkK THxe

1)(

xFJFx

xHJHx

kxHkz ee

kzkzki em )(

kikKkxkx e )( kKkSkKkPkP T

e )(

kQkJkPkJkP TFxFxe 1

kukxFkxe ,1

kRJkPJkS THxeHx )(

: Previous value

: Input and measure

: Function

: Computed value

iFL LFJi



Mono SLAM

What is Mono SLAM?EKF-SLAM framework (EKF : Extended Kalman Filter)

Single camera

Unknown user input

User inputKnown control input

Encoder information of robot or vehicle (odometry)

Most case of localization system,

odometry information is used as initial moving value.

Mono- slam don't use odometry information and it can be new feature.

kukxFkxe ,1 ?



Mono SLAM

World frame model

WY

World Frame WWZ

WX

RY

RX

RZ

Camera

frame R W : World coordinate

R : Local coordinate

r

r : Camera position

vector in W frame

yy : Landmark position

vector in W frame



Mono SLAM

Motion model

3D position and orientation

T

ZYXWR

W

p qqqqZYXq

r,,,,,, 0

x

form)n (Quaternio camera ofn orientatio:

camera ofposition -3D:

vectorstate :

W

W

p

q

r

x

This state vector is parameters for conventional SLAM .



Mono SLAM

Motion modelKey difference between Mono- and conventional-SLAM

In the robot case, there is in possession of the control inputs driving the motion, such as “moving forward 1m with steering angle 5 degree”

In the hand helded camera case, we do not have such prior information about a person’s movement.

Assumption (Mono SLAM)

In the case of a camera attached to a person, it takes account of the unknown intentions of the person, but these too can be statistically modeled.

Constant velocity, constant angular velocity model are chosen as initial value and added undetermined accelerations occur with a Gaussian profile.



Mono SLAM

Motion model (Mono SLAM)

3D position and orientation

camera ofn orientatio:

camera ofposition 3D:

SLAM-mono of vector state :

W

WR

v

q

r

x

W

W

WR

W

v

w

v

q

r

x

T

ZYX qqqqZYX ,,,,,, 0

TZYXZYX wwwvvv ,,,,,

motion camera oflocity angular ve :

motion camera ofocity linear vel:W

W

w

v

The total dimension of state

vector is 13.



Mono SLAM


Unknown user input (or noise vector)

In each time step, unknown acceleration and angular

acceleration processes of zero mean and Gaussian distribution.

locityangular ve of difference the:

velocityof difference the:

vectornoise the:

W

W

W

V

n

t

taW

W

W

W

V

n

WaW



Mono SLAM


State update function

function update state:

vectorstate previous:

vectorstate new :

F

x

xnew

WW

WW

WWWR

WWW

Wnew

Wnew

WRnew

Wnew

w

v

twqq

tvr

w

v

q

r

V

V

F

kkknew uxFx ,1

Quaternion trivially

defined by the

angle-axis rotation

vector tw WW

uV

n

t

taW

W

W

W

: Previous value

: Unknown user input



Mono SLAM

Motion model (Mono SLAM)Covariance update

In the EKF, the new state estimate must be accompanied by the increase in state uncertainty (process noise covariance) for the camera after this motion.

Qv is found via the Jacobian calculation

n vector noise of covariance:

yuncertaint state previous:

yuncertaint state new :

n

new

P

P

P

T

nv PQ

n

F

n

F

kQkJkPkJkP vT

new FxFx 1

uxF ,



Mono SLAM

Motion model (Mono SLAM)Covariance of noise vector

The rate of growth of uncertainty in this motion model is determined by the size of , and setting these parameters to small or large values defines the smoothness of the motion we expect.

small

- We expect a very smooth motion with small accelerations, and would be well placed to track motion but unable to cope with sudden rapid movements

High

- The uncertainty in the system increases significantly at

each time step.

- This can be cope with rapid accelerations.

nP

nP

nP



Simulation Demo

Condition

Simple circular motion

Five 3D landmarks

Observation is 2D using projective camera model

3D view 2D view

Estimated LM

Real LM

Estimated Pos

& CovarianceProjected LM



Conclusion

Conclusion

Localization is possible with out control input.

Simulation result

3D position can be estimated using SLAM through the projected landmark information.

It needs more debuging for perfect simulation.



Mono SLAM

Quaternion form for orientation (or rotation)Eular angle

Arbitrary 3D rotation is equal to one rotation (by scalar angle) around an axis.

The result of any sequence of rotation is equal to a single rotation around an axis.

3 degree of freedom in 3D space

Gimbal lock problem

Z

Y

X

),,( Rparameter



Mono SLAM

Quaternion form for orientation (or rotation)

Axis angle

Arbitrary 3D rotation composed by 3-d unit vector and 1-d angle value


Singularity problem

Z

Y

X),,( AAA ZYX

,,, AAA ZYXRparameter



Mono SLAM

Quaternion form for orientation (or rotation)Quaternion angle

Arbitrary 3D rotation composed by 3-d unit vector and 1-d angle value


Why quaternion?

Simpler algebra

Easy to fix numerical error

No singularity and Gimbal lock problem

Z

Y

X),,( AAA ZYX

2sin,

2sin,

2sin,

2cos AAA ZYXR

parameter

real-time simultaneous localization and mapping with a single camera (mono slam) 2005. 9. 26 young...

Documents

single cameracomputer

conventional slam

possiblerealtime slam

kalman filterrealtime

w framerealtime slam

computed valuerealtime

slam problemallow robots

single cameraandrew