Visual–inertial state estimation with camera and camera–IMU calibration

Robotics and Autonomous Systems 120 (2019) 103249

Contents lists available at ScienceDirect

Robotics and Autonomous Systems journal homepage: www.elsevier.com/locate/robot

Visual–inertial state estimation with camera and camera–IMU calibration Mohammadvali Arbabmir, Masoud Ebrahimi

∗

Department of Mechanical Engineering, Tarbiat Modares University, P.O.B. 14115-111, Tehran, Iran

article

info

Article history: Received 21 January 2019 Received in revised form 16 May 2019 Accepted 24 July 2019 Available online 1 August 2019 Keywords: Visual–inertial odometry Sensor fusion Sensors calibration State estimation Optimization

a b s t r a c t In the last two decades, the Visual–Inertial Odometry (VIO) has recently received much attention for efficient and accurate ego-motion estimation of unmanned vehicle systems. In particular, the VIO includes only an Inertial Measurement Unit (IMU) and a camera. In this paper, we present a novel calibration approach for accurate deployment of monocular VIO. For this purpose, the hybrid optimization algorithm is used for calibrating the camera intrinsic and camera–IMU extrinsic calibration, automatically and without knowing the mechanical configuration. It is a professional work to carefully calibrate the intrinsic and extrinsic parameters, and it is required to repeat this work when the mechanical configuration of the sensor suite changes. Quantitative comparisons our method with the offline conventional calibration method on the KITTI dataset verify the efficacy and accuracy of the proposed method. We also demonstrate the performance of the proposed approach in large scale outdoor experiments. © 2019 Elsevier B.V. All rights reserved.

1. Introduction In the robotics field, the ego-motion estimation is essential. For this purpose, various sensors such as the Global Positioning System (GPS), Inertial Measurement Units (IMU), and cameras have been used. Visual–Inertial Odometry (VIO) is currently used for autonomous driving, robotic navigation, virtual reality (VR), and augmented reality (AR) using the measurements. The monocular VIO, that includes a monocular camera and an Inertial Measurement Unit (IMU) sensor, become a very attractive choice due to its size, weight, and power characteristics. In fact, the VIO is the minimum sensor suite that allows accurate state. The VIO initial was approached by loosely couple fusion of sub sensors [1,2]. In VIO, the Extended Kalman Filter (EKF) is also used for update the current state by solving a linearized optimization problem for all state variables in a tightly-coupled fusion [3– 5]. Also, the Unscented Kalman Filter (UKF) is used to estimate all state variables in the VIO [6–8]. The filter base approaches can estimate the current states for real-time applications. On the other hand, the optimization base algorithms [9–12] have been developed for high accuracy application. These algorithms require higher computational cost and when the observations are poor, they will divergence. The filter base approaches are less accurate than the optimization base approaches because of the approximation in the update step.

In this research, we utilized an efficient approach to compute the camera intrinsic and camera–IMU extrinsic variables. For this purpose, the hybrid optimization algorithm, combination of Particle Swarm Optimization (PSO) and Genetic Algorithm (GA), was used. In the PSO–GA algorithm, the crossover and mutation of GA are adopted to avoid PSO into the local optimum. In this approach, by considering the pose RMSE of the VIO the optimization was considered to estimate the variables. This calibration approach is an automatic, simple execution, iterative, auto-initialized, and no need the mechanical configuration in the process. Furthermore, unlike the conventional methods, no extra equipment such as chessboard or optical instruments and calibration scenarios is required and the user does not take additional time for the calibration process. In addition, like the conventional methods, in this approach, we do not need to recalibrate the variables until the mechanical configuration of the sensor suite changes. The experiments based on monocular sliding VIO with the KITTI benchmark dataset [13] confirm that this approach can estimate the reliable camera intrinsic and camera–IMU extrinsic variables with minimum pose RMSE in the VIO compared to the dataset. The main contributions of our approach are summarized as follows:

• An automatic, simple execution, iterative, and autoinitialized approach.

∗ Corresponding author. E-mail address: [email protected] (M. Ebrahimi). https://doi.org/10.1016/j.robot.2019.103249 0921-8890/© 2019 Elsevier B.V. All rights reserved.

• Without knowing the mechanical configuration the intrinsic and extrinsic variables estimated.

2

M. Arbabmir and M. Ebrahimi / Robotics and Autonomous Systems 120 (2019) 103249

• Principal convergence criteria to identify and termination for the calibration procedure.

• Achieves appropriate accuracy on the well-known KITTI benchmark dataset. This paper is organized as follows. The related work presents in Section 2. In Section 3 we introduce a tightly-coupled VIO for continuously estimates the motion state by visual information and IMU measurement. The calibration principle and our calibration approach present in Section 4. The experimental results based on well-known benchmark dataset are presented in Section 5. Finally, the concluding remarks are suggested in Section 6.

