Visual Odometry and SLAM:
past, present, and the robust-perception age
Davide ScaramuzzaReferences
Scaramuzza, D., Fraundorfer, F., Visual Odometry: Part I - The First 30 Years andFundamentals, IEEE Robotics and Automation Magazine, Volume 18, issue 4, 2011. PDF
Fraundorfer, F., Scaramuzza, D., Visual Odometry: Part II - Matching, Robustness, andApplications, IEEE Robotics and Automation Magazine, Volume 19, issue 1, 2012. PDF
C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I.D. Reid, J.J. Leonard,
Simultaneous Localization And Mapping: Present, Future, and the Robust-Perception Age,IEEE Transactions on Robotics (cond. Accepted), 2016. PDF

Outline Theory Open Source Algorithms
 Event-based VisionVO is the process of incrementally estimating the pose of the vehicle byexamining the changes that motion induces on the images of its onboard
cameras
input

outputImage sequence (or video stream)from one or more cameras attached to a moving vehicle

Camera trajectory (3D structure is a plus):
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch 1980: First known VO real-time implementation on a robot by Hans Moraveck PhD
thesis (NASA/JPL) for Mars rovers using one sliding camera (sliding stereo).
 1980 to 2000: The VO research was dominated by NASA/JPL in preparation of2004 Mars mission (see papers from Matthies, Olson, etc. from JPL)

 2004: VO used on a robot on another planet: Mars rovers Spirit and Opportunity 2004. VO was revived in the academic environment
by David Nister «Visual Odometry» paper.
The term VO became popular (and Nisterbecame head of MS Hololens before moving
to TESLA in 2014)

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch 1980: First known VO real-time implementation on a robot by Hans Moraveck PhD
thesis (NASA/JPL) for Mars rovers using one sliding camera (sliding stereo).
 1980 to 2000: The VO research was dominated by NASA/JPL in preparation of2004 Mars mission (see papers from Matthies, Olson, etc. from JPL)

 2004: VO used on a robot on another planet: Mars rovers Spirit and Opportunity 2004. VO was revived in the academic environment
by David Nister «Visual Odometry» paper.
The term VO became popular (and Nisterbecame head of MS Hololens before moving
to TESLA in 2014)

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch Sufficient illumination in the environment
 Dominance of static scene over moving objects
 Enough texture to allow apparent motion to be extracted
 Sufficient scene overlap between consecutive frames

Is any of these scenes good for VO? Why?

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

SFM

VSLAM

VO

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

SFM is more general than VO and tackles the problem of 3D
reconstruction and 6DOF pose estimation from unordered image sets

Reconstruction from 3 million images from Flickr.com
Cluster of 250 computers, 24 hours of computation!
Paper: “Building Rome in a Day”, ICCV’09
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch VO is a particular case of SFM

 VO focuses on estimating the 3D motion of the camera

sequentially (as a new frame arrives) and in real time.
 Terminology: sometimes SFM is used as a synonym of VO

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

 Visual Odometry

 Focus on incremental estimation/local
consistency
 Visual SLAM: Simultaneous Localization And
Mapping
 Focus on globally consistent estimation
 Visual SLAM = visual odometry + loop detection+ graph optimization

Visual odometry

 The choice between VO and V-SLAM depends on

the tradeoff between performance andconsistency, and simplicity in implementation.
 VO trades off consistency for real-time

Visual SLAM
performance, without the need to keep track of all
the previous history of the camera.
Image courtesy from [Clemente et al., RSS’07]
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch1.

Compute the relative motion 𝑇𝑘 from images 𝐼𝑘−1 to image 𝐼𝑘
𝑇𝑘 =

2.

𝑅𝑘,𝑘−10𝑡𝑘,𝑘−1
1

Concatenate them to recover the full trajectory
𝐶𝑛 = 𝐶𝑛−1 𝑇𝑛

3.

An optimization over the last m poses can be done to refine locallythe trajectory (Pose-Graph or Bundle Adjustment)
𝑪𝟎

𝑪𝟏

𝑪𝟑

𝑪𝒏−𝟏

𝑪𝟒

𝑪𝒏

...

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

How do we estimate the relative motion 𝑇𝑘 ?

Image 𝐼𝑘−1

Image 𝐼𝑘

𝑇𝑘

𝑇𝑘
“An Invitation to 3D Vision”, Ma, Soatto, Kosecka, Sastry, Springer, 2003

Direct Image Alignment
It minimizes the per-pixel intensity difference
𝐼𝑘 𝒖′𝑖 − 𝐼𝑘−1 (𝒖𝑖 ) 2𝜎

𝑇𝑘,𝑘−1 = arg min
𝑇

𝑖

Dense

Semi-Dense

DTAM [Newcombe et al. ‘11]
300’000+ pixels

LSD [Engel et al. 2014]~10’000 pixels

SparseSVO [Forster et al. 2014, TRO’16]
100-200 features x 4x4 patch~ 2,000 pixels

Irani & Anandan, “All About Direct Methods,” Vision Algorithms: Theory and Practice, Springer, 2000

Direct Image Alignment
It minimizes the per-pixel intensity difference
𝐼𝑘 𝒖′𝑖 − 𝐼𝑘−1 (𝒖𝑖 ) 2𝜎

𝑇𝑘,𝑘−1 = arg min
𝑇

𝑖

Dense

Semi-Dense

DTAM [Newcombe et al. ‘11]
300,000+ pixels

LSD-SLAM [Engel et al. 2014]~10,000 pixels

SparseSVO [Forster et al. 2014]100-200 features x 4x4 patch~ 2,000 pixels

Irani & Anandan, “All About Direct Methods,” Vision Algorithms: Theory and Practice, Springer, 2000

