FG Neuroinformatik &

Kognitive Robotik

Egomotion Estimation using

Visual Odometry
Ruta Desai
Ruta Desai: Visual Odometry

NIT Surat
India

DAAD WISE 2010 Project

Guided By: Erik Einhorn
Neuroinformatics and Cognitive Robotics Lab
TU Ilmenau
1 / Ges
FG Neuroinformatik &
Kognitive Robotik

Flow of the Presentation

 Introduction
 Motivation
 Basics of the Central Algorithm
 Implemented Approaches
 Results
 Future work
 Resources used
 Bibliography
Ruta Desai: Visual Odometry

2 / Ges
FG Neuroinformatik &
Kognitive Robotik
Introduction: Visual Odometry & Egomotion Estimation

 Visual Odometry:
The process of determining the position and orientation of a robot by
analyzing the associated camera images. 

Egomotion:
 Egomotion refers to estimating a camera's motion relative to a rigid
scene.
 Visual odometry techniques are used on a sequence of images
captured by the moving camera.
Ruta Desai: Visual Odometry

3 / Ges
FG Neuroinformatik &
Kognitive Robotik

Motivation

 Structure from motion refers to the process of finding the three-

dimensional structure by analyzing the motion of an object over time.
Ruta Desai: Visual Odometry

4 / Ges
 Ruta Desai: Visual Odometry FG Neuroinformatik &
Kognitive Robotik

Motivation: The Big Picture..

5 / Ges
FG Neuroinformatik &
Kognitive Robotik

Various Approaches Implemented

 EKF and 1 point RANSAC with inlier rescue.

 Rotation Estimation using Homography.
 Rotation Estimation using Epipolar Geometry Constraint.
 Rotation Estimation using Projective Reconstruction Constraint.
Ruta Desai: Visual Odometry

6 / Ges
FG Neuroinformatik &
Kognitive Robotik

..Basics:Kalman Filter and Extended Kalman Filter..

 Provides an efficient
computational (recursive)
means to estimate the state
of a process.

 But, Kalman filter assumes

process to be linear.

 This limitation is overcome in

Ruta Desai: Visual Odometry

EKF, which is similar to

Kalman Filter except that a
Taylor series expansion is
used to linearise.

7 / Ges
FG Neuroinformatik &
Kognitive Robotik

..Basics: RANSAC..

 Used for robust estimation

from a data set that may
contain outliers.

 An outlying observation, or
outlier, is one that appears to
deviate markedly from other
members of the sample in
Ruta Desai: Visual Odometry

which it occurs.

log(1  p)
 How it works: N
log(1  (1   ) s )

8 / Ges
FG Neuroinformatik &
Kognitive Robotik

RANSAC without EKF

Consider a simple case of 2D line estimation from a set of points

contaminated with spurious data.
Ruta Desai: Visual Odometry

9 / Ges
FG Neuroinformatik &
Kognitive Robotik

The Central Algortihm:1 Point RANSAC with EKF

 The algorithm assumes that an a priori probability distribution over

the model parameters is known in advance. This prior knowledge
allows us to compute the random hypotheses using only 1 data point,
hence reducing the number of hypotheses and the computational
cost.
Ruta Desai: Visual Odometry

10 / Ges
FG Neuroinformatik &
Kognitive Robotik

Approach 1 (Inliers Rescue):The Flow

Reference:
Civera, J., Grasa,
O. G., Davison, A.
J., and Montiel, J.
M. M. (2010) 1-
Point RANSAC for
EKF Filtering.
Application to
Real-Time
Structure from
Motion and Visual
Ruta Desai: Visual Odometry

Odometry.

11 / Ges
FG Neuroinformatik &
Kognitive Robotik

State Model

 State vector of camera is obtained from odometry (for the first time)
or the last estimated state ( from camera extrinsic object)
 X-Y Position θ and Ф are also updated using motion model.
 Motion model error is also added.

 State Vector: bt = (xi , yi , Ф, θ)

 bt 1 = bt +(Rz( Δx, Δy), ΔФ*error, θ)
 Sigma+=R;
