Vision-Depth Landmarks and Inertial Fusion for Navigation in Degraded Visual Environments

TL;DR

This paper introduces a fusion method combining visual, depth, and inertial data to enable robust robot navigation in GPS-denied, poorly lit, and texture-less environments, demonstrated through experiments with handheld and aerial robots.

Contribution

It presents a novel tight fusion approach of multimodal sensory data at feature and sensor levels for enhanced navigation in degraded visual environments.

Findings

Reliable performance in challenging environments

Effective fusion of RGB-D and inertial data

Successful experiments with handheld and aerial robots

Abstract

This paper proposes a method for tight fusion of visual, depth and inertial data in order to extend robotic capabilities for navigation in GPS-denied, poorly illuminated, and texture-less environments. Visual and depth information are fused at the feature detection and descriptor extraction levels to augment one sensing modality with the other. These multimodal features are then further integrated with inertial sensor cues using an extended Kalman filter to estimate the robot pose, sensor bias terms, and landmark positions simultaneously as part of the filter state. As demonstrated through a set of hand-held and Micro Aerial Vehicle experiments, the proposed algorithm is shown to perform reliably in challenging visually-degraded environments using RGB-D information from a lightweight and low-cost sensor and data from an IMU.

Equations

$$\text{pazocal}{D}_s(x)=\max \left(\text{pazocal}L(x)-\sum _{\text{i=1}}^n\text{pazocal}N(i),0\right)$$

$$\text{pazocal}{C}_s(x)=\min (\gamma \text{pazocal}{V}_s(x)+\left(1-\gamma \right)\text{pazocal}{D}_s(x),s_{\text{sat}})$$

$$V\left(P_1,P_2\right)=\{\begin{array}{l}1,\text{if}{I}_1<I_2\\ 0,\text{otherwise}\end{array}$$

$$V\left(P_1,P_2\right)=\{\begin{array}{l}1,\text{if}\max \left(0,{\overline{I}}_1-I_{\text{DN}}\right)<\max \left(0,{\overline{I}}_2-I_{\text{DN}}\right)\\ 0,\text{otherwise}\end{array}$$

$$\mathbf{x}=[\underset{\text{robot states, ls}}{\underbrace{\stackrel{\text{pose, lp}}{\overbrace{\mathbf{r}\mathbf{q}}}\mathbf{\upsilon }{\mathbf{b}}_f{\mathbf{b}}_{\omega }}}\underset{\text{multimodal features states, lf}}{\underbrace{{\mathbf{\mu }}_0,\cdots {\mathbf{\mu }}_J{\rho }_0\cdots {\rho }_J}}{]}^T$$

Full text

1 INTRODUCTION

Robotic systems are being integrated in an increasingly large variety of applications such as infrastructure inspection [1, 2, 3, 4, 5], monitoring and surveillance [6], search and rescue [7] and more. However, in many critical tasks robots have to be able to cope with difficult conditions that challenge their autonomy. Of particular interest are cases involving GPS–denied Degraded Visual Environments (DVEs) that stress the abilities of onboard perception especially for Simultaneous Localization And Mapping (SLAM). Examples of relevant applications include navigating, mapping and characterization of underground settings, as well as search and rescue missions.

The focus of this work is on enabling reliable robotic autonomy in poorly illuminated, textureless and GPS–denied environments through the use of lightweight and ubiquitous sensing technology; specifically, miniaturized RGB-D sensors and Inertial Measurement Units (IMU). For this purpose we first develop a methodology for fusion of visual and depth information at the feature detection and descriptor extraction levels followed by the integration of these multimodal features with inertial sensor information in an Extended Kalman Filter (EKF) fashion. This approach reflects the fact that small RGB-D sensors have limited range, and is different from a) methods that fuse LiDAR and visual camera data in a loosely–coupled manner, b) RGB-D SLAM algorithms that depend on vision as prime sensing modality, or c) technological approaches that integrate onboard illumination or night vision.

To verify the proposed solution, a set of experimental studies were conducted including a) a proof–of–concept hand-held validation test inside a dark room where visual information is extremely limited, and b) an aerial robot trajectory in very low–light conditions. Comparison with ground truth and reconstructed maps are presented to demonstrate the reliability of the proposed localization system and its performance in such visually–degraded conditions.