Feature-based methods

𝑇𝑘,𝑘−1 = ?

1. Extract & match features (+RANSAC)
2. Minimize Reprojection error
minimization𝒖 ′ 𝑖 − 𝜋 𝒑𝑖

𝑇𝑘,𝑘−1 = arg min
𝑇

𝒖′𝑖

𝒖𝑖

𝒑𝑖

2
Σ

𝑖

Direct methods

𝑇𝑘,𝑘−1

1. Minimize photometric error
𝑇𝑘,𝑘−1 = arg min𝑇

𝐼𝑘 𝒖′𝑖 − 𝐼𝑘−1 (𝒖𝑖 )

2
𝜎

𝐼𝑘−1

𝐼𝑘

𝒖𝑖

𝒖′𝑖

𝑖

where 𝒖′𝑖 = 𝜋 𝑇 ∙ 𝜋 −1 𝒖𝑖 ∙ 𝑑[Jin,Favaro,Soatto’03] [Silveira, Malis, Rives, TRO’08], [Newcombe et al., ICCV ‘11],[Engel et al., ECCV’14], [Forster et al., ICRA’14]

𝑑𝑖
𝒑𝑖
16

Feature-based methods

 Large frame-to-frame motions

1. Extract & match features (+RANSAC)

 Accuracy: Efficient optimization of
structure and motion (Bundle Adjustment)

2. Minimize Reprojection error
minimization

 Slow due to costly feature extraction
and matching Matching Outliers (RANSAC)
𝒖 ′ 𝑖 − 𝜋 𝒑𝑖

𝑇𝑘,𝑘−1 = arg min
𝑇

2
Σ

𝑖

 All information in the image can be

Direct methods

exploited (precision, robustness)

1. Minimize photometric error
𝐼𝑘 𝒖′𝑖 − 𝐼𝑘−1 (𝒖𝑖 ) 2𝜎

𝑇𝑘,𝑘−1 = arg min𝑇𝑖where 𝒖′𝑖 = 𝜋 𝑇 ∙ 𝜋 −1 𝒖𝑖 ∙ 𝑑 Increasing camera frame-rate
reduces computational cost perframe Limited frame-to-frame motion
 Joint optimization of dense structure[Jin,Favaro,Soatto’03] [Silveira, Malis, Rives, TRO’08], [Newcombe et al., ICCV ‘11],and motion too expensive[Engel et al., ECCV’14], [Forster et al., ICRA’14]

17

VO computes the camera path incrementally (pose after pose)
Image sequence

Front-end

Feature detection

Feature matching (tracking)Motion estimation
2D-2D

3D-3D

Local optimization

3D-2D

Back-end

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch The Front-end is responsible for Feature extraction, matching, and outlier removal
 Loop closure detection
 The Back-end is responsible for the pose and structure

optimization (e.g., iSAM, g2o, Google Ceres)Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

VO computes the camera path incrementally (pose after pose)
Image sequence

Feature detection

Feature matching (tracking)Motion estimation
2D-2D

3D-3D

Example features tracks

3D-2D

Local optimization

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Feature Extraction

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch A corner is defined as the intersection of one or more edges
 A corner has high localization accuracy
 Corner detectors are good for VO It’s less distinctive than a blob
 E.g., Harris, Shi-Tomasi, SUSAN, FAST

 A blob is any other image pattern, which is not a corner, thatsignificantly differs from its neighbors in intensity and texture Has less localization accuracy than a corner Blob detectors are better for place recognition
 It’s more distinctive than a corner E.g., MSER, LOG, DOG (SIFT), SURF, CenSurE

 Descriptor: Distinctive feature identifier Standard descriptor: squared patch of pixel intensity values
 Gradient or difference-based descriptors: SIFT, SURF, ORB, BRIEF, BRISKDavide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Which of the patches below can be matched reliably?

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch How do we identify corners?
 We can easily recognize the point by looking through a small window Shifting a window in any direction should give a large change in intensity in atleast 2 directions“flat” region:no intensitychange“edge”:
no change along theedge direction“corner”:
significant change inat least 2 directions

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

[Rosten et al., PAMI 2010]

 FAST: Features from Accelerated Segment Test
 Studies intensity of pixels on circle around candidate pixel C
 C is a FAST corner if a set of N contiguous pixels on circle are: all brighter than intensity_of(C)+theshold, or all darker than intensity_of(C)+theshold• Typical FAST mask: test for 9 contiguous pixels in a 16-pixel circle• Very fast detector- Scaramuzzain the orderofof 100
Davide– University
Zurich –Mega-pixel/second
Robotics and Perception Group - rpg.ifi.uzh.ch

SIFT responds to local regions that look like Difference of Gaussian (~Laplacian ofGaussian)
LOG  DoG  Gks ( x, y )  Gs ( x, y )

s4

s3
Scale

s2

s

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

SIFT detector (location + scale)
SIFT keypoints: local extrema in both location and scale of the
DoG• Detect maxima and minimaof difference-of-Gaussian inscale space• Each point is compared toits 8 neighbors in thecurrent image and 9neighbors each in the scales
above and below
For each max or min found, output
is the location and the scale.

SIFT descriptor• Scale Invariant Feature Transform• Invented by David Lowe [IJCV, 2004]• Descriptor computation:– Divide patch into 4x4 sub-patches: 16 cells– Compute histogram of gradient orientations (8 reference angles) for all pixels
inside each sub-patch– Resulting SIFT descriptor: 4x4x8 = 128 values– Descriptor Matching: Euclidean-distance between these descriptor vectors(i.e., SSD)

David G. Lowe. "Distinctive image features from scale-invariant keypoints.” IJCV , 2004.