Ruta Desai: Visual Odometry

where Sigma=initial covariance

R=model noise
Rz=Rotation about Z axis

12 / Ges
FG Neuroinformatik &
Kognitive Robotik

RANSAC

 A feature is randomly
selected from the 3D feature
set.
 Measurement update is done
using this feature.
 After this measurement
update,all measurements are
predicted again using the
projection matrix
Ruta Desai: Visual Odometry

 If the original feature and the  This is done for nhyp(no of

new one(measurement) have iterations needed by RANSAC
difference below a algorithm) times till the biggest
threshold(Euclidean inliers set.
distance), the feature is  Final Meaurement update of
classified as inlier or LOW the state can be done using
INNOVATION inliers and thus this inliers set.
a set of inliers is constructed.
13 / Ges
FG Neuroinformatik &
Kognitive Robotik

Measurement Update
 Map 3D feature into image space using projection matrix.

 This is the homogeneous measurement and so it is converted to

Ruta Desai: Visual Odometry

Euclidean space.
 x' 
 y '    x' / w'
   y ' / w'
 w'  
 Jacobian of this measurement w.r.t camera state is calculated.
 Using Jacobian innovation and Kalman Gain is then calculated.
 Once innovation and Kalman Gain are obtained, mean and
covariance update is done.
14 / Ges
FG Neuroinformatik &
Kognitive Robotik

Inliers Rescue

 We assume that certain features that could be inliers are

misclassified as outliers.
 So, in order to obtain them we assume that the estimated camera
pose is correct and try to map covariance of features in image space
for which we obtain Jacobian of feature w.r.t feature and find
innovation.
 Then based on Mahalanobis distance threshold we obtain features
lying in 99% probability region.
 These are called HIGH INNOVATION inliers.
Ruta Desai: Visual Odometry

 Later update of state is done using these inliers.

15 / Ges
FG Neuroinformatik &
Kognitive Robotik

Modifications in Approach 1: Selection of Features

 Sorting of features based on difference from original Measurements.

 Use of Far Features.
Ruta Desai: Visual Odometry

16 / Ges
FG Neuroinformatik &
Kognitive Robotik

Results (With Inliers Rescue)

 Without Error:  With added Error:

Blue=estimated
Red=Ground truth
Ruta Desai: Visual Odometry

17 / Ges
FG Neuroinformatik &
Kognitive Robotik

Results (Without Inliers Rescue)

 Without Error:  With added Error:

Blue=estimated
Red=Ground truth
Ruta Desai: Visual Odometry

18 / Ges
 Ruta Desai: Visual Odometry FG Neuroinformatik &
Kognitive Robotik

Algorithm : The Big Picture..

19 / Ges
FG Neuroinformatik &
Kognitive Robotik

Motivation for further Approaches..

 The combination of EKF and 1 point RANSAC worked well for the
correction of angular orientation of camera pose.

 Moreover, correction of angular orientation was more important for

our problem statement.

 So, we decided to estimate and correct just the rotation of Camera

using this algorithm.
Ruta Desai: Visual Odometry

20 / Ges
FG Neuroinformatik &
Kognitive Robotik

Approach 2: Rotation Estimate using Homography

 State Vector: bt = (Ф, θ)

 State vector is updated using EKF.
 Position Vector is updated using Motion Model where velocity is
obtained from the odometry
Reference: R. Hartley and A.
c  x y h Zisserman. Multiple View
c t 1  c t  R  v Geometry in Computer Vision.
Cambridge University Press
v  o t 1  o t
Ruta Desai: Visual Odometry

where R=Rotation matrix, oi=odometry at time i, c=camera centre,

v=velocity
 If we use 3D feature mapping for measurement model, the error in
estimated 3D position would result in an error in the feature
measurement too.
 So, in order to obtain corresponding image pairs, Homography
between image planes was used in this approach.

21 / Ges
 Ruta Desai: Visual Odometry FG Neuroinformatik &
Kognitive Robotik

Here lies Homography! : The Big Picture Again..

