Smooth Mesh Estimation from Depth Data
using Non-Smooth Convex Optimization

We follow a similar notation than the one used in FLaME [11], since we use the same mathematical machinery. We define by $\mathcal{V}$ the set of vertices of the 3D mesh, and by $\mathcal{E}$ the set of edges of the 3D mesh. For each edge, $e=(i,j)\in\mathcal{E},$ we denote by $v^{i},v^{j}\in\mathcal{V}$ the associated vertices. The edges are directed from $i$ to $j$, but the sense is arbitrary and can be ignored. For each vertex, $v\in\mathcal{V}$, we assign a smoothed inverse depth value that we denote $v_{\xi}$, and an auxiliary variable $\mathbf{w}\in\mathbb{R}^{2}$ that represents the 2D projection of a 3D normal, such that $v_{\mathbf{w}}:=$ $(v_{w_{1}},v_{w_{2}})$. We encapsulate in $v_{\mathbf{x}}:=(v_{\xi},v_{\mathbf{w}})$ all the (primal) variables associated to a vertex $v$. We also denote by $\mathbb{v}_{\xi}$ the inverse depth of all the vertices of the mesh stacked into a column vector. $\langle\cdot,\cdot\rangle$ is the inner product between two vectors.

It is well-known from the computer graphics literature [34, Section 13.5] that the inverse depth map $\mathbf{b}$ depends linearly on the inverse depths $\mathbb{v}_{\xi}$ of the vertices of the mesh:

where the map $A$ encodes the barycentric coordinates of each pixel with respect to the triangle it belongs to. Here, $\mathbf{b}$ is a column vector with as many entries as there are pixels in the depth image, while $\mathbb{v}_{\xi}$ has as many entries as there are vertices in the mesh. Therefore, given a 2D mesh (in image space) with fixed topology, we can fit the inverse depths of the vertices such that the mesh minimizes the fit to a given depth-map in a least-squares fashion. In other words, the loss function for mesh reconstruction could be:

where $\mathbf{b}$ is now the measured inverse depth, and $A\mathbb{v}_{\xi}$ is the estimated inverse depth from the mesh. The optimal value of $\mathbb{v}_{\xi}$ that solves $\min_{\mathbb{v}_{\xi}}\mathcal{L}_{\text{depth}}$ is simply given by the normal equations. Nevertheless, we are not just interested in fitting a 3D mesh to the depth map, but rather in summarizing the depth map into a smooth 3D surface. Moreover, we expect our depth map to potentially contain outlier measurements. Therefore, we want to formulate our loss function such that it is robust to outliers, and we want to penalize solutions that are not smooth surfaces.

On the one hand, to reduce the influence of outliers, we can make use of the $\ell_{1}$ norm instead. In addition, we also want to quantify the difference between the estimated inverse depth $v_{\xi}$ and the inverse depth $v_{z}$ given by the visual odometry pipeline, which we denote by $\mathcal{L}_{\text{tracking}}$. In summary, our data loss $\mathcal{L}_{\text{data}}$ consists of two terms, $\mathcal{L}_{\text{depth}}$ and $\mathcal{L}_{\text{tracking}},$ defined as:

where $\mathcal{V}$ is the set of vertices of the mesh and $\mathcal{D}$ is the set of depth measurements. $\mathbf{a}_{d},b_{d}$ are the $d^{\text{th}}$ row of $A$ and $\mathbf{b}$ associated to the $d^{\text{th}}$ depth measurement in $\mathcal{D}$. While [23, 11, 10] only used the sparse features from VO tracking to estimate the mesh (using Delaunay triangulation), we further add Steiner points [35, Section 14.1], including the set of sparse features tracked by the VO. A Steiner point is an extra mesh vertex that is added to the optimization problem to create a better fit than it would be possible using only the tracked features from VO alone. On the other hand, to promote smooth 3D meshes, we can penalize deviations from piecewise affine solutions. We will quantify this desire by using the $\mathcal{L}_{smooth}$ term below.

We formulate this multi-objective optimization via scalarization, which introduces the free parameter $\lambda>0$ balancing the two objectives:

