FastFusion: Real-Time Indoor Scene Reconstruction with Fast Sensor Motion

Abstract

Real-time 3D scene reconstruction has attracted a great amount of attention in the fields of augmented reality, virtual reality and robotics. Previous works usually assumed slow sensor motions to avoid large interframe differences and strong image blur, but this limits the applicability of the techniques in real cases. In this study, we propose an end-to-end 3D reconstruction system that combines color, depth and inertial measurements to achieve a robust reconstruction with fast sensor motions. We involved an extended Kalman filter (EKF) to fuse RGB-D-IMU data and jointly optimize feature correspondences, camera poses and scene geometry by using an iterative method. A novel geometry-aware patch deformation technique is proposed to adapt the changes in patch features in the image domain, leading to highly accurate feature tracking with fast sensor motions. In addition, we maintained the global consistency of the reconstructed model by achieving loop closure with submap-based depth image encoding and 3D map deformation. The experiments revealed that our patch deformation method improves the accuracy of feature tracking, that our improved loop detection method is more efficient than the original method and that our system possesses superior 3D reconstruction results compared with the state-of-the-art solutions in handling fast camera motions.

1. Introduction

More and more real-time 3D reconstruction techniques have been proposed in recent years, with the development of the depth-sensing and parallel computing techniques. Real-time 3D reconstruction can be applied in many real applications. It could be used to generate high-fidelity 3D scene models for real-time interactions in augmented reality (AR) and offline highly realistic modeling and rendering in virtual reality (VR). For instance, a user could reconstruct his/her home and preview the home layout before purchasing the furniture [1]. It could also be used in robotics to help the robot to better navigate and explore the environment.

Many previous works [2,3,4,5,6] have demonstrated promising performances for real-time indoor scene reconstruction. However, they require some assumptions, such as slow camera motions, a static scene without dynamic objects, abundant texture information or invariant illumination. In many real cases, these assumptions cannot be satisfied [7,8]. In fact, it is important and valuable to robustly reconstruct a 3D scene without any idealistic assumptions.

In this study, we mainly focus on the assumption of camera motions. In traditional vision-based 3D reconstruction systems, camera poses are estimated by finding feature correspondences between consecutive frames, and 3D maps of each frame are then fused by the camera poses. For slow sensor motions, it is relatively easy to find correspondences between consecutive frames (either depth frames or color frames), since the corresponding features in two consecutive frames are similar to each other and their positions should also be similar, leading to a small searching space. As a consequence, camera poses can be accurately estimated in real time. However, during fast camera motions, color information will be severely degraded by image motion blurs, leading to the failure of traditional feature-based camera tracking and loop closure. The depth image is not that sensitive to fast camera motions, but the depth image is not enough to estimate the large interframe camera motions either, as the iterative closest point (ICP) algorithm used for depth-based camera pose estimation requires small interframe motions. With fast camera motions, the ICP algorithm will be liable to trap in a local optimum.

Performing image motion deblur to acquire sharp images is a theoretically feasible way to estimate fast camera motions, as well as to detect loops. However, deblurring an image and estimating camera motions is the chicken and egg problem. To perform image deblur, camera motions are required to calculate the blur kernels [9]. On the other hand, estimating accurate camera motion requires a sharp image to build correct feature correspondences.

We solve the problem by involving an inertial measurement unit (IMU), which is a device that measures its linear acceleration and angular velocity, and a geometry-aware feature tracking approach is proposed to accurately track patch features under fast sensor motions. With an extended Kalman filter (EKF) framework effectively integrating the multi-mode information of IMU, color and depth, the robust camera pose estimation and geometry fusion of an indoor scene are achieved in real time.

Compared to our preliminary version [10], which does not have a loop closure module, this paper further achieves globally consistent 3D reconstructions for both slow and fast sensor motions using a novel loop closure method. To achieve real-time loop closure, we applied a submap-based mechanism to close a loop between two submaps rather than all voxels. Additionally, we propose a depth-only loop detection method based on the keyframe encoding [11]. Our loop detection method is not affected by blurry color images and is able to detect loops under fast sensor motions. By performing the detection process on the keyframe lists of candidate submaps, our method achieves a more efficient and accurate loop detection than the method in [11]. To demonstrate the advantage of our system with fast camera motions, we also detail the definition of the speed of a sequence and list the speed information of public datasets. Additional experiments were performed in this version to compare our system with state-of-the-art methods, as well as the system in our preliminary version, for both slow and fast datasets.

In summary, the main contributions of our work are as follows:

(1) We present an RGB-D-inertial 3D reconstruction framework that tightly combines the three kinds of information to iteratively refine camera pose estimation using an extended Kalman filter.

(2) We present a geometry-aware feature-tracking method for handling fast camera motions that utilizes patch features to adapt blurred images and considers the deformation of patches in building feature matching for images with very different perspectives.

(3) An improved keyframe encoding method is proposed for loop detection, which uses submaps and the prior camera pose to increase the accuracy, as well as the computational efficiency, and is also able to detect loops in fast camera motions due to the use of the depth image only.

The experimental results demonstrate that our method is on par with state-of-the-art methods in slow camera motions, while it possesses a superior performance in fast camera motions. The ablation study also shows that our improved keyframe encoding method achieves a better accuracy and computational efficiency for loop detection. An example of our reconstruction result is shown in Figure 1.

2. Related Work

The technique in this paper mainly focuses on indoor scene reconstruction under fast camera motion. In this section, we discuss traditional 3D reconstruction techniques, as well as techniques related to the estimation of fast camera motion.

2.1. 3D Reconstruction

There have been many impressive works on 3D reconstruction in recent years, and we analyze these works based on the use of different sensors. Methods such as KinectFusion [12] and InfiniTAM [3] only use depth information to reconstruct the 3D model, and estimate the camera pose from distance data by variants of the iterative closest point (ICP) algorithms [13,14]. However, depth-only camera tracking is brittle in geometry-less scenes and bright windows, as well as depth sensor noises. Camera tracking has made breakthrough progress in monocular RGB methods, including direct methods [15,16,17] and feature point methods [18,19,20,21]; these methods cannot reconstruct the detailed 3D model, whereas the reconstruction of detailed and dense 3D models is the purpose of this paper. Furthermore, Refs. [2,3,4,5,22] use both color and depth information to estimate camera motions and generate dense 3D models based on implicit truncated signed distance functions (TSDFs) or surfel representation. Different from them, Ref. [23] reconstructs large-scale scenes by merging images and laser scans. Although these approaches are state-of-the-art in terms of 3D indoor scene reconstruction, they still can only work on situations that strictly follow the assumptions, such as static scenes without dynamic objects, sufficient texture, geometrical information and slow camera motions, as well as invariant illumination.

