Direct Visual Odometry for a Fisheye-Stereo Camera
Peidong Liu1 , Lionel Heng2 , Torsten Sattler1 , Andreas Geiger1,3 , and Marc Pollefeys1,4

Abstract— We present a direct visual odometry algorithm fora fisheye-stereo camera. Our algorithm performs simultaneous
camera motion estimation and semi-dense reconstruction. The
pipeline consists of two threads: a tracking thread and amapping thread. In the tracking thread, we estimate the camera
pose via semi-dense direct image alignment. To have a wider
field of view (FoV) which is important for robotic perception,
we use fisheye images directly without converting them toconventional pinhole images which come with a limited FoV.
To address the epipolar curve problem, plane-sweeping stereo
is used for stereo matching and depth initialization. Multiple
depth hypotheses are tracked for selected pixels to better
capture the uncertainty characteristics of stereo matching.
Temporal motion stereo is then used to refine the depth andremove false positive depth hypotheses. Our implementation
runs at an average of 20 Hz on a low-end PC. We run
experiments in outdoor environments to validate our algorithm,
and discuss the experimental results. We experimentally showthat we are able to estimate 6D poses with low drift, andat the same time, do semi-dense 3D reconstruction with highaccuracy. To the best of our knowledge, there is no other existingsemi-dense direct visual odometry algorithm for a fisheye-stereo
camera.

I. INTRODUCTION
Forster et al. [8] and Engel et al. [5] started a new waveof semi-direct and direct visual odometry (VO) methods
respectively for high-speed motion estimation. Previously,
traditional feature-based visual odometry methods [19, 10]were dominant, but have fallen out of favor as they involvethe computationally expensive tasks of feature descriptor
computation and outlier identification in a set of featurecorrespondences. These tasks have been made redundant bysemi-direct and direct visual odometry methods which directly use pixel intensity values instead of hand-crafted highdimensional feature descriptors. Consequently, these methods are able to run at high frame rates and with less computational resources, and are most suited for computationallyconstrained mobile robotic applications.
Semi-direct and direct visual odometry methods generally follow the simultaneous-tracking-and-mapping paradigm[14]. These two types of methods are similar in the aspectthat pixel intensity values are used for both motion estimationand stereo matching. However, semi-direct methods use
indirect pose and structure optimization which minimizes
feature-based reprojection errors while direct methods use
direct pose and structure optimization which minimizes
1 Computer Vision and Geometry Group, Department of Computer Science, ETH Zürich, Switzerland
2 Robotics Autonomy Lab, Manned - Unmanned Programme, Information
Division, DSO National Laboratories, Singapore
3 Autonomous Vision Group, MPI for Intelligent Systems, Tübingen,Germany
4 Microsoft, Redmond, USA

photometric errors. The semi-direct approach of Forster et al.
[8] has both direct and feature-based characteristics. The
direct method of image alignment is used to estimate theinitial motion. At the same time, feature-based methods areused to implicitly obtain feature correspondences throughfeature alignment, and feature-based optimization is used to
optimize the pose and landmark coordinates based on featurecorrespondences over a window of frames. In contrast, Engelet al. [5] exclusively uses direct techniques. A semi-dense
depth map is propagated from frame to frame, and refinedwith depth values that are obtained from variable-baseline
stereo matching for pixels with high gradient values. At
the same time, the depth map with respect to the mostrecent keyframe is used to track current frame via direct
image alignment. Another difference between the semi-direct
approach of Forster et al. [8] and the direct approach of Engelet al. [5] is the number of pixels used for motion estimation.
Similarly to conventional feature-based VO methods, thesemi-direct approach samples features very sparsely while
the direct approach uses a much larger number of pixels withhigh gradients. In other words, the direct approach is able
to do motion estimation and semi-dense 3D reconstruction
simultaneously. This motivates us to follow the formulationof the direct method proposed by Engel et al. [5] for ouralgorithm.
The use of a monocular pinhole camera in [5, 8, 20] leads
to scale drift. Researchers have proposed the use of inertial
measurement units (IMUs) [15, 17] and stereo cameras[4, 16] to eliminate scale drift. We see a stereo camera as
more robust than an IMU; noisy accelerometer measurements
on vehicle platforms with considerable amounts of vibration
do not allow precise observations of scale whereas a stereo
camera is immune to effects of vibration as long as themount does not allow the vibration to distort the extrinsic
stereo parameters. Furthermore, fisheye cameras improverobustness by significantly increasing the number of image
pixels corresponding to static areas in dynamic environments
and which can be used for accurate motion estimation.
Therefore, we propose a direct visual odometry algorithm
for a fisheye-stereo camera in this paper. The proposed
algorithm enables mobile robots to do motion estimation andsemi-dense 3D reconstruction simultaneously. Real world
experimental results show that our algorithm not only gives
low-drift motion estimation but also is able to do accuratesemi-dense 3D reconstruction.
The rest of the paper is organized as follows. Section II
provides an overview of the related work. Section III lists
the main contributions of our method. Section IV and Section V describes the formulations of our algorithm in detail.