where $\mathcal{L}_{\text{data}}=\mathcal{L}_{\text{depth}}+\mathcal{L}_{\text{tracking}}$. We could also balance the contribution of $\mathcal{L}_{\text{depth}}$ and $\mathcal{L}_{\text{tracking}}$ with another free parameter, but we avoid this for simplicity.

There are many possible choices for $\mathcal{L}_{smooth}$, but one that is particularly interesting is the second-order Total Generalized Variation ($\text{TGV}^{2}$) functional [36], because it promotes piecewise affine solutions. A non-local extension was proposed by [37] that allows using additional priors into the regularization term ($\text{NLTGV}^{2}$). Pinies et al.[38] showed that piecewise planarity in image space, afforded by $\text{NLTGV}^{2}$, translates to piecewise planarity in 3D space, when optimizing over the inverse depth. Leveraging this insight, Greene and Roy proposed FLaME [11] which discretizes $\text{NLTGV}^{2}$ for 3D meshes (instead of dense inverse depth maps), thereby achieving substantial computational savings. Similarly to FLaME [11], we will make use of the discrete $\text{NLTGV}^{2}$ to smooth our 3D mesh, which we now explain for completeness.

First, we need to introduce a key fact [38]: if two pixels $v_{\mathbf{u}}^{i}$ and $v_{\mathbf{u}}^{j}$ belong to the same planar surface in 3D, their inverse depth values $v^{i}_{\xi}$ and $v^{j}_{\xi}$ are constrained by the following relation:

where $v^{i}_{\mathbf{w}}$ is the normal vector of the surface at vertex $v^{i}.$

The $\text{NLTGV}^{2}$ smoothing term tries to penalize solutions that deviate from this relationship. Hence, the $\text{NLTGV}^{2}$ term over the mesh is given by:

Here $D_{e}$ is a linear operator that acts on the vertices $v_{\mathbf{x}}^{i}$ and $v_{\mathbf{x}}^{j}$ corresponding to edge $e$, where we recall that $v_{\mathbf{x}}:=(v_{\xi},v_{\mathbf{w}})$:

where $v_{\mathbf{u}}^{i}$ and $v_{\mathbf{u}}^{j}$ are the 2D pixel projections of the 3D vertices $v^{i}$ and $v^{j}$ of the mesh. The auxiliary variables $v^{i}_{\mathbf{w}}$, $v^{j}_{\mathbf{w}}$ are related to the 2D projection of the 3D normal of the plane associated to vertices $v^{i}$ and $v^{j}$. Intuitively, the first row of $\|{D}_{e}\left(v_{\mathbf{x}}^{i},v_{\mathbf{x}}^{j}\right)||_{1}$ penalizes the total deviation of the vertices from the expected plane going through the vertices, while the second and third row penalize having different normals for adjacent vertices. Weights $e_{\alpha},e_{\beta}\geq 0$ are assigned to each edge, and control the influence of the smoothness term. Setting one of these weights to $0$ would cancel the enforcement of planarity between vertices of the mesh. As in [11], we set the edge weight $e_{\alpha}=1/\|v_{\mathbf{u}}^{i}-v_{\mathbf{u}}^{j}\|_{2},$ i.e. inversely proportional to the edge length in pixels, and $e_{\beta}=1$. It will be useful in the remaining to express the operator ${D}_{e}$ as a matrix, in particular, we have:

The operator ${D}_{e}$ is applied on the per-edge vector of primal variables, namely: $[v_{\mathbf{x}}^{i},v_{\mathbf{x}}^{j}]=[v_{\xi}^{i},v_{w_{1}}^{i},v_{w_{2}}^{i},v_{\xi}^{j},v_{w_{1}}^{j},v_{w_{2}}^{j}].$

The resulting loss function combining the data terms and $\text{NLTGV}^{2}$ smoothness term is:

Eq. 4 is a non-smooth convex optimization problem. The lack of smoothness is due to the use of non-differentiable $\ell_{1}$ norms. Fortunately, this type of problems can be solved using first-order primal-dual optimization [39].

Let us first briefly introduce the primal-dual optimization proposed by Chambolle and Pock [39]. The problem considered is the nonlinear primal problem:

where the map $K:X\rightarrow Y$ is a continuous linear operator, and $G:X\rightarrow[0,+\infty)$ and $F:Y\rightarrow[0,+\infty)$ are proper, convex, lower semi-continuous functions.