These assumptions are invalid in many applications, so we have to improve the system’s robustness rather than establish more assumptions. Some researchers have noticed these problems and have published impressive works. For example, MixedFusion [24] focuses on dynamic scenes reconstruction, which decouples dynamic objects from static scenes and estimates both object motions and camera motions without any prior motion, such as a scene template [25] or motion model [26]. DRE-SLAM [27] builds an OctoMap for the static scene and the dynamic scene, respectively, with information of a RGB-D camera and two wheel-encoders. Refs. [28,29] allow multiple users to synergistically reconstruct a 3D scene rather than requiring only one input sequence for the reconstruction. This method does not require GPUS. Unlike traditional methods that rely on a simplified parallel lighting model, Refs. [30,31] estimate a lighting function from images captured under different illuminations. Ref. [32] reconstructs scenes with mirror and glass surfaces by detecting the mirrors and glasses from RGB-D inputs beforehand.

2.2. Estimation of Fast Camera Motion

The estimation of fast camera motions is a tough challenge. The fast camera motion not only blurs the color images but also induces the failure of the depth-based estimation method. For example, the fast interframe camera motion makes the real-time ICP algorithm easily fall into a local optimum.

Pushing up the frame-rate of the color camera could reduce the affection of image motion blur. However, we cannot arbitrarily increase the camera frame-rate due to the trade-off among the signal–noise ratio, motion blur and computational cost [33,34].

Hee Seok Lee et al. [35] and Haichao Zhang et al. [9] minimize the undesired effect of motion blur by deblurring images before feature extraction. However, these methods cannot be adopted in a real-time 3D reconstruction system, considering the computational cost.

Adopting the image patch matching could be a solution to adapt the blurs of color images, because the consecutive image frames could have a similar blurry effect that induces very closer pixel values. Traditional patch matching methods, such as [36], match patches by measuring the photometric error. Recently, Refs. [37,38,39] proposed modern neural networks to match patches using the extracted descriptors of each patch. However, the above methods cannot match patches in varied viewpoints. Ref. [40] achieves the matching of patches in varied viewpoints by proposing CNNs that learn a consistent descriptor for a patch, even in very different viewpoints.

Integrating IMU information is a way to effectively estimate the camera poses with fast motions. Refs. [3,41,42] use the rotation calculated by IMU as the initialization of the camera pose estimate. Tristan et al. [43] present the complete RGB-D-Inertial SLAM system based on ElasticFusion. Ref. [44] tracks camera motions via IMU pre-integration. Ref. [45] is devoted to the use of low-cost IMU sensors by calculating the acceleration bias as a variable during the initialization process. However, all of them fail to solve the influence of fast camera motions for real-time 3D reconstruction.

3. Overview

The pipeline of our system is illustrated in Figure 2. In our system, we tightly combined color, depth and inertial information for real-time indoor scene 3D reconstruction, and the system operated in an extended Kalman filter framework. We also propose a depth image encoding method and submap-based pose graph optimization for the loop closures under fast camera motions.

Kalman prediction (Section 4.2.1) was performed to predict the current camera motion by the camera state of the previous frame and the IMU measurements of the current frame. In the Kalman update step, we first performed feature tracking between two consecutive frames. During fast camera motions, the shape and appearance of an image patch may vary in two consecutive frames. We named this phenomenon the shrink and extend effect (SE effect), and we evaluated this SE effect for all patches by their intensity and geometric information (Section 4.1). The patches affected by the SE effect are deformed by our geometry-aware patch deformation for improved feature tracking. Then, we iteratively updated the camera pose and deformed the patches.

In the reconstruction process, we separated the reconstructed model into several small models called submaps (Section 4.3). When a new submap is created, the subsequent frames are fused into the submap and the global pose of the submap is initialized by its first frame. Patch features are updated after the fusion of the current frame; their geometries are refined by the so-far reconstructed 3D model.

In the loop closure module, the current depth image is encoded by a randomized fern encoding method, and compared with lists that store the codes of historical depth images. A loop is detected once an image that belongs to a historical submap successfully matches the current frame; then, a link between the two submaps is generated with the loop constraint. After that, we performed the submap-based pose graph optimization process to close the loop. Finally, the global 3D model of an indoor scene was rendered by fusing all submaps.

4. Method

We first introduce our novel geometry-aware feature-tracking method, which is the key to accurately estimate camera poses under fast sensor motions. We did not directly perform the feature tracking, but ran it in an extended Kalman filter framework, which integrates the IMU information to better achieve the sensor motion estimation. Then, we used the estimated camera pose to fuse the current depth into the current submap and update the features for the tracking of the next frame. Finally, we introduced our loop closure module, which detects loops during fast sensor motions and closes loops by deforming the submap-based geometry model.

4.1. Geometry-Aware Feature Tracking

During fast sensor motions, color images are severely blurred. Feature point methods such as ORB-SLAM [18] cannot detect enough feature points in the blurry images. Patch-based direct methods such as [36,46] outperform feature point methods in terms of the robustness of feature tracking on blurry images because the image intensities change similarly in two consecutive blurry images. However, patches may contain multiple objects with different depths, and, thus, the shape and appearance of a patch may change in interframes as fast camera motions generate larger perspective changes, leading to inaccurate feature tracking. As a consequence, a feature tracking method that considers the changes will improve the tracking performance.

We handled this by combining depth information with color to back-project 2D patches in one frame into 3D space, and then project them to the pixel coordinate by the camera pose of the next frame. The projection helps to deform the original patches to model the shape and appearance changes, and the patch tracking can be easily and accurately achieved by the deformed patches. To perform this in real time, we investigated the possible deformations, classified them into different cases and handled them separately.

4.1.1. SE Effect for Patch Deformation