The remainder of the paper is structured as follows: Section 2 provides an overview of related work. The proposed approach for common visual and depth features alongside fusion with IMU is presented in Section 3, followed by experimental evaluation in Section 4. Finally, conclusions are drawn in Section 5.

2 RELATED WORK

Methods for odometry estimation and SLAM have seen rapid growth in the recent years and employ a multitude of sensing modalities. For camera-based systems a set of feature–based [8, 9, 10], and semi–direct [11] visual odometry algorithms have been proposed. Furthermore, visual–inertial fusion approaches have presented increased robustness and reliability especially when dynamic motion is considered [12, 13]. Although visual odometry techniques have seen great growth in variety and robustness, yet the fact remains that all these techniques rely on proper scene illumination and availability of texture for their operation. On the other hand, direct depth sensors such as Light Detection And Ranging (LiDAR) and dense depth sensor are robust against illumination changes or lack of texture. Light Detection And Ranging (LiDAR) units can produce depth measurements at long ranges and return data in the form of sparse point clouds. Techniques using LiDAR along with IMU integration to generate odometry estimates have shown very robust results over long ranges [14] but tend to suffer when matched with short–range sensors and/or when they operate in structure-less environments where geometric constraints are not enough to constrain the underlying optimization process [15]. Similarly, dense depth sensors produce dense depth data at short ranges and can be easily combined with RGB images. Techniques such as [16, 17, 18], have shown good odometry estimation results when using RGB-D data. However, despite the fact that these methods take advantage of the availability of direct depth estimates, on closer inspection it can be noted that handling of depth and visual data is done separately. In [16, 17] feature detection and matching is done solely on the visual image for odometry estimation and depth data is utilized for scale estimation and mapping purposes. Similarly, visual approaches that deal with cases of poor illumination usually do not integrate any other type of information and require an external light source [19, 20]. Due to this separate handling of the two sensing modalities, the overall odometry estimation remains prone to illumination changes and lack of texture. To remedy these problems, recent work [21, 22] proposes to encode visual and depth information on the feature detection and descriptor level. Although these approaches improve the robustness in large illumination changes, they are sensitive to the quality of depth data and can become computationally burdensome for real time operations.

Motivated by the discussion above, in this work we present an EKF framework that fuses inertial, visual and depth information for odometry estimation. We use a robot-centric formulation and use inertial measurements to predict feature pixel positions between frames and use the re-projection error as an innovation term for the update step. Visual and depth information are encoded at the feature detector and descriptor level making them more robust in certain DVEs, i.e. low illumination and texture-less conditions. Only a small number of features are tracked as part of the filter state making the whole odometry computationally tractable for real time on–board robot navigation tasks. To the best of our knowledge this tightly integrated multimodal framework has no precedent in the robot odometry estimation literature.

3 PROPOSED APPROACH

Our proposed approach consists of three main components, namely a) Visual–Depth Feature generation, b) Descriptor Extraction and c) inertial fusion using an EKF, as shown in Figure 2.

3.1 Feature Detection

Upon receiving a pair of visual and depth images our method generates a combined score image that identifies keypoints in both sensing modalities by making use of the intuition that edges in both domains often lie along same image coordinates [23]. For this purpose we perform registration between the two images using the intrinsic and extrinsic calibration parameters of the cameras. For detection of features in the visual image we make use of the ORB feature detector [24] due to its robustness to image noise and scale invariant properties. We threshold detected ORB points by imposing a quality metric $\lambda$, representing the minimum acceptable Harris corner score, to ensure repeatable detection in low light conditions. We normalize the scores of the remaining visual keypoints and annotate them to create a visual score map $\pazocal{V}_{s}(x)$ which represents the score of a visual keypoint at pixel location $x$. For identification of key points in the depth image we calculate the second derivative at each pixel location by applying the Laplacian operator to the image. This allows us to identify edges in the depth domain, as well as to differentiate between points that lie near object edges as part of background or foreground since points in the background take a negative value. Next, we filter out noisy edge points by comparing the score at the pixel location to the depth error of the neighbouring pixels used in the calculation, which is a quadratic function of depth as mentioned in [25]. This operation is defined as:

[TABLE]

