Adaptive Direct RGB-D Registration and Mapping for Large Motions

Abstract

Dense direct RGB-D registration methods are widely used in tasks ranging from localization and tracking to 3D scene reconstruction. This work addresses a peculiar aspect which drastically limits the applicability of direct registration, namely the weakness of the convergence domain. First, we propose an activation function based on the conditioning of the RGB and ICP point-to-plane error terms. This function strengthens the geometric error influence in the first coarse iterations, while the intensity data term dominates in the finer increments. The information gathered from the geometric and photometric cost functions is not only considered for improving the system observability, but for exploiting the different convergence properties and convexity of each data term. Next, we develop a set of strategies as a flexible regularization and a pixel saliency selection to further improve the quality and robustness of this approach.

The methodology is formulated for a generic warping model and results are given using perspective and spherical sensor models. Finally, our method is validated in different RGB-D spherical datasets, including both indoor and outdoor real sequences and using the KITTI VO/SLAM benchmark dataset. We show that the different proposed techniques (weighted activation function, regularization, saliency pixel selection), lead to faster convergence and larger convergence domains, which are the main limitations to the use of direct methods.

Appendix A: Error Jacobians and Optimization

The pose $\mathbf {T(x)} \in \mathbb {SE}(3)$ is parametrized as function of angular and linear velocities $\mathbf {x} = (\varvec{\upsilon }\delta t,\varvec{\omega }\delta t) \in \mathbb {R}^6$ and the optimization will be related to this twist parametrization. The pose is related to the twist velocities by the exponential mapping $\mathbf {T(x)} = \exp (se3(\mathbf {x}))$, with

$$\begin{aligned} se3(\mathbf {x}) = \left[ \begin{array}{cc} \mathbf {S}(\varvec{\omega })\delta t &{} \varvec{\upsilon } \delta t\\ \mathbf {0}_{(1\times 3)} &{} 0 \end{array} \right] \in \mathfrak {se}(3) \end{aligned}$$

(11)

which is the Lie algebra of $\mathbb {SE}(3)$ at the identity element, $\mathbf {S(z)}$ represents the skew symmetric matrix associated to vector $\mathbf {z}$ and $\delta t = 1$.

The respective Jacobians will be derived following this parametrization. We ask the reader to see [19] for details about the photometric Jacobian $\mathbf {J}^I$. Next, for the geometric point-to-plane direct Jacobian $\mathbf {J}^D \in \mathbb {R}^{1\times 6}$, we denote the 3D point error $\mathbf {\zeta }(\mathbf {x})$:

$$\begin{aligned} \begin{array}{cl} \mathbf {\zeta }(\mathbf {x}) &{} = - \mathbf {\hat{T}}\mathbf {T(x)}\left[ \begin{array}{c} g^*(\mathbf {p}))\\ 1 \end{array} \right] + g(w(\mathbf {p},\mathbf {\hat{T}}\mathbf {T(x)})) \\ &{} = -\mathbf {\hat{R}}\mathbf {R(x)}g^*(\mathbf {p}) - \mathbf {\hat{R}t(x)} - \mathbf {\hat{t}} + g(w(\mathbf {p},\mathbf {\hat{T}}\mathbf {T(x)})) \end{array} \end{aligned}$$

(12)

From Eqs. (3), (12) and the product rule:

$$\begin{aligned} \mathbf {J}^D(\mathbf {0}) = \begin{array}{l} \lambda _D\mathbf {n^*}^T \left( \frac{\partial (\mathbf {R(x)}^T\mathbf {\hat{R}}^T\mathbf {\zeta }(\mathbf {z}))}{\partial \mathbf {x}}\Bigg |_{\mathbf {z = x}} + \mathbf {R(x)}^T\mathbf {\hat{R}}^T \frac{\partial (\mathbf {\zeta }(\mathbf {x}))}{\partial \mathbf {x}}\right) \Bigg |_{\mathbf {x = 0}} \end{array} \end{aligned}$$

(13)

For clarity, the first term in Eq. (13) is $\mathbf {{J}_{d1}}$ and we decompose the second term in two Jacobians $\mathbf {{J}_{d2}}$ and $\mathbf {{J}_{d3}}$, such as $\mathbf {J}^D(\mathbf {0}) = \lambda \mathbf {n^*}^T (\mathbf {{J}_{d1}} (\mathbf {0}) + \mathbf {{J}_{d2}}(\mathbf {0}) + \mathbf {{J}_{d3}}(\mathbf {0}))$. From $\frac{\partial (\mathbf {R(x)}\mathbf {\zeta })}{\partial \mathbf {x}} = \frac{\partial (\mathbf {R(x)}\mathbf {\zeta })}{\partial \mathbf {R(x)}} \frac{\partial \mathbf {R(x)}}{\partial \mathbf {x}}$ the first term is

$$\begin{aligned} \mathbf {{J}_{d1}}(\mathbf {0}) = \left[ \begin{array}{c}\mathbf {0}_{3\times 3} ~~~ \mathbf {S(\mathbf {\hat{R}}^T\mathbf {\zeta }(\mathbf {0}))} \end{array} \right] \end{aligned}$$

(14)

The second term is decomposed in two Jacobians

$$\begin{aligned} \mathbf {{J}_{d2}}(\mathbf {0}) = \left[ \begin{array}{c}- \mathbf {I}_{3\times 3} ~~~ \mathbf {S}(g^*(\mathbf {p})) \end{array} \right] \end{aligned}$$

(15)

And finally the last Jacobian is the one corresponding to $\frac{\partial (g(w(\mathbf {p},\mathbf {\hat{T}}\mathbf {T(x)}))}{\partial \mathbf {x}}$. This derivative can be seen as an extended version of the image photometric gradient $\mathbf {J^I}$, for each component of $g(w(\mathbf {p},\mathbf {\hat{T}}\mathbf {T(x)})$. Then

$$\begin{aligned} \mathbf {{J}_{d3}}(\mathbf {0}) = \left[ \begin{array}{c}\mathbf {J_g}\big |_{[g(\mathbf {p}_w)]_1}^T ~ \mathbf {J_g}\big |_{[g(\mathbf {p}_w)]_2}^T ~ \mathbf {J_g}\big |_{[g(\mathbf {p}_w)]_3}^T \end{array} \right] ^T\mathbf {J_wJ_T} \end{aligned}$$

(16)

And $\mathbf {J_g}\big |_{[g(\mathbf {p}_w)]_i}$ is the image gradient (as in the photometric term) of an image produced with the ith-coordinate of $g(w(\mathbf {p},\mathbf {\hat{T}}\mathbf {T(0)}))$. Note that this Jacobian is small for points belonging to planar surfaces. Therefore, $\mathbf {{J}_{d3}}$ is neglected since only a fraction of the scene is on geometric discontinuities and since these points have higher sensitivity to depth error estimates and self-occlusions effects. Finally, we use the ESM formulation [19] for defining the optimization step for the RGB cost, while a Gauss-Newton step is employed for the geometric Jacobian. The reader is asked to see [33] for more details on the different optimization available techniques.