[Calonder et. al, ECCV 2010]• Binary Robust Independent Elementary
Features• Goal: high speed (in description and matching)• Binary descriptor formation:• Smooth image• for each detected keypoint (e.g. FAST),• sample 256 intensity pairs p=(𝑝1 , 𝑝2 ) withina squared patch around the keypoint• for each pair p• if 𝑝1 < 𝑝2 then set bit p of descriptor
to 1• else set bit p of descriptor to 0
• The pattern is generated randomly only once;
then, the same pattern is used for all patches• Not scale/rotation invariant• Allows very fast Hamming Distance matching:count the number of bits that are different in
the descriptors matched

Pattern for intensity pair samples –generated randomly

Calonder, Lepetit, Strecha,Davide
Fua, Scaramuzza
BRIEF: Binary
RobustIndependent
Elementary– Universityof Zurich– Robotics and Perception
Group Features,- rpg.ifi.uzh.ch ECCV’10]

ORB descriptor[Rublee et al., ICCV 2011]
 Oriented FAST and Rotated

BRIEF
 Alterative to SIFT or SURF,
designed for fast computation
 Keypoint detector based on
FAST BRIEF descriptors are steered
according to keypointorientation (to provide
rotation invariance) Good Binary features arelearned by minimizing thecorrelation on a set of training
patches.
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

[Leutenegger, Chli, Siegwart, ICCV 2011]• Binary Robust Invariant Scalable Keypoints• Detect corners in scale-space using FAST
• Rotation and scale invariant• Binary, formed by pairwise intensity
comparisons (like BRIEF)• Pattern defines intensity comparisons in
the keypoint neighborhood• Red circles: size of the smoothing kernel
applied• Blue circles: smoothed pixel value used
• Compare short- and long-distance pairs
for orientation assignment & descriptor
formation• Detection and descriptor speed: ~10times faster than SURF
• Slower than BRIEF, but scale- androtation- invariant
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Detector

Descriptor

Accuracy

Relocalization &Loop closing

Efficiency

Harris

Patch

++++

-

+++

Shi-Tomasi

Patch

++++

-

+++

SIFT

SIFT

++

++++

+

SURF

SURF

++

++++

++

FAST

BRIEF

++++

+++

++++

ORB

ORB

++++

+++

++++

FAST

BRISK

++++

+++

++++

ORB & BRISK:• 128-to-256-bit binary descriptors• Fast to extract and match (Hamming distance)• Good for relocalization and Loop detection• Multi-scale detection  same point appears on several scalesVO computes the camera path incrementally (pose after pose)
Image sequence

Feature detection
Ck+1

Feature matching (tracking)Tk+1,kCk

Motion estimation
2D-2D

3D-3D

3D-2D
Tk,k-1

Local optimization

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Ck-1

Motion estimation
2D-2D

Motion from Image Feature Correspondences Both feature points 𝑓𝑘−1 and 𝑓𝑘 are specified in 2D
 The minimal-case solution involves 5-point correspondences The solution is found by minimizing the reprojection error:𝝅 Popular algorithms: 8- and 5-point algorithms [Hartley’97, Nister’06]

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

3D-2D

3D-3D

Motion estimation
2D-2D

3D-2D

Motion from 3D Structure and Image Correspondences 𝑓𝑘−1 is specified in 3D and 𝑓𝑘 in 2D
 This problem is known as camera resection or PnP (perspective from n points)
 The minimal-case solution involves 3 correspondences (+1 for disambiguating the 4
solutions) The solution is found by minimizing the reprojection error:

 Popular algorithms: P3P [Gao’03, Kneip’11]

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

3D-3D

Motion estimation
2D-2D

3D-2D

3D-3D

Motion from 3D-3D Point Correspondences (point cloud registration)
 Both 𝑓𝑘−1 and 𝑓𝑘 are specified in 3D. To do this, it is necessary to triangulate 3D points(e.g. use a stereo camera) The minimal-case solution involves 3 non-collinear correspondences The solution is found by minimizing the 3D-3D Euclidean distance:𝝅 Popular algorithm: [Arun’87] for global registration, ICP for local refinement or Bundle
Adjustment (BA)Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Type ofcorrespondences

Monocular

Stereo

2D-2D

X

X

3D-3D
3D-2D

X
X

X

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.chKeyframe 1

Initial pointcloud(bootstrapping)

Keyframe 2

Current frame
New keyframe

New triangulated points

Typical visual odometry pipeline used in many algorithms[Nister’04, PTAM’07, LIBVISO’08, LSD-SLAM’14, SVO’14, ORB-SLAM’15]
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch When frames are taken at nearby positions compared to the scene distance, 3D

points will exibit large uncertainty

Small baseline → large depth uncertainty

Large baseline → small depth uncertainty

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch When frames are taken at nearby positions compared to the scene distance, 3D

points will exibit large uncertainty
 One way to avoid this consists of skipping frames until the average uncertainty of
the 3D points decreases below a certain threshold. The selected frames arecalled keyframes
keyframe distance Rule of the thumb: add a keyframe when> threshold (~10-20 %)average-depth...

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch Matched points are usually contaminated by outliers
 Causes of outliers are:

 image noise occlusions
 blur changes in view point and illumination
 For the camera motion to be estimated accurately, outliers must be removed
 This is the task of Robust Estimation

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch Error at the loop closure: 6.5 m
 Error in orientation:
5 deg
 Trajectory length:
400 m

Before removing the outliers
After removing the outliers

Outliers can be removed using RANSAC [Fishler & Bolles, 1981]Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

VO computes the camera path incrementally (pose after pose)
Image sequence

Front-end

Feature detection