22 / Ges
FG Neuroinformatik &
Kognitive Robotik

Homography

x=PX
X(λ)=P+x+ λC
X  = P+x (λ=0)
x’=P’ X 
x’=P’ P+x
x’=Hx

where H=homography
matrix
Ruta Desai: Visual Odometry

The homography transfers points from

one view to another.

23 / Ges
FG Neuroinformatik &
Kognitive Robotik

Results

Without With Odometry:

Odometry:
Blue=estimated
Red=Ground truth
Ruta Desai: Visual Odometry

24 / Ges
FG Neuroinformatik &
Kognitive Robotik

Approach 3: Epipolar Geometry Constraint

 It is the intrinsic projective geometry between two images. It depends

only on camera ‘s internal parameters and relative pose.
 Fundamental matrix( F) encapsulates this intrinsic geometry.
 The Fundamental matrix is the relation between any 2 images of
same scene that constraints where the projection of scene can occur
in both images.
 Given the projection of a scene point in one of the images , the
corresponding point in the other image is constrained to a line.
 With homogeneous image coordinates x and x’ of corresponding
Ruta Desai: Visual Odometry

points in image pairs; Fx describes the line (epipolar line) on which

corresponding point x’ on other image will lie.

Reference:B.Williams and I.Reid. On

Combining Visual SLAM and Visual
Odometry.

25 / Ges
 Ruta Desai: Visual Odometry FG Neuroinformatik &
Kognitive Robotik

Here lies Epipolar Constraint! :The Big Picture Again..

26 / Ges
FG Neuroinformatik &
Kognitive Robotik
Calculation of Innovation

x'T Fx=0 (Since x’ should lie on epipolar line l ' = Fx)

Ruta Desai: Visual Odometry

27 / Ges
FG Neuroinformatik &
Kognitive Robotik

Approach 4: Projective Reconstruction Constraint

 Once P1 : Projection matrix, has been set ; each 3D point is
constrained to lie on the ray back projected from the optical centre.
 Once P2 : Projection matrix for different pose is also set; 3D point is
then constrained to lie at the intersection point of the pair of back
projected ray.
 Back projected rays may be skew due to noise in the image
measurements.
 When camera position is updated correctly, the rays will intersect and
hence the perpendicular distance between them will be zero. Thus,
Ruta Desai: Visual Odometry

the perpendicular distance between skew rays can be used as

innovation for EKF update.
Reference:
P.A. Beardsley, A.
Zisserman and D.W.
Murray. Sequential
Updating of Projective and
Affine Structure from
Motion 28 / Ges
 Ruta Desai: Visual Odometry FG Neuroinformatik &
Kognitive Robotik

Here lies the Projective Constraint!...

29 / Ges
FG Neuroinformatik &
Kognitive Robotik

Calculation of Innovation

 l = P1+ x1
 m = P2+ x2
 
(C2C1) (l m)
 perpendicular distance between the skew rays=
(l m)

where Ci= camera centre

l, m
=direction cosines of the backprojected rays
Ruta Desai: Visual Odometry

30 / Ges
FG Neuroinformatik &
Kognitive Robotik

Resources used

 Eclipse C++
Ruta Desai: Visual Odometry

 Maple
 Monocular vision and Eigen
C++ library

31 / Ges
FG Neuroinformatik &
Kognitive Robotik

The Growth of the Idea..

●
Literature Survey and Building the Basics.
Getting acquainted with Eclipse and C++

MAY
●

●
Understanding the Problem Statement, and Coding for
Inliers Rescue approach

●
Modifications in Inliers Rescue approach
JUNE

●
Coding for Homography based Rotation Estimate.
Coding For Epipolar Geometry based Rotation Estimate.
Ruta Desai: Visual Odometry

●
LY

●
Use of Maple for Jacobian Calculation.
U
J

●
Coding for Projective Reconstruction Constraint based Rotation Estimate.
●
Final presentation and compilation into paper.
●
Implementation on robot.

32 / Ges
FG Neuroinformatik &
Kognitive Robotik

