Autonomous Robots
August 2019 , Volume 43, Issue 6, pp 1605–1622 | Cite as
Autonomous flight with robust visual odometry under dynamic lighting conditions
Pyojin Kim
Hyeonbeom Lee
H. Jin Kim
Sensitivity to light conditions poses a challenge when utilizing visual odometry (VO) for autonomous navigation of small aerial vehicles in various applications. We present an illumination-robust direct visual odometry for a stable autonomous flight of an aerial robot under unpredictable light condition. The proposed stereo VO achieves robustness with respect to the light-changing environment by employing the patch-based affine illumination model to compensate abrupt, irregular illumination changes during direct motion estimation. We furthermore incorporate a motion prior from feature-based stereo visual odometry in the optimization, resulting in higher accuracy and more stable motion estimate. Thorough analyses of convergence rate and linearity index for the feature-based and direct VO methods support the effectiveness of the usage of the motion prior knowledge. We extensively evaluate the proposed algorithm on synthetic and real micro aerial vehicle datasets with ground-truth. Autonomous flight experiments with an aerial robot show that the proposed method successfully estimates 6-DoF pose under significant illumination changes.
Aerial robotics Stereo visual odometry Robustness Illumination changes
The online version of this article ( https://doi.org/10.1007/s10514-018-9816-4) contains supplementary material, which is available to authorized users.
This research was supported by the Ministry of Science, ICT, under the Information Technology Research Center (ITRC) program (IITP-2018-2017-0-01637) supervised by the Institute for Information & communications Technology Promotion, and Automation and Systems Research Institute (ASRI), and Samsung Research, Samsung Electronics Co., Ltd.
Supplementary material 1 (mp4 63455 KB)
In this section, we derive the analytic form of the dimensionless linearity index (DLI) for each observation model in feature-based and direct estimation in detail. The DLI of each measurement equation in terms of the 6-DoF camera motion can be written as follows:
$$\begin{aligned} L&= \left| \frac{\left. \frac{\partial ^2 h}{\partial {\varvec{\xi }}^2}\right| _{{\varvec{\xi }} = {\varvec{\xi }}_{0}}\varDelta {\varvec{\xi }}}{\left. \frac{\partial h}{\partial {\varvec{\xi }}}\right| _{{\varvec{\xi }} = {\varvec{\xi }}_{0}}} \right| \nonumber \\ {\hat{x}}&= h_{x}({\varvec{\xi }}) = \left[ \pi (T({\varvec{\xi }})\cdot \pi ^{-1}({\mathbf {x}},Z({\mathbf {x}}))) \right] _{x}\nonumber \\ {\hat{I}}&= h_{I}({\varvec{\xi }}) = I(\pi (T({\varvec{\xi }})\cdot \pi ^{-1}({\mathbf {x}},Z({\mathbf {x}})))) \end{aligned}$$
where the h is the observation model in each estimation method. The division in the Eq. (14) denotes element-wise operation between two vectors with a slight abuse of notation. For the sake of simplicity, we can rewrite the x component of the warping function in the feature-based method as \({\hat{x}}\), and the pixel intensity of the next image frame in the direct method as \({\hat{I}}\).
$$\begin{aligned} T = T({\varvec{\xi }}) = \exp ({\hat{\xi }}) = \exp \left( \sum _{i=1}^{6} {\varvec{\xi }}_{i}E_{i} \right) \end{aligned}$$
The 6-DoF camera motion, \(T({\varvec{\xi }})\in \) SE(3), is written as T in this section for simplicity. In the Eq. (4), \({\varvec{\xi }}_{i}\) is a ith component of the Lie algebra explained in Sect. 3, and \(E_{i}\) is one of the six Lie algebra se(3) bases, each corresponding to either infinitesimal translations or rotations along each axis Blanco (2010).
Feature-based estimation
The first and second-order partial derivatives of \({\hat{x}}\) with respect to \({\varvec{\xi }}\), i.e., the Jacobian and Hessian matrices, can be written as follows:
$$\begin{aligned} J_{x} = \frac{\partial {\hat{x}}}{\partial {\varvec{\xi }}} \in {\mathbb {R}}^{1 \times 6}, H_{x} = \frac{\partial ^2 {\hat{x}}}{\partial {\varvec{\xi }}^2} \in {\mathbb {R}}^{6 \times 6} \end{aligned}$$
By applying the chain rule, we can derive analytical Jacobian matrix as follows:
$$\begin{aligned} J_{x} = \frac{\partial {\hat{x}}}{\partial {\mathbf {X}}^{\prime }} \cdot \frac{\partial {\mathbf {X}}^{\prime }}{\partial T} \cdot \frac{\partial T}{\partial {\varvec{\xi }}} \end{aligned}$$
where \({\mathbf {X}}^{\prime }=\left[ X^{\prime }, Y^{\prime }, Z^{\prime } \right] ^{\top }\) is a transformed 3D point from \({\mathbf {X}} = \left[ X, Y, Z \right] ^{\top }\) with respect to T. The analytical Jacobian matrix of warping function is:
$$\begin{aligned} J_{x} = \begin{bmatrix} f_{x} \frac{1}{Z^{\prime }}&0&-f_{x} \frac{X^{\prime }}{{Z^{\prime }}^2}&-f_{x} \frac{X^{\prime }Y^{\prime }}{{Z^{\prime }}^2}&f_{x} \left( 1+ \frac{{X^{\prime }}^2}{{Z^{\prime }}^2} \right)&-f_{x} \frac{Y^{\prime }}{Z^{\prime }} \end{bmatrix} \end{aligned}$$
For full details of each step, see Blanco (2010) and Kerl (2012).
In the following, the second order partial derivatives of warping function with respect to \({\varvec{\xi }}\), the Hessian matrix, can be obtained by applying the chain rule same as above:
$$\begin{aligned} \begin{aligned} H_{x}&= \frac{\partial }{\partial {\varvec{\xi }}} \left( \frac{\partial {\hat{x}}}{\partial {\varvec{\xi }}} \right) = \frac{\partial }{\partial {\varvec{\xi }}} \left( \frac{\partial {\hat{x}}}{\partial T} \cdot \frac{\partial T}{\partial {\varvec{\xi }}} \right) \\&= \frac{\partial }{\partial {\varvec{\xi }}} \left( \frac{\partial {\hat{x}}}{\partial T} \right) \cdot \frac{\partial T}{\partial {\varvec{\xi }}} + \frac{\partial {\hat{x}}}{\partial T} \cdot \frac{\partial ^2 T}{\partial {\varvec{\xi }}^2} \end{aligned} \end{aligned}$$
The (i, j)th element of the Hessian matrix \(H_{x}\) can be written as follows:
$$\begin{aligned} \begin{aligned} H_{x \left( i,j \right) }&= \frac{\partial }{\partial T} \left( \frac{\partial {\hat{x}}}{\partial T} \right) \cdot \frac{\partial T}{\partial {\varvec{\xi }}_{j}} \cdot \frac{\partial T}{\partial {\varvec{\xi }}_{i}} + \frac{\partial {\hat{x}}}{\partial T} \cdot \frac{\partial ^2 T}{\partial {\varvec{\xi }}_{i}\partial {\varvec{\xi }}_{j}} \\&= \left( \frac{\partial T}{\partial {\varvec{\xi }}_{j}} \right) ^{\top } \cdot \frac{\partial ^2 {\hat{x}}}{\partial T^2} \cdot \left( \frac{\partial T}{\partial {\varvec{\xi }}_{i}} \right) + \frac{\partial {\hat{x}}}{\partial T} \cdot \frac{\partial ^2 T}{\partial {\varvec{\xi }}_{i}\partial {\varvec{\xi }}_{j}} \end{aligned} \end{aligned}$$
where the first and second-order derivatives of T with respect to \({\varvec{\xi }}_{i}\) can be computed as follows:
$$\begin{aligned}&\frac{\partial T}{\partial {\varvec{\xi }}_{i}} = E_{i} \cdot T\\&\frac{\partial ^2 T}{\partial {\varvec{\xi }}_{i}\partial {\varvec{\xi }}_{j}} = \frac{1}{2}\left( E_{i}E_{j} + E_{j}E_{i} \right) \cdot T \end{aligned}$$
Note that, \(J_{y}\) and \(H_{y}\), the Jacobian and Hessian matrices of y component of the warping function with respect to the camera motion, are omitted because they are symmetric to \(J_{x}\) and \(H_{x}\). With the equations derived above, we obtain the Jacobian and Hessian matrices of \({\hat{x}}\) with respect to the \({\varvec{\xi }}\), and compute the DLI of the observation model in the feature-based method in terms of the 6-DoF camera pose.
Direct estimation
The first and second-order partial derivatives of \({\hat{I}}\) with respect to \({\varvec{\xi }}\), the Jacobian and Hessian matrices of the observation model in the direct method, can be written as follows:
$$\begin{aligned} J_{I} = \frac{\partial {\hat{I}}}{\partial {\varvec{\xi }}} \in {\mathbb {R}}^{1 \times 6}, H_{I} = \frac{\partial ^2 {\hat{I}}}{\partial {\varvec{\xi }}^2} \in {\mathbb {R}}^{6 \times 6} \end{aligned}$$
To obtain the analytical Jacobian and Hessian matrices, we repeat similar calculation procedure again as above. Based on the analytical Jacobian and Hessian matrices of warping function calculated in the previous section and the chain rule, we can easily derive the analytical Jacobian matrix of direct method as follows:
$$\begin{aligned} J_{I} = \frac{\partial {\hat{I}}}{\partial \hat{{\mathbf {x}}}} \cdot \frac{\partial \hat{{\mathbf {x}}}}{\partial {\varvec{\xi }}} = \frac{\partial {\hat{I}}}{\partial \hat{{\mathbf {x}}}} \cdot \frac{\partial \hat{{\mathbf {x}}}}{\partial {\mathbf {X}}^{\prime }} \cdot \frac{\partial {\mathbf {X}}^{\prime }}{\partial T} \cdot \frac{\partial T}{\partial {\varvec{\xi }}} \end{aligned}$$
where \(\hat{{\mathbf {x}}} = \left[ {\hat{x}}, {\hat{y}} \right] ^{\top }\) is a 2D pixel point in the image \({\hat{I}}\). The first term \(\frac{\partial {\hat{I}}}{\partial \hat{{\mathbf {x}}}}\) denotes the gradient of the image \({\hat{I}}\) given by the image derivatives in the horizontal and vertical directions and the latter terms are the Jacobian matrix of warping function written in Eq. (16). The analytical Jacobian matrix of the observation model in the direct method is:
$$\begin{aligned} \begin{aligned} J_{I}&= \begin{bmatrix} \frac{\partial {\hat{I}}}{\partial {\hat{x}}}&\frac{\partial {\hat{I}}}{\partial {\hat{y}}} \end{bmatrix} \cdot \begin{bmatrix} \frac{\partial {\hat{x}}}{\partial {\mathbf {X}}^{\prime }} \cdot \frac{\partial {\mathbf {X}}^{\prime }}{\partial T} \cdot \frac{\partial T}{\partial {\varvec{\xi }}}\\ \frac{\partial {\hat{y}}}{\partial {\mathbf {X}}^{\prime }} \cdot \frac{\partial {\mathbf {X}}^{\prime }}{\partial T} \cdot \frac{\partial T}{\partial {\varvec{\xi }}} \end{bmatrix} \\&= \begin{bmatrix} \frac{\partial {\hat{I}}}{\partial {\hat{x}}}&\frac{\partial {\hat{I}}}{\partial {\hat{y}}} \end{bmatrix} \cdot \begin{bmatrix} \frac{\partial {\hat{x}}}{\partial {\varvec{\xi }}}\\ \frac{\partial {\hat{y}}}{\partial {\varvec{\xi }}} \end{bmatrix} \\&= \begin{bmatrix} \nabla {\hat{I}}_{x}&\nabla {\hat{I}}_{y} \end{bmatrix} \cdot \begin{bmatrix} J_{x}\\ J_{y} \end{bmatrix} \end{aligned} \end{aligned}$$
where \(\nabla {\hat{I}}_{x}\) and \(\nabla {\hat{I}}_{y}\) are image gradients of \({\hat{I}}\) along the x and y direction in the image plane.
The Hessian matrix used in the direct estimation can be also derived by applying the chain rule and the Eq. (20) as follows:
$$\begin{aligned} H_{I} = \frac{\partial }{\partial {\varvec{\xi }}} \left( \frac{\partial {\hat{I}}}{\partial {\varvec{\xi }}} \right) = \frac{\partial }{\partial {\varvec{\xi }}} \left( \frac{\partial {\hat{I}}}{\partial \hat{{\mathbf {x}}}} \cdot \frac{\partial \hat{{\mathbf {x}}}}{\partial {\varvec{\xi }}} \right) \end{aligned}$$
Each element of the Hessian matrix, an element in the ith row and jth column of \(H_{I}\), can be written as follows:
$$\begin{aligned} \begin{aligned} H_{I \left( i,j \right) }&= \frac{\partial }{\partial {\varvec{\xi }}_{j}} \left( \frac{\partial {\hat{I}}}{\partial {\varvec{\xi }}_{i}} \right) \\&= \frac{\partial }{\partial {\varvec{\xi }}_{j}} \left( \frac{\partial {\hat{I}}}{\partial {\hat{x}}} \cdot \frac{\partial {\hat{x}}}{\partial {\varvec{\xi }}_{i}} + \frac{\partial {\hat{I}}}{\partial {\hat{y}}} \cdot \frac{\partial {\hat{y}}}{\partial {\varvec{\xi }}_{i}} \right) \\&= \frac{\partial }{\partial {\varvec{\xi }}_{j}} \left( \frac{\partial {\hat{I}}}{\partial {\hat{x}}} \right) \cdot \frac{\partial {\hat{x}}}{\partial {\varvec{\xi }}_{i}} + \frac{\partial {\hat{I}}}{\partial {\hat{x}}} \cdot \frac{\partial }{\partial {\varvec{\xi }}_{j}} \left( \frac{\partial {\hat{x}}}{\partial {\varvec{\xi }}_{i}} \right) \\&\quad + \frac{\partial }{\partial {\varvec{\xi }}_{j}} \left( \frac{\partial {\hat{I}}}{\partial {\hat{y}}} \right) \cdot \frac{\partial {\hat{y}}}{\partial {\varvec{\xi }}_{i}} + \frac{\partial {\hat{I}}}{\partial {\hat{y}}} \cdot \frac{\partial }{\partial {\varvec{\xi }}_{j}} \left( \frac{\partial {\hat{y}}}{\partial {\varvec{\xi }}_{i}} \right) \\&= \left( \frac{\partial ^2 {\hat{I}}}{\partial {\hat{x}}^2} \cdot J_{x \left( 1,j \right) } + \frac{\partial ^2 {\hat{I}}}{\partial {\hat{y}} \partial {\hat{x}}} \cdot J_{y \left( 1,j \right) } \right) \cdot J_{x \left( 1,i \right) } \\&\quad + \frac{\partial {\hat{I}}}{\partial {\hat{x}}} \cdot H_{x \left( i,j \right) } \\&\quad + \left( \frac{\partial ^2 {\hat{I}}}{\partial {\hat{x}} \partial {\hat{y}}} \cdot J_{x \left( 1,j \right) } + \frac{\partial ^2 {\hat{I}}}{\partial {\hat{y}}^2} \cdot J_{y \left( 1,j \right) } \right) \cdot J_{y \left( 1,i \right) } \\&\quad + \frac{\partial {\hat{I}}}{\partial {\hat{y}}} \cdot H_{y \left( i,j \right) } \end{aligned} \end{aligned}$$
where \(J_{x}, J_{y}\) and \(H_{x}, H_{y}\) are the Jacobian and Hessian matrices of warping function derived in the previous section, and \(\frac{\partial ^2 {\hat{I}}}{\partial {\hat{x}}^2}, \frac{\partial ^2 {\hat{I}}}{\partial {\hat{x}} \partial {\hat{y}}}, \frac{\partial ^2 {\hat{I}}}{\partial {\hat{y}}^2}\) are the second image derivatives in the horizontal and vertical directions. With the above analytical Jacobian and Hessian matrices of the observation model in the direct method, we can compute the DLI of the image intensity observation model with respect to the 6-DoF camera motion.
Achtelik, M., Achtelik, M., et al. (2012). Sfly: Swarm of micro flying robots. In: 2012 IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 2649–2650). IEEE.Google Scholar
Alismail, H., Kaess, M., Browning, B., & Lucey, S. (2017). Direct visual odometry in low light using binary descriptors. IEEE Robotics and Automation Letters, 2(2), 444–451.CrossRefGoogle Scholar
Benhimane, S., & Malis, E. (2004). Real-time image-based tracking of planes using efficient second-order minimization. In: 2004 IEEE/RSJ international conference on intelligent robots and systems, 2004. IROS 2004. Proceedings (Vol. 1, pp 943–948). IEEE.Google Scholar
Bergmann, P., Wang, R., & Cremers, D. (2018). Online photometric calibration of auto exposure video for realtime visual odometry and slam. IEEE Robotics and Automation Letters, 3(2), 627–634.CrossRefGoogle Scholar
Blanco, J. L. (2010). A tutorial on se (3) transformation parameterizations and on-manifold optimization (p. 3). University of Malaga, Tech Rep.Google Scholar
Burri, M., & Nikolic, J. (2016). The EuRoc micro aerial vehicle datasets. The International Journal of Robotics Research, 35, 1157–1163.CrossRefGoogle Scholar
Civera, J., Davison, A. J., & Montiel, J. M. (2008). Inverse depth parametrization for monocular slam. IEEE Transactions on Robotics, 24(5), 932–945.CrossRefGoogle Scholar
Engel, J., Schöps, T., & Cremers, D. (2014). LSD-SLAM: Large-scale direct monocular slam. In: European Conference on Computer Vision (pp. 834–849). Springer.Google Scholar
Engel, J., Stückler, J., & Cremers, D. (2015). Large-scale direct slam with stereo cameras. In: 2015 IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 1935–1942). IEEE.Google Scholar
Engel, J., Koltun, V., & Cremers, D. (2018). Direct sparse odometry. IEEE Transactions on Pattern Analysis and Machine Intelligence, 40(3), 611–625.CrossRefGoogle Scholar
Fang, Z., & Scherer, S. (2014). Experimental study of odometry estimation methods using RGB-D cameras. In: IROS (pp. 680–687). IEEE.Google Scholar
Forster, C., Pizzoli, M., & Scaramuzza, D. (2014). SVO: Fast semi-direct monocular visual odometry. In: 2014 IEEE international conference on robotics and automation (ICRA) (pp. 15–22). IEEE.Google Scholar
Forster, C., Zhang, Z., Gassner, M., Werlberger, M., & Scaramuzza, D. (2017). SVO: Semidirect visual odometry for monocular and multicamera systems. IEEE Transactions on Robotics, 33(2), 249–265.CrossRefGoogle Scholar
Geiger, A., Roser, M., & Urtasun, R. (2010). Efficient large-scale stereo matching. In: Asian conference on computer vision (pp. 25–38). Springer.Google Scholar
Geiger, A., Ziegler, J., & Stiller, C. (2011). Stereoscan: Dense 3d reconstruction in real-time. In: 2011 IEEE Intelligent Vehicles Symposium (IV) (pp. 963–968). IEEE.Google Scholar
Huang, A. S., Bachrach, A., Henry, P., Krainin, M., Maturana, D., Fox, D., et al. (2011). Visual odometry and mapping for autonomous flight using an RGB-D camera. In: ISRR, pp. 1–16.Google Scholar
Jin, H., Favaro, P., & Soatto, S. (2001). Real-time feature tracking and outlier rejection with changes in illumination. In ICCV, pp. 684–689.Google Scholar
Kerl, C. (2012). Odometry from RGB-D cameras for autonomous quadrocopters. Master's thesis, Technical University Munich, Germany.Google Scholar
Kerl, C., Sturm, J., & Cremers, D. (2013). Robust odometry estimation for RGB-D cameras. In: 2013 IEEE international conference on robotics and automation (ICRA) (pp. 3748–3754). IEEE.Google Scholar
Kerl, C., Souiai, M., Sturm, J., & Cremers, D. (2014). Towards illumination-invariant 3D reconstruction using ToF RGB-D cameras. In: 3DV.Google Scholar
Kim, P., Lim, H., & Kim, H. J. (2015). Robust visual odometry to irregular illumination changes with RGB-D camera. In: 2015 IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 3688–3694). IEEE.Google Scholar
Kitt, B., Geiger, A., & Lategahn, H. (2010). Visual odometry based on stereo image sequences with ransac-based outlier rejection scheme. In: 2010 IEEE Intelligent Vehicles Symposium (IV) (pp. 486–492). IEEE.Google Scholar
Klose, S., Heise, P., & Knoll, A. (2013). Efficient compositional approaches for real-time robust direct visual odometry from RGB-D data. In: 2013 IEEE/RSJ international conference on intelligent robots and systems (pp. 1100–1106). IEEE.Google Scholar
Krombach, N., Droeschel, D., & Behnke, S. (2016). Combining feature-based and direct methods for semi-dense real-time stereo visual odometry. In: International conference on intelligent autonomous systems (pp. 855–868). Springer.Google Scholar
Lee, H., Kim, H., & Kim, H. J. (2018). Planning and control for collision-free cooperative aerial transportation. IEEE Transactions on Automation Science and Engineering, 15(1), 189–201.CrossRefGoogle Scholar
Ma, Y., Soatto, S., Kosecka, J., & Sastry, S. S. (2012). An invitation to 3-d vision: From images to geometric models (Vol. 26). Berlin: Springer.zbMATHGoogle Scholar
Maimone, M., Cheng, Y., & Matthies, L. (2007). Two years of visual odometry on the mars exploration rovers. Journal of Field Robotics, 24(3), 169–186.CrossRefGoogle Scholar
Mur-Artal, R., Montiel, J., & Tardós, J. D. (2015). ORB-SLAM: A versatile and accurate monocular slam system. IEEE Transactions on Robotics, 31(5), 1147–1163.CrossRefGoogle Scholar
Nikolic, J., Rehder, J., et al. (2014). A synchronized visual-inertial sensor system with FPGA pre-processing for accurate real-time slam. In: 2014 IEEE international conference on robotics and automation (ICRA) (pp. 431–437). IEEE.Google Scholar
Nistér, D., Naroditsky, O., & Bergen, J. (2004). Visual odometry. In: Proceedings of the 2004 IEEE computer society conference on computer vision and pattern recognition, 2004. CVPR 2004 (Vol. 1, pp. I–I). IEEE.Google Scholar
Park, S., Schöps, T., & Pollefeys, M. (2017). Illumination change robustness in direct visual slam. In: 2017 IEEE international conference on robotics and automation (ICRA) (pp. 4523–4530). IEEE.Google Scholar
Scaramuzza, D., Achtelik, M., et al. (2014). Vision-controlled micro flying robots: From system design to autonomous navigation and mapping in GPS-denied environments. IEEE Robotics & Automation Magazine, 21(3), 26–40.CrossRefGoogle Scholar
Scaramuzza, D., & Fraundorfer, F. (2011). Visual odometry [tutorial]. IEEE Robotics & Automation Magazine, 18(4), 80–92.CrossRefGoogle Scholar
Sturm, J., et al. (2012). A benchmark for the evaluation of RGB-D slam systems. In: 2012 IEEE/RSJ international conference on intelligent robots and systems (pp. 573–580). IEEE.Google Scholar
Valenti, R. G., et al. (2014). Autonomous quadrotor flight using onboard RGB-D visual odometry. In: 2014 IEEE International Conference on Robotics and Automation (ICRA) (pp. 5233–5238). IEEE.Google Scholar
von Stumberg, L., Usenko, V., Engel, J., Stückler, J., & Cremers, D. (2016). Autonomous exploration with a low-cost quadrocopter using semi-dense monocular slam. ArXiv preprint arXiv:1609.07835.
Wang, R., Schwörer, M., & Cremers, D. (2017). Stereo DSO: Large-scale direct sparse visual odometry with stereo cameras. In: International conference on computer vision (ICCV), Venice, Italy.Google Scholar
Zhang, J., Kaess, M., & Singh, S. (2017). A real-time method for depth enhanced visual odometry. Autonomous Robots, 41(1), 31–43.CrossRefGoogle Scholar
View author's OrcID profile
1.Department of Mechanical and Aerospace EngineeringSeoul National UniversitySeoulSouth Korea
2.Department of Electronics EngineeringKyungpook National UniversityDaeguSouth Korea
Kim, P., Lee, H. & Kim, H.J. Auton Robot (2019) 43: 1605. https://doi.org/10.1007/s10514-018-9816-4
First Online 31 October 2018