SLAM (Simultaneously Localization and Mapping)

Presenter : Jeongkyun Lee

2

What is SLAM SfM-based SLAM Filter-based SLAM Comparison Other SLAMs Research topics

Contents

3

Simultaneously Localization and Mapping

What is SLAM

Unknown Environment

Unknown Pose

Given only images

4

How to localize & map

1) Structure-from-Motion based

2) Filtering based

Pay attention to :

Initialization

Measures ( Matching features )

Localization & Mapping

What is SLAM

5

Geometry

Fundamentals

1 [ | ] 0x K x R t X

0 x Fx

0 0 0 x Ex

F

E

[ | ] x PX K R t X

Projection matrix

Calibration matrix

Rotation matrix

Translation vector

3D homogeneous vector2D image point

Normalized point

Fundamental matrix

Essential matrix where E KFK[ ]E t R

* http://www.umiacs.umd.edu/~ramani/cmsc828d/lecture27.pdf* Multiple View Geometry in Computer Vision, R. Hartley and A. Zisserman, Cambridge, University Press, 2000

6

5-point algorithm1)

Rotation matrix : 3 DoF (Rodrigues’ formula)Translation vector : 3 DoFThus, is 5 DoF. ( 3 + 3 – 1, 1 DoF for scaling factor )

Given 5 pairs of points on the image planes,We can obtain .

PnP problem (Perspective-n-Point problem)Given n 3D-to-2D point correspondencesWe can obtain .

- Grunert’s algorithm2) (P3P)- EPnP3)

- Robust PnP4)

- ...

Fundamentals

SfM-based SLAM

E

E

exp( )

R u

u

1) D. Nister, An Efficient Solution to the Five-Point Relative Pose Problem, IEEE Conference on Computer Vision and Pattern Recognition, Volume 2, pp. 195-202, 2003.2) R. M. Haralick, C. N. Lee, K. Ottenberg and M. Nolle, Review and Analysis of Solutions of the Three Point Perspective Pose Estimation Problem, International Journal of Com-puter Vision, 1994. 3) F. Moreno-Noguer, V. Lepetit and P. Fua , Accurate Non-Iterative O(n) Solution to the PnP Problem, IEEE International Conference on Computer Vision, Rio de Janeiro, Brazil, October 2007.4) S. Li, C. Xu, M. Xie, A Robust O(n) Solution to the Perspective-n-Point Problem, IEEE Transactions on Pattern Analysis and Machine Intelligence (2012) Volume 34, Issue 7, pp. 1444-1450

Known Environment

Corresponding image points

Unknown poseE

7

Visual Odometry1)

– Feature Detection : Harris corners– Feature Matching : Normalized Corss Correlation (NCC)

Only matches between detected features within a fixed dis-tance.– Procedure

SfM-based SLAM

5-point algorithm3),4) & RANSAC3)

Given 3 frames

P3P algorithm & RANSAC3)

Triangulation

Re-triangulationusing first & last observations

Given 1 frames… …

P3P

5P 5P

P3P

1) D. Nister, O. Naroditsky, J. Bergen, Visual odometry, Computer Vision and Pattern Recognition, July 2004.2) D. Nister, Preemptive RANSAC for Live Structure and Motion Estimation, IEEE International Conference on Computer Vision, 2003.3) D. Nister, An Efficient Solution to the Five-Point Relative Pose Problem, IEEE Conference on Computer Vision and Pattern Recognition, Volume 2, pp. 195-202, 2003.4) D. Nister, An Efficient Solution to the Five-Point Relative Pose Problem, IEEE Transactions on Pattern Analysis and Machine Intelligence, 2004.

8

Real Time Localization and 3D Reconstruction1)

- LBA2) (Levenberg-Marquardt algorithm (LM))

Minimize

where

SfM-based SLAM

1) E. Mouragnon, M. Lhuillier, M. Dhome, F. Dekeyser, P. Sayd, Real Time Localization and 3D Reconstruction, Computer Vision and Pattern Recognition, 2006.2) B. Triggs, P. F. McLauchlan, R. I. Hartley & A. W. Fitzibbon, Bundle adjustment – A modern synthesis, in Vision Algorithms: Theory and Practice, LNCS, pp. 298-375, Springer Verlag, 2000.

Visual odometry Local bundle adjustment (LBA)+

: Extrinsic parameters: Projection matrix : The square of Euclidean distance : Estimated projection of point through the camera : Observation

9

Real Time Localization and 3D Reconstruction

SfM-based SLAM

n : number of optimized camera posesN : number of cameras used for reprojection criterion minimization

10

Real Time Localization and 3D Reconstruction– Key frame selection

1. Number of matched points2. Uncertainty of camera pose ( Obtained from the hessian inverse )

– Complexity :

– Experiments512 x 384 pixes, 75 fps, 94 key frames from a series of 445.

SfM-based SLAM

11

MonoSLAM1)

– EKF-based

Filter-based SLAM

1) A. J. Davison, I. D. Reid, N. D. Molton, O. Stasse, MonoSLAM: Real-Time Single Camera SLAM, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 29, no. 6, June 2007.

Filter initialization

