Keyframe-based Direct Thermal-Inertial Odometry

TL;DR

This paper introduces a novel thermal-inertial odometry method that fuses thermal camera data with inertial measurements to enable reliable navigation in GPS-denied and visually degraded environments, including darkness and obscurants.

Contribution

It presents an optimization-based fusion approach for thermal and inertial data, extending robotic navigation capabilities in challenging conditions.

Findings

Successfully verified in indoor laboratory tests with ground-truth.

Demonstrated robustness in underground mine with severe visual degradation.

Achieved accurate localization in GPS-denied, dark, and obscurant-filled environments.

Abstract

This paper proposes an approach for fusing direct radiometric data from a thermal camera with inertial measurements to extend the robotic capabilities of aerial robots for navigation in GPS-denied and visually degraded environments in the conditions of darkness and in the presence of airborne obscurants such as dust, fog and smoke. An optimization based approach is developed that jointly minimizes the re-projection error of 3D landmarks and inertial measurement errors. The developed solution is extensively verified against both ground-truth in an indoor laboratory setting, as well as inside an underground mine under severely visually degraded conditions.

Tables

Table 1. TABLE I: RMSE in Pose Estimation with respect to VICON

RMSE in Pose Estimation
Axis	Translation(m)	Rotation(deg)
X	0.2741	1.1822
Y	0.0910	0.6380
Z	0.0481	0.4701

Table 2. TABLE II: Trajectory error during the underground mine experiments

	Serpentine Path	Flight Path
Error(m)	0.5350	0.6001

Equations

$$p^{\prime }=K\left(R{K}^{-1}\left(p,d^{-1}\right)+t\right)$$

$${\mathbf{e}}_{\mathbf{radio}}=\sum _{iϵP}\sum _{pϵN_i}\Vert T\left(p^{\prime }\right)-T\left(p\right){\Vert }_2$$

$${\mathbf{e}}_{\mathbf{reproj}}=p_{\text{pazocal}C}-K\left(T_{\text{pazocal}C\text{pazocal}I}{T}_{\text{pazocal}I\text{pazocal}W}{l}_{\text{pazocal}W}\right)$$

$$\mathbf{x}=[\mathbf{r}\mathbf{q}\mathbf{v}{\mathbf{b}}_a{\mathbf{b}}_{\omega }{]}^{{}^{\prime }}$$

$$\begin{array}{rl}& \dot{\mathbf{r}}=R_{\text{pazocal}W\text{pazocal}I}\mathbf{v}\\ & \dot{\mathbf{q}}=-q(\hat{\omega })\\ & \dot{\mathbf{v}}=-\widehat{{\omega }^{\times }}\mathbf{v}+\hat{\mathbf{a}}+R_{\text{pazocal}I\text{pazocal}W}\mathbf{g}\\ & \dot{{\mathbf{b}}_a}=w_a\\ & \dot{{\mathbf{b}}_{\omega }}=w_{\omega }\end{array}$$

$$\begin{array}{rl}& \hat{\mathbf{a}}=\tilde{a}-b_a+w_a\\ & \hat{\omega }=\tilde{\omega }-b_{\omega }+w_{\omega }\end{array}$$

$${\mathbf{e}}_{\mathbf{imu}}={\hat{\mathbf{x}}}^{k+1}-{\mathbf{x}}^k$$

$$J=\sum _{k=1}^K\sum _{lϵL(k)}{\mathbf{e}}_{\mathbf{reproj}}^{{}^{\prime }\mathbf{k},\mathbf{l},}{W}_{reproj}^{k,l}{\mathbf{e}}_{\mathbf{reproj}}^{\mathbf{k},\mathbf{l}}+\sum _{k=1}^K{\mathbf{e}}_{\mathbf{imu}}^{{}^{\prime }\mathbf{k}}{W}_{imu}^k{\mathbf{e}}_{\mathbf{imu}}^{\mathbf{k}}$$

Full text

I INTRODUCTION

Aerial robots have recently seen an increased utilization in a wide variety of tasks typically reserved for humans as their flexibility makes them suitable for a diverse set of applications, while also mitigating risk to human life and lowering costs [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]. An important aspect of their application is towards performing critical tasks in GPS–denied, poorly–illuminated and sensor–degraded environments, such as underground mines and tunnels. To navigate GPS–denied environments aerial robots rely on their on–board sensing to estimate their pose. Traditionally RGB cameras have been the sensor of choice due to their low weight, cost and power requirements. However, in poorly–illuminated conditions and in the presence of airborne obscurants, such as dust, fog and smoke, the image quality of RGB cameras degrades significantly making them unreliable for pose estimation. In contrast thermal cameras, e.g. Long Wave Infrared (LWIR) sensors, are not affected by scene illumination changes or by the presence of certain obscurants [11], making them a viable sensing alternative.