This nonlinear problem can be formulated as the following saddle-point problem [39]:

where $F^{*}$ is the convex conjugate of $F$. From now on, we refer to variable $x$ as the primal, and $y$ as the dual. Chambolle and Pock proposed to solve this saddle-point problem by performing descent steps on the primal, and ascent steps on the dual. In particular, for each iteration $n\geq 0$, the algorithm updates $x^{n},y^{n},\bar{x}^{n}$ as follows:

where $\sigma,\tau>0$ are the dual and primal steps, and $\theta\in[0,1]$. $K^{*}$ is the adjoint of $K$. $(I+\tau\partial F^{*})^{-1}$ and $(I+\tau\partial G)^{-1}$ are the resolvent operators, defined as:

The main assumption of [39] is that the resolvent operators for $F$ and $G$ are easy to compute.

As noted by Greene and Roy [11], the summation over the edges $\mathcal{E}$ and the summation over the vertices $\mathcal{V}$ in Eq. 4 can be encoded in $F({K}\mathbf{x})$ and $G(\mathbf{x}),$ respectively, in Eq. 5. It remains to be seen where does the third term $|\mathbb{a}_{d}\mathbb{v}_{\xi}-b_{d}|$ fit.

On the one hand, we could consider the term $|\mathbb{a}_{d}\mathbb{v}_{\xi}-b_{d}|$ to be part of $G(\mathbf{x})$, but then we need to find its proximal operator¹¹1The proximal operator is the resolvent of the subdifferential operator.. Unfortunately, there is no known closed-form proximal operator for the $\ell_{1}$ norm of an affine function [40]. On the other hand, we can consider it part of $F({K\mathbf{x}})$. In this case, we need to dualize the function with respect to $G(\mathbf{x})$. Unfortunately, doing so will introduce as many dual variables as pixels in the image, which ideally we would like to avoid for computational reasons. We proceed hence with the second choice, and implement a fast GPU-based solver (Fig. 3).

The dual representation of $\|{A}\mathbb{v}_{\xi}-{\mathbf{b}}\|_{1}$ is given by (Appendix VI):

where $\delta_{P}(\mathbb{p})$ is the convex conjugate of the $\ell_{1}$ norm, and corresponds to the indicator function of the set $P=\{\mathbf{p}:\|\mathbf{p}\|_{\infty}\leq 1\}$. Together with the dual representation of the $\text{NLTGV}^{2}$ term [11]:

where each $e_{\mathbf{q}}\in\mathbb{R}^{3}$ corresponds to a dual variable for each edge, and $\delta_{Q}(e_{\mathbf{q}})$ is also the convex conjugate of the $\ell_{1}$ norm. We have the primal-dual formulation of our original problem:

Let us now formulate this primal-dual optimization problem under the formalism of Eq. 6. In particular, Eq. 8 can be cast as Eq. 6 by defining $F^{*}$ and $G$ as follows:

We now detail the resolvent operators $\left(I+\sigma\partial F^{*}\right)^{-1}$ and $(I+\tau\partial G)^{-1}$. Since $F^{*}$ is the sum of separable functions, we can first find the resolvent for $q$ and then $p$. For the indicator function of a convex set $\delta_{Q}(q)$, the resolvent operator reduces to point-wise projectors onto unit $\ell_{\infty}$ balls. Similarly, for the sum of an indicator function of a convex set $\delta_{P}(\mathbf{p})$ and the linear map $\lambda\langle\mathbf{b},\mathbf{p}\rangle$, the resolvent operator remains a point-wise projector onto the unit $\ell_{\infty}$ ball, where the argument is offset by ${\lambda\mathbf{b}}$.

Similarly, the resolvent operator for $G(\mathbf{v}_{\xi})$ is given by the pointwise shrinkage operations:

We now detail the primal descent and dual ascent steps. First, we need to define the linear operator $K$ for the dual ascent. The dual ascent over $\mathbf{q,p}$ is given by:

where we use $D$ to represent the linear operator that stacks all of the ${D}_{e}$ operators acting on a per-edge basis. $\sigma>0$ is the dual step size.

