视觉里程计，即仅仅依靠视觉来跟踪机器人的位置，在计算机视觉和机器人领域有着悠久的历史。激光扫描仪常用于估计机器人的运动。激光扫描通常使用迭代最近的点(ICP)匹配算法的变体。一般的想法是迭代地分配两个扫描之间的对应关系，并匹配它们。在实践中，由于配置倾向于加强可能次优的初始对应关系，这种变化(alternation)局部最小值容易出现。这个问题在一定程度上得到了缓解，而不是点对点分配。相比之下，许多最先进的方法使用单目相机图像提取关键点，并通过描述子匹配等一系列处理步骤RANSAC与之前图像的关键点匹配光束调整。虽然关键点的稀疏度大大提高了计算速度，但与场景相关的信息却丢失了很多。Newcombe等人最近通过GPGPU并行化，利用单目图像流构建密集的视觉里程和3D表面重建。正如我们将在本文中展示的，比如微软Kinect这样的RGB-D传感器打开了直接从输入数据构建视觉里程计的新思路，大大降低了计算成本。

我们提出了一种利用密集能量最小化的方法RGB-D图像构建视觉里程计。关键是通过最小化反投影误差来解决潜在的反向问题。我们的目标是从特殊的欧几里得群 S E ( 3 ) SE(3) SE(3)找到刚体变换 g g g表示相机运动，使匹配的第二个图像与第一个图像完全匹配。我们通过序贯凸优化来最小化这种非凸能量。我们线性化能量，解决刚体运动的三维扭转坐标(twist coordinates)方程。由于线性化只适用于小扭转，我们采用从粗到精的策略来处理大型相机运动。

我们在最近提出的数据集的图像序列中验证了我们的方法，并验证了我们的方法和最先进的方法ICP实现，广义ICP(GICP)，性能比较。虽然我们发现了ICP更大的相机位移更鲁棒，但我们的评估表明，我们的方法在相机运动时提供了更好的结果。此外，我们的方法比ICP快两个数量级。

令 I R G B : Ω × R → [ 0 , 1 ] 3 , ( x , t ) ? I R G B ( r , t ) (1) I_{RGB}: \ \Omega \times \mathbb{R}_{ } \rightarrow \ [0, 1]^3, \ \ \ \ (x,t) \mapsto I_{RGB}(r,t) \tag{1} IRGB:Ω×R+​→ [0,1]3,    (x,t)↦IRGB​(r,t)(1) h :   Ω × R + →   R + ,      ( x , t ) ↦ h ( x , t ) (2) h:\ \Omega \times \mathbb{R}_{+} \rightarrow \ \mathbb{R}_{+}, \ \ \ \ (x,t) \mapsto h(x,t) \tag{2} h: Ω×R+​→ R+​,    (x,t)↦h(x,t)(2) 表示 t ∈ R + t\in \mathbb{R}_{+} t∈R+​时刻RGB-D传感器获取的图像平面 Ω ⊂ R 2 \Omega \subset \mathbb{R}^2 Ω⊂R2上的彩色图像数据和高度场，其中 I R G B I_{RGB} IRGB​给出了RGB值， h h h给出了深度值，单位为米。根据深度场信息，我们可以计算表面 S S S， S :   Ω → R 3 ,      x ↦ S ( x ) (3) S:\ \Omega \rightarrow \mathbb{R}^3, \ \ \ \ x \mapsto S(x) \tag{3} S: Ω→R3,    x↦S(x)(3) S ( x ) = ( ( x + o x ) ⋅ h ( x ) f x ,   ( y + o y ) ⋅ h ( x ) f y ,   h ( x ) ) T S(x) = \bigg(\frac{(x+o_x)\cdot h(x)}{f_x}, \ \frac{(y+o_y)\cdot h(x)}{f_y}, \ h(x)\bigg)^T S(x)=(fx​(x+ox​)⋅h(x)​, fy​(y+oy​)⋅h(x)​, h(x))T 其中 ( o x , o y ) T (o_x,o_y)^T (ox​,oy​)T表示相机的像主点， f x f_x fx​和 f y f_y fy​表示焦距。

我们用向量 ξ = ( ω 1 , ω 2 , ω 3 , v 1 , v 2 , v 3 ) T ∈ R 6 \xi=(\omega_1,\omega_2,\omega_3,v_1,v_2,v_3)^T\in \mathbb{R}^6 ξ=(ω1​,ω2​,ω3​,v1​,v2​,v3​)T∈R6表示李群 S E ( 3 ) SE(3) SE(3)中刚体运动 g g g的六个自由度。在 t ∈ R + t\in \mathbb{R}_{+} t∈R+​时刻的李代数 s e ( 3 ) se(3) se(3)中，它定义了一个扭转(twist)， ξ ^ = ( 0 − ω 3 ω 2 v 1 ω 3 0 − ω 1 v 2 − ω 2 ω 1 0 v 3 0 0 0 0 ) (4) \hat{\xi}=\begin{pmatrix} 0 & -\omega_3 & \omega_2 & v_1 \\ \omega_3 & 0 & -\omega_1 & v_2 \\ -\omega_2 & \omega_1 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{pmatrix} \tag{4} ξ^​=⎝⎜⎜⎛​0ω3​−ω2​0​−ω3​0ω1​0​ω2​−ω1​00​v1​