Master Thesis Ludovico Russo - Presentation

Robot Navigation Using a Single Camera

Ludovico Orlando Russo

MSc in Mechatronic EngineeringPolitecnico di Torino

Supervisors: Basilio Bona, Luca Carlone, Matteo MatteucciTelecom Italia

October 15th 2013

Ludovico Orlando Russo MSc in Mechatronic Engineering October 15th 2013 1/13

Outline

1 IntroductionRobot NavigationSLAMThe Chosen Approach

2 Mono-SLAMThe Mono-SLAM AlgorithmThe developed solutionResults

3 Conclusion and Future Works


Introduction Robot Navigation

Navigation

LocalizationMapping

SLAM

Robot Navigation

Capabilities anautonomous robot musthave



Navigation

LocalizationMapping

SLAM

LocalizationAbility of a robot toestablish its position in aknown environment



Navigation

LocalizationMapping

SLAM

Mapping

Ability to build arepresentation of anunknown environment



Navigation

LocalizationMapping

SLAM

SLAMSimultaneous Localizationand Mapping


Introduction SLAM

SLAMClassic

Approaches

VisualApproaches

Dense

Sparse

Semantic

Classic Approaches

Based on Laser ScannersSensors


Introduction SLAM

SLAMClassic

Approaches

VisualApproaches

Dense

Sparse

Semantic

Visual Approaches

Based on visual systemsMonocular cameraDepth-map system


Introduction SLAM

SLAMClassic

Approaches

VisualApproaches

Dense

Sparse

Semantic

Dense Approaches

Build up a dense map! Robustness% Computational power


Introduction SLAM

SLAMClassic

Approaches

VisualApproaches

Dense

Sparse

Semantic

Sparse Approaches

Build up a sparse map% Robustness! Computational power


Introduction SLAM

SLAMClassic

Approaches

VisualApproaches

Dense

Sparse

Semantic

Semantic Approaches

Build up a semantic map! Robustness! Computational power% Very complex


Mono-SLAM The Mono-SLAM Algorithm

Mono-SLAM MonocularCamera

EKFFast

Real Time

GeneralPurposeHW

Sparse Probabilistic VisualSLAM approach [1]

Andrew J Davison et al.“MonoSLAM: Real-timesingle camera SLAM”.In: Pattern Analysisand Machine Intelligence,IEEE Transactions on 29.6(2007)




EKFFast

Real Time

GeneralPurposeHW

Based on a monocularcamera




EKFFast

Real Time

GeneralPurposeHW

Built upon the ExtendedKalman Filter




EKFFast

Real Time

GeneralPurposeHW

Developed to be fast




EKFFast

Real Time

GeneralPurposeHW

Able to meet real timeconstraints




EKFFast

Real Time

GeneralPurposeHW

Does not need dedicatedhardware



State of the Mono-SLAMµk =

(xT

k f T1 . . . f T

m)T ← Complete state of the EKF

xk =(rTk qT

k vTk wT

k)T ← 3D camera pose and velocities

f i ← represents each point in the map

yx

z

Ck

f i

y

x

z

W

(rk qk

)

yx

z

Ck+1

(vk wk

)∆t



Features Matching

Using information from EKFPatch matching algorithm1-Point RANSAC outlier rejection algorithm



Features Matching




Features Matching




Features Matching




Features Matching




Features Matching




Features Matching




Features Matching



Mono-SLAM The developed solution



Contribution ROS Imple-mentation

Motion blurhandling




Motion blurhandling

Camera

Camera Topic

frameMono-SLAM

Mono-SLAM Topics

Map

Path

Camera Pose

Elaborated Frame

Rviz




Motion blurhandling




Motion blurhandling

[2] L. O. Russo, G. Airò Farulla, M. Indaco, Rolfo D. Rosa S., and B.Bona. “Blurring prediction in Monocular SLAM”. In: InternationalDesign and Test Symposium, IEEE Conference (2013).



Featuresmatching

UpdateEKF Step

PredictionEKF Step

MeasurementsPrediction

BlurPredictionz−1

z−1

z−1

Patcheshandler

Capture andpreprocessCamera

Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM



Featuresmatching

UpdateEKF Step

PredictionEKF Step


BlurPredictionz−1

z−1

z−1

Patcheshandler


Sk+1Sk

yk|k−1

µk|k

Σk|k

µk|k+1

Σk|k+1

P ′k+1|kP ′k|k−1

fk

yk+1|k

−yk ek

Pk

µk|k

Mono-SLAM


Mono-SLAM Results

10m

Starting point

Ground TruthMono-SLAM

10m

Starting point

Starting point50m

20m

Starting point


Conclusion and Future Works

Open Issues

RobustnessScale DriftLarge Scale Navigation

Future Works

ImprovementsLoop ClosingMulti Camera SLAMMulti Robot SLAM



Open Issues


Future Works




Open Issues


Future Works




Open Issues


Future Works




Open Issues


Future Works




Open Issues


Future Works




Open Issues


Future Works




Open Issues


Future Works




Open Issues


Future Works




Thank you!


Real Time

15 20 25 30 35 40 45 50 55 60 65 70

20

40

60

number of features

t[m

s]


Blur Handling

2 4 6 8 10 12 14 16 18

1

1.1

1.2

1.3

low speed medium speed high speed

vp [pixel/frame]

r


The 3D Reconstruction Problem

The 3D Reconstruction ProblemExtrapolate 3D information from a set of 2D images


The 3D Reconstruction Problem Stereo Triangulation

Stereo Triangulation

Computing 3D information from a stereo system

V2

V1

pb

f

h

h = bf∆v



Stereo Triangulation: Step 1

Select a point on the first image

V2

V1

pb

f

h

h = bf∆v




Match the selected point with the second image

V2

V1

pb

f

h

h = bf∆v




Triangulate

V2

V1

pb

f

h

h = bf∆v



Stereo Triangulation: Uncertainty Propagation

Uncertainty on depth h increases when disparity ∆v decreases, i.e.when depth increases:

δh = bf δv1

∆v2 =δvbf

h2

δh





δh = bf δv1

∆v2 =δvbf

h2

δh





δh = bf δv1

∆v2 =δvbf

h2

δh


The 3D Reconstruction Problem Structure from Motion

Structure from MotionComputing 3D information on map and camera path from a video

3 v3y3

y2

2v2

1 v1y1

p

y



Structure from Motion: Step 1

Select a point on the first image

3 v3y3

y2

2v2

1 v1y1

p

y




Match the selected point with all the others image

3 v3y3

y2

2v2

1 v1y1

p

y




Compute poses of each camera and triangulate the map

3 v3y3

2v2y2

1 v1y1

p

y



Structure from Motion: Scale LossSfM and similar approach are not able to evaluate the scale of theenvironment

p




p

p∗




p

DimensionlessreformulationEquivalentdimensionlessproblem based onangles



Features Codings: Euclidian Representation

y =(x y z

)T

Simple (3 componets)Unable to represents features with low parallaxPoor Linearity

yx

z

C

p

y

r0

1ρµ(θ, φ)

y

x

z

W



Features Codings: Inverse Depth Representation

y =(rT0 θ φ ρ

)T

More complex (6 components)Able to represents features with low parallax (at infinity)Good linearity

yx

z

C

p

y

r0

1ρµ(θ, φ)

y

x

z

W



Features Codings InverseDepth Init

Points atInfinity

EucledianWellknowndepth




Points atInfinity





Points atInfinity





Points atInfinity



Documents

Master Thesis Ludovico Russo - Presentation