This paper presents a computationally efficient sensor-fusion algorithm for visual inertial odometry (VIO). The paper utilizes trifocal tensor geometry (TTG) for visual measurement model and a nonlinear deterministic-sampling-based filter known as cubature Kalman filter (CKF) to handle the system nonlinearity. The TTG-based approach is developed to replace the computationally expensive three-dimensional-feature-point reconstruction in the conventional VIO system. This replacement has simplified the system architecture and reduced the processing time significantly. The CKF is formulated for the VIO problem, which helps to achieve a better estimation accuracy and robust performance than the conventional extended Kalman filter (EKF). This paper also addresses the computationally efficient issue associated with Kalman filtering structure using cubature information filter (CIF), the CKF version on information domain. The CIF execution avoids the inverse computation of the high-dimensional innovation covariance matrix, which in turn further improves the computational efficiency of the VIO system. Several experiments use the publicly available datasets for validation and comparing against many other VIO algorithms available in the recent literature. Overall, this proposed algorithm can be implemented as a fast VIO solution for high-speed autonomous robotic systems.

References

References
1.
Wong
,
X. I.
, and
Majji
,
M.
,
2017
, “
Extended Kalman Filter for Stereo Vision-Based Localization and Mapping Applications
,”
ASME J. Dyn. Syst. Meas. Control
,
140
(
3
), p.
030908
.
2.
Rogne
,
R. H.
,
Bryne
,
T. H.
,
Fossen
,
T. I.
, and
Johansen
,
T. A.
,
2018
, “
Redundant MEMS-Based Inertial Navigation Using Nonlinear Observers
,”
ASME J. Dyn. Syst. Meas. Control
,
140
(
7
), p.
071001
.
3.
Santoso
,
F.
,
Garratt
,
M. A.
, and
Anavatti
,
S. G.
,
2017
, “
Visual-Inertial Navigation Systems for Aerial Robotics: Sensor Fusion and Technology
,”
IEEE Trans. Autom. Sci. Eng.
,
14
(
1
), pp.
260
275
.
4.
Kottas
,
D. G.
, and
Roumeliotis
,
S. I.
,
2015
, “
An Iterative Kalman Smoother for Robust 3D Localization on Mobile and Wearable Devices
,”
IEEE
International Conference on Robotics and Automation
,
Seattle, WA
,
May 26–30
, pp.
6336
6343
.
5.
Hartley
,
R.
, and
Zisserman
,
A.
,
2004
,
Multiple View Geometry in Computer Vision
,
Cambridge University Press
,
New York
.
6.
Delmerico
,
J.
, and
Scaramuzza
,
D.
,
2018
, “
A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots
,”
IEEE
International Conference on Robotics and Automation
,
Brisbane, Australia
,
May 21–25
, pp.
2502
2509
.
7.
Mourikis
,
A. I.
, and
Roumeliotis
,
S. I.
,
2007
, “
A Multi-State Constraint Kalman Filter for Vision-Aided Inertial Navigation
,”
IEEE
International Conference on Robotics and Automation
,
Roma, Italy
,
Apr. 10–14
, pp.
3565
3572
.
8.
Li
,
M.
, and
Mourikis
,
A. I.
,
2013
, “
High-Precision, Consistent EKF-Based Visual-Inertial Odometry
,”
Int. J. Rob. Res.
,
32
(
6
), pp.
690
711
.
9.
Qin
,
T.
,
Li
,
P.
, and
Shen
,
S.
,
2018
, “
Vins-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
,”
IEEE Trans. Rob.
,
34
(
4
), pp.
1004
1020
.
10.
Leutenegger
,
S.
,
Lynen
,
S.
,
Bosse
,
M.
, and
Furgale
,
P.
,
2015
, “
Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization
,”
Int. J. Rob. Res.
,
34
(
3
), pp.
314
334
.http://www.roboticsproceedings.org/rss09/p37.pdf
11.
Pakki
,
K.
,
Chandra
,
B.
,
Gu
,
D.-W.
, and
Postlethwaite
,
I.
,
2013
, “
Square Root Cubature Information Filter
,”
IEEE Sens. J.
,
13
(
2
), pp.
750
758
.
12.
Forster
,
C.
,
Carlone
,
L.
,
Dellaert
,
F.
, and
Scaramuzza
,
D.
,
2017
, “
On-Manifold Preintegration for Real-Time Visual—Inertial Odometry
,”
IEEE Trans. Rob.
,
33
(
1
), pp.
1
21
.
13.
Arasaratnam
,
I.
, and
Haykin
,
S.
,
2009
, “
Cubature Kalman Filters
,”
IEEE Trans. Autom. Control
,
54
(
6
), pp.
1254
1269
.
14.
De Silva
,
O.
,
Mann
,
G. K. I.
, and
Gosine
,
R. G.
,
2016
, “
The Right Invariant Nonlinear Complementary Filter for Low Cost Attitude and Heading Estimation of Platforms
,”
ASME J. Dyn. Syst. Meas. Control
,
140
(
1
), p.
011011
.
15.
Yu
,
X.
,
Baker
,
T.
,
Zhao
,
Y.
, and
Tomizuka
,
M.
,
2017
, “
Fast and Precise Glass Handling Using Visual Servo With Unscented Kalman Filter Dual Estimation
,”
ASME J. Dyn. Syst. Meas. Control
,
140
(
4
), p.
041008
.
16.
Wan
,
E. A.
, and
Merwe
,
R. V. D.
,
2000
, “
The Unscented Kalman Filter for Nonlinear Estimation
,”
IEEE
Adaptive Systems for Signal Processing, Communications, and Control Symposium
,
Lake Louise, AB, Canada
,
Oct. 4
, pp.
153
158
.
17.
Loianno
,
G.
,
Watterson
,
M.
, and
Kumar
,
V.
,
2016
, “
Visual Inertial Odometry for Quadrotors on SE(3)
,”
IEEE
International Conference on Robotics and Automation
,
Stockholm, Sweden
,
May 16–21
, pp.
1544
1551
.
18.
Hu
,
J.-S.
, and
Chen
,
M.-Y.
,
2014
, “
A Sliding-Window Visual-IMU Odometer Based on Tri-Focal Tensor Geometry
,”
IEEE
International Conference on Robotics and Automation
,
Hong Kong, China
,
May 31–June 7
, pp.
3963
3968
.
19.
Mutambara
,
A. G.
,
1998
,
Decentralized Estimation and Control for Multisensor Systems
,
CRC Press
,
Boca Raton, FL
.
20.
Wang
,
S.
,
Feng
,
J.
, and
Tse
,
C. K.
,
2014
, “
A Class of Stable Square-Root Nonlinear Information Filters
,”
IEEE Trans. Autom. Control
,
59
(
7
), pp.
1893
1898
.
21.
Hesch
,
J. A.
,
Kottas
,
D. G.
,
Bowman
,
S. L.
, and
Roumeliotis
,
S. I.
,
2014
, “
Camera IMU Based Localization: Observability Analysis and Consistency Improvement
,”
Int. J. Rob. Res.
,
33
(
1
), pp.
182
201
.
22.
Peretroukhin
,
V.
,
Vega-Brown
,
W.
,
Roy
,
N.
, and
Kelly
,
J.
,
2016
, “
PROBE-GK: Predictive Robust Estimation Using Generalized Kernels
,”
IEEE
International Conference on Robotics and Automation
,
Stockholm, Sweden
,
May 16–21
, pp.
817
824
.
23.
Heo
,
S.
,
Cha
,
J.
, and
Park
,
C. G.
,
2018
, “
EKF-Based Visual Inertial Navigation Using Sliding Window Nonlinear Optimization
,”
IEEE Trans. Intell. Transp. Syst.
(epub).
24.
Andreff
,
N.
, and
Tamadazte
,
B.
,
2015
, “
Laser Steering Using Virtual Trifocal Visual Servoing
,”
Int. J. Rob. Res.
,
35
(
6
), pp.
672
694
.
25.
Rameau
,
F.
,
Ha
,
H.
,
Joo
,
K.
,
Choi
,
J.
, and
Kweon
,
I.
,
2016
, “
A Real-Time Vehicular Vision System to Seamlessly See-Through Cars
,”
European Conference on Computer Vision
,
Amsterdam, The Netherlands
,
Oct. 8–10 and 15–16
, pp.
209
222
.https://link.springer.com/chapter/10.1007/978-3-319-48881-3_15
26.
Min
,
H.-G.
,
Li
,
X.-C.
,
Sun
,
P.-P.
,
Zhao
,
X.-M.
, and
Xu
,
Z.-G.
,
2015
, “
Visual Odometry for on-Road Vehicles Based on Trifocal Tensor
,”
IEEE
First International Smart Cities Conference
,
Guadalajara, Mexico
,
Oct. 25–28
, pp.
1
5
.
27.
Chen
,
Y.
,
Yang
,
G. L.
,
Jiang
,
Y. X.
, and
Liu
,
X. Y.
,
2018
, “
Monocular Visual Odometry Based on Trifocal Tensor Constraint Monocular
,”
J. Phys.: Conf. Ser.
,
976
(
1
), pp.
1
6
.https://iopscience.iop.org/article/10.1088/1742-6596/976/1/012002
28.
Trawny
,
N.
, and
Roumeliotis
,
S. I.
,
2005
, “
Indirect Kalman Filter for 3D Attitude Estimation: A Tutorial for Quaternion Algebra
,” Department of Computing Science and Engineering, University of Minnesota, Minneapolis, MN, Report No. 2005-002.
29.
Weiss
,
S.
, and
Siegwart
,
R.
,
2011
, “
Real-Time Metric State Estimation for Modular Vision-Inertial Systems
,”
IEEE
International Conference on Robotics and Automation
,
Shanghai, China
,
May 9–13
, pp.
4531
4537
.
30.
Arasaratnam
,
I.
,
Haykin
,
S.
, and
Hurd
,
T. R.
,
2010
, “
Cubature Kalman Filtering for Continuous-Discrete Systems: Theory and Simulations
,”
IEEE Trans. Signal Process.
,
58
(
10
), pp.
4977
4993
.
31.
Bay
,
H.
,
Ess
,
A.
,
Tuytelaars
,
T.
, and
Van Gool
,
L.
, “
Speeded-Up Robust Features (SURF)
,”
Comput. Vision Image Understanding
,
110
(
3
), pp.
346
359
.
32.
Lowe
,
D. G.
,
2004
, “
Distinctive Image Features From Scale-Invariant Key points
,”
Int. J. Comput. Vision
,
60
(
2
), pp.
91
110
.
33.
Civera
,
J.
,
Grasa
,
O. G.
,
Davison
,
A. J.
, and
Montiel
,
J. M. M.
,
2010
, “
1-Point RANSAC for Extended Kalman Filtering: Application to Real-Time Structure From Motion and Visual Odometry
,”
J. Field Rob.
,
27
(
5
), pp.
609
631
.
34.
Geiger
,
A.
,
Lenz
,
P.
,
Stiller
,
C.
, and
Urtasun
,
R.
,
2013
, “
Vision Meets Robotics: The Kitti Dataset
,”
Int. J. Rob. Res.
,
32
(
11
), pp.
1231
1237
.
35.
Arasaratnam
,
I.
, “
Matlab Code Examples of Cubature Kalman Filter
,”
The MathWorks
, Natick, MA, accessed Mar. 5,
2019
, https://haranarasaratnam.com/software.html
36.
Mur-Artal
,
R.
, and
Tardos
,
J. D.
,
2017
, “
ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras
,”
IEEE Trans. Rob.
,
33
(
5
), pp.
1255
1262
.
37.
Burri
,
M.
,
Nikolic
,
J.
,
Gohl
,
P.
,
Schneider
,
T.
,
Rehder
,
J.
,
Omari
,
S.
,
Achtelik
,
M. W.
, and
Siegwart
,
R.
,
2016
, “
The Euroc Micro Aerial Vehicle Datasets
,”
Int. J. Rob. Res.
,
35
(
10
), pp.
1157
1163
.
38.
Bloesch
,
M.
,
Burri
,
M.
,
Omari
,
S.
,
Hutter
,
M.
, and
Siegwart
,
R.
,
2017
, “
Iterated Extended Kalman Filter Based Visual-Inertial Odometry Using Direct Photometric Feedback
,”
Int. J. Rob. Res.
,
36
(
10
), pp.
1053
1072
.
39.
Maybeck
,
P. S.
,
1979
, “
Stochastic Models, Estimation and Control
,”
Academic Press
,
New York
.
40.
Agarwal
,
S.
, and
Mierle
,
K.
, 2018, “
Ceres Solver
,” accessed Mar. 5, 2019, http://ceres-solver.org
41.
Lee
,
D.-J.
,
2008
, “
Nonlinear Estimation and Multiple Sensor Fusion Using Unscented Information Filtering
,”
IEEE Signal Process. Lett.
,
15
(
1
), pp.
861
864
.
You do not currently have access to this content.