CuFusion: Accurate Real-Time Camera Tracking and Volumetric Scene Reconstruction with a Cuboid

Abstract

Given a stream of depth images with a known cuboid reference object present in the scene, we propose a novel approach for accurate camera tracking and volumetric surface reconstruction in real-time. Our contribution in this paper is threefold: (a) utilizing a priori knowledge of the precisely manufactured cuboid reference object, we keep drift-free camera tracking without explicit global optimization; (b) we improve the fineness of the volumetric surface representation by proposing a prediction-corrected data fusion strategy rather than a simple moving average, which enables accurate reconstruction of high-frequency details such as the sharp edges of objects and geometries of high curvature; (c) we introduce a benchmark dataset CU3D that contains both synthetic and real-world scanning sequences with ground-truth camera trajectories and surface models for the quantitative evaluation of 3D reconstruction algorithms. We test our algorithm on our dataset and demonstrate its accuracy compared with other state-of-the-art algorithms. We release both our dataset and code as open-source (https://github.com/zhangxaochen/CuFusion) for other researchers to reproduce and verify our results.

1. Introduction

Real-time camera tracking and simultaneous dense scene reconstruction has been one of the most actively studied problems in computer vision over recent years. The advent of depth cameras based either on structured light (e.g., Asus Xtion, Kinect 1.0) or time-of-flight (ToF) (e.g., Kinect 2.0) sensing offers dense depth measurements directly in real-time as video streams. Such dense depth sensing technologies have drastically simplified the process of dense 3D modeling, which turns the widely available Kinect-style depth cameras into consumer-grade 3D scanners.

KinectFusion [1] is one of the most famous systems for registering each incoming frame of depth images captured during the scanning into one integrated volumetric representation of the scene. An iterative closest point (ICP) algorithm [2] is performed to align the current depth map to the reconstructed volumetric truncated signed distance function (TSDF) [3] surface model to get the camera pose estimation. Each depth measurement is fused into the TSDF model directly to update the reconstruction. A triangulated 3D mesh model could finally be extracted using a Marching Cubes type algorithm [4].

Existing geometric alignment approaches based on ICP and its variants [5] are prone to drift in the presence of structure-less surfaces. Drift might be accumulated and even cause the failure of camera tracking when scanning larger man-made environments. Meanwhile, the weighted moving average TSDF fusion strategy makes the assumption of a Gaussian noise model on the depth measurements with a naïve surface visibility predicate that every surface point is visible from all sensor viewpoints [6]. This predicate is only locally true and usually violated due to surface occlusions [1] when scanning around the scene. Although truncation of the signed distance function (SDF) is performed to avoid surfaces interfering, surface blurring and the inflating problem (as shown in Figure 1c) may happen when scanning around tiny objects or sharp geometries in the scene.

Existing algorithms have been proposed to keep globally consistent camera trajectory estimation. Pose graphs are created and optimized when large loop closures are found [7], which may substantially reduce the odometry error accumulation. On the task of scanning small-sized scenes or objects, however, even small camera drift may cause deformation of the reconstruction. We propose a novel algorithm called CuFusion, which particularly focuses on the application of reconstructing small-sized scenes and objects precisely in real-time, with the accuracy of both camera tracking and data fusion significantly improved. With a priori knowledge of the planar faces and occluding contours of the cuboid reference object partly or totally present in the scene, each data frame is aligned against both the reconstructed scene and the localized cuboid model, and thus drift-free camera trajectories are maintained.

The predicate that every surface point is visible from all sensor viewpoints is only locally true due to surface occlusions [1]. In our work, we drop such assumptions and implement a “prediction-corrected” data fusion algorithm to integrate all incoming data into one geometrically consistent 3D model in the global reference frame. Instead of a simple moving average surface reconstruction, our work extends the TSDF representation by adding components storing the locally consistent TSDF value, the pixel ray and surface normal vector in each voxel grid for the detection of the camera view variation and correction of the global TSDF value. Experimental results (Figure 1) show the ability of our fusion method to keep the structural details of surfaces, which is on par with, or better than, existing state-of-the-art reconstruction systems that focus mostly on camera tracking accuracy.

Many scanning and reconstruction systems use both RGB and depth images. Feature-based registration is combined with dense ICP shape matching to estimate the best alignment between consecutive frames. Our system exploits only depth information as input to maximize tracking accuracy for the following reasons: First, some depth cameras such as ASUS Xtion PRO are not accompanied by RGB cameras. Second, for RGB-D cameras which provide both color and depth streams, the spatiotemporal alignment of RGB and depth information in pixel level may not be perfect. Third, by using only depth data, our system enables scanning in complete darkness regardless of the ambient lighting conditions.

We evaluate our algorithm qualitatively and quantitatively using both noiseless synthetic and noisy real-world data captured by a hand-held Kinect. The synthesized data provide both ground-truth (GT) camera trajectories and GT mesh models enabling both the trajectories and reconstructions to be quantitatively evaluated. For real-world image sequences, unfortunately we do not have GT camera trajectories. We 3D printed several rigid models using a high precision 3D printer (http://www.dowell3d.com/3d/3.html) for scanning and evaluate the quality of our reconstructions directly compared with the GT models.

2. Related Work

The research into the real-time 3D model reconstruction problem has been extensively studied in recent decades. The advance of range sensing technology has facilitated the development of real-time interactive range scanners for dense 3D surface model acquisition. Such range sensors, particularly on active sensing technologies, could be categorized into different types including laser scanners [8,9], time-of-flight (ToF) [10,11] sensing and structured-light cameras [12]. The introduction of Microsoft’s Kinect—based on structured-light sensing—has brought dense depth sensors to wide consumer-grade accessibility.

KinectFusion [1] of Newcombe et al. is one of the founding systems for real-time dense SLAM, taking a sequence of depth maps streamed from a Kinect-style sensor as the input to create a globally consistent 3D model of the scene. Despite its enlightenment, this algorithm has limitations in several aspects. First, pure geometric alignment of ICP is prone to drift in the presence structure-less surfaces. Second, the regular volumetric representation is memory consuming, which limits the size of the reconstructed model to medium sized rooms, also with limited resolution. Third, it cannot detect loop closures and therefore lacks the ability to recover from accumulating drift, leading to mesh artifacts.

Researchers have been making efforts to address the problems mentioned above. Henry et al. [13] were the first to combine texture feature matching with Generalized-ICP [14] using RGB-D data to reduce drift result from pure geometric alignment and increase the robustness of visual odometry [15]. Loop closure is detected when the previously seen region is revisited, and a pose graph is optimized to create a globally consistent map in [13], as well as in the work of Endres et al. [7,16], Whelan et al. [17,18] and Kerl et al. [19]. Whelan et al. [20] further proposed ElasticFusion, a novel algorithm for loop closure optimization without a pose graph. Moreover, higher-level primitives such as edges [21,22], occluding contours [23], curvature information [24], lines [25] and planes [26,27,28,29] are used as additional information to constrain the pose estimation process.

On dense scene representation, Whelan et al. [17] extended the KinectFusion algorithm spatially to support large unbounded scenes, with a cyclical buffer data structure. Endres et al. [7,16] used an octree-based mapping framework OctoMap [30] to generate a volumetric 3D map of the environment at scale, yet no mesh model is created. Other researchers have been using points and surfels [20,24,31,32,33,34] to represent the scene and render it with the surface-splatting technique [35]. Such point-based scene representation has significantly reduced computational complexity and lowered the memory overhead compared with the volumetric approaches and is therefore adequate for reconstructing large-scale environments. Note that Lefloch et al. [24] use curvature information as an independent surface attribute for their real-time reconstruction, leading not only to camera drift reduction but also to improved scene reconstruction.

However, despite the efforts exerted, both the camera pose estimation and the reconstructed models are far from perfect. On small-sized scenes particularly, slight camera drift may lead to reconstruction deformation and sharp depth edges or highly concave scenes are problematic for these approaches [36]. We tackle these problems and focus on fidelity preservation in this paper.

3. Method

We base our work on an open-sourced implementation of the KinectFusion algorithm from the PCL library [37]. Our reconstruction pipeline is illustrated in Figure 2, which is described in detail in the following sections.

3.1. Notation

We define the image domain as $\Omega \subset {\mathbb{N}}^2$, and a depth image ${\mathrm{D}}_k:\Omega \to ℝ$ at time k. We represent the camera pose at time k in the global coordinate frame ${ℱ}_g$ by a rigid transformation matrix:

$${\mathrm{T}}_{g,k}=\left[\begin{array}{cc}{\mathrm{R}}_{g,k}& {\mathrm{t}}_{g,k}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in \mathbb{S}\mathbb{E}\left(3\right),$$

(1)

with a $3\times 3$ rotation matrix ${\mathrm{R}}_{g,k}\in \mathbb{S}\mathbb{O}\left(3\right)$ and a $3\times 1$ translation vector ${\mathrm{t}}_{g,k}\in {ℝ}^3$, which transforms a point $p_k\in {ℝ}^3$ in the camera coordinate frame ${ℱ}_k$ to a global point $p_g={\mathrm{R}}_{g,k}{p}_k+{\mathrm{t}}_{g,k}\in {ℝ}^3$. We model the depth camera by the simple pinhole model, and use a constant camera intrinsic matrix $\mathrm{K}$ to transform points on the sensor plane into image pixels:

$$\mathrm{K}=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right],$$

(2)

where $\left(f_x,f_y\right)$ are the horizontal and vertical focal lengths and $\left(c_x,c_y\right)$ is the image coordinate of the principal point.

We define the 3D back-projection of an image pixel $u\in \Omega$ as $p={\mathrm{K}}^{-1}\dot{u}\mathrm{D}\left(u\right)$, where $\dot{u}:={\left(u^{\mathrm{T}}|1\right)}^{\mathrm{T}}$ is the homogeneous form of $u$. And inversely, we define the perspective projection of point $p={\left(x,y,z\right)}^{\mathrm{T}}$ as $u=\mathsf{\pi }\left(\mathrm{K}p\right)$, where function $\mathsf{\pi }\left(p\right)={\left(x/z,y/z\right)}^{\mathrm{T}}$ performs perspective projection including de-homogenization process.

Prior to registration, an organized vertex map ${\mathrm{V}}_k$ is computed by bilateral-filtering and back-projecting the raw depth image ${\mathrm{D}}_k$. The normal map ${\mathrm{N}}_k$ is computed using the PCA method. Given the camera pose ${\mathrm{T}}_{g,k}$ at time k, we could transform both ${\mathrm{V}}_k,{\mathrm{N}}_k$ to the global frame of coordinate:

$$\{\begin{array}{l}{\dot{\mathrm{V}}}_k^g\left(u\right)={\mathrm{T}}_{g,k}{\dot{\mathrm{V}}}_k\left(\mathbf{u}\right)\\ {\mathrm{N}}_k^g\left(u\right)={\mathrm{R}}_{g,k}{\mathrm{N}}_k\left(\mathbf{u}\right)\end{array},$$

(3)

3.2. Cuboid Localization

Given a depth image ${\mathrm{D}}_k$ and a rectangular cuboid with edge lengths ${\mathrm{𝒫}}_{cu}=(\mathrm{a},\mathrm{b},\mathrm{c})$ present in the image, we localize the cuboid and calculate its pose in the global coordinate frame ${ℱ}_g$. Live depth frames will be latterly aligned against the reference cuboid when scanning around it to mitigate the accumulating camera drift.

We first perform plane segmentation using the Agglomerative Hierarchical Clustering (AHC) algorithm [38], as illustrated in Figure 3c. Then we check the orthogonality of the segmented planes. Two planes are considered to be orthogonal if the angle ${\Theta }_p$ between their normal vectors is approximately 90° (i.e., $|{\Theta }_p-90°|<{\epsilon }_{\Theta };{\epsilon }_{\Theta }=5°).$ Once we find three planes that are orthogonal to each other, we check the length of the intersecting line segments between the planes. If the three line segments’ lengths match the cuboid edge length parameter ${\mathrm{𝒫}}_{cu}$ approximately (differences below a threshold ${\epsilon }_{\mathrm{𝒫}}=10\mathrm{mm}$), we claim to find the cuboid and mark the three planes as its adjacent planes.

We consequently define the cuboid coordinate frame of reference. We set frame origin ${\mathrm{O}}_{cu}$ to the intersection point of the three orthogonal planes, and draw the system axes from the normal vectors. Due to the inaccuracy of the depth measurement and camera intrinsic calibration, orthogonality between the normal vectors of the segmented adjacent planes are not guaranteed strictly. We obtain the nearest orthogonal axes $\left[X_{cu},Y_{cu},Z_{cu}\right]$ of the frame by solving the Orthogonal Procrustes Problem. The cuboid pose in the camera frame at time k is:

$${\mathrm{T}}_{k,cu}=\left[\begin{array}{cc}{\mathrm{R}}_{k,cu}& {\mathrm{t}}_{k,cu}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in \mathbb{S}\mathbb{E}\left(3\right),$$

(4)

$${\mathrm{R}}_{k,cu}=\left[X_{cu},Y_{cu},Z_{cu}\right],$$

(5)

$${\mathrm{t}}_{k,cu}={\mathrm{O}}_{cu}^{\mathrm{T}},$$

(6)

Assuming the camera pose ${\mathrm{T}}_{g,k}$ at time k is known, the cuboid pose ${\mathrm{T}}_{g,cu}=\left[\begin{array}{cc}{\mathrm{R}}_{g,cu}& {\mathrm{t}}_{g,cu}\\ 0^{\mathrm{T}}& 1\end{array}\right]$ in the global frame of coordinate could then be derived: ${\mathrm{T}}_{g,cu}={\mathrm{T}}_{g,k}{\mathrm{T}}_{k,cu}$. Figure 4 illustrates the notations used in the paper.

3.3. Camera Pose Estimation

Since we use depth maps as input sequences, only geometric alignment is performed. For each input frame ${\mathrm{D}}_k$ at time k, we estimate the pose ${\mathrm{T}}_{g,k}$ of the depth camera frame ${ℱ}_k$ with respect to the global frame ${ℱ}_g$ by registering the live depth map to both the global reconstructed surface model and the cuboid reference object.

A. Frame to Model Registration

Given the implicit TSDF surface model $S$, the surface prediction w.r.t. the camera pose ${\mathrm{T}}_{g,k-1}$ is obtained as an organized vertex and normal map $({\widehat{\mathrm{V}}}_{k-1},{\widehat{\mathrm{N}}}_{k-1})$, and transformed into the global frame as $({\widehat{\mathrm{V}}}_{k-1}^g,{\widehat{\mathrm{N}}}_{k-1}^g)$. For frame-to-model registration, a transformation ${\mathrm{T}}_{g,k}$ is pursued to minimize the point-to-plane error between ${\mathrm{T}}_{g,k}{\mathrm{V}}_k$ and ${\widehat{\mathrm{V}}}_{k-1}^g$:

$${\mathrm{E}}_{frame2model}\left({\mathrm{T}}_{g,k}\right)={\displaystyle {{\displaystyle \sum }}_{\left(u,\widehat{u}\right)\in {\mathbb{K}}_1}}{\left(\left({\mathrm{T}}_{g,k}{\dot{\mathrm{V}}}_k\left(u\right)-{\widehat{\mathrm{V}}}_{k-1}^g\left(\widehat{u}\right)\right){\widehat{\mathrm{N}}}_{k-1}^g\left(\widehat{u}\right)\right)}^2,$$

(7)

where ${\mathbb{K}}_1=\left\{\left(u,\widehat{u}\right)\right\}$ is the set of correspondences obtained by projective data association [1]:

$$\widehat{u}=\mathsf{\pi }\left({\mathrm{K}\tilde{\mathrm{T}}}_{k-1,k}{\dot{\mathrm{V}}}_k\left(u\right)\right),$$

(8)

${\tilde{\mathrm{T}}}_{k-1,k}$ denotes the transformation from current time k to time (k − 1) during each ICP iteration.

B. Frame to Cuboid Registration

Assuming the cuboid pose w.r.t., the global coordinate frame is already known. For each camera pose ${\mathrm{T}}_{g,k}$, per-pixel ray casting is performed on the global cuboid to synthesize a proxy depth map ${\widehat{\mathrm{D}}}_k^{cu}$. An organized vertex and normal map in the global frame as $\left({\widehat{{\mathrm{V}}_{\mathrm{cu}}}}_{k-1}^g,{\widehat{{\mathrm{N}}_{\mathrm{cu}}}}_{k-1}^g\right)$ is then obtained using back projection of the depth map and local to global transformation. Similar to the frame-to-model registration, a frame is aligned against the cuboid surface in the global coordinate frame by minimizing the point-to-plane error:

$${\mathrm{E}}_{frame2cuboid}\left({\mathrm{T}}_{g,k}\right)={{\displaystyle \sum }}_{\left(u,\widehat{u}\right)\in {\mathbb{K}}_2}{\left(\left({\mathrm{T}}_{g,k}{\dot{\mathrm{V}}}_k\left(u\right)-{\widehat{{\mathrm{V}}_{\mathrm{cu}}}}_{k-1}^g\left(\widehat{u}\right)\right){\widehat{{\mathrm{N}}_{\mathrm{cu}}}}_{k-1}^g\left(\widehat{u}\right)\right)}^2,$$

(9)

In addition, we adopt the edge-to-edge error metric as a constraint to mitigate the potential camera drift. Given the inpainted depth map ${\mathrm{D}}_k^{\prime }$, we find the edge points (i.e., pixels at depth discontinuities) on the live depth map along the contour generator set $C_k$ as proposed in [23]:

$$C_k=\left\{s\in {\mathrm{D}}_k:\exists \mathrm{t}\in {\mathcal{N}}_s^8,\mathrm{s}.\mathrm{t}.{\mathrm{D}}_k^{\prime }\left(s\right)-{\mathrm{D}}_k^{\prime }\left(t\right)>{\delta }_c\right\},$$

(10)

where ${\mathcal{N}}_s^8$ is the 8-neighborhood of pixel $s\in {\mathrm{D}}_k$ and ${\delta }_c$ is the depth discontinuity threshold, set to 50 mm according to the sensor noise magnitudes [39]. Figure 3d demonstrates the contour generators with white lines labeled on the depth map. Edge points set ${\mathrm{Ve}}_k$ of the live depth map is obtained by back-projection of $C_k$.

On the other hand, the cuboid edges are discretized into a 3D point set ${\mathrm{Ve}}_g^{cu}$ in the global frame with an interval of 1 mm. ${\mathrm{Ve}}_g^{cu}$ is invariant to the camera pose, and is obtained once the cuboid is successfully localized, prior to the ICP registration procedure. We also set up a KD-tree over ${\mathrm{Ve}}_g^{cu}$ beforehand for fast correspondence search for each point in ${\mathrm{Ve}}_k$. The edge-to-edge error to minimize is:

$${\mathrm{E}}_{edge2edge}\left({\mathrm{T}}_{g,k}\right)={{\displaystyle \sum }}_{\left(s,t\right)\in {\mathbb{K}}_3}{\left(\left({\mathrm{T}}_{g,k}{\dot{\mathrm{Ve}}}_k\left(s\right)-{\mathrm{Ve}}_g^{cu}\left(t\right)\right){\widehat{{\mathrm{N}}_{\mathrm{cu}}}}_{k-1}^g\left(t\right)\right)}^2,$$

(11)

where ${\mathbb{K}}_3=\left\{\left(s,t\right)\right\}$ is the correspondence set obtained by nearest neighbor search with KD-tree.

C. Joint Optimization

We combine Equations (7), (9) and (11) to form a joint cost function:

$${\mathrm{E}}_{track}={\mathrm{E}}_{frame2model}+{𝓌}_{f2c}{\mathrm{E}}_{frame2cuboid}+{𝓌}_{e2e}{\mathrm{E}}_{edge2edge},$$

(12)

where ${𝓌}_{f2c}$ and ${𝓌}_{e2e}$ are the weights that determine the influence of correspondences on the cuboid surfaces and edges. When setting ${𝓌}_{f2c}={𝓌}_{e2e}=0$, our optimization objective is equivalent to KinectFusion. We set ${𝓌}_{f2c}=1$ and ${𝓌}_{e2e}=4$ in our experiments empirically, enforcing the constraint of the edge correspondences.

The camera pose ${\mathrm{T}}_{g,k}$ is then obtained by minimizing the overall cost function ${\mathrm{E}}_{track}$ iteratively. A linear approximation is made to solve the system, assuming the orientation change between consecutive frames is very small [1,40]. Using the small angle assumption at each iteration, we approximate the incremental rotation matrix as:

$${\mathrm{R}}_{inc}=[\begin{array}{ccc}1& -\gamma & \beta \\ \gamma & 1& -\alpha \\ -\beta & \alpha & 1\end{array}],$$

(13)

where $\alpha$, $\beta$, and $\gamma$ are the rotation in radians about the X, Y, and Z axis, respectively. Similar to KinectFusion, we compute and sum the linear system in parallel on the GPU, and solve it on the CPU using a Cholesky decomposition.

3.4. Improved Surface Reconstruction

Although we are trying to stabilize camera tracking, surface reconstruction is yet to be perfect. The TSDF volumetric representation allows for online surface extraction as a polygon mesh, while the simple moving average TSDF fusion strategy proposed in KinectFusion suffers from the inflation problem, and lower the reconstruction accuracy. Figure 5 illustrates one of our synthetic datasets “armadillo.” Even with noiseless depth images and GT camera trajectory as input, surface reconstruction is smoothed and inflated, particularly at the cuboid edges, the claws and ears of the armadillo, which is far less satisfactory than the GT surface model.

The reason for fusion inflation is illustrated in Figure 6. Due to the simple moving average TSDF fusion algorithm based on the predicate that every surface point is visible from all sensor viewpoints [6], voxel grids with negative TSDF values interfere with the positive ones. To tackle this problem, we extend the storage of TSDF $S\left(p\right)$ from the truncated signed distance value $\mathrm{F}\left(p\right)$ and its weight $\mathrm{W}\left(p\right)$ to:

$$S\left(p\right)\mapsto \left[\mathrm{F}\left(p\right),\mathrm{W}\left(p\right),\mathrm{F}\prime \left(p\right),\mathrm{W}\prime \left(p\right),{\mathrm{R}}_g\left(p\right),{\mathrm{N}}_g\left(p\right),\mathrm{Cv}\left(p\right),\mathrm{Cn}\left(p\right)\right],$$

(14)

where for each voxel grid $p$:

• $\left[\mathrm{F}\left(p\right),\mathrm{W}\left(p\right)\right]$ are the original TSDF components, and $\left[\mathrm{F}\prime \left(p\right),\mathrm{W}\prime \left(p\right)\right]$ are “ghost” distance value and weight for correction of the existing TSDF prediction;
• ${\mathrm{R}}_g\left(p\right)$ and ${\mathrm{N}}_g\left(p\right)$ are the view ray and the normal vector in the global coordinate frame respectively, which are used to check if a new surface patch is observed from a different view;
• $\mathrm{Cv}\left(p\right)$ and $\mathrm{Cn}\left(p\right)$ are two integer counters as the confidence indices of voxel $p$ and its normal vector ${\mathrm{N}}_g\left(p\right)$. When $\mathrm{Cv}\left(p\right)>{\delta }_v$, we think the distance value $\mathrm{F}\left(p\right)$ of voxel has been robustly estimated; when $\mathrm{Cn}\left(p\right)>{\delta }_n$, the normal vector ${\mathrm{N}}_g\left(p\right)$ is believed to be stable enough against the measurement noise. A simple Boolean semantic function is defined to check if a new face is observed from a new view point:

  $$\mathrm{IsNewFace}\left(p\right)=\mathrm{True}\mathrm{iff}\{\begin{array}{}\mathrm{Cn}\left(p\right)>{\delta }_n,\mathrm{and}\\ \mathrm{Angle}\left({\mathrm{R}}_g\left(p\right),{\mathrm{R}}_{{\mathrm{D}}_k}\left(p\right)\right)>{\theta }_r,\mathrm{and}\\ \mathrm{Angle}\left({\mathrm{N}}_g\left(p\right),{\mathrm{N}}_{{\mathrm{D}}_k}\left(p\right)\right)>{\theta }_n.\end{array},$$

  (15)

  where the thresholds are set to ${\mathsf{\delta }}_v=15,{\mathsf{\delta }}_n=5,{\mathsf{\theta }}_r=15°,{\mathsf{\theta }}_n=30°$ empirically. We define a weight map ${\mathcal{W}}_k$ for each input frame ${\mathrm{D}}_k$:

  $${\mathcal{W}}_k\left(u\right)=cos\left({\theta }_I\right)\ast \frac{{\mathrm{L}}_k\left(u\right)}{{\mathrm{D}}_k\left(u\right)},$$

  (16)

  with ${\theta }_I=\mathrm{Angle}\left({\mathrm{R}}_{{\mathrm{D}}_k}\left(p\right),{\mathrm{N}}_{{\mathrm{D}}_k}\left(p\right)\right)$ denoting the incidence angle of the view ray to the surface, and ${\mathrm{L}}_k$ is a distance transform map obtained from the contour generator map $C_k$. For each grid $p$ in the TSDF volume, we obtain the adaptive fusion weight ${\mathrm{W}}_k\left(p\right)$ and the truncation distance threshold ${\mu }_k\left(p\right)$:

  $$\{\begin{array}{c}{\mathrm{W}}_{{\mathrm{D}}_k}\left(p\right)={\mathrm{W}}_{base}\ast {\mathcal{W}}_k\left(u\right)\\ {\mu }_{{\mathrm{D}}_k}\left(p\right)={\mu }_{base}\ast {\mathcal{W}}_k\left(u\right)\end{array},$$

  (17)

  where $u$ is the projection of $p$ given the camera pose ${\mathrm{T}}_{g,k}$, and ${\mathrm{W}}_{base}$, ${\mu }_{base}$ are the base weight and base truncation distance which are set empirically. Our prediction-corrected TSDF fusion algorithm is then detailed as a flowchart in Figure 7. We categorize the fusion procedure into three sub-strategies:
  Moving Average: Identical to the TSDF update procedure of KinectFusion, simple moving average TSDF fusion is performed when a voxel has high uncertainty (e.g., at glancing incidence angle or too close to the depth discontinuity edge):

  $$\{\begin{array}{c}{\mathrm{F}}_k\left(p\right)={\mathrm{W}}_{k-1}\left(p\right){\mathrm{F}}_{k-1}\left(p\right)+{\mathrm{W}}_{{\mathrm{D}}_k}\left(\mathbf{p}\right){\mathrm{F}}_{{\mathrm{D}}_k}\left(\mathbf{p}\right)\\ {\mathrm{W}}_k\left(p\right)={\mathrm{W}}_{k-1}\left(p\right)+{\mathrm{W}}_{{\mathrm{D}}_k}\left(p\right)\end{array},$$

  (18)
  Ignore Current: We ignore the TSDF value at the current time when a previously robustly estimated voxel is at glancing incidence angle along the view ray. This is also the case when the current TSDF value with higher uncertainty is observed from a new perspective.
  Fix Prediction: When a voxel with previously stable TSDF value ${\mathrm{F}}_{k-1}\left(p\right)<0$ is observed to increase from a new point of view—either with ${\mathrm{F}}_{{\mathrm{D}}_k}\left(p\right)>0$ or $\left({\mathrm{F}}_{{\mathrm{D}}_k}\left(p\right)\langle 0{\mathrm{and}\mathrm{F}}_{{\mathrm{D}}_k}\left(p\right)\rangle {\mathrm{F}}_{k-1}\left(p\right)\right)$—we believe the live TSDF estimation is more trustworthy as a correction of the previous prediction. In the case of measurement noise, we fuse the live estimation into the ghost storage:

  $$\{\begin{array}{c}{\mathrm{F}}_k^{\prime }\left(p\right)={\mathrm{W}}_{k-1}^{\prime }\left(p\right){\mathrm{F}}_{k-1}^{\prime }\left(p\right)+{\mathrm{W}}_{{\mathrm{D}}_k}\left(\mathbf{p}\right){\mathrm{F}}_{{\mathrm{D}}_k}\left(\mathbf{p}\right)\\ {\mathrm{W}}_k^{\prime }\left(p\right)={\mathrm{W}}_{k-1}^{\prime }\left(p\right)+{\mathrm{W}}_{{\mathrm{D}}_k}\left(p\right)\end{array},$$

  (19)

  and replace the global TSDF with the ghost storage when ${\mathrm{W}}_k^{\prime }\left(p\right)$ is above a threshold:

  $$\{\begin{array}{c}{\mathrm{F}}_k\left(p\right)={\mathrm{F}}_k^{\prime }\left(p\right)\\ {\mathrm{W}}_k\left(p\right)={\mathrm{W}}_k^{\prime }\left(p\right)\end{array},$$

  (20)

Note the update of $\left[{\mathrm{R}}_g\left(p\right),{\mathrm{N}}_g\left(p\right),\mathrm{Cv}\left(p\right),\mathrm{Cn}\left(p\right)\right]$ is performed independently from the three fusion strategies. With our subdivided fusion algorithm, different surface areas are reconstructed elaborately, resulting in the good preservation of high-curvature surface areas, as illustrated in Figure 6e.

4. Evaluation

We compare our algorithm with four other dense tracking and mapping approaches: KinectFusion [1] (PCL implementation [37]), the work of Zhou et al. [23], ElasticFusion [20] of Whelan et al., and the NICP algorithm [32] of Serafin et al. ElasticFusion jointly aligns RGB and depth information, while the other four are pure depth camera tracking and reconstruction approaches. We set the weight $w_{rgb}=0$ for RGB alignment component in ElasticFusion, to make it relies only on depth camera tracking as in others’ work. On the evaluation of NICP, we run their CPU implementation offline at full resolution, with default configuration (only the camera parameters are updated). We use the point clouds for reconstruction accuracy evaluation.

Since the scales of our scanned objectives are small, we use a volume of size $1{\mathrm{m}}^3$ with ${256}^3$ voxels for all the compared algorithms, where each voxel is approximately $3.9{\mathrm{mm}}^3$.

4.1. Dataset

A. Noiseless Synthetic Data

We synthesize three depth image sequences with ground-truth (GT) mesh surface models and GT camera trajectories. A camera intrinsic matrix ${\mathrm{K}}_s$ is given to generate images of resolution $640\times 480$, as shown in

Table 1. Camera intrinsic parameters used in our dataset, including the focal lengths $\left(f_x,f_y\right)$ and the optical center $\left(c_x,c_y\right)$. Note that on real-world data the RGB and the depth camera share one intrinsic matrix ${\mathrm{K}}_r$ since they are pre-aligned together.

Scenario	Intrinsic Matrix	${\mathit{f}}_{\mathit{x}}$	${\mathit{f}}_{\mathit{y}}$	${\mathit{c}}_{\mathit{x}}$	${\mathit{c}}_{\mathit{y}}$
synthetic	${\mathrm{K}}_s$ (RGB)	525.50	525.50	320.00	240.00
real-world	${\mathrm{K}}_r$ (RGB-D)	529.22	528.98	313.77	254.10

. We choose from “The Stanford Models” [41] the armadillo, dragon and bunny, and scale and place them respectively on top of a synthetic cuboid of edge lengths ${\mathrm{𝒫}}_{cu}=(400,300,250)$ mm. We then move the camera freely around the scene to generate GT trajectories and depth images, as illustrated in Figure 8. Note that neither the depth measurement noise nor the motion blur is modeled and the only measurement inaccuracy comes from data type casting from floats to integers when saving the depth images.

B. Noisy Real-World Data

We manufacture six rigid objects using a 3D printer and put them on a precisely manufactured cuboid with dimensions $400\times 300\times 250{\mathrm{mm}}^3$, same as the one used in our synthetic data. The cuboid is placed on a turntable which is turned by hand, and we held and moved a Kinect camera slowly to perceive more details of the objectives. $640\times 480$ pre-aligned RGB-D images are generated at $30\mathrm{Hz}$, with the camera intrinsic matrix ${\mathrm{K}}_r$ (Table 1). We pre-process the depth sequences by truncating depth pixels of values larger than 1.5 m, to remove static background areas. Figure 9 demonstrates our GT mesh models, the 3D printed objects and the captured depth images with the scanning objectives placed on top of the cuboid reference object. Note that in data “lambunny,” a simplified bunny model with merely 640 vertices and 1247 faces is used, and in data “mug,” a regular hexagonal mug resting upside down on the cuboid is scanned.

4.2. Error Metrics

On synthetic data, both GT camera trajectories and GT mesh surfaces are provided. We quantify the accuracy of camera trajectory using absolute trajectory error (ATE) proposed by Sturm et al. [42], and evaluate the root mean squared error (RMSE) of the translational components over all time indices, which gives more influence to outliers. We further quantify the surface reconstruction accuracy using the cloud to mesh (C2M) distance metric [43] after aligning the GT model with the reconstructed model using the CloudCompare software [44]. We use two standard statistics: Mean and Std. over the C2M distances for all vertices in the reconstruction. On our real-world data, GT camera trajectories are not available nor do we have GT surface models of the entire scenes. We focus on the evaluation of the reconstructed 3D printed models using the C2M error metric.

4.3. Camera Trajectory Accuracy

We evaluate the absolute trajectory error (ATE) of the camera trajectories on synthetic depth image sequences. Although planar surfaces of the cuboid occupy the majority of the depth images, KinectFusion [1], Zhou et al. [23], ElasticFusion [20] and our approach achieve decent camera trajectories without prominently accumulating drift, as listed in

Table 2. Evaluation of the odometry accuracy with absolute trajectory error (ATE) RMSE metric in millimeters.

Synthetic Data	KinectFusion [1]	Zhou et al. [23]	ElasticFusion [20]	NICP [32]	Our Approach
armadillo	3.2	6.4	7.1	454.6	1.5
dragon	4.2	6.7	8.0	292.8	1.7
bunny	3.9	5.1	6.6	417.8	1.3

. However, NICP [32] produces inaccurate trajectories with ATE up to hundreds of millimeters. With the additional information from the cuboid a priori, our approach significantly outperforms the reference algorithms, reducing the RMS odometry error from 3~8 mm to less than 2 mm.

Since the errors of all the trajectory estimations (NICP excluded) on synthetic data are small ($<10$ mm), we plot the per frame ATE (as in Figure 10) for each algorithm (NICP excluded) rather than the trajectory overviews. Our approach (cyan line) keeps the least drift on most of the frames compared with the other three algorithms.

4.4. Surface Reconstruction Accuracy

The surface reconstruction accuracy is evaluated with the cloud to mesh (C2M) distances between the reconstructions and the ground-truth mesh models. For our synthetic data, GT models of the whole scenes are provided while for our real-world data, we have only GT models for the 3D printed objectives placed on the reference cuboid. Surface reconstructions are first aligned against the GT models for C2M distance computation, and heat maps of the C2M distances are plotted in Figure 11 for qualitatively reconstruction accuracy evaluation. Rows 1~3 show the reconstruction of the synthetic data inputs, and rows 4~9 show the real-world ones. The outputs of ElasticFusion in column 3 are not watertight, since it outputs clouds instead of meshes. NICP is excluded from comparison, since its inaccurate camera trajectories result in invalid scene clouds on our benchmark dataset. Note how tightly our approach preserves the scale of the reconstruction and maintains high-fidelity particularly on sharp geometries.

We quantitatively evaluate the C2M errors for each algorithm with Mean and Std. statistics, as shown in

Table 3. Surface Reconstruction accuracy on our synthetic data, with C2M error metric ($\mathrm{Mean}\pm \mathrm{Std}.$) in millimeters.

Synthetic Data	KinectFusion [1]	Zhou et al. [23]	ElasticFusion [20]	Our Approach
armadillo	$0.9\pm 1.1$	$1.6\pm 1.4$	$1.3\pm 1.6$	$0.2\pm 0.5$
dragon	$1.0\pm 1.2$	$1.5\pm 1.4$	$1.3\pm 1.6$	$0.3\pm 0.6$
bunny	$0.9\pm 1.9$	$1.3\pm 1.8$	$1.0\pm 1.1$	$0.4\pm 1.1$

and

Table 4. Surface Reconstruction accuracy on our real-world data, with C2M error metric ($\mathrm{Mean}\pm \mathrm{Std}.$) in millimeters. Note that for real-world data, the evaluation is only performed on the 3D printed objectives but not the whole scene.

Real-World Data	KinectFusion [1]	Zhou et al. [23]	ElasticFusion [20]	Our Approach
lambunny	$4.0\pm 3.3$	$4.5\pm 3.0$	$3.5\pm 3.7$	$1.3\pm 1.5$
owl	$4.4\pm 3.1$	$4.9\pm 2.9$	$5.1\pm 4.4$	$1.1\pm 1.3$
wingedcat	$5.0\pm 3.3$	$5.2\pm 3.1$	$3.2\pm 3.1$	$1.5\pm 1.8$
buddhahead	$4.8\pm 3.3$	$5.3\pm 3.0$	$4.5\pm 3.7$	$1.0\pm 0.8$
tooth	$4.4\pm 1.7$	$4.8\pm 1.8$	$3.9\pm 3.6$	$1.2\pm 1.0$
mug	$2.7\pm 2.0$	$3.5\pm 2.3$	$5.0\pm 3.2$	$0.9\pm 0.8$

. Our approach keeps the minimum values on both Mean and Std. in all experimental datasets, indicating its superiority in accuracy over the compared algorithms. Close-up views of the reconstructions are detailed in Figure 12, for further comparison between KinectFusion and our approach.

Additionally, we evaluate the reversed C2M errors, namely the distance from the point clouds of the GT models to the mesh of the surface reconstructions. ElasticFusion is excluded from this comparison, since it produces no surface meshes.

Table 5. Reversed C2M error in millimeters on our synthetic data.

Synthetic Data	KinectFusion [1]	Zhou et al. [23]	Our Approach
armadillo	$1.7\pm 1.5$	$2.0\pm 1.5$	$0.5\pm 0.6$
dragon	$1.8\pm 2.1$	$2.0\pm 2.2$	$0.9\pm 2.0$
bunny	$1.4\pm 2.3$	$1.7\pm 2.3$	$0.9\pm 2.3$

and

Table 6. Reversed C2M error in millimeters on our real-world data.

Real-World Data	KinectFusion [1]	Zhou et al. [23]	Our Approach
lambunny	$3.3\pm 2.6$	$3.9\pm 2.5$	$1.2\pm 1.0$
owl	$4.0\pm 2.1$	$4.8\pm 2.1$	$1.5\pm 1.6$
wingedcat	$7.6\pm 5.9$	$8.0\pm 6.0$	$4.3\pm 5.0$
buddhahead	$4.7\pm 1.8$	$5.6\pm 1.6$	$1.1\pm 0.8$
tooth	$4.1\pm 1.6$	$4.6\pm 1.7$	$1.0\pm 1.0$
mug	$2.3\pm 1.5$	$3.1\pm 1.9$	$1.1\pm 1.2$

show the quantitative results of this evaluation. On average, the error of this metric is slightly larger than that of the normal C2M distance metric shown in Table 3 and Table 4, which results from the inabilities of the compared algorithms to accurately reconstruct extremely sharp surface geometries.

5. Discussion and Conclusions

We have presented a novel approach called CuFusion for real-time 3D scanning and accurate surface reconstruction using a Kinect-style depth camera. A man-made cuboid, the scale of which is accurately known, is used as a reference object for accurate camera localization without explicit loop closure detection, and a novel prediction-corrected TSDF fusion strategy is employed for reconstruction update. By solving the surface inflation problem introduced by the simple moving average fusion strategy, our approach preserves the surface details especially when scanning tiny objects or edge areas with high curvatures, resulting in high-fidelity surface reconstruction, which also improves the camera odometry accuracy in turn. We provide a dataset CU3D for the quantitative evaluation of our algorithm and have made our code open-source for scientific verification.

There are several limitations for future work to overcome. First, our modified dense volumetric representation needs 16 bytes per voxel—four times as much memory as KinectFusion at the same resolution—which limits our reconstruction to small-sized scenes. Second, to be capable of reconstructing high-curvature geometries, the camera should be moved as steadily as possible to reduce motion blur and uncertainty in depth measurements. Our algorithm trades off the robustness for reconstruction accuracy, which may fail in the presence of camera jitter or large motion. Figure 13 shows an example of the reconstruction failure result from depth motion blur artifact. Although no noticeable tracking drift happens, the reconstruction is delicate due to our prediction-corrected TSDF fusion strategy. Third, despite our efforts, the reconstructions are yet to be perfected due to sensor noise and the limitation of the volume resolution. As illustrated in Figure 12, engraved surfaces such as the armadillo shell, the facial expression of the owl, wingedcat and buddhahead are smoothed out—additionally, very thin geometries such as the owl’s ears and the mug’s handle are partly gone.

Our future work will focus on the memory efficiency of our modified volumetric representation, enabling higher volume resolution and a larger scale of reconstruction. The octree-based framework OctoMap [30] could be used for volume data compression. Another interesting challenge might be the surface smoothing problem, which we will focus on mitigating using the surface curvature consistency among the captured frames.