My Contribution

 Imbibing the problem

statement and understanding
the basics for the same.

 Implementing various
approaches for correction of
camera pose for effective
working of 3D structure
reconstruction on the
Ruta Desai: Visual Odometry

framework made by Erik.

 Out of the various

approaches implemented,
Rotation Estimate using
Homography can be readily
integrated into the SFM
algorithm.
33 / Ges
FG Neuroinformatik &
Kognitive Robotik

Future Work

 First and foremost, to make Projective Reconstruction approach

work.

 Implementation of Rotation Estimate using Homography on the robot.

 Use of Adaptive Sigma and Noise matrices.

 Moving Obstacle Avoidance using EKF and 1 point RANSAC

algorithm.
Ruta Desai: Visual Odometry

 Comparison of results and Compilation into a Paper.

34 / Ges
FG Neuroinformatik &
Kognitive Robotik

Bibliography
 Civera, J., Grasa, O. G., Davison, A. J., and Montiel, J. M. M.1-Point
RANSAC for EKF Filtering. Application to Real-Time Structure from
Motion and Visual Odometry, ICRA 2010.

 R. Hartley and A. Zisserman. Multiple View Geometry in Computer

Vision. Cambridge University Press, Page number:239-247 and 270.

 E. Einhorn, Ch. Schröter,and H.-M. Gross. Monocular Scene

Reconstruction for Reliable Obstacle Detection and Robot
Ruta Desai: Visual Odometry

Navigation, Proc. 4th European Conference on Mobile Robots

(ECMR) 2009.

 J. Civera, A.J. Davison, and J. Montiel. Inverse Depth

Parametrization for Monocular SLAM,IEEE Transactions on Robotics,
24(5):932–945, October 2008.

35 / Ges
FG Neuroinformatik &
Kognitive Robotik

Bibliography

 D.Scaramuzza, F.Fraundorfer, and R. Siegwart .Real-Time Monocular

Visual Odometry for On-Road Vehicles with 1-Point RANSAC,ICRA
2009.

 P.A. Beardsley, A. Zisserman and D.W. Murray. Sequential Updating

of Projective and Affine Structure from Motion, International Journal
of Computer Vision 23(3), 235–259 (1997).

 B.Williams and I.Reid. On Combining Visual SLAM and Visual

Ruta Desai: Visual Odometry

Odometry.

 S.Thrun,W.Burgurd and D.Fox.Probabilistic Robotics.MIT Press,

Page number:39-44 and 54-57.

 www.wikipedia.org

36 / Ges
FG Neuroinformatik &
Kognitive Robotik

Acknowledgements

 First and foremost, I would like to thank Erik Einhorn for his guidance,
help and patience. 

 Professor Gross for giving me an opportunity to come to this lab and

always encouraging me. 

 Sandra and Christoph for answering my doubts and for all the super
discussions about Germany: from Soccer to weather 
Ruta Desai: Visual Odometry

 Heike for being so friendly and helpful and always being around
when I needed her 

 And all others who welcomed me so heartily into this family and
helped me mingle into it like family’s own child.

37 / Ges
 Ruta Desai: Visual Odometry FG Neuroinformatik &
Kognitive Robotik

Questions??..
Thank you…

38 / Ges
FG Neuroinformatik &
Kognitive Robotik

Fundamental Matrix and Epipolar line

 x=PX
 X(λ)=P1+x+ λC
 x’(λ)=P2P1+x+ P2 λC
 x'
=P2P1+x (  0)
 x’=P2 C (  )
 
l'
 =P2 C X P2P1+x
= Fx
Ruta Desai: Visual Odometry

where F=Fundamental Matrix

 P’=KR’[I | -C] and P+=RTK-1

39 / Ges
FG Neuroinformatik &
Kognitive Robotik

Epipolar Geometry : Calculation of Innovation

n ( x 'T  l ' )
d  2
n

where n = normal of epipolar line l’

d =measurement as well as innovation for the EKF update
||.||=norm
Ruta Desai: Visual Odometry

40 / Ges