Section VI discusses the evaluation results of our proposed
method with real-world datasets.
II. R ELATED W ORK
A. Monocular vs. Stereo
Forster et al. [9] and Engel et al. [7] make extensions
to their seminal work in semi-direct [8] and direct visual
odometry [5] respectively for monocular cameras moving athigh speeds. In [9], edgelet features are tracked in addition tocorner features. In turn, motion estimates are more accurate,
especially in environments lacking in corner features. Theyuse motion priors from either a constant velocity model orgyroscopic measurements to make tracking more robust andfaster. With motion priors, the initial motion estimate forimage alignment is closer to the actual value, resulting inconvergence at the global minimum, especially in the presence of local minima. Furthermore, convergence is faster due
to a fewer number of iterations in optimisation. In contrast to
Forster et al. [9] which both minimizes photometric and geometric (reprojection) errors, Engel et al. [7] only minimizes
photometric errors. In this case, for more accurate results,
a photometric calibration is used in addition to a standard
geometric calibration that models image projection and lensdistortion. Such a photometric calibration accounts for a nonlinear response function, lens vignetting, and exposure time.
Besides camera poses, they also optimize affine brightness
parameters, inverse depth values, and camera intrinsics.
Stereo visual odometry resolves scale ambiguity in monocular visual odometry by estimating metric depth from stereo
images with a known baseline. Forster et al. [9] expand their
original semi-direct visual odometry method [8] for monocular cameras to multi-camera systems. Omari et al. [18] follow
the direct approach advocated by Engel et al. [5] but neither
propagate nor update a semi-dense depth map. Instead, they
directly obtain the depth map from stereo block matching
applied to rectified stereo images, and uses this depth map to
track new frames. Engel et al. [6] improve on their original
direct visual odometry method [5] by incorporating depth
measurements from static stereo in addition to those from
temporal stereo.
B. Pinhole vs. Fisheye
With a fisheye camera comes a large field of view. Thiscan be advantageous in urban settings with many moving
vehicles which can otherwise occlude a pinhole camera’s
field of view, making tracking impossible. Both semi-direct
and direct visual odometry methods for pinhole cameras
typically perform disparity search along the epipolar line.
However, epipolar curves instead of epipolar lines exist in
fisheye images. A fisheye camera brings added computational
complexity to epipolar stereo matching as epipolar curves
are much more expensive to compute. Caruso et al. [3]do a disparity search along the epipolar curve. We cancircumvent the problem of epipolar curves by using thenaı̈ve step of extracting rectified pinhole images from fisheye
images at the expense of a significant loss of field-of-view.
Heng and Choi [13] propose a semi-direct approach for afisheye-stereo camera and which directly operates on fisheye
images. They use a fisheye camera model in both imagealignment, and pose and structure refinement, and leverageplane-sweeping stereo for direct stereo matching with fisheye
images. However, there exists no direct approach for afisheye-stereo camera to the best of our knowledge.
III. CONTRIBUTIONS
The main contribution of this paper is a direct visual
odometry algorithm for a fisheye-stereo camera. To the bestof our knowledge, no direct visual odometry algorithm exists
for a fisheye-stereo camera.
Our paper is most similar in spirit to that of Engel et al.
[5] with three key differences:
1) We use fisheye cameras instead of pinhole cameras.
2) We avoid scale ambiguity by using depth measurements from wide-baseline stereo.
3) We keep track of multiple depth hypotheses when
initializing depth values for new pixels to track.
In wide-baseline stereo, it is common to encounter many
local minima, making it difficult to find correct depth values.
We solve this problem by keeping track of multiple depth
hypotheses corresponding to local minima, and eliminating
incorrect hypotheses over time until we are left with a single
depth hypothesis.
IV. NOTATION
We denote the world reference frame as !
F W , the stereocamera reference frame as !
F S , and the individual camera
reference frames as !
F C1 and !
F C2 . Here, C1 is the reference
camera in the stereo camera, and thus, !
F C1 coincides withF.
We
denote
the
image
from
cameraCi at time step k as
S!
IkCi , and the stereo frame at time step k as FSk = {IkC1 , IkC2 }.
IkCi (u) is the intensity of the pixel with image coordinate u.
The camera projection model ⇡ : R3 7! R2 projects alandmark with coordinates Ci p = [x y z]T in !
F Ci to anTimage point u = [u v] in ICi :u = ⇡(Ci p).