During fast camera motions, an image patch feature is observed in frames with very different viewports, and, thus, the shape and appearance of the patch feature in the image domain vary from different frames. To model the deformations of patch features in different frames, different from Bloesch et al. [47], who only consider the 2D planar information of patches, we involved the 3D geometry of a patch to determine the 2D shape deformations of patches in interframes.

Because of the different 3D shapes of patches and the irregularity of camera motions, patches may produce different deformations in consecutive frames. Figure 3 shows the three representative cases of patch deformations:

• Case 1. When the depths of a patch are consistent, the shape and appearance of the patch will not be influenced by the camera motions in interframes.
• Case 2. In a static scene with an uneven surface, the shape of patches in interframes will not change in slow camera motions because the occlusion does not change over two consecutive frames with almost the same camera views.
• Case 3. Different from case 1 and case 2, if the large depth variance in a patch is simultaneous with the aggressive camera motion, the intensity distribution and the shape of the patch will change. As shown in Figure 3, if the camera moves from ${\mathbf{V}}_0$ to ${\mathbf{V}}_2$, the yellow region is occluded and the patch shrinks in the frame of ${\mathbf{V}}_2$. In addition, the black region occluded in ${\mathbf{V}}_0$ is visible once the camera moves to ${\mathbf{V}}_1$, and thus the patch shape extends in the frame of ${\mathbf{V}}_1$. We named these phenomena as the shrink effect and extend effect (SE effect).

From the above cases, the patch deformation in interframes is mainly caused by the changes in occlusion. The camera rotation (yaw, patch, roll) will not generate the patch deformation, as the occlusion does not change in rotation-only camera motion.

Matching patches that are severely affected by the SE effect will generate a large margin of error. Thus, we designed a geometry-aware patch deformation method to handle the SE effect. The process of the deformation method is shown in Figure 4 and is formularized as follows.

Each pixel i in a patch of frame $k-1$ is defined as $P_i^{k-1}:=({\mathbf{p}}_i^{k-1},I_i^{k-1},d_i^{k-1},{\mathbf{n}}_i^{k-1})$, where ${\mathbf{p}}_i$ and $I_i$ denote the pixel coordinate and the intensity of the pixel i. $d_i^{k-1}$ and ${\mathbf{n}}_i^{k-1}$ denote the depth and the 3D normal of the pixel i in frame $k-1$. Note that the depth and the normal are from the so-far reconstructed model rather than the raw depth image of frame $k-1$.

We first back-projected each pixel into the camera coordinate to obtain the local vertex position:

$${\mathbf{v}}_i^{k-1}={\pi }^{-1}\left(P_i^{k-1}\right),$$

(1)

where $\pi (·)$ is projecting a point from the 3D space to the 2D pixel coordinate, and ${\pi }^{-1}(·)$ denotes the inverse operation. Then, we projected ${\mathbf{v}}_i^{k-1}$ to the pixel coordinate of frame k:

$${\widehat{\mathbf{p}}}_i^k=\pi \left({\widehat{\mathbf{v}}}_i^k\right)=\pi \left({\mathbf{T}}_k^{-1}{\mathbf{T}}_{k-1}{\mathbf{v}}_i^{k-1}\right).$$

(2)

Here, ${\mathbf{T}}_k$ is the camera pose of the k-th frame, and ${\mathbf{T}}_k\mathbf{v}={\mathbf{R}}_k\mathbf{v}+{\mathbf{t}}_k$ ($3\times 3$ rotation matrix ${\mathbf{R}}_k$, $3\times 1$ translation vector ${\mathbf{t}}_k$) transforms a 3D point from a local camera coordinate of the k-th frame to the global coordinate. Note that the global coordinate is defined to be the camera coordinate of the first frame. The camera pose ${\mathbf{T}}_k$ is to be solved and affects the 2D position of the projection.

We used ${\widehat{P}}_i^k$ to represent the pixel that is projected from frame $k-1$ to frame k, not originally from frame k. The projected pixel coordinate ${\widehat{\mathbf{p}}}_i^k$ belongs to the pixel ${\widehat{P}}_i^k$, and its depth value was set as the z-coordinate of ${\widehat{\mathbf{v}}}_i^k$. The intensity ${\widehat{I}}_i^k$ and the normal ${\widehat{\mathbf{n}}}_i^k$ were directly set as the values of $P_i^{k-1}$.

As shown in Figure 4, the SE effect can be evaluated after the projection. In the pixel coordinate, when the distance between two projected pixels ${\widehat{P}}_i^k$ and ${\widehat{P}}_j^k$ is smaller than a half-pixel distance, the pixel with the greater depth is covered by another one, and the shrink effect occurs in the patch of frame k. Therefore, the pixel with the greater depth value should be removed from the projected patch. The extend effect was evaluated by comparing the height and width between the projected patch and the original one. Note that the projected patch was also set as a rectangle for easy operation, and that empty pixels were marked and assigned as black.

If neither of these effects appear after projection, we considered the patch to not be affected by the SE effect. Note that the SE effect evaluation and the patch deformation are initialized by the camera pose predicted from the Kalman prediction, and are iteratively updated in the Kalman update. We specify these steps in Section 4.2.

4.1.2. Objective

In feature tracking, patches affected by the SE effect are replaced with the corresponding deformed patches. Inspired by Jaesik Park et al. [48], who presented an algorithm for aligning two colored geometry maps, we tracked the projected patch features by both the intensity and depth information of them.

The photometric error of each deformed patch was computed as follows. Firstly, pixel $P_i^k$ of a patch in the current frame k was extracted by the pixel coordinate ${\widehat{\mathbf{p}}}_i^k$ of the corresponding pixel ${\widehat{P}}_i^k$ of the deformed patch. Then, we calculated the intensity difference between the extracted patch and the projected patch. The photometric error can be formularized as:

$$E_p\left({\mathbf{T}}_k\right)=\sum _i^Y{∥I\left[P_i^k\right]-\widehat{I}\left[{\widehat{P}}_i^k\right]∥}_2^2,$$

(3)

Y is the valid pixel amount of a patch. The geometric errors were calculated by the point-to-plane ICP algorithm:

$$E_g\left({\mathbf{T}}_k\right)=\sum _i^Y{∥{\left({\mathbf{T}}_{k-1}^{-1}·{\mathbf{T}}_k·({\mathbf{v}}_i^k-{\widehat{\mathbf{v}}}_i^k)\right)}^{\top }·\widehat{\mathbf{n}}\left[{\widehat{P}}_i^k\right]∥}_2^2,$$

(4)

where ${\mathbf{v}}_i^k$ and ${\widehat{\mathbf{v}}}_i^k$ are the vertexes of the corresponding pixels in the camera coordinate of the k-th frame. Given $E_p$ and $E_g$, the cost function for patch tracking was formulated as:

$$E\left({\mathbf{T}}_k\right)=\lambda \sum _j^M{E}_p^j\left({\mathbf{T}}_k\right)+(1-\lambda )\sum _j^M{E}_g^j\left({\mathbf{T}}_k\right),$$

(5)

with $\lambda =0.1$, which is similar to related work [2]. M denotes the patch amount, and j is the index of a patch.

4.2. Extended Kalman Filtering Framework

We did not directly solve for the energy minimization of Equation (5). Instead, we used it in our extended Kalman filter (EKF) framework. Our EKF framework aims to tightly combine the color, depth and inertial measurements. To be specific, we modeled the camera pose of each frame as the state of the EKF. The observations of the EKF are color and depth images. The relationship between the state and the observations was measured by the energy defined in Equation (5). If a state fits exactly to the observation, the energy is zero. On the other hand, the inertial information was used in the Kalman prediction step, which serves to build the motion prediction model.

We followed the traditional extended Kalman filter to define the variables. A nonlinear discrete time system with state $\mathbf{x}$, observation term $\mathbf{z}$, process noise $\omega \sim N(0,\mathbf{Q})$ and update noise $\mu \sim N(0,\mathbf{U})$ in the k-th frame can be written as:

$${\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\omega }_k),$$

(6)

$${\mathbf{z}}_k=h({\mathbf{x}}_k,{\mu }_k).$$

(7)

In our framework, the state of the filter was composed of the following elements $\mathbf{x}:=(\mathbf{R},\mathbf{t},\mathbf{v},{\mathbf{b}}_a,{\mathbf{b}}_g)$, where $\mathbf{v}$ is the camera linear velocity and ${\mathbf{b}}_a,{\mathbf{b}}_g$ are biases of IMU (accelerometer and gyroscope); process noise $\omega :=({\mathbf{n}}_a,{\mathbf{n}}_w,{\mathbf{n}}_{b_a},{\mathbf{n}}_{b_w})$, where ${\mathbf{n}}_a,{\mathbf{n}}_w$ are noises of IMU and ${\mathbf{n}}_{b_a},{\mathbf{n}}_{b_w}$ are white Gaussian noises of IMU’s biases that are modeled as a random walk process. $\mathbf{Q}$ is the covariance matrix of IMU’s noises and biases, which is derived by the offline calibration using Kalibr [49,50]. In the following, we use the symbol $\tilde{}$ to represent values calculated from the Kalman prediction step, such as the predicted state $\tilde{\mathbf{x}}$.

4.2.1. Kalman Prediction

Given the a posteriori estimation ${\mathbf{x}}_{k-1}$ with covariance ${\mathbf{P}}_{k-1}$, the prediction step of the EKF yields an a priori estimation at the next frame k:

$${\tilde{\mathbf{x}}}_k=f({\mathbf{x}}_{k-1},0),$$

(8)

$${\tilde{\mathbf{P}}}_k={\mathbf{F}}_k{\mathbf{P}}_{k-1}{\mathbf{F}}_k^{\top }+{\mathbf{G}}_k\mathbf{Q}{\mathbf{G}}_k^{\top },$$

(9)

with the Jacobians:

$${\mathbf{F}}_k={\left.\frac{\partial f}{\partial \mathbf{x}}\right|}_{{\mathbf{x}}_{k-1},0},{\mathbf{G}}_k={\left.\frac{\partial f}{\partial \omega }\right|}_{{\mathbf{x}}_{k-1},0}.$$

(10)

The key in the Kalman prediction step is to define the function f. In our work, we employed the inertial measurements in the definition. Assuming IMU is synchronized with the camera, and acquires measurements with a time interval $\tau$, which is much smaller than that of the camera, then, similar to [47,51], the IMU pre-integration method [52] was performed to integrate the IMU measurements acquired between two consecutive frames:

$$\Delta \mathbf{R}=\prod _{n=1}^N\mathrm{Exp}({\mathbf{w}}_n·\tau )$$

(11)

$$\Delta \mathbf{v}=\sum _{n=1}^N\Delta {\mathbf{R}}_n·{\mathbf{a}}_n·\tau$$

(12)

$$\Delta \mathbf{t}=\sum _{n=1}^N(\Delta {\mathbf{v}}_n\tau +\frac{1}{2}\Delta {\mathbf{R}}_n·{\mathbf{a}}_n·{\tau }^2),$$

(13)

where $\mathrm{Exp}(·)$ denotes the exponential map from Lie-algebra to Lie-group. $\mathbf{a},\mathbf{w}$ are the bias-corrected linear acceleration and angular velocity of IMU, and are already transformed to the camera coordinate via the extrinsic matrix calibrated offline [49,50]. N denotes the number of inertial measurements acquired between two consecutive camera frames. $\Delta {\mathbf{R}}_n,\Delta {\mathbf{v}}_n$ represent the integration results from the first measurement to the n-th (rather than N-th). Details about this pre-integration can be found in [52].

Finally, the state of k-th frame predicted in the Kalman prediction step was formulated as:

$$\begin{array}{c}\hfill {\tilde{\mathbf{R}}}_k={\mathbf{R}}_{k-1}·\Delta \mathbf{R},{\tilde{\mathbf{v}}}_k={\mathbf{v}}_{k-1}+\mathrm{g}N\tau +{\mathbf{R}}_{k-1}\Delta \mathbf{v},\\ \hfill {\tilde{\mathbf{t}}}_k={\mathbf{t}}_{k-1}+{\mathbf{v}}_{k-1}N\tau +\frac{1}{2}\mathrm{g}{\left(N\tau \right)}^2+{\mathbf{R}}_{k-1}\Delta \mathbf{t},\\ \hfill {\tilde{\mathbf{b}}}_{{\mathbf{a}}_k}={\mathbf{b}}_{{\mathbf{a}}_{k-1}}+{\mathbf{n}}_{b_a},{\tilde{\mathbf{b}}}_{{\mathbf{w}}_k}={\mathbf{b}}_{{\mathbf{w}}_{k-1}}+{\mathbf{n}}_{b_w},\end{array}$$