Let us now define the adjoint $K^{*}$ for the primal descent over $v_{\xi}$. Since we have formulated our linear operators in terms of matrices ${D}_{e}$ and ${A}$, the adjoints are simply the transpose of these matrices: ${D}_{e}^{*}={D}_{e}^{T}$ and ${A}^{*}={A}^{T}$. We now further decompose ${D}_{e}^{*}$ to make the subsequent primal descent iteration explicit. ${D}_{e}^{*}$ operates on the incoming edges $\mathcal{N}_{in}(v)$ and outgoing edges $\mathcal{N}_{out}(v)$ to a vertex $v$, as noted by [11], and therefore can be decomposed:

where ${D}_{in}^{*}$ and ${D}_{out}^{*}$ partition $D_{e}^{*}$ into the first and last three rows, respectively. Furthermore, we need to make explicit the primal updates on the inverse depths, since the adjoint $A^{*}$ only affects these. Therefore, we use that:

The descent equations for each $v_{\xi}$ are then given by:

where $\tau>0$ is the primal step size, and $[\cdot]_{v}$ in $[{A}^{*}\mathbf{p}^{n+1}]_{v}$ extracts the row corresponding to vertex $v$. The descent equations for each $v_{\mathbf{w}}$ are given by:

With the primal descent Eqs. 10 and 11 and dual ascent Eq. 9 in hand, we can now iteratively apply Eq. 7 until convergence.

In summary, our optimization proceeds as follows:

and we implement the dual step in the GPU as illustrated in Fig. 3.

The EuRoC dataset provides ground-truth 3D reconstructions of the scene for the Vicon Room (V) datasets. Therefore, we can directly evaluate the accuracy of the per-frame 3D mesh reconstructions. Nevertheless, the dataset was recorded with a stereo camera instead of an RGB-D camera. Hence, we perform dense stereo reconstruction (using OpenCV’s block-matching approach) to fit our 3D mesh.

Table I compares our approach with two VO pipelines, LSD-SLAM[41] and MLM[42], as well as FLaME [11], which is the work closest to our approach. LSD-SLAM is a semi-dense VO pipeline, and as such, it partially reconstructs the scene. While being semi-dense, LSD-SLAM remains practically unable to reconstruct the scene, staying below $25\%$ in terms of accurate depth estimates. MLM is built on top of LSD-SLAM and was designed to densify LSD-SLAM’s map, as such, it clearly achieves denser reconstructions than its counterpart (approximately $50\%$ denser reconstructions). FLaME further densifies the 3D reconstruction by using 3D meshes and a primal-dual formulation like ours. Their approach clearly improves the MLM estimates (by an average factor of $42\%$), but most importantly, provides topological information with the 3D mesh. Our approach further uses the depth data coming from RGB-D or stereo, and fuses this information into the 3D mesh, which further densifies the 3D reconstruction, while also keeping the topological properties of the scene. Fig. 4 shows the quality of the reconstruction in EuRoC.

Using the RGB-D images in uHumans2 dataset, we run and evaluate our approach using an increasing number of Steiner points. Table II shows that increasing the number of Steiner points has a clear positive effect on the quality of the depth estimates. The improvements are most noticeable when comparing the use of no Steiner points (No Steiner), and adding Steiner points every 100 pixels ($S=100$). Subsequently, the addition of Steiner points increases the accuracy up to around one Steiner point every 10 pixels. Note that the computation time increases according to the number of points added, and beyond $S=50$ our approach might not be fast enough for real-time operations. Fig. 2 shows a reconstruction using the $S=50$ configuration.

All timings were processed using an Nvidia RTX 2080 Ti GPU on an Intel i9 CPU. The computation of the dual contribution takes less than $2.1$ms overall (worst case timing recorded, including data transfer). Nevertheless, since this operation needs to be repeated several times until convergence, we record an average processing time of $13.2$ms. This includes performing the Delaunay triangulation, uploading depth and 2D mesh coordinates to the GPU, solving the dual problem, sending the primal contribution to the CPU, optimizing, and repeating the process until convergence (for $6$ iterations). Ultimately, the bottleneck in terms of computation comes from Kimera-VIO which takes $20$ms per frame, despite being an optimized VIO pipeline that has its frontend and backend working in parallel.