However, previous approaches that utilized thermal cameras for robot pose estimation present a set of limitations. First, most previously proposed solutions have operated on re–scaled thermal images rather than utilizing full radiometric information provided by thermal cameras [12, 13, 14, 15, 16]. The motivation behind this choice relates to the fact that thermal cameras typically capture LWIR data in more than 8–bit resolution (e.g., 14–bit) but feature matching methods for visual images require the data to be re–scaled to 8–bit resolution. However, re–scaling of LWIR data results in loss of information observed in the form of lower contrast, resulting in poor feature matching performance, as shown in [17]. As a workaround, these approaches use histogram equalization techniques, usually implemented as Automatic Gain Control (AGC), to improve contrast of LWIR images and by extension improving feature matching performance. However, as AGC adapts the image according to the range of LWIR spectrum currently present in the scene, the image contrast can change significantly if hot or cold objects enter the Field–of–View (FoV) of the thermal camera. This is especially problematic for a moving camera, such as one on–board an aerial robot, as the observed view of the environment is dynamic. Another approach is to disable AGC completely and set an acceptable range of thermal information to be re–scaled into 8-bit resolution [18]. However, this requires manually setting a range for operation in a particular environment under certain thermal conditions and is also not suitable for long term navigation, because as thermal cameras operate, they accumulate sensor noise. Hence, if only a small range of thermal information is scaled to 8–bit information then the effect of noise can also be amplified, resulting in image artifacts. Thermal cameras reduce this noise accumulation by performing a periodic Flat Field Correction (FFC), during which camera operation is suspended for up to $500$ milliseconds and a uniform temperature (flat field) is presented to the camera sensor for the estimation of noise correction parameters. Application of FFC operation in itself presents two challenges. First, if only a small range of thermal information is re–scaled, the difference in image intensity values will be very different after a correction is applied. Such a large change in intensity is problematic for both direct and feature–based methods to establish robust correspondences between images. Secondly, the interruption of image data can make odometry estimates prone to drift, especially in filter–based solutions such as [19] where state estimates are progressively propagated. However, this is less of a problem for optimization–based odometry approaches that operate over a temporal window and state estimate is only propagated when new data is available and optimized against a temporal history.

Motivated by the discussion above, in this work we present a key–frame based odometry estimation approach that uses direct 14–bit radiometric data from a monocular LWIR thermal camera to establish correspondences between successive images. Working directly on 14–bit radiometric data allows our approach to overcome many of the problems associated with image re–scaling, to operate without relying on feature detection and description methods designed for visual systems, and to remain generalizable to a variety of environments under different thermal conditions. Similarly, our approach is able to remain robust against data interruption due to its optimization based estimation nature. Furthermore, we integrate measurements from an Inertial Measurement Unit (IMU) to formulate a joint cost function for our thermal–inertial odometry estimation approach. The motivation behind integrating inertial measurements is threefold. First, IMU measurements provide a transformation prior for the image alignment process. Second, they provide direct observation of two rotation states reducing the number of unobserved pose degrees of freedom from $6$ to $4$. Third, they provide a better estimation of scale in the case of monocular vision. To verify our proposed solution, a set of experiments are conducted including, a) comparison of odometry estimation of an aerial robot, in complete darkness, against ground truth provided by a VICON system, as well as, b) odometry estimation of an aerial robot traversing through an underground mine in dark and dust-filled conditions. An instance of this experiment is shown in Figure 1, and a video of the conducted experiments and the derived results can be found at https://youtu.be/-hnL5kLqT4Q.

The remainder of the paper is structured as follows: Section: II details the proposed approach, followed by the experimental evaluation results presented in Section: III. Finally, conclusions are drawn in Section: IV.

II PROPOSED APPROACH

Our proposed approach of fusing direct 14–bit radiometric data with inertial measurements for odometry estimation can be divided into a front–end component and a back–end component. This bifurcation allows us to run our odometry estimation framework in a multi–threaded manner on a modern CPU. The key responsibility of the front–end component is to perform an alignment between an incoming image to previous images in the camera coordinate frame $(\pazocal{C})$ based on the minimization of radiometric error and to initialize $3\textrm{D}$ landmarks in the world coordinate frame $(\pazocal{W})$. Given a set of $3\textrm{D}$ landmarks, the key responsibility of the back–end component is to estimate odometry by jointly minimizing the re–projection errors in landmark positions and the intra–frame inertial measurement errors over a sliding window. An overview of the approach is shown in Figure 2. Each component is detailed below:

II-A Front–End Design

In our framework, the front–end component replaces all of the tasks usually associated with feature detection, matching and pruning in feature–based approaches. Our front–end component can be divided into four sub–components, namely: i) Image Alignment, ii) Point Initialization, iii) Point Refinement, and iv) Landmark Initialization and Pruning. Each of these sub–components are discussed below.

II-A1 Image Alignment

To align and establish correspondences between two images we minimize the radiometric error between them, making our approach belong to the category of direct approaches. Historically, direct visual odometry methods based on the minimization of photometric error such as [20, 21, 22], require optimization to be performed on a large number of points and thus becoming computationally expensive. Inspired by [23], we instead track a sparse set of points for performing image alignment. Given a set of points in a reference image, we project them into the incoming image as:

[TABLE]

where $p$ and ${p}^{\prime}$ are the original and new projected point locations in pixels respectively, $K$ is the intrinsic camera matrix, $R$ and $t$ are the rotation matrix and translation vector between frames respectively and $d$ is the estimated depth of the point. As $R$ and $t$ are unknown, we estimate them by minimizing the radiometric error between points. If the depth of a point is known from its previous association to a $3D$ landmark, it is used, otherwise $t$ is estimated only up to scale. In order to make our radiometric error minimization process more robust, we calculate the error over a small neighborhood around each point:

[TABLE]

where $\mathbf{e_{radio}}$ is the squared sum of radiometric errors for a set of tracked points $(P)$ calculated over the neighborhood $(N_{i})$ of each point with $T$ representing the thermal value of each point in 14–bit resolution. A weighted Gauss–Newton optimization is then performed to estimate the transformation parameters between the two images. However, performing an optimization for a large number of points can be computationally expensive. Instead we perform our alignment operation over the levels of an image scale–space pyramid in a top–down manner, where the coarsest levels of the two images are aligned first and their transformation estimate becomes an alignment prior for the next levels. This allows the solution to converge at the lowest level of the image pyramid in very few iterations. Furthermore, we start the alignment process at the coarsest level with a transformation prior provided by the IMU, as shown in Figure 3. Once the transformation parameters between the frames are estimated, points in the reference image are projected into the new image and are considered matched points. These matched points are then used for image alignment for the next image. During the alignment process we align the incoming image to the last two incoming images and the latest two key–frames. The last two incoming images form a small sliding temporal window whereas the latest two key–frames are outside of this window and further in the past.

II-A2 Point Initialization

To choose points for tracking we first calculate image gradients over the whole image. We then divide the image into $32\times 32$ pixel blocks and calculate the median gradient $(g_{med})$ in each block. A constant offset $(g_{off})$ is added to the $g_{med}$ of each block and all gradient values in the block below this $g_{med}+g_{off}$ value are suppressed. To ensure a good distribution of points is chosen across the scene, we start by picking the points with largest non-suppressed gradient in each block and insert them into a $2\textrm{D}$ point selection grid equal in size to the image. This $2\textrm{D}$ point selection grid allows us to quickly check if the next candidate point has sufficient distance from the previously selected points without explicitly calculating a distance metric. This approach ensures that the points selected have sufficient gradient values and are well distributed across the image. We select points in this manner over the image scale–space such that the selected number of points at a given level are half in number to those in the level below it. New points are initialized in an image only when the number of tracked points falls below a certain threshold. In such a case the successfully tracked points are first inserted into the $2\textrm{D}$ grid before inserting new points. Each time new points are initialized we set that image as a key–frame.

II-A3 Point Refinement

Once a reference and a new image are aligned and pixel locations of points in the new image are determined, these pixel locations are then further refined to understand how well a point can be localized in its neighborhood. We calculate the localization quality of a point by calculating its radiometric residual along the vector of its motion in a search window centered around the point as shown in Figure 2. We then compute the ratio of the lowest residual to the second lowest residual in the search window and reject the point if this ratio is lower than a threshold. This step ensures that points that are sufficiently constrained in their location are allowed to progress, while eliminating points that due to alignment errors separate from corner or edge locations on to a uniform planar surface and become unconstrained in their location.

II-A4 Landmark Initialization and Pruning

Once point correspondences between images have been established, we attempt to initialize them as $3\textrm{D}$ landmarks. We use the OpenGV [24] library to triangulate points and check if rays emanating from points are not parallel, indicating points that are far away. If a point can be successfully triangulated, it is assigned a landmark ID. New landmarks are only initialized when image alignment is successful between current frame and the latest frame in the temporal window. However, for each successful alignment of the current frame with the oldest frame in the temporal window and the key–frames, new observations are added to the already initialized landmarks. If enough landmarks are observable between two frames, we check for the quality of landmarks by using their depth estimates to back–project their pixel locations from one frame to the other and compare the back–projected pixel location to the one obtained after the image alignment and point refinement process. Landmarks with inconsistent depth have larger error in their pixel locations and can be pruned out. The remaining landmarks, if having a minimum number of observations, are added to the optimization back–end for odometry estimation.