(14)

where $\mathrm{g}$ is the gravity acceleration in the global coordinate. The predicted camera state is used in Section 4.1.1 for the initialization of patch deformation. The Jacobian calculations in Equation (10) are complicated but a tractable matter of differentiation, and are similar to the ones in [51]; for brevity, we do not present the results here.

4.2.2. Kalman Update

In a traditional Kalman update step, the measurement residual is modeled as:

$${\mathbf{y}}_k={\mathbf{z}}_k-h({\tilde{\mathbf{x}}}_k,\mathbf{0}).$$

(15)

Here, $\mathbf{0}$ means that we directly used ${\tilde{\mathbf{x}}}_k$ to calculate the residual without adding any Gausian noise. The updated state was formulated as:

$${\mathbf{x}}_k={\tilde{\mathbf{x}}}_k+{\mathbf{K}}_k·{\mathbf{y}}_k,$$

(16)

where ${\mathbf{K}}_k$ is the Kalman gain. In our method, we defined the residual as the photometric and geometric errors of patches (Equation (5)); thus, the residual is:

$${\mathbf{y}}_k=0-E\left({\tilde{\mathbf{T}}}_k\right)=-E\left(\left[\begin{array}{cc}{\tilde{\mathbf{R}}}_k& {\tilde{\mathbf{t}}}_k\\ \mathbf{0}& 1\end{array}\right]\right).$$

(17)

The patch deformations used in calculating ${\mathbf{y}}_k$ by Equation (17) were greatly affected by the camera poses. Therefore, after acquiring an updated camera pose by Equation (16), we used the pose to iteratively derive the deformations of patches, and refined the camera pose again by Equation (16). Specifically, we used m to denote the iterations, and, thus, we had:

$$h({\mathbf{x}}_k^m,0)=E\left(\left[\begin{array}{cc}{\mathbf{R}}_k^m& {\mathbf{t}}_k^m\\ \mathbf{0}& 1\end{array}\right]\right),$$

(18)

and the Kalman gain with respect to each iteration is:

$${\mathbf{K}}_k^m={\tilde{\mathbf{P}}}_k·{\left({\mathbf{H}}_k^m\right)}^{\top }·{\left({\mathbf{S}}_k^m\right)}^{-1}$$

(19)

$${\mathbf{S}}_k^m={\mathbf{H}}_k^m·{\tilde{\mathbf{P}}}_k·{\left({\mathbf{H}}_k^m\right)}^{\top }+{\mathbf{J}}_k^m·\mathbf{U}·{\left({\mathbf{J}}_k^m\right)}^{\top }.$$

(20)

$\mathbf{U}=\mathrm{diag}({\sigma }_u^2,{\sigma }_v^2,{\sigma }_d^2)$ is the covariance matrix of $3\times 1$ image noise ${\mu }_k$, which is assigned by the depth image model of [53]. In addition, the Jacobians updated in each iteration are

$${\mathbf{H}}_k^m={\left.\frac{\partial h}{\partial \mathbf{x}}\right|}_{{\mathbf{x}}_k^m,0},{\mathbf{J}}_k^m={\left.\frac{\partial h}{\partial \mu }\right|}_{{\mathbf{x}}_k^m,0},$$

(21)

where the calculation of ${\mathbf{H}}_k^m$ is equal to the Jacobian calculation of $R_k^m,t_k^m$ in Equation (5). Then, the updated state of each iteration was calculated as follows:

$${\mathbf{x}}_k^{m+1}={\mathbf{x}}_k^m-{\mathbf{K}}_k^m·h({\mathbf{x}}_k^m,\mathbf{0}).$$

(22)

Notice that ${\mathbf{x}}_k^0$ is initialized by ${\widehat{\mathbf{x}}}_k$. The covariance matrix is only updated after the convergence:

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k·{\mathbf{H}}_k)·{\tilde{\mathbf{P}}}_k$$

(23)

4.3. Model Fusion and Patch Update

Similar to [3], we used TSDF [54] and a hierarchical hashing branch [55] to incrementally fuse each depth image by the camera poses obtained from the Kalman update. To perform a non-rigid deformation for the TSDF-represented geometry model in loop closure, we used the submap mechanism introduced in [56] to build the whole model via several small submaps $\mathcal{M}$. We generated a new submap once the camera moved far away from the “central point” of the current submap, which was implemented by checking whether the “central point” could be projected into the current camera viewport. The “central point” is the geometry center of the current submap and was online updated with the fusion. The subsequent depth frames were fused to the newly generated submap, and the global pose of the new was is initialized by the camera pose of its first frame. Similar to [56], we also used a pose graph to represent the submaps, where the pose of each submap was denoted as a node and two consecutive submaps were linked with one edge. Note that the pose of a submap is updated by loop closure (described in the next section).

After the fusion of the current frame, we updated the patch features for the subsequent tracking. Firstly, features with great intensity errors should be excluded. Therefore, we excluded those bad features by comparing the average pixel intensity error of the patches. Then, we utilized the FAST corner detector [57] and the number of pixels with available depth information to add new features with distinct intensity gradients and sufficient depth information. Note that the SE patches were also replaced by their normal patches. Finally, the intensity information of the patch features was updated by the current color image, and their depth information was set by the fused geometry, which has better quality than the current depth image.

4.4. Loop Closure

4.4.1. Loop Detection

During fast camera motions, color images are severely blurred, leading to the failure of the color-based loop detection methods, such as [58,59]. Fortunately, depth images captured by the structured-light depth sensor are insensitive to fast sensor motions, thanks to the high frame-rate of the IR projector (more than 1000 FPS). Therefore, we utilized the keyframe encoding method [11] to only encode depth images for the detection of latent loops.