Feature matching (tracking)Motion estimation
2D-2D

3D-3D

3D-2D

𝑪𝟎

𝑪𝟏

𝑪𝟑

𝑪𝒏−𝟏

𝑪𝟒

...
Local optimization (back-end)

Back-end

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

𝑪𝒏

Pose-Graph Optimization
 So far we assumed that the transformations are between consecutive frames
𝑪𝟎

𝑪𝟏
𝑻𝟏,𝟎

𝑻𝟐,𝟏
𝑻𝟐,𝟎

𝑪𝒏−𝟏

𝑪𝟑

𝑪𝟐

...

𝑻𝟑,𝟐
𝑻𝟑,𝟎

𝑪𝒏

𝑻𝒏,𝒏−𝟏

𝑻𝒏−𝟏,𝟐

 Transformations can be computed also between non-adjacent frames 𝑇𝑖𝑗 (e.g., whenfeatures from previous keyframes are still observed). They can be used as additional
constraints to improve cameras poses by minimizing the following:

𝐶𝑖 − 𝑇𝑖𝑗 𝐶𝑗𝑖2𝑗 For efficiency, only the last 𝑚 keyframes are used

 Gauss-Newton or Levenberg-Marquadt are typically used to minimize it. For large graphs,
efficient open-source tools: g2o, GTSAM, Google Ceres

Bundle Adjustment (BA)
𝑪𝟎

𝑪𝟏
𝑻𝟏,𝟎

𝑻𝟐,𝟏
𝑻𝟐,𝟎

𝑪𝒏−𝟏

𝑪𝟑

𝑪𝟐

...

𝑻𝟑,𝟐
𝑻𝟑,𝟎

𝑪𝒏

𝑻𝒏,𝒏−𝟏

𝑻𝒏−𝟏,𝟐

 Similar to pose-graph optimization but it also optimizes 3D points
𝑋 𝑖 , 𝐶𝑘 = 𝑎𝑟𝑔𝑚𝑖𝑛𝑋𝑖 ,𝐶𝑘 ,𝜌𝐻 𝑝𝑘 𝑖 − 𝜋 𝑋 𝑖 , 𝐶𝑘𝑖,𝑘 𝜌𝐻 () is a robust cost function (e.g., Huber cost) to downweight wrong matches
 In order to not get stuck in local minima, the initialization should be close to the minimum
 Gauss-Newton or Levenberg-Marquadt can be used
 Very costly: example: 1k images and 100k points, 1s per LM iteration. For large graphs,
efficient open-source software exists: GTSAM, g2o, Google Ceres can be used BA is more precise than pose-graph optimization because it adds additional

constraints (landmark constraints)
𝑞𝑀 + 𝑙𝑁 3 with 𝑀 and 𝑁 being the number of points
and cameras poses and 𝑞 and 𝑙 the number of parameters for points andcamera poses. Workarounds:

 But more costly: 𝑂

 A small window size limits the number of parameters for the optimization and thus
makes real-time bundle adjustment possible.

 It is possible to reduce the computational complexity by just optimizing over thecamera parameters and keeping the 3D landmarks fixed, e.g., (motion-only BA)

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

 Relocalization problem: During VO, tracking can be lost (due to occlusions, low
texture, quick motion, illumination change)Solution: Re-localize camera pose and continue

Loop closing problem
 When you go back to a previously mapped area:
 Loop detection: to avoid map duplication
 Loop correction: to compensate the accumulated drift
 In both cases you need a place recognition techniqueDavide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Visual Place Recognition
 Goal: find the most similar images of a query image in a database of 𝑵 images
𝑁2 ∙𝑀2
 Complexity: 2 feature comparisons (worst-case scenario)Each image must be compared with all other images!



𝑁 is the number of all images collected by a robot- Example: 1 image per meter of travelled distance over a 100𝑚2 house with one
robot and 100 feature per image → M = 100, 𝑁 = 100 → 𝑁 2 𝑀2 /2=~ 50 𝑀𝑖𝑙𝑙𝑖𝑜𝑛 feature comparisons!
Solution: Use an inverted file index!
Complexity reduces to 𝑵 ∙ 𝑴[“Video Google”, Sivic & Zisserman, ICCV’03][“Scalable Recognition with a Vocabulary Tree”, Nister & Stewenius, CVPR’06]
See also FABMAP and Galvez-Lopez’12’s (DBoW2)]

University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.chIndexing local features: inverted file text
 For text documents, an efficient way
to find all pages on which a word
occurs is to use an index

 We want to find all images in which afeature occurs

 To use this idea, we’ll need to map
our features to “visual words”

University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Building the Visual Vocabulary
Image Collection

Extract Features

Cluster Descriptors
Descriptors space

Examplesof
VisualWords:

Limitations of VO-SLAM systems
 Limitations
 Monocular (i.e., absolute scale is unknown)
 Requires a reasonably illuminated area Motion blur
 Needs texture: will fail with large plain walls
 Map is too sparse for interaction with the environment

 Extensions
 IMU for robustness and absolute scale estimation
 Stereo: real scale and more robust to quick motions
 Semi-dense or dense mapping for environment interaction
 Event-based cameras for high-speed motions and HDR environments
 Learning for improved reliability

Visual-Inertial Fusion

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.chAbsolute Scale Determination
 The absolute pose 𝑥 is known up to a scale 𝑠, thus
𝑥 = 𝑠𝑥 IMU provides accelerations, thus𝑣 = 𝑣0 +𝑎 𝑡 𝑑𝑡 By derivating the first one and equating them𝑠𝑥 = 𝑣0 +𝑎 𝑡 𝑑𝑡 As shown in [Martinelli, TRO’12], for 6DOF, both 𝑠 and 𝑣0 can be determined in closed form
from a single feature observation and 3 views

 This is used to initialize the asbolute scale [Kaiser, ICRA’16]
 The scale can then be tracked with