(1)

We use the unified projection model [2, 11] whose intrinsic
parameters are known from calibration. Given the inverse
projection function ⇡ 1 , we recover the coordinates of thelandmark corresponding to an image point u in ICi for which
the depth du 2 R is known:
Ci p = ⇡1(u, du ).

(2)

If the depth is not known, we recover the ray fu in !
F Ci
and passing through the landmark that corresponds to theimage point u in Ci :Ci fu = ⇡

1

(u).

(3)

We denote the stereo camera pose at time step k as arigid body transformation TkSW 2 SE(3) from !
F W to
k
F
and
whose
rotation
matrixpart
isR,
corresponding
S
SW!
quaternion part is qkSW , and translation part is tkSW . Similarly, we denote the pose of camera Ci at time step k with

RefineModel-based
motion prediction

Motion stereo

i2⌦(IrC )InsertLocal map
Direct imagealignment

step r by minimizing the following cost function which isthe sum of squared photometric errors:
X
E(Tk,rri2(7)C1 ) =

MapperTrackerRequest KF=

Plane-sweeping
stereoTkCi W = TCi S TkSW . TCi S is known from calibration. The
rigid body transformation TkCi W maps a 3D point W p in
F W to a 3D point Ci p in !
F Ci :!
(4)

Given the fact that a rigid body transformation matrix
is over-parameterized, on-manifold optimization requires aminimal representation of the rigid body transformation. In
this case, we use the Lie algebra se(3) corresponding to thetangent space of SE(3). We denote the algebra elements,
also known as twist coordinates, with ⇠ = [v !]T where v
is the linear velocity and ! is the angular velocity. We use
the exponential map to map the twist coordinates ⇠ in se(3)to a rigid body transformation T in SE(3):(5)

Similarly, we use the logarithm map to map a rigid bodytransform T in SE(3) to twist coordinates ⇠ in se(3):⇠ = log(T(⇠)).

1
IkC1 (⇡(Tk,r(ui , dui )))2 ,C1 ⇡1Fig. 1: System overview

T(⇠) = exp(⇠).