where $\pazocal{D}_{s}(x)$ is the score at depth pixel location $x$, $\pazocal{L}(x)$ is the result of applying Laplacian operation at the pixel location and $\pazocal{N}(i)$ is the depth error at pixel location $i$ among the $n$ neighboring pixels used in the calculation. Furthermore, to reduce the number of points along the edges and to identify corner points, we apply non-maxima suppression and only keep points with a gradient direction in the range of $30^{\circ}\leq\theta\leq 60^{\circ}$ in each quadrant. We normalize the scores to get our final depth score map $\pazocal{D}_{s}(x)$. The visual and depth score maps are then combined into a single score map which allows the method to identify multimodal keypoints and also maintain the best keypoints from each modality. This is given as:

[TABLE]

where $\pazocal{C}_{s}(x)$ represents the combined score at every pixel location, $\gamma$ defines the contribution factor of each sensing modality, and $s_{\textrm{sat}}$ is a fixed value used to saturate the value of $\pazocal{C}_{s}(x)$. The best keypoints from the combined score map are selected using an adaptive Euclidean distance constraint to ensure the method balances having enough features tracked in the filter while maintaining a distribution of features across the image frame. Figure 3 illustrates an example of this process for common feature detection.

3.2 Descriptor Extraction

Given a set of multimodal keypoints we extract a descriptor that utilizes both visual and depth information. For this purpose we chose to use the binary descriptor BRAND [21] because of its ability to encode visual and depth information, while maintaining fast performance during the descriptor extraction and matching processes. BRAND generates two binary strings that encode the visual and depth neighborhood information of keypoints individually, by performing pair–wise intensity and geometric tests on visual and depth images respectively. These bit strings are then combined using a bit–wise $OR$ operation to create a combined descriptor. To generate the visual part of the descriptor, for a pair of pixel locations $P_{1}$ and $P_{2}$ with intensities $I_{1}$ and $I_{2}$, BRAND performs the following visual comparison $V$:

[TABLE]

However, differing from the original work, our method modifies the above comparison for two reasons. First, comparing pixel intensities directly is susceptible to pixel noise and can cause erroneous bit switching in the descriptor. Secondly, in poorly illuminated conditions, visual camera sensors are susceptible to dark noise which can generate false intensity values that can introduce noise in the visual part of the descriptor. To reduce the sensitivity of the descriptor to pixel noise, instead of comparing pixel intensity values we compare mean intensity values using patches of size $9\times 9$ created around sampled pair locations. Secondly, to reduce the effect of dark noise we subtract a small intensity value, representing dark noise, from the mean intensity values of the patches before performing the pair-wise comparison. This intensity representation of dark noise $\left(I_{\textrm{DN}}\right)$ can be calculated by collecting images in a very dark environment where we expect the intensity value to be zero and deriving the mean intensity value across these images. The modified intensity comparison function for a pair of pixel locations $P_{1}$ and $P_{2}$ with mean patch intensities $\overline{I}_{1}$ and $\overline{I}_{2}$ takes the form:

[TABLE]

The $\max$ function in the above equation ensures that the minimum allowed intensity value is [math]. The depth part of the descriptor is maintained as describe in [21].

3.3 Extended Kalman Filter for IMU Fusion

We fuse inertial information with our multimodal features (landmarks) by tracking them as part of the state of an EKF, where state propagation is done by using proper acceleration $\hat{\mathbf{f}}$ and rotational rate measurements $\hat{\boldsymbol{\omega}}$ provided by an IMU. This formulation allows us to predict the feature locations between successive frames hence reducing the search space for feature matching without the need for feature mismatch pruning. As features are part of the filter state we have an estimate of their uncertainty which we utilize to dynamically scale the search patch for every feature individually for feature matching purposes. Our filter structure is similar to the one proposed in [12]. In our formulation, three coordinate frames namely, IMU fixed coordinate frame ($\pazocal{B}$), the camera fixed frame $\pazocal{V}$, and the world inertial frame $\pazocal{W}$, are used. As we register our depth image with respect to the visual image, all the depth data is expressed in the camera fixed frame $\pazocal{V}$. Our multimodal features are expressed in $\pazocal{V}$ and are parameterized using a landmark approach which models $3\textrm{D}$ feature locations by using a $2\textrm{D}$ bearing vector $\boldsymbol{\mu}$, parametrized with azimuth and elevation angles, and a depth parameter $\boldsymbol{d}$. By using this parameterization, feature locations in the camera frame and their depth estimates are decoupled which allows us to use multimodal, vision–only and depth–only features interchangeably as part of the state vector. Hence, the employed state vector with dimension $l$ and associated covariance matrix $\mathbf{\Sigma}_{l}$ is:

[TABLE]

where $l_{p},l_{s},l_{f}$ are dimensions, $\mathbf{r}$ and $\boldsymbol{\upsilon}$ are the robot-centric position and velocity of the IMU respectively, expressed in $\pazocal{B}$, $\mathbf{q}$ is the IMU attitude represented as a map from $\pazocal{B}\rightarrow\pazocal{W}$, $\mathbf{b}_{f}$ and $\mathbf{b}_{\omega}$represents the additive accelerometer and gyroscope biases respectively expressed in $\pazocal{B}$, while $\boldsymbol{\mu}_{j}$ is the bearing vector to feature $j$ expressed in $\pazocal{V}$ and $\rho_{j}$ is the depth parameter of the $j^{th}$ feature such that the feature distance $d_{j}$ is $d(\rho_{j})=1/\rho_{j}$. Given the estimation of the robot pose, this is then expressed on the world frame $\pazocal{W}$ and the relevant pose transformations are available. This enables state feedback control and allows autonomy in difficult DVE cases of darkness and broadly poor illumination and lack of texture.

4 EXPERIMENTAL EVALUATION

To evaluate the proposed solution for multimodal sensor fusion , a visual-depth-inertial perception unit consisting of a lightweight and low–cost Intel Realsense D435 Depth Camera, and a software synchronized VN–100 IMU from VectorNav was employed. Intel Realsense D435 provides RGB images as well as reliable depth information in the range from $0.75m$ to $6.0m$. Beyond intrinsics calibration, camera–to–IMU extrinsics are identified based on the work in [26]. A set of experimental studies were conducted, in particular a) a hand-held evaluation study inside a dark room, alongside b) an experiment using a small aerial robot operating in low–light conditions. For both studies, the method processes Realsense D435 data at $10\textrm{Hz}$, while IMU updates are received at $200\textrm{Hz}$.

4.1 Hand-held Evaluation

The first experimental evaluation refers to the localization and mapping inside a $7.6\times 5\times 2.3\textrm{m}$ dark room. Utilizing the Intel Realsense D435 Depth Camera and the VN–100 IMU, the method was found to be able to reliably estimate the motion trajectory and therefore allow consistent reconstruction of the $3\textrm{D}$ map. Most notably, the method maintains robustness even in very dark subsets of the environment, where visual camera data is non–informative and therefore traditional visual or visual–inertial odometry pipelines cannot cope with in a reliable manner. In these areas, furniture provides depth information that allows the framework to work. Figure 4 presents results from this study.

4.2 Micro Aerial Vehicle Flight Test

For the flight experiment, a custom–built hexarotor Micro Aerial Vehicle (MAV) is employed and has a weight of $2.6\textrm{kg}$. The system relies on a Pixhawk–autopilot for its attitude control, while integrating an Intel NUC5i7RYH and executing a complete set of high–level tasks on-board with the support of the Robot Operating System (ROS). For the purposes of position control, a Linear Model Predictive Control strategy has been deployed following previous work in [27]. The robot integrates the same perception unit, i.e., the Intel Realsense D435 Depth Camera and the VN–100 IMU. An instance of the robot during the flight experiment is shown in Figure 1.

The conducted experimental study relates to that of tracking a prescribed rectangle trajectory ($\textrm{length}=4.8\textrm{m}$, $\textrm{width}=1.95\textrm{m}$) in a low–light environment. The real–time estimated trajectory is compared against ground–truth information provided by a VICON Motion Capture system. Figure 5 presents the derived results with the proposed method running on–board the MAV, alongside the reconstructed map of the environment. As shown, the derived trajectory is mostly on par with the ground–truth data and therefore reliable mapping is also achieved. Figure 6 presents an error plot for each axis of the robot trajectory. The video of the experiment can be seen at (https://tinyurl.com/DVEResults)

5 CONCLUSIONS

A method for common visual and depth data features alongside their fusion with IMU cues in order to enable autonomous localization in degraded visual environments and specifically low–light, dark, and textureless conditions was proposed. The focus is on an approach that exploits lightweight and ubiquitous RGB-D sensors and therefore can be integrated onboard small systems such as Micro Aerial Vehicles. A set of experimental evaluation studies are presented and demonstrate the ability of the system to provide reliable localization and mapping data in sensing–degraded conditions of darkness and low–light flight.