EKF [Mourikis & Roumeliotis, IJRR’10], [Weiss, JFR’13]
Non-linear optimization methods [Leutenegger, RSS’13] [Forster, RSS’15]

Davide Scaramuzza – Robotics and Perception Group - rpg.ifi.uzh.ch

Visual-Inertial Fusion [RSS’15]
 Fusion solved as a non-linear optimization problem
 Increased accuracy over filtering methods

IMU residuals

Reprojection residuals

Forster, Carlone, Dellaert, Scaramuzza, IMU Preintegration on Manifold for efficient Visual-Inertial
Maximum-a-Posteriori Estimation, Robotics Science and Systens’15, Best Paper Award Finalist

Comparison with Previous Works
Open Source
SVO + GTSAM (Forster et al. RSS’15)(optimization based, pre-integrated
IMU): https://bitbucket.org/gtborg/gtsam
Instructions here:
http://arxiv.org/pdf/1512.02363
YouTube: https://youtu.be/CsJkci5lfco

Accuracy: 0.1% of the travel distance
Proposed

Google Tango

OKVIS

Forster, Carlone, Dellaert, Scaramuzza, IMU Preintegration on Manifold for efficient Visual-Inertial
Maximum-a-Posteriori Estimation, Robotics Science and Systens’15, Best Paper Award Finalist

Open-source
VO & VSLAM algorithms

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Intro: visual odometry algorithms
 Popular visual odometry and SLAM algorithms


ORB-SLAM (University of Zaragoza, 2015)- ORB-SLAM2 (2016) supports stereo and RGBD camera



LSD-SLAM (Technical University of Munich, 2014)



DSO (Technical University of Munich, 2016)

SVO (University of Zurich, 2014/2016)- SVO 2.0 (2016) supports wide angle, stereo and multiple cameras

57

ORB-SLAM
Large-scale Feature-based SLAM
[Mur-Artal, Montiel, Tardos, TRO’15]

58

 It combines all together: Tracking
 Mapping Loop closing
 Relocalization (DBoW) Final optimization
 ORB: FAST corner + Oriented Rotated Brief descriptor

 Binary descriptor
 Very fast to compute and compare Real-time (30Hz)

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

ORB-SLAM: overview

60

ORB-SLAM: ORB feature ORB: Oriented FAST and Rotated Brief

256-bit binary descriptor

Fast to extract and match (Hamming distance)Good for tracking, relocation and Loop detectionMulti-scale detection: same point appears on several scales

61

ORB-SLAM: tracking

 For every new frame:First track w.r.t. last frame
Find matches from last frame in the new frame -> PnPThen track w.r.t. local map
Find matches from local keyframes in the new frame -> PnP

62

ORB-SLAM: mapping
 Map representationKeyframe poses

Points- 3D positions- Descriptor- Observations in frames

 Functions of the mapping part

Triangulate new pointsRemove redundant keyframes/points



Optimize poses and points

Q: why do we need keyframes instead of just using points?
63

ORB-SLAM: video

64

LSD-SLAM
Large-scale Semi-Dense SLAM
[Engel, Schoeps, Cremers, ECCV’14]

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Direct (photometric error) + Semi-Dense formulation
 3D geometry represented as semi-dense depth maps.
 Optimizes a photometric error Separateley optimizes poses (direct image alignment) &geometry (pixel-wise filtering)

Includes: Loop closing
 Relocalization
 Final optimization

 Real-time (30Hz)

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

LSD-SLAM: overview

 Direct image alignment Depth refinement and regularization
Instead of using features, LSD-SLAM uses pixels with large gradients.
67

LSD-SLAM: Direct Image Alignment
 New frame w.r.t. last keyframe• Finding pose that Minimizes photometric error rp over all selected pixels• Weighted by the photometric covariance Keyframe w.r.t. global map• Also minimizing geometric error: distance between the points in thecurrent keyframe and the points in the global map.
68

LSD-SLAM: Depth Refinement/Regularization
 Depth estimation: per pixel stereo:Using the estimated pose from image alignment, we can perform stereo
matching for each pixel.

Using the stereo matching result to refine the depth.

 Regularization


Average using adjacent depthRemove outliers and spurious estimations: visually appealing69

LSD-SLAM: video

70

DSO
Direct Sparse Odometry[Engel, Koltun, Cremers, Arxiv’16]

71

DSO: Tracking frontend
 Direct Image Alignment• Using points of large gradients• Incorporate photometric correction: robust to exposure time change• Using exposure time ti tj to compensate exposure time change• Using affine transformation if no exposure time is known72

DSO: Optimization backend
 Sliding window estimator

Not full bundle adjustment



Only keep a fixed length window (e.g., 3 keyframes) of past framesInstead of simply dropping the states out of the window, marginalizing thestates:Advantage:• Help improve accuracy• Still able to operate in real-time73

DSO: Video

74

SVO
Fast, Semi-Direct Visual Odometry[Forster, Pizzoli, Scaramuzza, ICRA’14, TRO’16]

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

SVO: overview
Direct (minimizes photometric error)• Corners and edgelets• Frame-to-frame motion
estimation

Feature-based (minimizes photometric error)• Frame-to-Keyframe pose refinement

Mapping• Probabilistic depth estimation

Extensions••••

Omni-cameras
Multi-camera systems
IMU pre-integration
Dense → REMODE

EdgeletCornerSVO: Semi-Direct Visual Odometry [ICRA’14]
Direct• Frame-to-frame motion
estimation