Fig. 1. IMU, camera and global frames and the relationship between them.

2. Related work The VIOs focus on highly accurate pose estimation of a vehicle by fusing camera and IMU information. Recent VIO algorithms can be classified into the filtering-based approach and the optimization-based approach. The evaluation results of the filter base and optimization base VIO systems were presented by Delmerico and Scaramuzza [14]. The filter base systems are faster than the optimization base methods since they use linearized motion and observation models. On the other hand, the optimization base methods are more accurate than the filter base systems. In 2017, Rehder and Siegwart [15] proposed a revisited camera–IMU calibration with the direct model for the camera measurements. The camera calibration method based on the GA was presented in [16,17]. In 2011, He et al. [18] presented an approach for the automatic estimation of the intrinsic variables from images by using the vanishing points in orthogonal directions. The camera calibration method based on PSO algorithm was presented in [19, 20]. Li et al. [21] proposed the combination of GA with PSO for camera self-calibration. The combination of differential evolution and PSO algorithm is used to calibrate the external and internal variables and calculate the distortion coefficients of the camera effectively by Deng et al. [22]. The experimental results show that the proposed algorithm has a good optimization ability to avoid the local optimum and can complete the visual identification tasks accurately. In 2010, Hol et al. [23] presented a new calibration method to determine the relative position and orientation of the IMU to a camera coordinate system based on a physical model of the sensor unit. Kelly and Sukhatme [24] proposed an algorithm based on the UKF for self-calibration of the transformation between a camera and an IMU. The result of a monocular camera and low-cost IMU demonstrates the accurate estimation of the calibration variables. Tang et al. [18] proposed a fully automatic calibration of the camera–odometry system with two-step calibration algorithm, consisting a non-iterative auto-initialization step for spatial extrinsic calibration and a joint optimization step to estimate both the spatiotemporal extrinsic variables and the odometric parameters. Tang and Liu [25] proposed a two-step fully automatic calibration algorithm for a mobile robot system which equipped with odometric devices and a monocular camera. This calibration algorithm initially estimates both the odometric and the extrinsic variables, through a non-iterative auto-initialization process, and then improves the calibration result by iterative optimization. The calibration of the camera intrinsic and camera–IMU extrinsic variables is critical in the VIO system because the vehicle pose is estimated from it. Nevertheless, we discuss in Section 4 how to calculate the intrinsic and extrinsic variables of VIO system using the PSO–GA optimization algorithm.

Fig. 2. The overview of proposed VIO. We used the synchronized IMU and camera dataset.

3. Visual-inertial odometry The goal of the VIO is to estimate the current state of the vehicle using the camera and IMU measurement at every time. For this purpose, the reference frames of our VIO are shown in Fig. 1. ) ( According to the above figure, the transformation RIC , pIC is between the camera frame with respect to the IMU frame. Additionally, ( the) transformation of the IMU frame into the global frame is RGI , pGI . The proposed VIO algorithm presented in Fig. 2. In this algorithm, after capturing the images by the camera, the Kanade– Lucas–Tomasi (KLT) feature tracker [26] to find the feature points. At this rate, the state and covariance of VIO are predicted at each step by the initial values and IMU outputs in the Filter Prediction. For this purpose, the nominal state is predicted by the Runge Kutta 4th order (RK4) method, and the error state and its covariance are obtained by Multi-State Constraint Kalman Filter (MSCKF) procedure. The RANSAC algorithm [27] rejects the outlier matched points which are located on independently moving objects or mismatch. This algorithm works with the KLT and Filter Prediction outputs based on 1-point solver [28,29]. The output of RANSAC is the inlier and outlier matched points between three sequential images. To move one step further, the measurement model of observation is obtained. To this end, to avoid estimating the global coordinate of the feature points and updating the filter states, the camera geometry constraints including the epipolar geometry between two images and the trifocal tensor between three images are used [30]. Then, the state and covariance are updated by Filter Prediction outputs and visual observation in Filter Update. Finally, in order to keep only three poses in the filter state vector, the old state is replaced by the Filter Update outputs. Consequently, this VIO is a sliding window odometry with a balance between computational cost and accuracy. The sliding window sample is illustrated in Fig. 3. In this VIO the INS measurements are obtained by Eq. (1):

( )(

am = RT qGI

)

aG + g G + b a + na ωm = ωI + bg + ng

(1)

M. Arbabmir and M. Ebrahimi / Robotics and Autonomous Systems 120 (2019) 103249

3

Fig. 3. Sliding window with four IMU states xi and three features fk . This sliding window model applies to VIO (Section 3).

( )

where am is the measured acceleration, RT qGI is the transposed of the Direction Cosine Matrix (DCM) calculated from the quaternion, q is the quaternion vector, T represents the transpose, aG and g G are the linear acceleration of the IMU with respect to the global frame and the gravitational acceleration in the global frame, respectively. ωm is the angular velocity in the IMU frame, ωI denotes the real angular velocity in the IMU frame. The na and ng are the white Gaussian noises of the accelerometer and gyroscope, respectively. Moreover, the ba and bg are respectively the biases of the accelerometer and gyroscope. On the other hand, the visual measurements are obtained by Eq. (2). By assuming the jth feature point in three sequential images correspondences as (m1 , m2 , m3 ), the observation model is obtained as Eq. (2):

⎡

[

mT2 K −T RT1,2 t1,2

] ]×