The original keyframe encoding method compares the code of the current frame with codes of all so-far harvested keyframes to detect loops and harvest keyframes. Its processing time grows with the number of keyframes, which goes against the real-time performance of the reconstruction system for long-term large-scale indoor scene reconstruction. In our work, we stored image codes separately into the code lists of their corresponding submaps, and a new code was only compared with the codes of the same submap. This design greatly reduces the computation cost by matching the code of the current frame only with the lists of nearby submaps. In addition, our method also improves the detection accuracy by removing outliers implicitly. Note that outliers are usually from regions with similar geometries, such as different corners in a room.

The encoding process is similar to Glocker et al. [11]; we utilized the randomized ferns to encode a binary code B for a depth image. The binary code consisted of N four-bit binary code blocks b, and each code block was calculated by a random pixel. The dissimilarity between the current frame I and a keyframe J was calculated via the Hamming distance:

$$\mathrm{BlockHD}(B^I,B^J)=\frac{1}{N}\sum _{i=1}^N{b}_i^I\equiv b_i^J$$

(24)

where the operation ≡ returns 0 only if two code blocks are identical. In our experiment, N was assigned as 1000.

Since we maintained one list for each submap, a code $B^I$ of the current frame I was stored in the code list $\left\{{\mathcal{B}}_i^I\right\}$ of the current submap ${\mathcal{M}}_i$. During the loop detection of each frame, we only matched the current frame with the candidate keyframes in lists that belong to the k nearest submaps. The nearest submaps were selected by comparing the poses of the current frame and each submap. We set $k=5$ by considering the experimental results, as well as our practical experience. If the smallest dissimilarity is higher than a threshold ${\sigma }_1=0.1$, then the current frame is added as a new keyframe and the code is included in the code list of the current submap. Otherwise, a loop is successfully detected when the smallest dissimilarity (except the ones of the current submap) is lower than the threshold ${\sigma }_2=0.05$.

4.4.2. Pose Graph Optimization

Once a loop was successfully detected between the current submap and a previous submap, we relocated the camera pose of the current frame via ICP between the current frame and the previous submap [11]. Note that the ICP was initialized by the pose of the matched keyframe in loop detection. A new edge between the two submaps was also added in the pose graph (Section 4.3) by comparing the camera poses of the two submaps. As the pose graph was updated, we used the pose graph optimization [60] to close the detected loop.

5. Experiments

In this section, we first introduce the hardware information of our platform and our computational performance. Then, we define the speed of sensor motion in a recorded sequence. After that, we evaluate the effectiveness of our geometry-aware feature tracking and the importance of integrating the inertial information by sequences with different speeds, as well as the accuracy and efficiency of our loop detection method. Next, we compare our indoor scene 3D reconstruction method with state-of-the-art methods in terms of both quantitative and qualitative experiments.

5.1. Performance and Parameters

All experiments were performed in a laptop with Intel Core i7-7820 HK CPU at 2.9 GHz, 32 GB of RAM and GPU GeForce GTX 1080 with 8 GB memory. We exhibit the computational performance of our system in Figure 5. The overall processing time of each frame does not increase with the frame number thanks to our improved loop detection method. We allocated voxel blocks with voxel size 0.005 m × 0.005 m × 0.005 m; thus, the size of one voxel block was 0.04 m × 0.04 m × 0.04 m, and we pre-allocated 16,384 blocks for each submap.

5.2. Speed Definition

The TUM RGB-D dataset [61] sets the average sensor motion speed of a sequence as the speed of the sequence. However, we find that it is the fastest part of a sequence that defines the real difficulty for camera tracking, so we plan to use the fastest camera motion speed to define a sequence as ‘fast’ or ‘slow’. In practice, we did not directly use the fastest speed value but the value at the top 10% because the obtained camera motion speed is not very accurate and thus the fastest speed may be easily affected by errors. Then, we empirically set a sequence belonging to ‘fast’ as long as its linear speed is greater than 1 m/s or its angular speed is greater than 2 rad/s; as we found for common sensors, this speed led to considerable motion blur.

In

Table 1. The Speed of sequences in different datasets.

Sequence	Linear Velocity (m/s)	Angular Velocity (rad/s)
ICL_kt0	0.27	0.33
ICL_kt1	0.09	0.40
ICL_kt2	0.40	0.46
ICL_kt3	0.24	0.31
VaFRIC	1.81	0.22
TUM_fr1_desk	0.66	0.94
TUM_fr2_xyz	0.1	0.08
TUM_fr3_office	0.36	0.35
ETH3D_camera_shake1	0.64	2.65
ETH3D_camera_shake2	0.48	3.30
ETH3D_camera_shake3	0.51	3.13
Ours_Dorm1_slow	0.59	1.07
Ours_Dorm1_fast1	0.92	2.59
Ours_Dorm1_fast2	0.93	2.42
Ours_Dorm2_fast1	1.60	2.16
Ours_Dorm2_fast2	1.43	2.49
Ours_Hotel_slow	0.68	1.16
Ours_Hotel_fast1	1.26	2.34
Ours_Hotel_fast2	1.55	2.10
Ours_Bedroom_slow	0.25	0.42