Feature-based• Frame-to-Keyframe poserefinement

77

SVO: Semi-Direct Visual Odometry [ICRA’14]
Direct• Frame-to-frame motion
estimation
Feature-based• Frame-to-Kreyframe poserefinement
Mapping Feature extraction only forevery keyframe Probabilistic depth estimationof 3D points

78

Probabilistic Depth Estimation in SVO
Depth-Filter:• Depth-filter for every new feature• Recursive Bayesian depth estimation• Epipolar search using ZMSSD

Measurement Likelihood models outliers:• 2-Dimensional distribution: Depth 𝒅 and inliner ratio 𝝆• Mixture of Gaussian + Uniform• Inverse depth
79

Probabilistic Depth Estimation in SVO
 Based on the model by (Vogiatzis &Hernandez, 2011) but withinverse depth

(1)
(2)

 The posterior in (1) can be approximated by
(3)
𝜌
𝑑
𝜌

𝜌

𝜌
𝑑

𝜌
𝑑

𝑑

𝜌
𝜌
Modeling the posterior
as a dense 2D histogramis very expensive!
𝜌𝑑The parametric model𝑑𝑑describes the pixel depth at time k.

𝑑80

SVO: Video

https://youtu.be/hR8uq1RTUfA

81

SVO for Autonomous Drone Navigation

Video
https://www.youtube.com/watch?v=4X6Voft4Z_0
RMS error: 5 mm, height: 1.5 m – Down-looking camera

Speed: 4 m/s, height: 1.5 m – Down-looking camera

Video: https://youtu.be/fXy4P3nvxHQ

Faessler, Fontana, Forster, Mueggler, Pizzoli, Scaramuzza, Autonomous, Vision-based Flight and Live Dense 3D
Mapping with a Quadrotor Micro Aerial Vehicle, Journal of Field Robotics, 2015.

SVO on 4 fisheye Cameras from AUDI dataset
Video: https://www.youtube.com/watch?v=gr00Bf0AP1k[Forster, et al., «SVO: Semi Direct Visual Odometry for Monocular and Multi-Camera Systems», TRO’16]

83

Processing Times of SVO[Forster, et al., «SVO: Semi Direct Visual Odometry for Monocular and Multi-Camera Systems», TRO’16]

84

Processing Times of SVO
Laptop (Intel i7, 2.8 GHz)
400 frames per second
Embedded ARM Cortex-A9, 1.7 GHz
Up to 70 frames per second

Source Code

 Open Source available at: github.com/uzh-rpg/rpg_svo Works with and without ROS
 Closed-Source professional edition (SVO 2.0): available for companies
85Summary: Feature-based vs. direct
Feature-based (ORB-SLAM, part of SVO/DSO) Large frame-to-frame motions
1. Feature extraction
2. Feature matching
3. RANSAC + P3P
4. Reprojection error
minimization
𝑇𝑘,𝑘−1 = arg min𝑇𝒖′𝑖 − 𝜋 𝒑𝑖

 Slow (20-30 Hz) due to costly feature
extraction and matching Not robust to high-frequency andrepetive texture Outliers

2𝑖Direct approaches (LSD, DSO, SVO)  Every pixel in the image can be

exploited (precision, robustness)

1. Minimize photometric error
𝑇𝑘,𝑘−1 = arg min𝑇𝑖

𝐼𝑘 𝒖′𝑖 − 𝐼𝑘−1 (𝒖𝑖 )2 Increasing camera frame-rate reducescomputational cost per frame Limited to small frame-to-frame
motion86Comparison among SVO, DSO, ORB-SLAM,LSD-SLAM [Forster, TRO’16]
 See next two slides
 For a thorough evaluation please refer to [Forster, TRO’16] paper,
where all these algorithms are evaluated in terms of accuracy

against ground truth and timing on several datasets: EUROC, TUMRGB-D, ICL-NUIM[Forster, et al., «SVO: Semi Direct Visual Odometry for Monocular and Multi-Camera Systems», TRO’16]

Accuracy (EUROC Dataset) [Forster, TRO’16][Forster, et al., «SVO: Semi Direct Visual Odometry for Monocular and Multi-Camera Systems», TRO’16]

Timing (EUROC Dataset) [Forster, TRO’16][Forster, et al., «SVO: Semi Direct Visual Odometry for Monocular and Multi-Camera Systems», TRO’16]

Review of Direct Image Alignment
It minimizes the per-pixel intensity difference
𝐼𝑘 𝒖′𝑖 − 𝐼𝑘−1 (𝒖𝑖 ) 2𝜎

𝑇𝑘,𝑘−1 = arg min
𝑇

𝑖

Dense

Semi-Dense

DTAM [Newcombe et al. ‘11]
300’000+ pixels

LSD [Engel et al. 2014]~10’000 pixels

SparseSVO [Forster et al. 2014, TRO’16]
100-200 features x 4x4 patch~ 2,000 pixels

Irani & Anandan, “All About Direct Methods,” Vision Algorithms: Theory and Practice, Springer, 2000
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.chGoal: study the magnitude of the perturbation for which image-to-model alignment iscapable to converge as a function of the distance to the reference image
 The performance in this experiment is a measure of robustness: successful pose
estimation from large initial perturbations shows that the algorithm is capable of dealing
with rapid camera motions
 1000 Blender simulations
 Alignment considered converged when the estimated relative pose is closer than 0.1
meters from ground-truth
 Result: difference between semi-dense image alignment and dense image alignment ismarginal. This is because pixels that exhibit no intensity gradient are not informative forthe optimization (their Jacobians are zero).
◦ Using all pixels becomes only useful when considering motion blur and image defocus
Convergence [%]



100