(IrC1 (ui )i2⌦(IrC )KF?

kCi p = TCi W W p.

X1

(6)

V. ALGORITHM
In this section, we describe our semi-sense visual odometry algorithm for a fisheye-stereo camera. As shown in Fig. 1,
we follow the simultaneous-tracking-and-mapping paradigm[14], which encompasses a tracking thread and a mapping
thread. The tracking thread tracks the current frame withrespect to a keyframe. The mapping thread initializes andrefines the depth map corresponding to the current keyframe;
binocular stereo and temporal stereo are used for depth
initialization and refinement respectively. The tracking thread
only processes images from the reference camera in thestereo pair for efficiency considerations while the mapping
thread processes images from both cameras. We describe
each step in the pipeline in detail.
A. Semi-Dense Image Alignment
The tracking thread of our pipeline tracks the current
frame against a keyframe by using semi-dense direct image alignment. In particular, we estimate the camera posecorresponding to the stereo-camera reference frame at time(8)

where ⌦(IrC1 ) is the set of high-gradient pixels in the
keyframe, and Tk,rC1 is the transformation from the keyframeto the current frame at time k. The camera pose corresponding to the current frame can be recovered from the known
camera pose corresponding to the keyframe.
The above cost function is minimized iteratively by using
the Gauss-Newton method. To achieve better efficiency, we
employ the inverse compositional algorithm from [1]. Here,
we do a one-time computation of the Jacobian and Hessian
matrices instead of re-computing them for each iteration.
Specifically, we minimize the following cost function foreach iteration,
X
E(⇠) =(IrC1 (⇡(T(⇠)⇡ 1 (ui , dui ))(9)i2⌦(IrC )1

1
IkC1 (⇡(Tk,r(ui , dui )))2 .
C1 ⇡

(10)

Using the chain rule, we derive the Jacobian matrix:
Ji =@(T(⇠) C1 pi )@⇡@ri (⇠)= rIrC1 ··.
@⇠@ Ci p i@⇠(11)

According to the Gauss-Newton method, we can computethe update ⇠:XX⇠= (JTi Ji ) 1
ri JTi .
(12)
i2⌦(IrC )1

i2⌦(IrC )1

The motion estimate between the current frame and the
keyframe is then updated:
k,r
1
Tk,r
.
C1 = TC1 T(⇠)

(13)

We use the Huber robust weighting scheme for the residual error of each pixel to suppress the effect of outliers.
To achieve better convergence performance, a coarse-tofine approach using multiple pyramid levels is used in the
optimization method. Furthermore, the initial camera posefor the above optimization procedure is estimated using a
constant velocity motion model as shown in Fig. 1.
B. Plane-Sweeping Stereo
For a fisheye-stereo frame, pixel correspondences lie on
epipolar curves instead of straight epipolar lines. Thus,
conventional epipolar line disparity search algorithms cannot
be used for fisheye stereo matching. To use fisheye images
directly without rectifying them to pinhole images, we use
the plane-sweeping stereo algorithm [12] for fisheye stereo
matching to initialize the depth map of the keyframe. Planesweeping stereo assumes that scenes are locally planar andtests a set of plane hypotheses. These plane hypotheses areused to warp pixels from a reference view to the current
view for similarity matching. The plane hypothesis which
gives the maximum similarity measure is recorded and used
to compute the ray depth for the corresponding pixel. The
algorithm is implemented based on [12], and we will discuss
the details as follows.
We define a set of plane hypotheses {n1 , d1 },. . . ,{nm , dm }, . . . ,{nM , dM } where nm and dm are the normal
and depth respectively of the mth plane in !
F C1 . For eachhigh-gradient pixel in the keyframe image IrC1 , we evaluateeach plane hypothesis by computing the corresponding homography HC2 C1 and using it to warp an image patch from
IrC1 to IrC2 such that the warped 7x7 image patch in IrC1 iscentered on the selected pixel point:
tC2 C 1 n m T
.
(14)
dm
For the plane hypotheses, we use all possible permutations
drawn from 64 depth values over the range [0.5 30] mwith a constant disparity step size, and fronto-parallel andground plane orientations. In total, 128 plane hypotheses are
generated.
We compute the similarity score based on zero-mean
normalized cross-correlation (ZNCC) between the image
patch in IrC1 centered on the selected high-gradient pixel
point, and warped image patch from IrC2 . For a given plane
hypothesis {ni , di }, the depth du of the selected pixel u in
IrC1 can be further computed as:
HC 2 C 1 = R C 2 C 1

di
.
(15)
fu · n i
Stereo matching usually has ambiguities due to repetitive
textures, occlusions, and so on. Experimentally, we find theestimated depth map using a winner-takes-all approach to berather noisy. To better capture the uncertainty characteristicsof the estimated depth, we propose to keep track of multiple
depth hypotheses for each pixel. Since the procedure to
get these depth hypotheses are the same for all pixels, we
illustrate the concept for one pixel. For each pixel, we
accumulate a 1D (⇢i , Si ) volume, where ⇢i is the inverse
depth and Si is its corresponding ZNCC score. We select
only the elements in the volume such that their ZNCC scores
Si are larger than a pre-defined threshold. We use a thresholdvalue of 0.85 in our experiments; a ZNCC score of 1.0
is considered perfect. All the selected elements from thisvolume are then clustered according to ⇢i . In particular,
we sort the elements according to ⇢i . We separate twoneighboring elements ⇢i and ⇢j into two different clusters
if their inverse ray depth difference |⇢i ⇢j | is larger than apre-defined separation distance. We use a cluster separation
distance of 0.1. We fit a Gaussian model to each cluster asP
i Si · ⇢i
µ= P
,
(16)
Si
P i
Si · (⇢i µ)2
2
= i P
.
(17)
i Si
du =

Furthermore, we choose the best score among all Si belonging to the current cluster as the representative score forthe current cluster. The cluster with the best score for eachpixel is used for direct image alignment. All clusters foreach pixel are tracked for temporal motion stereo so that
depth ambiguities can be eliminated and the ray depth canbe refined iteratively.
C. Temporal Motion Stereo
We use temporal motion stereo to refine the estimateddepth and to eliminate depth ambiguity in the case that there
is more than one inverse depth cluster for each pixel. Dueto the epipolar curve issue for fisheye images mentioned
earlier, we cannot use the conventional epipolar line disparity
search method for stereo matching. Instead, we propose atechnique to refine the mean depth estimate for each ray
segment corresponding to a cluster, which is bounded bya two-sigma distance from the mean inverse depth of thatcluster. Since this technique is applied in the same way toeach cluster and each pixel, we use one cluster to explain
the concept.
Each cluster or each depth hypothesis is parameterizedby five parameters: its mean inverse ray depth µ⇢ , thevariance of its inverse ray depth ⇢2 , its best matching score
Sbest , a good matching count nInliers, and a bad matching
count nOutliers. µ⇢ , ⇢2 and Sbest are initialized fromplane-sweeping stereo. Both nInliers and nOutliers are
initialized to be 2 in our experiment to avoid the zero division
problem when we compute either the inlier ratio or theoutlier ratio. Based on the initial µ⇢ and ⇢2 , we assume
that the correct inverse ray depth would lie in the interval[µ⇢ 2 ⇢ , µ⇢ + 2 ⇢ ], which has a probability of 95.45%
statistically. To get subpixel projection matching accuracy,
we subdivide this interval iteratively until the pixel distance
between two neighboring projected pixels falls below 1 pixel.
For all the sampled inverse ray depths within this interval, we
compute their ZNCC matching scores between the reference
image and the current image in the same way we compute thescores in plane-sweeping stereo. We use the fronto-paralleland ground plane orientations and current sampled inverseray depth to compute the homography matrices. Using these
homography matrices, we then compute the ZNCC scoresfor a 7⇥7 patch with the current refined pixel centered at
the patch. As in plane-sweeping stereo, each cluster willthen accumulate its own local volume. If this volume is not
empty, we fit a new Gaussian model for it and fuse it withthe original model:µprevµref ined =

2
prev

1

2
prev

2
ref ined =

1

2
prev

+ µcur
2
1

,

(18)

1

.

(19)

cur

+
1
+

2
cur

2
cur

If no good matching (i.e., Sbest < 0.85) is found, then
we increment nOutliers by 1. Otherwise, we increment
nOutliers
nInliers by 1. If the outlier ratio (i.e., nInliers+nOutliers)
is larger than a pre-defined threshold, we label the current
cluster or depth hypothesis as an outlier and stop tracking
it. If only one cluster remains after several motion stereo

refinement iterations, and its corresponding variance falls
below a certain threshold, we mark the current hypothesis as
the true depth estimate and further mark this pixel’s depth asconverged. If all hypotheses are marked as outliers, we label
this pixel as a bad pixel and consider its ray depth estimation
to have diverged. Only pixels not labeled as either convergedor diverged are refined in the next iteration.
VI. EXPERIMENTS AND RESULTS
We evaluate our direct fisheye-stereo visual odometry
pipeline using real world datasets. In addition, we compare
our direct fisheye-stereo visual odometry pipeline against thesemi-direct fisheye-stereo visual odometry pipeline of Heng
and Choi [13]. The data was collected on a ground vehicle
as shown in Fig. 4. Datasets from two different environments
are used to evaluate our pipeline: a vegetated environment
and an urban environment. Fig. 2 and Fig. 3 show sample
images captured on our ground vehicle in both environments.
We describe our testbed platform in Section VI-A andshow and discuss experimental results in Section VI-B. For
ground truth data, we use the post-processed pose estimatesfrom a GPS/INS system, which according to manufacturer
specifications, has a position error of 2 cm in the absence ofGPS outages.

Fig. 2: A view of the vegetatedenvironment.

Fig. 3: A view of the urban
environment.

A. Testbed Platform
We have two fisheye-stereo cameras with a 50 cm baselineon our vehicle platform. Fig. 4 shows the locations of thefisheye-stereo cameras marked by red ellipses. The left andright fisheye-stereo cameras look 45 to the left and rightrespectively. All cameras output 1280⇥960 color images at
30 frames per second, and are hardware-time-synchronizedwith the GPS/INS system.
We calibrate the multi-camera system using a grid ofAprilTag markers, and use hand-eye calibration to computethe transformation between the reference frames of the multicamera system and the INS. This transformation allows directcomparison of visual odometry pose estimates with the postprocessed GPS/INS pose estimates which are used as groundtruth.
B. Experiments
To achieve real-time performance, we use downsampled
640⇥480 images from the left fisheye-stereo camera as input
to our direct visual odometry pipeline. The vehicle wasdriven with a speed range of 10-15 km/h for both datasets.

Fig. 4: DSO’s Isuzu D-Max platform equipped with two fisheyestereo cameras which are shown enclosed in red ellipses.

The trajectory lengths are 289 m and 199 m for the vegetatedand urban datasets respectively. Fig. 5 and Fig. 8 show thepost-processed pose estimates from the GPS/INS system,
our visual odometry implementation, and a state-of-the-art
semi-direct visual odometry implementation from Heng andChoi [13] in red, green and blue respectively. A black circle
marks the starting point, while red, green and blue circles
mark the end of the trajectories. Fig. 6 and Fig. 9 plot theabsolute x y position errors against traveled distance forthe vegetated and urban environments respectively. Similarly,
Fig. 7 and Fig. 10 plot the absolute yaw errors for thevegetated and urban environments. We compute the position
error at a given time by computing the norm of the difference
between the x y components of the pose estimates fromvisual odometry implementations and the GPS/INS system.
We compute the yaw error at a given time by computing theabsolute difference between the estimated yaw from visual
odometry and the GPS/INS system at that time.
VO-vegetated-ours
VO-vegetated-Heng
VO-urban-ours
VO-urban-Heng

xy-drift
0.86%
0.96%
0.6%
1.6%

yaw-drift
0.013 deg/m
0.0138 deg/m
0.0063 deg/m
0.011 deg/m

TABLE I: Pose accuracy comparison between our direct visual
odometry implementation and [13]We compare the accuracy of our direct visual odometry implementation against that of [13] using position andorientation drift metrics. We compute the x y and yaw
errors averaged over all possible subsequences of length{100, 200} meters. Table I shows that our direct visual
odometry implementation outperforms Heng and Choi [13]for both the vegetated and urban environments. In addition,
we are able to generate a semi-dense point cloud that canbe used for dense mapping. Furthermore, Fig. 11 shows thereconstructed and colored point cloud generated by our visual
odometry pipeline. The point cloud only includes points
which are labeled as converged by our motion stereo. Tobetter show the reconstruction quality of our pipeline, wechoose different viewpoints for purposes of comparison. A
supplementary video showing the experimental results canbe found at https://youtu.be/NOiIVO0jzuc.
Our pipeline is implemented and evaluated on a PC withan Intel i7 CPU @ 2.9 GHz and a low-end Nvidia GTX
480 GPU. We run our tracking thread at 20 Hz on one CPU
core. Both plane sweeping stereo and motion stereo run on
the GPU at around 5 Hz.
One possible reason that our algorithm performs better
than that of [13] could be the use of more informationfrom observed images. Our algorithm uses most of thehigh gradient pixels (>20k) while [13] only samples several
hundred sparse feature points. The use of more information
increases the robustness of the whole algorithm, especiallyin poorly textured environments where [13] does not perform
well as shown in Fig. 10.
VII. CONCLUSIONSWe present a direct visual odometry algorithm for a fisheye
stereo camera in this paper. Our algorithm uses fisheye
images directly instead of converting them to conventional
pinhole images, and in turn, reducing the field of view. Planesweeping stereo is used to do stereo matching for fisheye
images which cannot be processed by traditional epipolarline disparity search algorithms. The estimated depths arefurther refined via temporal stereo. By using the refined semidense depth map, a direct image alignment tracker is used toestimate the current camera pose in real-time. Experimental
results show that our pipeline not only achieves significantly
accurate motion estimates but also outputs a high-quality
point cloud at the same time. In other words, our pipeline
can facilitate real-time localization and dense mapping formobile robots.
R EFERENCES[1] S. Baker and I. Matthews. Lucas-kanade 20 years on: A
unifying framework. International Journal of Computer
Vision, 56(3):221–255, 2004.
[2] J. Baretto and H. Araujo. Issues on the geometry of central catadioptic image formation. In IEEE Conference
on Computer Vision and Pattern Recognition (CVPR),
2001.
[3] D. Caruso, J. Engel, and D. Cremers. Large-scale direct
slam for omnidirectional cameras. In IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2015.
[4] B. Clipp, J. Lim, J.-M. Frahm, and M. Pollefeys.
Parallel, Real-Time Visual SLAM. In IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2010.
[5] J. Engel, J. Sturm, and D. Cremers. Semi-dense
visual odometry for a monocular camera. In IEEE
International Conference on Computer Vision (ICCV),
2013.
[6] J. Engel, J. Stückler, and D. Cremers. Large-scale direct
slam with stereo cameras. In IEEE/RSJ International

Conference on Intelligent Robots and Systems (IROS),
2015.
[7] J. Engel, V. Koltun, and D. Cremers. Direct sparse
odometry. IEEE Transactions on Pattern Analysis andMachine Intelligence (PAMI), 2017.
[8] C. Forster, M. Pizzoli, and D. Scaramuzza. SVO:
Fast semi-direct monocular visual odometry. In IEEE
International Conference on Robotics and Automation(ICRA), 2014.
[9] C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and
D. Scaramuzza. SVO: Semi-direct visual odometry formonocular and multi-camera systems. IEEE Transactions on Robotics (T-RO), PP(99):1–17, 2016.
[10] F. Fraundorfer and D. Scaramuzza. Visual odometry:
Part ii - matching, robustness, and applications. IEEE
Robotics And Automation Magazine (RAM), 19(2):78–
90, 2012.
[11] C. Geyer and K. Daniilidis. A unifying theory forcentral panaromic systems and practical implications.
In European Conference on Computer Vision (ECCV),
2000.
[12] C. Häne, L. Heng, G. H. Lee, A. Sizov, and M. Pollefeys. Real-time direct dense matching on fisheye
images using plane-sweeping stereo. In International
Conference on 3D Vision (3DV), 2015.
[13] L. Heng and B. Choi. Semi-direct visual odometry
for a fisheye-stereo camera. In IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS),
2016.
[14] G. Klein and D. Murray. Parallel tracking and mapping
for small ar workspaces. In International Symposiumon Mixed and Augmented Reality, 2007.
[15] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, andP. T. Furgale. Keyframe-based visualinertial odometry
using nonlinear optimization. International Journal of
Robotics Research, 2015.
[16] J. Lim, J.-M. Frahm, and M. Pollefeys. Online Environment Mapping. In IEEE Conference on Computer
Vision and Pattern Recognition (CVPR), 2011.
[17] A. I. Mourikis and S. I. Roumeliotis. A multi-state
constraint kalman filter for vision-aided inertial navigation. In IEEE International Conference on Robotics
and Automation (ICRA), 2007.
[18] S. Omari, M. Blösch, P. Gohl, and R. Siegwart. Dense
visual-inertial navigation system for mobile robots. In
IEEE International Conference on Robotics and Automation (ICRA), 2015.
[19] D. Scaramuzza and F. Fraundorfer. Visual odometry:
Part i - the first 30 years and fundamentals. IEEE
Robotics And Automation Magazine (RAM), 18(4):80–
92, 2011.
[20] P. Tanskanen, T. Ngeli, M. Pollefeys, and O. Hilliges.
Semi-Direct EKF-based Monocular Visual-Inertial
Odometry. In IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), 2015.

2.5

30

2

20

10

4

1.5

1

0

-10

0

10
x (meters)

20

30

40

Fig. 5: Red, green, and blue lines represent
the trajectories estimated by the GPS/INS
system, our implementation, and [13] respectively in the vegetated environment.
20

0

50

100

150
Distance (meters)

200

250

2

3.5

0

300

Fig. 6: Red and green lines represent thex y position drift of the pose estimates
output by our implementation and [13] inthe vegetated environment.

Ground Truth
Odometry-ours
Odometry-Heng

3

1

0.5

0

Yaw-drift-ours
Yaw-drift-Heng

xy-drift-ours
xy-drift-Heng

Attitude Drift (degrees)

40

-10
-20

5

3

Ground Truth
Odometry-ours
Odometry-Heng

Position Drift (meters)y (meters)

50

0

50

100

150
Distance (meters)

200

250

300

Fig. 7: Red and green lines represent theyaw drift of the pose estimates output by
our implementation and [13] in the vegetatedenvironment.
2.5

xy-drift-ours
xy-drift-Heng

Yaw-drift-ours
Yaw-drift-Heng

3

0

2

-20

-40

Attitude Drift (degrees)

Position Drift (meters)y (meters)

2.5

2

1.5

1.5

1

1
0.5

-60
0.5

-80
-60

0

-40

-20

0
x (meters)

20

40

60

Fig. 8: Red, green, and blue lines represent
the trajectories estimated by the GPS/INS
system, our implementation, and [13] respectively in the urban environment.

0

50

100
Distance (meters)

150

200

Fig. 9: Red and green lines represent the xy position drift of the pose estimates output
by our implementation and [13] in the urban
environment.

0

0

50

100
Distance (meters)

150

200

Fig. 10: Red and green lines represent theyaw drift of the pose estimates output by
our implementation and [13] in the urban
environment.

Fig. 11: Each subfigure shows a reconstructed point cloud from our VO pipeline and the corresponding image as well as estimatedsemi-dense depth maps.