II-B Back-End Design

Given a set of $3\textrm{D}$ landmarks and inertial measurements between image frames we estimate the pose of the robot by solving a non–linear optimization problem that minimizes the re–projection errors of the observed landmarks, while respecting inertial constraints. Our back–end design takes inspiration from [25] and the re–projection error of a landmark can be written as:

[TABLE]

where $p_{\pazocal{C}}$ is the point coordinates in $\pazocal{C}$, $l_{\pazocal{W}}$ is the corresponding $3D$ landmark location in $\pazocal{W}$, $T_{\pazocal{C}\pazocal{I}}$ is the transformation from IMU in the IMU frame $\pazocal{I}$ to $\pazocal{C}$ and $T_{\pazocal{I}\pazocal{W}}$ is the transformation of the IMU in $\pazocal{W}$. Furthermore, to write the inertial measurement error model we first define our IMU state equations similarly to the ones described in [19, 25]:

[TABLE]

where $\mathbf{r}$ and $\mathbf{q}$ are the position and orientation of the IMU in $\pazocal{W}$ respectively, $\mathbf{v}$ is the velocity of the IMU in $\pazocal{I}$, $\mathbf{b}_{a}~{}$ and $\mathbf{b}_{\omega}~{}$ are the estimated bias of the accelerometer and gyroscope expressed in $\pazocal{I}$. The differential equations for the state elements are defined as:

[TABLE]

where $R_{\pazocal{W}\pazocal{I}}$ is the rotation matrix $\pazocal{I}\xrightarrow{}\pazocal{W}$, × represents skew-symmetric matrix of a vector, $\mathbf{g}$ is the gravity vector in $\pazocal{W}$, $w_{\star}$ are white Gaussian noise processes, while $\mathbf{\hat{a}}$ and $\mathbf{\hat{\omega}}$ represent the bias corrected proper acceleration and angular velocity and are given as:

[TABLE]

where $\tilde{a}$ and $\tilde{\omega}$ are the uncompensated IMU measurements. Therefore, the error term for inertial measurements between two frames can be written as:

[TABLE]

where $\mathbf{x}^{k}$ represents the IMU state at the acquisition time of frame $k$ and $\mathbf{\hat{x}}^{k+1}$ represents the predicted state of the IMU at the acquisition time of frame $k+1$. A forward Euler integration scheme is used to calculated the predicted IMU state. Given the re–projection and inertial measurement error equations, the cost function for the joint thermal–inertial problem can be written as:

[TABLE]

where $k$ represents the frame being processed in the sliding window $K$ containing temporal frames and key-frames, $L(k)$ represents the set of landmarks observable in frame $k$, $\mathbf{e_{reproj}}$ represents the stacked vector of re–projection errors of every landmark $l$ visible in frame $k$, $W^{k,l}_{reproj}$ represents the co–variance matrix for landmark measurements in frame $k$, and $W^{k}_{imu}$ represents the co–variance matrix for IMU measurements. This cost function is then minimized using the Google Ceres [26] optimization framework and produces an estimate of the robot pose.

III EXPERIMENTAL EVALUATION

To evaluate the performance of our thermal–inertial odometry estimation approach, a set of experiments were performed both indoors and in an underground mine using an aerial robot in challenging conditions of poor–illumination and in the presence of airborne obscurants. A video detailing the experiments can be found at https://youtu.be/-hnL5kLqT4Q

III-A System Overview

For the purpose of experimental studies a DJI Matrice 100 quad–rotor was used. An Intel NUC Core–i7 computer (NUC7i7BNH) was carried on–board the robot. A FLIR Tau 2 thermal camera was mounted on the robot to provide thermal images of $640\times 512$ resolution at $30$ frames per second. The intrinsic calibration parameters of the thermal camera were calculated using our custom designed thermal checker board pattern [18]. To provide inertial measurements a VN–100 MEMS IMU from VectorNav was employed. The camera–to–IMU extrinsics were identified based on the work in [27].

III-B Indoor Ground Truth Comparison