30%
overlap0
Distance between frames[Forster, et al., «SVO: Semi Direct
Visual Odometry for Monocular and Multi-Camera Systems», TRO’16]
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Summary: keyframe and filter-based method Why the parallel structure in all these algorithms?


Mapping is often expensive- Local BA- Loop detection and graph optimization- Depth filter per featureUsing the best map available for real-time tracking [1] Why not filter-based method?


Keyframe-based: more accuracy per unit computing time [2]Still useful in visual-inertial fusion- MSCKF- ROVIO[1] Klein, Georg, and David Murray. "Parallel tracking and mapping for small AR workspaces.
[2] Strasdat, Hauke, José MM Montiel, and Andrew J. Davison. "Visual SLAM: why filter?."92

Error Propagation

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Ck+1
Ck

Tk+1,kTk,k-1

Ck-1

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch The uncertainty of the camera pose 𝐶𝑘 is a combination of theuncertainty at 𝐶𝑘−1 (black-solid ellipse) and the uncertainty of thetransformation 𝑇𝑘 (gray dashed ellipse)
 𝐶𝑘 = 𝑓(𝐶𝑘−1 , 𝑇𝑘 )

Ck+1
Ck

 The combined covariance ∑𝑘 is

Tk+1
Tk

Ck-1

 The camera-pose uncertainty is always increasing when concatenatingtransformations. Thus, it is important to keep the uncertainties of theindividual transformations smallDavide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Commercial Applications of SVO

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Application: Autonomous Inspection of Bridges and Power Masts
Project with Parrot: Autonomous vision-based navigation

Video: https://youtu.be/mYKrR8pihAQ

Albris drone

Dacuda VR solutions

Dacuda Fully immersive virtual reality with 6-DoF for VR and AR content (running on
iPhone): https://www.youtube.com/watch?v=k0MLs5mqRNo

 Powered by SVO

3DAround iPhone App

DacudaZurich-Eye – www.zurich-eye.com
Vision-based Localization and Mapping Solutions for Mobile Robots
Started in Sep. 2015, became Facebook-Oculus R&D Zurich in Sep. 2016

Event-based Vision

Open Problems and Challenges in Agile Robotics
Current flight maneuvers achieved with onboard cameras are still to slow
compared with those attainable by birds or FPV pilots

FPV-Drone race

To go faster, we need faster sensors!
At the current state, the agility of a robot is limited by the latency andtemporal discretization of its sensing pipeline.

Currently, the average robot-vision algorithms have latencies of 50-200 ms.
This puts a hard bound on the agility of the platform.
next frame

frame

time
computation

latencycommandcommandtemporal discretization

Can we create a low-latency, low-discretization perception pipeline?
- Yes, if we combine cameras with event-based sensors[Censi & Scaramuzza, «Low Latency, Event-based Visual Odometry», ICRA’14]

Human Vision System 130 million photoreceptors
 But only 2 million axons!

Dynamic Vision Sensor (DVS) Event-based camera developed by Tobi Delbruck’s group (ETH & UZH).
 Temporal resolution: 1 μs
 High dynamic range: 120 dB
 Low power: 20 mW
 Cost: 2,500 EUR

Image of the solar eclipse (March’15) captured by
a DVS (courtesy of IniLabs)[Lichtsteiner, Posch, Delbruck. A 128x128 120 dB 15µs Latency Asynchronous Temporal Contrast
Vision Sensor. 2008]

Camera vs DVSA traditional camera outputs frames at fixed time intervals:

frame

next frametime By contrast, a DVS outputs asynchronous events at microsecond resolution.
An event is generated each time a single pixel detects an intensity changes value

events streamtime
𝑑𝐼(𝑥, 𝑦)event: 𝑡, 𝑥, 𝑦 , 𝑠𝑖𝑔𝑛
𝑑𝑡
sign (+1 or -1)
Lichtsteiner, Posch, Delbruck. A 128x128 120 dB 15µs Latency Asynchronous Temporal
Contrast Vision Sensor. 2008

Camera vs Dynamic Vision Sensor
 Video with DVS explanation

Video: http://youtu.be/LauQ6LWTkxMCamera vs Dynamic Vision Sensor
 Video with DVS explanation

Video: http://youtu.be/LauQ6LWTkxM

DVS Operating Principle [Lichtsteiner, ISCAS’09]
Events are generated any time a single pixel sees a change in brightness larger than 𝐶∆log 𝐼 ≥ 𝐶

V = log 𝐼(𝑡)

𝑂𝑁

𝑂𝑁

𝑂𝑁

𝑂𝑁

𝑂𝐹𝐹 𝑂𝐹𝐹 𝑂𝐹𝐹

𝑂𝐹𝐹 𝑂𝐹𝐹 𝑂𝐹𝐹

The intensity signal at the event time can be reconstructed by integration of ±𝐶[Cook et al., IJCNN’11][Kim et al., BMVC’15][Lichtsteiner, Posch, Delbruck. A 128x128 120 dB 15µs Latency Asynchronous Temporal Contrast
Vision Sensor. 2008]

Pose Tracking and Intensity Reconstruction from a DVS

Dynamic Vision Sensor (DVS)
Advantages•

low-latency (~1 micro-second)•

high-dynamic range (120 dB instead 60 dB)•

Very low bandwidth (only intensity changes are transmitted): ~200Kb/s•

Low storage capacity, processing time, and power

Disadvantages•

Require totally new vision algorithms•

No intensity information (only binary intensity changes)

Lichtsteiner, Posch, Delbruck. A 128x128 120 dB 15µs Latency Asynchronous Temporal Contrast Vision Sensor. 2008