, we calculate the speeds of sequences for several public datasets, such as the ICL NUIM [53], VaFRIC [33], TUM RGB-D [61] and ETH3D [22]. Since most of the public datasets are recorded under slow camera motions (the ETH3D provides three fast sequences with small camera shakes), we made a dataset with both slow and fast sequences to further evaluate the scene reconstruction performance of our method. Our dataset was recorded in indoor scenes “Dorm1, Dorm2, Bedroom, Hotel” using the sensor Intel Realsense ZR300, and the sensor provided color, depth and IMU information. We have released our dataset to the public (https://github.com/zhuzunjie17/FastFusion).

Figure 6 shows the linear and angular camera velocities of the three sequences recorded by ourselves. In the two fast sequences, the camera velocities of consecutive frames change violently; this is mainly caused by great accelerations of the camera. In fact, other fast sequences used in our experiments also contain this kind of phenomenon. Notice that the speeds of the pubic datasets are derived from ground truth camera trajectories that are acquired by Vicon systems, and the speeds of the dataset recorded by ourselves are derived from successfully tracked camera trajectories.

5.3. Evaluation

5.3.1. Feature Tracking

To test whether the proposed geometry-aware feature tracking method tracks features accurately, we compared our feature tracking method with and without considering the SE effect. The method without considering the SE effect matches the patches directly by the shape of the previous frame. For the two compared methods, we set the patch size to $10\times 10$ and extracted no more than 100 patches of each frame. Figure 7 shows the qualitative tracking results for patches affected by the SE effect. The method without considering the SE effect tracks patches directly by the patch shapes of previous frames, whereas our method successfully detects the SE effect and deforms the patches for tracking purpose. The results show that our method obtains more accurate positions and smaller residuals for patches affected by the SE effect than for patches not affected. We list the average pixel intensity errors (AIE) of various sequences in

Table 2. Comparison of patch feature tracking.

	Sequence	Frame	SE Patch	AIE
		Num.	Num.	w/o SE	w/ SE
Slow	ICL_kt2	882	91	4.90	4.08
	ICL_kt3	1240	298	6.3	5.87
	TUM_fr1_desk	613	323	13.38	9.53
	TUM_fr3_office	2585	1302	12.93	12.1
	Dorm1_slow	1534	403	8.13	7.89
	Hotel_slow	1283	277	7.78	7.35
Fast	VaFRIC (20FPS)	300	1032	17.22	7.83
	ETH3D_cs1	318	988	15.61	8.01
	Dorm1_fast1	1477	1940	13.90	7.93
	Dorm1_fast2	484	1639	16.45	9.2
	Dorm2_fast1	564	1351	20.80	11.02
	Hotel_fast1	645	1367	14.83	9.69

to quantitatively compare the two methods. In this table, ICL_kt2 is the “livingroom” sequence and ICL_kt3 is the “officeroom” sequence. From the table, we find that our method obtains smaller AIE for all sequences than those obtained by the competing method, especially in sequences with fast camera motions. In addition, the numbers of frames and SE patches are also listed.

5.3.2. IMU

To verify whether the integration of IMU helps in the reconstruction of the scene model for fast camera motions, we compared the results with and without IMU in the three sequences, Hotel_slow, Hotel_fast1 and Hotel_fast2. Note that the loop closure function is closed in this comparison. Figure 6 already demonstrates the details of camera motions in the three sequences. For our system without IMU, we used the uniform motion model in the Kalman prediction step, i.e., the predicted camera states are identical to the previous frame.

Figure 8 shows the corresponding reconstruction results. In the sequence with slow camera motions, the system without IMU works on par with the full system. In sequences with aggressive camera motions, our system without IMU reconstructs models with several fractures. As shown in Figure 8b, a large fracture appears in the reconstructed model of Hotel_fast2, which is essentially caused by the bad initialization of the SE patch deformation during frames from 300 to 350. The bad initialization of the SE patch deformation degenerates the geometry-aware patch feature tracking, leading to inaccurate camera pose estimation. A side-by-side comparison for the reconstruction process of our system with and w/o IMU can be viewed in our video in Supplementary Materials.

5.3.3. Loop Detection

Computional Efficiency. As described in Section 4.4.1, we improved the original keyframe encoding method [11] for more efficient loop detection. Therefore, we show the processing time of the original and our improved methods on a recorded long sequence “Ours_House” with more than 20k frames. The reconstruction of “Ours_House” (first 5k frames) is shown in Figure 9 and our video in Supplementary Materials. Note that both methods are run in CPU on the same platform (detailed in Section 5.1). In this experiment, the two methods share identical parameters, such as the code block amount being 1000. Figure 5 shows the relationship between the computational time and the number of keyframes. A frame is named as a keyframe as long as its code is added into a list. It is obvious that the computational time of the original method [11] is linearly correlated with the number of keyframes; the reason for this is that all keyframes are used for the dissimilarity calculation, as well as the loop detection. This affects the real-time performance of a 3D reconstruction system, even when running the loop detection process in parallel on another CPU core, because the computational time is going to grow indefinitely. On the contrary, our improved loop detection method only calculates dissimilarity for candidate keyframes of $k=5$ nearby submaps, and the candidate keyframe amount does not increase with the total keyframe amount. Consequently, the computational time of our method is always lower than a threshold; this guarantees the real-time performance of the 3D reconstruction.

Accuracy. We further evaluated the accuracy of our method on the public dataset “7 scenes” [11]. This dataset provides training and test image sequences in seven indoor scenes. The training sequences are used for keyframe harvesting and the test sequences are for an accuracy evaluation of camera relocalization by the harvested keyframes. In this experiment, we first reconstructed indoor scenes by training sequences, then initialized test sequences in the same global coordinate. The successful relocalization of a frame is defined in the same way as [11], i.e., the recovered pose of a frame after ICP camera tracking (which is initialized by the pose of the matched keyframe) is within a 2 cm translational error and 2 degrees angular error. Figure 10 shows the accuracy of the original method and our method with depth input only, i.e., it only uses the depth information for the relocalization. Interestingly, the accuracy of our method increases in all seven scenes, especially in the scene “Stairs”. The reason for this is that, with the selection of candidate keyframes by the prior camera pose, our method implicitly removes the outliers that are produced by the very similar geometry of two different regions; for example, depth images that record different sections of stairs in the scene “Stairs”.

We further evaluated the effect of the nearest submap number k; the results are summarized in Figure 11. In the figure, the accuracy increases with the nearest submap number when k is small, and decreases when more and more submaps are selected, because the outliers are rejoined. In summary, we set $k=5$ as a trade-off between the performance and processing time. Note that a 120 m² indoor scene costs nearly 30 submaps (the one in Figure 9 “House”); thus, $k=5$ is enough for most house-grade indoor scenes to cover potential loops, and it can also be tuned to adapt other scenes.

5.4. Comparison

5.4.1. Quantitative Comparison

We quantitatively compared our full system with state-of-the-arts, such as ORB-SLAM2 [44], ElasticFusion [5], InfiniTAM-v3 [56], BundleFusion [4], BAD-SLAM [22] and our system without loop closure [10]. ORB-SLAM2, BundleFusion and BAD-SLAM use feature point for the loop detection, whereas ElasticFusion, InfiniTAM-v3 and ours use the keyframe encoding method; note that InfiniTAM-v3 also uses depth only for the detection.

We used the absolute trajectory errors (ATE) (as used in [61]; smaller is better) for the comparison; the errors calculate the root-mean-square of the Euclidean distances between the estimated camera trajectory and the ground truth camera trajectory. We evaluated three slow sequences of the TUM RGB-D dataset [61] and three fast sequences of the ETH3D [22]. The TUM RGB-D dataset is a recognized and widely used real-world benchmark. Both datasets provide color and depth images, while ETH3D further provides IMU measurements.

Table 3. ATE RMSE (cm) on TUM RGB-D and ETH3D datasets.

	TUM RGB-D [61]			ETH3D [22]
	Desk	xyz	Office	cs1	cs2	cs3
ORB-SLAM2 [62]	1.6	0.4	1.0	-	6.9	-
ElasticFusion [5]	2.0	1.1	1.7	8.4	-	-
InfiniTAM-v3 [56]	1.8	1.5	2.5	3.6	3.3	7.1
BundleFusion [4]	1.6	1.1	2.2	5.2	4.0	-
BAD-SLAM [22]	1.7	1.1	1.7	-	-	-
Ours w/o loop [10]	3.3	1.5	4.6	1.7	1.6	2.8
Ours	1.8	1.2	2.4	1.7	1.6	2.8

shows the results in sequences fr1/desk, fr2/xyz and fr3/office of TUM RGB-D, as well as the results in sequences camera_shake1(cs1), camera_shake2(cs2) and camera_shake3(cs3) of ETH3D. The speeds of these sequences are listed in Table 1. Note that the TUM RGB-D dataset does not provide full IMU information (acceleration only), so we used color and depth images of the TUM RGB-D dataset, and used the uniform motion model in our Kalman prediction step. In addition, since all of the other methods (except InfiniTAM-v3) cannot utilize IMU data, we only input IMU measurements for our system and InfiniTAM-v3.

The TUM RGB-D dataset neither contains IMU nor fast camera motions, but we see that our system still generate reasonable results in this situation, and that our system with loop closure outperforms our original one on these sequences with multiple loops. Both ours and other methods track camera poses well and successfully perform loop detection, as well as global optimization, in slow camera motion. Our strategies for fast camera motion (i.e., SE effect) are of little help under slow movement. Therefore, there is not much difference in performance between ours and other methods in slow camera motion. The advantage of our work is adequately demonstrated in sequences cs1, cs2 and cs3 with fast camera motions. The three fast sequences in the ETH3D dataset are collected under fast camera shakes, which capture a same small scene without loop (the reconstructed model of the scene is shown in Figure 12). Therefore, our method with and without loop closure outputs the same RMSE. The results in Table 3 show that most approaches cannot successfully track the cameras in all of the three fast sequences, whereas our method outperforms other methods on the three fast sequences.

5.4.2. Qualitative Comparison

In the qualitative experiments, we only compared our system with the methods that focus on the 3D reconstruction, including InfiniTAM-v3 [56], which uses the rotation values calculated by IMU information for pose estimation; BundleFusion [4], which has an efficient global pose optimization algorithm; ElasticFusion [5], which contains loop closure and model refinement via non-rigid surface deformation; BAD-SLAM [22], which performs direct RGB-D bundle adjustment; and our system without loop closure [10].

We compared them on our dataset that provides slow and fast sequences with IMU and RGB-D images; each sequence of our dataset aims to reconstruct an indoor scene. We first compared all methods in our slow sequence “Bedroom_slow” with a looped camera trajectory. The reconstruction results are shown in Figure 13. As we can see, our system without loop closure cannot consistently reconstruct the scene because it cannot eliminate the accumulated error for the camera tracking. On the contrary, our system with loop closure successfully detects and closes loops to reconstruct a globally consistent 3D model. Meanwhile, due to the good color images and small interframe motions of the slow sequence, other methods also successfully track the camera and close loops.

Then, we evaluated them on three fast sequences: two from our dataset (named “Dorm2_fast2” and “Livingroom_fast1”) and one from the ETH3D dataset (named “Camrea_shake1”). The reconstruction results are shown in Figure 12. BAD-SLAM fails at the very beginning of these fast sequences, which is also indicated in Table 3. In addition, because of the cautious tracking and reconstruction strategy of BundleFusion, it fails when the camera quickly scans large unknown regions, and then it restarts the tracking and reconstruction when the scanning speed slows down. Therefore, BundleFusion restarts repeatedly and outputs incomplete 3D maps of the three sequences. The reason for why BundleFusion still works on the fast sequences cs1 and cs2 shown in Table 3 is that those sequences only reconstruct a small region (see Figure 12) with camera shakes.

The performance of ElasticFusion and InfiniTAM-v3 is much better than BAD-SLAM and BundleFusion. However, due to the severe image motion blur caused by aggressive camera motions, the loop closure of ElasticFusion, which only depends on color information, does not work well, leading to the inconsistent reconstructions. Though InfiniTAM-v3 uses depth images to successfully detect loops, it still fails to rectify the great accumulated errors caused by fast camera motions. Different from them, our camera tracking and loop closure are both robust for fast camera motions, and the indoor scenes are well reconstructed.

More reconstruction results can be viewed in Figure 9 and our video in Supplementary Materials; we encourage the reader to watch our video in Supplementary Materials for a better visualization of the comparison.

6. Limitation

A large loop cannot be detected in time by our loop detection method. The reason for this is that the target submap may not be included in the candidate submaps due to a huge gap between the current camera and the target submap. A solution is to adaptively increase the nearest submap number k, i.e., the size of k can be controlled by the submap number to adapt scenes with arbitrary magnitude.

In addition, our method cannot recover the sharp texture of the reconstructed 3D model; the blurry images should be recovered during or after the reconstruction.

7. Conclusions

We present a real-time system for indoor scene reconstruction by tightly coupling RGB-D-IMU information with an extended Kalman filter, and our system is demonstrated to be able to estimate camera poses and reconstruct 3D scene models under fast camera motions. In our system, we reconstructed a static scene quickly and casually without requiring slow sensor motions; this significantly enhances the user-friendliness and system robustness of scene reconstruction. In addition, we explored the SE effect caused by fast camera motions and handled it using a geometry-aware patch deformation method for accurate feature tracking under fast camera motions. The globally consistent reconstruction was achieved in our system for both slow and fast camera motions. We detected loops via a depth-based keyframe encoding method and reduced the computational time of detection using a submap-based candidate selection mechanism. When a loop is successfully detected between two submaps, we performed the pose graph optimization to close the loop and update the poses of all submaps.