To evaluate the performance of our odometry solution, an experimental flight was performed in an indoor environment. To simulate the typical thermal profile of an indoor setting a computer, a room heater and some wires are introduced into the scene. A prescribed rectangular trajectory of $\textrm{length}=4.0\textrm{m}$ and $\textrm{width}=2.5\textrm{m}$ was executed 5 times in complete darkness. The estimated trajectory is compared against ground–truth provided by a VICON motion capture system. Figure 4 and Figure 5 present the derived translation and orientation results for each axis with the proposed method. The root mean squared error over the full flight including landing and take–off are presented in Table: I. It can be noted that the overall RMSE error is small.

III-C Comparison to other methods

To provide a thorough evaluation of our method, we compare the performance of our approach against three state–of–the–art visual and visual–inertial odometry estimation methods namely, OKVIS [25], DSO [23] and ROVIO [19], on thermal imagery instead of other approaches that use thermal data for odometry estimation as mentioned in Section I. This choice was made primarily for two reasons 1) None of the approaches that utilize thermal data for odometry estimation work purely on thermal imagery like our approach and instead require both visual and thermal data 2) These approaches do not make use of full radiometric information but instead work on re–scaled images making them comparable to visual odometry approaches. Therefore it is more reasonable to compare our proposed method directly against state–of–art methods.

For odometry estimation comparison with state–of–the–art methods, we re–scale thermal images from the indoor flight data–set. As the thermal camera faces the same environment constantly, instead of using AGC we use pre–defined thresholds to re–scale the images to provide better contrast. However, to be fair to these approaches, the image quality is much lower as compared to the quality of visual data. The quality is even further degraded due to accumulation of noise as shown in Figure 6, where a sharp change in contrast can be noted. Due to low image quality, all of the evaluated approaches perform poorly on such image data causing their pose estimates to diverge significantly. By trial–and–error we try to find the best parameters for each approach, however even with significant fine tuning the odometry estimates of these approaches diverge significantly as shown in Figure 7.

To provide context to the results shown in Figure 7, we discuss the strengths and weaknesses of each of these state–of–the–art methods when operating on low contrast data. OKVIS, is a key–frame based optimization approach that relies on the detection and matching of BRISK [28] features. These features perform poorly on such low contrast data and provide very few matches. Lowering of feature detection threshold forces the detection of more features, however, matching performance of remains low resulting in unreliable odometry estimation. However, it should be noted that being an optimization–based approach, OKVIS was able to work through data interruption during the application of FFC operations. ROVIO is a semi–direct filter–based method which relies on IMU data for its state propagation. ROVIO relies on image data to apply correction to its propagated state estimate. Delays in image data, as in case of thermal images, causes the state estimate to drift. As a filter–based method, ROVIO is not able to correct for drift errors. However, operating as semi–direct approach, ROVIO is able to find and track features even in low contrast images. DSO is a sparse direct odometry approach that relies on minimizing photometric error between images using a windowed optimization. Being a direct method DSO is sensitive to sudden intensity changes and constantly lost tracking during testing when operating on thermal images and hence was removed from plot in Figure 7. Continuous tracking loss forced a number re–initializations, which in turn lead to poor estimation of scale. DSO solely relies on image data for scale estimation unlike visual–inertial methods such as the method presented, OKVIS, and ROVIO that take advantage of inertial measurements for better estimation of scale. However, as DSO works on direct image data it was able to find many point correspondences even in low contrast images. This comparison study verifies the choices made during the development of our approach.

III-D Underground Mine Experiment

To demonstrate the real world application and performance of our proposed method, we conducted tests in an underground mine in the conditions of darkness and in the presence of airborne dust. To provide ground–truth markers were placed along the mine shaft with the final marker position being $50.0\textrm{m}$ away from the take–off position. For verification a hand–held test was carried out by moving the robot along a serpentine path towards the final marker location. A flight test was then carried out with the robot facing towards the end of the mine shaft throughout its flight. Figure 8 shows both of these paths as well as the marker locations. The error estimation in the direction of the mine shaft length is presented in Table: II. It can be seen that even in complete darkness and in the presence of airborne dust our approach was able to estimate robot pose with very low error.

IV CONCLUSIONS

In this paper we presented an approach to estimate robot odometry using direct radiometric data from a thermal camera. Our approach fuses direct thermal data with inertial measurements in a windowed optimization manner to overcome problems associated with long term thermal camera operation. We demonstrate the accuracy of our approach by comparing our odometry estimates for an aerial robot against the ground truth as well as state–of–the–art visual and visual–inertial odometry methods. We show the application of our method by using it for the estimation of odometry of an aerial robot deployed in an underground mine in the conditions of darkness and in the presence of airborne dust. In the future, we would focus on extending our method to perform re–localization when tracking is lost in order to enable long term operation of aerial robots in dark, dirty and dangerous environments.