Generative Model [Censi & Scaramuzza, ICRA’14]
The generative model tells us that the probability that an event is generated depends on thescalar product between the gradient 𝛻𝐼 and the apparent motion 𝐮∆𝑡P(e)∝ 𝛻𝐼, 𝐮∆𝑡Generative event model: 𝛻 ∆log 𝐼 , 𝐮∆𝑡 = 𝐶

P

u
v

𝐮
O

Zc
C
Y

p

X
c

[Event-based Camera Pose Tracking using a Generative Event Model, Arxiv]c[Censi & Scaramuzza, Low Latency, Event-based Visual Odometry, ICRA’14]

𝛻𝐼

Event-based 6DoF Pose Estimation Results

Video: https://youtu.be/iZZ77F-hwzs[Event-based Camera Pose Tracking using a Generative Event Model, Arxiv][Censi & Scaramuzza, Low Latency, Event-based Visual Odometry, ICRA’14]

Robustness to Illumination Changes and High-speed Motion

Video: https://www.youtube.com/watch?v=EUX3Tfx0KKE

BMVC’16 : EMVS: Event-based Multi-View Stereo, Best Industry Paper Award

Possible future sensing architecture[Censi & Scaramuzza, Low Latency, Event-based Visual Odometry, ICRA’14]

DAVIS: Dynamic and Active-pixel Vision Sensor [Brandli’14]
Combines an event camera with a frame-based camera in the same pixel array!

CMOS frames

DVS eventstime

Brandli, Berner, Yang, Liu, Delbruck, "A 240× 180 130 dB 3 µs Latency Global Shutter Spatiotemporal Vision
Sensor." IEEE Journal of Solid-State Circuits, 2014.

Event-based Feature Tracking [IROS’16]
 Extract Harris corners on images
 Track corners using event-based Iterative Closest Points (ICP)

IROS’16 : Low-Latency Visual Odometry using Event-based Feature Tracks, Best application paper award finalist
Elias Mueggler – Robotics and Perception Group117

Event-based, Sparse Visual Odometry [IROS’16]

Video: https://youtu.be/RDu5eldW8i8

IROS’16 : Event-based Feature Tracking for Low-latency Visual Odometry, Best application paper award finalist
Elias Mueggler – Robotics and Perception Group

118

Conclusions
 VO & SLAM theory well established Biggest challenges today are reliability and robustnessHDR scenes
High-speed motion
Low-texture scenes

 Which VO/SLAM is best?


Depends on the task and how you measure the performance!
- E.g., VR/AR/MR vs Robotics

 99% of SLAM algorithms are passive: need active SLAM!
 Event cameras open enormous possibilities! Standard cameras have been studied
for 50 years!
 Ideal for high speed motion estimation and robustness to HDR illumination
changesVO (i.e., no loop closing)
Modified PTAM: (feature-based, mono): http://wiki.ros.org/ethzasl_ptam
 LIBVISO2 (feature-based, mono and stereo): http://www.cvlibs.net/software/libviso
 SVO (semi-direct, mono, stereo, multi-cameras): https://github.com/uzh-rpg/rpg_svo
 DSO (direct sparse odometry): https://github.com/JakobEngel/dso


VIO
ROVIO (tightly coupled EKF): https://github.com/ethz-asl/rovio
 OKVIS (non-linear optimization): https://github.com/ethz-asl/okvis
 SVO + GTSAM (Forster et al. RSS’15) (optimization based, pre-integrated IMU):
https://bitbucket.org/gtborg/gtsam
 Instructions here: http://arxiv.org/pdf/1512.02363


VSLAM
ORB-SLAM (feature based, mono and stereo): https://github.com/raulmur/ORB_SLAM
 LSD-SLAM (semi-dense, direct, mono): https://github.com/tum-vision/lsd_slamDavide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch GTSAM: https://collab.cc.gatech.edu/borg/gtsam?destination=node%2F299
 G2o: https://openslam.org/g2o.html
 Google Ceres Solver: http://ceres-solver.org/

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.chVO (i.e., no loop closing)
Modified PTAM (Weiss et al.,): (feature-based, mono): http://wiki.ros.org/ethzasl_ptam
 SVO (Forster et al.) (semi-direct, mono, stereo, multi-cameras): https://github.com/uzhrpg/rpg_svo


IMU-Vision fusion:Multi-Sensor Fusion Package (MSF) (Weiss et al.) - EKF, loosely-coupled:
http://wiki.ros.org/ethzasl_sensor_fusion
 SVO + GTSAM (Forster et al. RSS’15) (optimization based, pre-integrated IMU):
https://bitbucket.org/gtborg/gtsam
 Instructions here: http://arxiv.org/pdf/1512.02363
 OKVIS (non-linear optimization): https://github.com/ethz-asl/okvis
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch Open source: MAVMAP: https://github.com/mavmap/mavmap
 Closed source: Pix4D: https://pix4d.com/

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch DBoW2: https://github.com/dorian3d/DBoW2
 FABMAP: http://mrg.robots.ox.ac.uk/fabmap/

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

VO Datasets Malaga dataset: http://www.mrpt.org/malaga_dataset_2009
 KITTI Dataset: http://www.cvlibs.net/datasets/kitti/
VIO Datasets
These datasets include ground-truth 6-DOF poses from Vicon and synchronized
IMU and images:
 EUROC MAV Dataset (forward-facing stereo):
http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets RPG-UZH dataset (downward-facing monocular)
http://rpg.ifi.uzh.ch/datasets/dalidation.bag
More Check out also this:
http://homepages.inf.ed.ac.uk/rbf/CVonline/Imagedbase.htm
Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch

Davide Scaramuzza – University of Zurich – Robotics and Perception Group - rpg.ifi.uzh.ch