K − 1 m1

⎤

⎥ ⎢ T −T T [ ⎢m3 K R2,3 t2,3 K −1 m2 ⎥ × ⎞ ⎥ ⎢ ⎛ Yj = ⎢ ⎥ ∑ ⎥ ⎢ −1 T⎠ ⎣ K⎝ l2 ⎦ K m1,j Tj

(2)

j

where the first two components are calculated by epipolar constraint and the third component contains two parts calculated by ( ) trifocal constraint. The pair Rk,k+1 , tk,k+1 is the transformation from image k to k + 1 and is calculated as Eq. (3). K is the camera calibration matrix, T is jth trifocal tensor column and l2 is the line perpendicular to the epipolar line.

(

Rk,k+1 = RGC,k tk,k+1 =

(

RGC,k

)T (

)T

RGC,k+1

pGC,k+1

−

pGC,k

)k = 1, 2

(3)

In the above equations, the RGC and pGC are the transformation of the camera frame to the global frame. In order to modeling and description of the VIO, the state vector is introduced as the following:

Fig. 4. The pipeline of PSO–GA algorithm for calibrating the sensors variables of VIO.

The VIO state vector is calculated by solving the state-space equations of a dynamic system: p˙ GI = vIG , q˙ GI =

1 G q 2 I

[ ⊗ 0

( I ) T ]T ω

v˙ IG = aG , b˙ a = nba , b˙ g = nbg

xVIOi

[

= pGI i

qGI i

vIG i

bai

bgi

pGVIO

i−2

qGVIO

i−2

pGVIO

i−1

qGVIO

]T

pGVIO

˙

i −1

(4) where xVIOi is the current state vector of VIO, pGI i is the current position vector of the INS in the global frame, qGI i is the current quaternion vector G of the INS respect to the global frame, ( and vI i is the ) velocity ( vector of the ) INS in the global frame. The pairs

pGVIO

i−2

, qGVIOi−2

and

pGVIO

i−1

, qGVIOi−1

are the poses state of the VIO related to the first and second prior poses of the camera, respectively.

= 0, ˙

qGVIO

(5)

=0

where, p˙ GI is derivate of the position vector of the INS in the global frame, q˙ GI is derivate of the quaternion vector of the INS respect to the global frame, v˙ IG is derivate of the velocity vector of the INS in the global frame, b˙ a is derivate of the accelerometer bias vector, b˙ g is derivate of the gyroscope bias vector, nba is Gaussian vector noise of the accelerometer bias, and nbg is Gaussian vector noise of the gyroscope bias. p˙ GVIO and q˙ GVIO are the derivation of past poses and since past poses in the filter prediction have no dynamic, assume its process model is zero.

4

M. Arbabmir and M. Ebrahimi / Robotics and Autonomous Systems 120 (2019) 103249

Fig. 5. Detailed illustration of the camera intrinsic variables on trajectory shown in Fig. 8. The satisfied convergence criteria is separated by dashed line.

Fig. 7. Detailed illustration of the camera–IMU translation variables on trajectory shown in Fig. 8. The satisfied convergence criteria is separated by dashed line.

Fig. 8. Trajectory for the trial shown in Figs. 5, 6, and 7.

4. Calibration principle and approach Fig. 6. Detailed illustration of the camera–IMU rotation variables on trajectory shown in Fig. 8. The satisfied convergence criteria is separated by dashed line.

4.1. Camera intrinsic calibration For calibrating the camera, the pinhole model [30] is used basically. The pinhole model is presented as follow:

In order to achieve linearization purpose and minimizing the dimension of the filter state vector, the INS state vector is divided into nominal and error so the above state vector xINS is calculated by the following equations:

[

pGI = pˆ GI + ˜ pGI , qGI = qˆ GI ⊗ 0

(

0.5˜ θIG

) T ]T

vIG = vˆ IG + ˜ vIG , ba = bˆ a + ˜ ba , bg = bˆ g + ˜ bg [ ( ) T ]T G G G G G G pVIO = pˆ VIO + ˜ pVIO , qVIO = qˆ VIO ⊗ 0 0.5˜ θVIO

(ˆ)

(˜)

(6)

In the above equations, · and · are the nominal and error state, respectively. In order to do further investigation, the trajectory of the three navigation systems are compared in five sharp edges of the trajectory shown in Fig. 9.

(7)

x = K [R|t] X

where x is the mapped point of X in the image plane and X is a point in the global frame. K is the calibration matrix and contains the intrinsic variables of the camera and the pair (R, t ) is the extrinsic rotation matrix and the translation vector. The matrix K is defined as Eq. (8):

⎡

f sx K =⎣ 0 0

S f sy 0

⎤

Ox Oy ⎦ 1

(8)

In the above matrix, sx and sy are the number of pixels per unit distance in the image coordinates and in the x and y directions, f is the focal length, f sx and f sy represent the focal length of the camera in pixel dimensions and in the x and y directions, Ox and Oy are the pixel

M. Arbabmir and M. Ebrahimi / Robotics and Autonomous Systems 120 (2019) 103249

Fig. 9. The trajectory estimation with (1) AVIO, (2) VIO with Geiger et al. [13] calibration variables and (3) GPS/INS as Real Data. The total trajectory length is about 540 m and 78 s.

coordinates of image center and finally S is the skew variable which is zero for most normal cameras. The rotation matrix R is the product of the IMU to camera rotation matrix RCI and the Global to IMU rotation matrix RIG . RCI is obtained from ( ) Eq. (9) and RIG is equal to RT qGI in Eq. (4). Finally, the translation vector I t is the summation of TC and TIG . TCI is defined in Eq. (10) and TIG is pGI in Eq. (4).

4.2. Camera–IMU extrinsic calibration In order to camera–IMU registration, we should calculate the rotation and translation between two coordinate frames. For this purpose, according to [31] the rotation matrix of the camera to IMU coordinate is as follow: RCI cos ψ cos θ ⎢ = ⎣ sin ψ cos θ − sin θ

⎡

cos ψ sin θ sin ϕ − sin ψ cos ϕ

sin ψ sin ϕ + cos ψ sin θ cos ϕ

cos ψ cos ϕ + sin ψ sin θ sin ϕ

sin ψ sin θ cos ϕ − cos ψ sin ϕ ⎦

cos θ sin ϕ

⎤ ⎥

cos θ cos ϕ

global optimum solution with a high probability most of the time. This algorithm is based on the concepts of natural genetics and selection. The reproduction, crossover, and mutation are the basic elements of genetics and are used in GA. On the other hand, the PSO algorithm, that was proposed in [33], is a population-based search method. The indicated algorithm is based on the demeanor of a colony or swarm of ants, termites, bees, and wasps; or a group of birds or fishes. PSO has a fast convergence velocity in the early stages of the search toward the global optimum and has a slow convergence near the global optimum. Nevertheless, although two optimization algorithms are widely used in many applications they have advantages and disadvantages [34]. Due to the random nature of genetic operators, the GA is very sensitive to the initial population. This algorithm performs crossover and mutation operations to recombine chromosomes. It has a strong global search capability. However, since this algorithm does not take the memory mechanism, its convergence speed is slow. On the other hand, PSO is the much more powerful algorithm, because the knowledge of valuable solutions is preserved by the particles and then shared between the other particles. In addition, this algorithm has the same sensitivity against initial point such as GA. According to the advantages and disadvantages of both optimization algorithms, a combination of them has better performance relative to each of them. In this way, Angeline and Eberhard have suggested a combination of the PSO and GA algorithms that could be very effective for search method [35,36]. In the hybrid algorithm, the PSO is combined with GA by applying crossover and mutation operators on solutions constructed by particles. To that end, this algorithm helps to solve the defects of two optimization algorithms. The hybrid algorithm is a single objective optimization algorithm and avoids the premature convergence and time complexity in conventional PSO and GA algorithms. Moreover, this algorithm is very fast and the accuracy of the answers are acceptable [37]. 4.4. PSO-GA for the camera and camera-IMU calibration Since the camera intrinsic and camera–IMU extrinsic variables calibration is critical in the VIO system and vehicle pose estimation, the PSO–GA algorithm is applied in the simulation base optimization to obtain calibration variables. Nevertheless, the intrinsic and extrinsic variables are to be optimized in this research based on VIO. For this purpose, the cost function is established as Eq. (11).

(9) In above matrix, ϕ is the rotation angle about z axis of camera, θ is the rotation angle about x axis of camera and ψ is the rotation angle about y axis of camera. Also, the translation vector TIC is the distance between the two coordinate systems and is calculated as the following equation: tx

[ ] TCI

= ty

(10)

tz where, tx , ty , and tz are the distances between two coordinate systems in the x, y, and z directions. 4.3. PSO-GA overview Many real optimum design problems are defined by discontinuous and no convex design spaces or mixed continuous and discrete variables. GA was presented systematically first by Holland in [32]. This optimization algorithm has remarkable performance for solving these type of problems, and it can find the

5

fcost =

N )2 1 ∑ (( latReali − latVIOi (C.V) N i=1

( )2 + lonReali − lonVIOi (C.V) ( )2 ) + altReali − altVIOi (C.V)

(11)

where fcost is the mean of square position error of VIO than real position, N is the number of image frames in the trajectory. (·)Reali are the real latitude, longitude, and altitude state in the ith image frame, respectively. (·)VIOi (C.V) are the VIO latitude, longitude, and altitude state in the ith image frame corresponding to Calibration Variables (C.V), respectively. The cost function is minimized by PSO–GA, and then the optimal stable solution of camera intrinsic and camera–IMU extrinsic calibration variables can be obtained. If Calibration Variables are defined as xC . V , the optimal stable solution is calculated as Eq. (12). x∗C .V = arg min (fcost (xC.V ))0.5 xC.V

(12)

6

M. Arbabmir and M. Ebrahimi / Robotics and Autonomous Systems 120 (2019) 103249

Fig. 10. The trajectory estimation. (a) Section 1. (b) Section 2. (c) Section 3. (d) Section 4. (e) Section 5.

where (fcost (xC.V ))0.5 is position RMSE of VIO. The convergence criteria to identify and termination for the calibration procedure is used for this algorithm. For this purpose, the convergence criteria or stop condition is defined as Eq. (13).

∆xTC.V ∆xC.V ≤ ε

(13)

where ∆xC.V is the minus of sequential obtained xC.V in the PSO– GA and ε is 0.001. When the above equation is satisfied then the end of the process and the optimal results are obtained. The application process of PSO–GA optimization algorithm for calibrating the sensors variables of VIO is given as Fig. 4. The description of the presented pipeline is given as follow: Step 1. Create the initial population: The initial population is created randomly based on the boundary constraints of the calibration variables. The calibration variables were defined in Sections 4.1 and 4.2. For this purpose, the cost function is evaluated for each particle, and then the best particle is selected as initial values based on Eq. (12). Step 2. Continue with PSO: According to the initial population or last GA population, the velocity and population in PSO are updated by evaluation of cost function (Eq. (11)) and then the best PSO population is selected based on Eq. (12). Step 3. Continue with GA: The updated population of the PSO is chosen as the initial population in the GA and then is updated. Then the cost function (Eq. (11)) is evaluated for each particle of the new population and the best population is selected. Step 4. Stop condition: If the convergence criteria (Eq. (13)) is satisfied, go to step 5, otherwise, go to step 2. Step 5. Show results. 5. Experimental results 5.1. Implementation details and calibration performance The presented optimization algorithm employed on a part of benchmark dataset KITTI [13] in MATLAB. In this dataset, a GPS/INS (OXTS RT 3003, 100 Hz), and a grayscale camera (Point Grey Flea 2, 10 fps, 1392 × 512 pixel resolution, 90◦ ×35◦ opening angle) were used for recording the related data. It is worth mentioning, we used the synchronized dataset for experiments R test. Our algorithm runs on an Intel⃝ Core(TM) i7-6700K CPU @ 4.00 GHz. With this PC type.

Table 1 The PSO–GA and Geiger et al. [13] as a real reference. Item Variables Geiger et al. [13] Lower bound Upper bound PSO–GA 1 2 3 4 5 6 7 8 9 10

fsx fsy Ox Oy

ϕ θ ψ tx ty tz

718.8560 718.8560 607.1928 185.2157 119.5262 −89.4279 −28.722 1.1022 −0.3191 0.7461

700 700 600 170 110 −100 −50 0.8 −0.5 0.5

740 740 620 200 140 −80 −20 1.3 −0.1 1

717.4954 720.5287 602.0016 180.1210 132.3246 −89.5779 −41.6586 1.0169 −0.2687 0.7957

In this experiment, the performance of our camera and camera–IMU calibration approach is evaluated. It is worth mentioning that without knowing the mechanical configuration and the precise camera–IMU calibration, the intrinsic and extrinsic variables estimated in the simulation base PSO–GA optimization algorithm. Our approach is an automatic, simple execution, iterative, and auto-initialized process. Also, no extra equipment such as chessboard or optical instruments and calibration scenarios is required and the user does not take additional time for the calibration process. We use results calculated by Geiger et al. [13], a conventional method, as a real reference. Figs. 5, 6, and 7 displays all camera intrinsic and camera– IMU extrinsic variable during calibration on the trials, with the trajectory shown in Fig. 8. The calibration variables are converged after the 83 s. Since in this study the intrinsic and extrinsic variables are calibrated by minimization of position RMSE, in order to achieve proper calibration, remarkable changes are needed in position and attitude of vehicle trajectory. Nevertheless, PSO–GA calibration approach is used on the trajectory shown in Fig. 8. The optimal calibrated variables are shown in Table 1. It is worth mentioning that the boundary constraints of calibration variables for first step of PSO–GA algorithm, create the initial population, are defined in this table. The obtained variables by PSO–GA algorithm are close to conventional Geiger et al. [13] approach, as a real reference. This shows the precision of PSO–GA in determining the calibration variables based on VIO.

M. Arbabmir and M. Ebrahimi / Robotics and Autonomous Systems 120 (2019) 103249

7

Table 2 The pose statistic errors on trajectory shown in Fig. 9. Third left to right column is Root Mean Square Error (RMSE) of poses error, fourth left to right column is Mean error of poses error, and fifth left to right column is the Standard deviation (Std) of poses error. State

Method

RMSE

Mean

Std

Position (m)

VIO based on [13] AVIO

4.8944 2.0736

2.3549 1.7936

1.1641 0.8551

Velocity (m/s)

VIO based on [13] AVIO

0.5281 0.1513

0.4598 0.1472

0.1705 0.1368

Attitude (deg)

VIO based on [13] AVIO

2.3558 1.9108

0.6493 0.5101

1.1323 1.0023

Table 3 The pose statistic errors on trajectory shown in Fig. 12. Third left to right column is Root Mean Square Error (RMSE) of poses error, fourth left to right column is Mean error of poses error, and fifth left to right column is the Standard deviation (Std) of poses error. State

Method

RMSE

Mean

Std

Position (m)

VIO based on [13] AVIO

27.3307 19.8791

18.3963 12.6847

7.2341 2.3142

Velocity (m/s)

VIO based on [13] AVIO

1.5723 1.0134

0.9824 0.7925

0.9876 0.6541

Attitude (deg)

VIO based on [13] AVIO

2.2124 1.3482

1.2344 0.9821

1.9856 1.4422

This experiment demonstrates the competitive accuracy of our system against Geiger et al. [13] approach. Since our approach is based on position RMSE of VIO, so the obtained optimal calibration variables are adjusted.

5.2. Performance in large-scale environments In two outdoor experiments, we evaluate the performance of the VIO with optimal calibration variables. Since the optimal calibration variables are adjusted so we had an Adjusted VIO (AVIO). In the first experiment, (1) AVIO, (2) VIO with Geiger et al. [13] calibration variables and (3) GPS/INS as Real Data were compared in the trajectory of about 540 m length, which takes 78 s with an average speed about 7 m/s. This trajectory is a part of identical benchmark dataset KITTI. Fig. 9 shows the trajectory estimation from three mentioned navigation systems. As shown in Fig. 10, the position error of the AVIO is less than the VIO with Geiger et al. [13] calibration variables in the most points of the trajectory. Table 2, represents statistics errors of position, velocity and attitude corresponding to AVIO and the VIO with Geiger et al. [13] calibration variables compared to Real Data. Fig. 11 displays the position, velocity, and attitude state errors of AVIO and VIO with Geiger et al. [13] calibration variables, respectively. The navigation results corresponding to first outdoor experiment demonstrates the competitive accuracy of AVIO, presented simulation base calibration approach, against VIO with Geiger et al. [13] calibration variables. As well as the first experiment, in the second experiment, we consider another outdoor navigation tasks. We investigated AVIO on a trajectory is a part of identical benchmark dataset KITTI. This trajectory is about 928 m length, which takes 115 s with average speed about 8 m/s. The (1) AVIO, (2) VIO with Geiger et al. [13] calibration variables and (3) GPS/INS as Real Data were compared in this trajectory. Fig. 12 shows the trajectory estimation from three mentioned navigation systems.

Fig. 11. Compression of the position, velocity, and attitude state errors corresponding AVIO against VIO on trajectory shown in Fig. 9. (a) Position state errors. (b) Velocity state errors. (c) Attitude state errors.

Table 3, represents statistics errors of position, velocity and attitude corresponding to AVIO and the VIO with Geiger et al. [13] calibration variables compared to Real Data.

8

M. Arbabmir and M. Ebrahimi / Robotics and Autonomous Systems 120 (2019) 103249

Fig. 12. Comparison of estimated trajectory with (1) AVIO, (2) VIO with Geiger et al. [13] calibration variables and (3) GPS/INS as Real Data. The total trajectory length is about 928 m and 115 s. In some parts of this trajectory, the Real Data or GPS/INS get away from the real trajectory because the GPS had error or outage.

Fig. 13 displays the position, velocity, and attitude state errors of AVIO and VIO with Geiger et al. [13] calibration variables, respectively. The navigation results corresponding to the second outdoor experiment demonstrates the competitive accuracy of AVIO, presented simulation base calibration approach, against VIO with Geiger et al. [13] calibration variables. These results validate the calibrated variables and the calibration approach. 6. Conclusion In the present study, we present a novel calibration approach based on monocular VIO. Especially, our approach calibrates the camera intrinsic and camera–IMU extrinsic variables automatically by simulation base hybrid PSO–GA optimization algorithm. Our approach is appropriate for calibrating the intrinsic and extrinsic variables without any added equipment such as chessboard and optical instruments and knowing the mechanical configuration of the sensor suite. Also, the complexity and time consuming are resolved in our approach. Our approach is able to identify the convergence of the calibration variables so that the calibration can be terminated. The experimental results are presented to demonstrate the performance of our approach. Also, we show the improved navigation accuracy based on two outdoor well-known benchmark dataset. In our future work, we plan to correct the time-synchronize between camera and IMU measurements. Declaration of competing interest The authors declare that they have no known competing financial interests or personal relationships that could have appeared to influence the work reported in this paper.

Fig. 13. Compression of the position, velocity, and attitude state errors corresponding AVIO against VIO on trajectory shown in Fig. 12. (a) Position state errors. (b) Velocity state errors. (c) Attitude state errors.

Acknowledgments The authors gratefully acknowledge the contribution of Mohammad Norouz, M.Sc graduate of Mechanical Engineering at

Tarbiat Modares University for his help on the implementation process.

M. Arbabmir and M. Ebrahimi / Robotics and Autonomous Systems 120 (2019) 103249

References [1] S. Weiss, M.W. Achtelik, S. Lynen, M. Chli, R. Siegwart, Real-time onboard visual-inertial state estimation and self-calibration of mavs in unknown environments, in: 2012 IEEE International Conference on Robotics and Automation (ICRA), 2012, pp. 957–964. [2] S. Lynen, M.W. Achtelik, S. Weiss, M. Chli, R. Siegwart, A robust and modular multi-sensor fusion approach applied to mav navigation, in: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2013, pp. 3923–3929. [3] A.I. Mourikis, S.I. Roumeliotis, A multi-state constraint Kalman filter for vision-aided inertial navigation, in: 2007 IEEE International Conference on Robotics and Automation, 2007, pp. 3565–3572. [4] M. Li, A.I. Mourikis, High-precision, consistent EKF-based visual-inertial odometry, Int. J. Robot. Res. 32 (6) (2013) 690–711. [5] J. Civera, O.G. Grasa, A.J. Davison, J.M.M. Montiel, 1-Point RANSAC for extended kalman filtering: Application to real-time structure from motion and visual odometry, J. F. Robot. 27 (5) (2010) 609–631. [6] G.R. Mueller, H.-J. Wuensche, Continuous extrinsic online calibration for stereo cameras, in: Intelligent Vehicles Symposium (IV), 2016 IEEE, 2016, pp. 966–971. [7] J.-S. Hu, M.-Y. Chen, A sliding-window visual-imu odometer based on trifocal tensor geometry, in: 2014 IEEE International Conference on Robotics and Automation (ICRA), 2014, pp. 3963–3968. [8] M. Norouz, M. Ebrahimi, M. Arbabmir, Modified Unscented Kalman Filter for improving the integrated visual navigation system, in: 2017 Iranian Conference on Electrical Engineering (ICEE), 2017, pp. 753–758. [9] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, P. Furgale, Keyframe-based visual–inertial odometry using nonlinear optimization, Int. J. Robot. Res. 34 (3) (2015) 314–334. [10] R. Mur-Artal, J.D. Tardós, Visual-inertial monocular SLAM with map reuse, IEEE Robot. Autom. Lett. 2 (2) (2017) 796–803. [11] T. Qin, P. Li, S. Shen, Vins-mono: A robust and versatile monocular visual-inertial state estimator, IEEE Trans. Robot. 34 (4) (2018) 1004–1020. [12] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, On-manifold preintegration for real-time visual–inertial odometry, IEEE Trans. Robot. 33 (1) (2017) 1–21. [13] A. Geiger, P. Lenz, R. Urtasun, Are we ready for autonomous driving? the kitti vision benchmark suite, in: Computer Vision and Pattern Recognition (CVPR), 2012 IEEE Conference on, 2012, pp. 3354–3361. [14] J. Delmerico, D. Scaramuzza, A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots, Memory 10 (2018) 20. [15] J. Rehder, R. Siegwart, CaMera/IMU calibration revisited, IEEE Sens. J. 17 (11) (2017) 3257–3268. [16] N.B. Hui, D.K. Pratihar, Camera calibration using a genetic algorithm, Eng. Optim. 40 (12) (2008) 1151–1169. [17] Y. Xing, Q. Liu, J. Sun, L. Hu, Camera calibration based on improved genetic algorithm, in: 2007 IEEE International Conference on Automation and Logistics, 2007, pp. 2596–2601. [18] H. Tang, Y. Liu, A fully automatic calibration algorithm for a camera odometry system, IEEE Sens. J. 17 (13) (2017) 4208–4216. [19] K. Deep, M. Arya, M. Thakur, B. Raman, Stereo camera calibration using particle swarm optimization, Appl. Artif. Intell. 27 (7) (2013) 618–634. [20] J. Zhang, J. Lu, H. Li, M. Xie, Particle swarm optimisation algorithm for non-linear camera calibration, Int. J. Innov. Comput. Appl. 4 (2) (2012) 92–99. [21] J. Li, Y. Yang, G. Fu, Camera self-calibration method based on GA-PSO algorithm, in: 2011 IEEE International Conference on Cloud Computing and Intelligence Systems (CCIS), 2011, pp. 149–152. [22] L. Deng, G. Lu, Y. Shao, M. Fei, H. Hu, A novel camera calibration technique based on differential evolution particle swarm optimization algorithm, Neurocomputing 174 (2016) 456–465.

9

[23] J.D. Hol, T.B. Schön, F. Gustafsson, Modeling and calibration of inertial and vision sensors, Int. J. Robot. Res. 29 (2–3) (2010) 231–244. [24] J. Kelly, G.S. Sukhatme, Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration, Int. J. Robot. Res. 30 (1) (2011) 56–79. [25] H. Tang, Y. Liu, Automatic simultaneous extrinsic-odometric calibration for camera-odometry system, IEEE Sens. J. 18 (1) (2018) 348–355. [26] C. Tomasi, T. Kanade, Detection and Tracking of Point Features, 1991. [27] K.G. Derpanis, Overview of the RANSAC algorithm, Image Rochester NY 4 (1) (2010) 2–3. [28] D. Scaramuzza, F. Fraundorfer, R. Siegwart, Real-time monocular visual odometry for on-road vehicles with 1-point ransac, in: IEEE International Conference on Robotics and Automation, 2009. ICRA’09, 2009, pp. 4293–4299. [29] D. Scaramuzza, 1-point-ransac structure from motion for vehicle-mounted cameras by exploiting non-holonomic constraints, Int. J. Comput. Vis. 95 (1) (2011) 74–85. [30] R. Hartley, A. Zisserman, Multiple View Geometry in Computer Vision, Cambridge university press, 2003. [31] D. Titterton, J.L. Weston, J. Weston, Strapdown Inertial Navigation Technology, Vol. 17, IET, 2004. [32] J.H. Holland, Adaptation in Natural and Artificial Systems. An Introductory Analysis with Application to Biology, Control, and Artificial Intelligence, Ann Arbor, MI Univ. Michigan Press, 1975. [33] R.C. Eberchart, J. Kennedy, Particle swarm optimization, in: IEEE International Conference on Neural Networks, Perth, Australia, 1995. [34] N. Ru, Y. Jianhua, A GA and particle swarm optimization based hybrid algorithm, in: IEEE Congress on Evolutionary Computation, 2008. CEC 2008.(IEEE World Congress on Computational Intelligence), 2008, pp. 1047–1050. [35] P. Angeline, Evolutionary optimization versus particle swarm optimization: Philosophy and performance differences, in: Evolutionary Programming VII, 1998, pp. 601–610. [36] R. Eberhart, Y. Shi, Comparison between genetic algorithms and particle swarm optimization, in: Evolutionary Programming VII, 1998, pp. 611–616. [37] H.-C. Huang, C.-C. Tsai, Global path planning for autonomous robot navigation using hybrid metaheuristic GA-PSO algorithm, in: 2011 Proceedings of SICE Annual Conference (SICE), 2011, pp. 1338–1343.

Mohammadvali Arbabmir Received his bachelor’s and M.S. degrees in electronic engineering from Malekashtar Technical University, Shahinshahr and Tehran, Iran, in 2001 and 2010. He is currently pursuing his Ph.D. degree in dynamics and control in the Department of Mechanical Engineering at Tarbiat Modares University. His research interests are robotic, machine vision, guidance, navigation, and control.

Masoud Ebrahimi Received his bachelor’s degree in mechanical engineering from Isfahan Technical University, Isfahan, Iran, in 2003 and his M.S. and Ph.D. degrees in aerospace engineering from K. N. Toosi University of Technology, Tehran, Iran, in 2006 and 2012. He is currently an assistant professor of the aerospace group at Tarbiat Modares Mechanical University. His research interests include multidisciplinary optimization, guidance, navigation, and control.