Map management( Generate & delete features )

Prediction

Measurements acquisition

Data association

Update

Prediction

Measure-ments Ac-quisition

Update

To easily explain….

12

MonoSLAM

Filter-based SLAM

Prediction Measure-ments Ac-quisition

Update

Dynamic System Model(Constant Velocity Model)

( )

( )( )

( )

( )

W

WR

v W

R

t

tt

t

t

r

qx

v

ω

: 3D position vector

: orientation quaternion

: linear velocity vector

: angular velocity vector

: landmark position vectoriy

State

1

2

( 1)

( 1)

v t

t

x

yx

y

Prediction

( 1) ( 1)

( 1) ( ( 1) )ˆ ( )

( 1)

( 1)

W W

WR R

v W

R

t t t

t t tt

t

t

r v

q q ωx

v

ω

13

MonoSLAM

Filter-based SLAM

Prediction Measure-ments Ac-quisition

Update

For

Matching the patch by NCC

at

Max NCC value at > threshold

Measurement

Prediction of measurements Find measurements

ˆ( ) ( ( ))

i i v

ut h t

vh x

( )1

i

ku

kv t

k

yPK

ˆ ( )W tr

ˆ ( )WR tq

Active search1),2)

T 1( , ) | iu vu u S u

( ) i th u

( ) i th u

( ) ( ) i it tz h u

1) A. J. Davison, Active Search for Real-Time Vision, International Conference Computer Vision, 2005.2) M. Chli, A. J. Davison, Active Matching for Visual Tracking, Robotics and Autonomous Systems, 57(12):1173-1187, 2009.

iS: a covariance matrix for the 2D position of i th landmark

14

MonoSLAM

Filter-based SLAM

Prediction Measure-ments Ac-quisition

Update

1) A. J. Davison, Active Search for Real-Time Vision, International Conference Computer Vision, 2005.2) M. Chli, A. J. Davison, Active Matching for Visual Tracking, Robotics and Autonomous Systems, 57(12):1173-1187, 2009.

Update

1 1( ) ( )

ˆ( ) ( ) ( )

( ) ( )

n n

t t

t t t

t t

z h

x x K

z h

: a Kalman gain at time t ( )tK

15

MonoSLAM

– Initialization of features• Delayed : SfM• Undelayed : Inverse depth parameterization1)

– Data association• 1-point RANSAC2)

• Joint Compatibility Branch and Bound3) (JCBB)

– Experiment• 1.6GHz Pentium M processor

Filter-based SLAM

1) J. Civera, A. J. Davison, J. M. M. Montieal, Inverse Depth Parametrization for Monocular SLAM, IEEE Transactions on Robotics 24(5):932-945, 2008.2) J. Civera, O. G. Grasa, A. J. Davison, J. M. M. Montiel, 1-Point RANSAC for EKF Filtering. Application to Real-Time Structure from Motion and Visual Odometry , Journal of Field Robotics, 20103) J. Neira, J. D. Tardos, Data association in stochastic mapping using the joint compatibility test, IEEE Transactions on Robotics and Automation, 17(6):890-897, Dec 2001.

16

Comparison

SfM-based Filter-based

Initialization

• 5-point algorithm

• Delayed : SfM

• Undelayed : Inverse depth parameteri-

zation

Measurement • NCC matching

(from extracted feature points)

• KLT tracker

• Active search

(prediction & templete matching)

• KLT tracker

Estimation tech-nique • LBA

(after p3p algorithm)

• Kalman filtering

(prediction & update)

• Tracking 3~400 points in a frame • Working in real time within 100 landmarks.

17

PTAM (Parallel Tracking and Mapping)1)– Separate Tracking / Mapping

• Redundancy : use only key frames.• Accuracy : available to optimization.

Other SLAMs

1) G. Klein, D. Murray, Parallel Tracking and Mapping for Small AR Workspaces, ACM International Symposium on Mixed and Augmented Reality, 2007.

Tracking

Pose estimate &Map point projection

Searching a small number (50) of the coarsest-scale features by

pyramid

Pose update

Searching a large number (1000) of the re-projected features

Final pose estimation

Mapping

18

PTAM (Parallel Tracking and Mapping)– Features

• Matching : zero mean SSD• Key frame

1. > 20 frames from the last key frame.2. Minimum distance away from the nearest key point.

• Point initialization– Epipolar search

• Data association refinement– Create new features in older keyframes.– Re-measure outlier measurements.

– Experiments• Intel Core 2 Duo 2.66GHz, 600x480 pixels• 6000 point, 150 keyframes.

Other SLAMs

19

Filter-based…– Divergence

• Relocalization• Multiple model• Resilience• Other filtering techniques

– Error accumulation• Loop-closing• Combining LBA, visual odometry

– Data association• 1-point RANSAC• ICNN, SCNN, JCBB

– Dynamic environment• SLAMMOT (SLAM and Moving Object Tracking)

– Multi-view, Sensor fusion

– Application• Dense 3d reconstruction• AR• Deblurring

Topics

Beyond Spatial Pyramids: A New Feature Extraction Framework with Dense Spatial Sampling for Image Classifi-cation

20

Thank you!