CN106875482A - 一种同时定位与稠密三维重建方法 - Google Patents

Abstract

本发明公开了一种同时定位与稠密三维重建方法，该方法能够鲁棒的处理快速运动和频繁回路闭合，在较大规模的场景下可以无限时间运行。该方法除了可以用来扫描高质量的三维模型，还可以满足虚拟现实和增强现实的需求。该方法提出了一种基于关键帧的同时定位与地图构建的框架。首先，同时利用深度和颜色信息，基于关键帧的定位可以在CPU上高速运行，并且可以鲁棒地运行在具有挑战性的场景下。为减少累计误差，本发明引入了增量集束调整，可以极大的减少运算量，同时将局部和和全局集束调整在一个统一的框架下完成。其次，本发明提出了基于关键帧的一种融合方法，可以在线生成模型并在关键帧姿态调整时实时更新三维模型。

Description

一种同时定位与稠密三维重建方法

技术领域

本发明涉及一种可应用于增强现实、虚拟现实、机器人导航等领域的同时定位与稠密三维重建方法，尤其涉及一种同时定位与稠密三维重建方法。

背景技术

同时定位与地图构建(SimultaneouslyLocatingandMapping，SLAM)是机器人和计算机视觉领域的一个基本问题。在过去的几时年来，实时运动结构学(StructurefromMotion)和视觉SLAM有很多成功的应用。然而，视觉SLAM难以处理无纹理的场景，而且要做到实时稳定地恢复稠密三维结构也比较困难。使用深度传感器可以帮助解决这两个问题。随着深度传感器(如微软Kinect和英特尔RealSense 3D相机)的流行，大量基于RGB-D的SLAM方案得到提出。

大多数稠密的SLAM方法使用帧到帧或帧到模型的对准方法，这类方法很容易累计误差，在具有挑战性的场景下容易失败。一些方法提出使用非刚性网格变形优化模型，减小累积误差。然而由于不精确的对准导致的模型误差无法得到消除，并且会进一步增大对准误差。

最近，BundleFusion[A.Dai,M.Nieβner,M.S.Izadi,andC.Theobalt.BundleFusion:Real-time globally consistent 3d reconstruction usingonline surface re-integration.arXiv preprint arXiv:1604.01093,2016.]提出了端到端的实时重建***。BundleFusion使用所有的RGB-D输入数据，层次化的优化所有的相机姿态和三维模型。不同于前面的帧到模型的对准方法，BundleFusion采用每一帧和所有帧的暴力匹配求解相机姿态。为此，BundleFusion使用两颗强GPU(NVIDIA GeForce GTXTitan X和GTX Titan Black)以达到实时运行。同时，它保存所有的输入数据，有极大的内存消耗从而限制了其运行场景大小和运行时间。

近年来，许多研究者提出了不同的基于RGB-D相机进行稠密三维重建和实时同事定位与地图构建的方法。Huang[A.S.Huang,A.Bachrach,P.Henry,M.Krainin,D.Maturana,D.Fox,and N.Roy.Visual odometry and mapping for autonomous flight using anRGB-D camera.In Robotics Research-The 15th International Symposium ISRR,9-12December 2011,Flagstaff,Arizona,USA,pages 235–252,2011.]等人提出使用RGB-D数据进行实时的运动测量，使用稀疏的BA进行离线稠密三维重建。Endres[F.Endres,J.Hess,J.Sturm,D.Cremers,and W.Burgard.3-d mapping with an rgb-d camera.IEEETransactions on Robotics,30(1):177–187,2014.]等提出使用视觉特征结合深度信息估计相机姿态，用三维网格占据率表示场景。Kerl[C.Kerl,J.Sturm,and D.Cremers.Densevisual SLAM for RGB-D cameras.In 2013 IEEE/RSJ International Conference onIntelligent Robots and Systems,Tokyo,Japan,November 3-7,2013,pages 2100–2106,2013]等提出使用稠密的RGB-D数据，直接最小化光学误差和深度误差来实现相机姿态估计，这种方法比基于稀疏视觉特征的方法具有更高的精度。Newcombe[.R.A.Newcombe,S.Izadi,O.Hilliges,D.Molyneaux,D.Kim,A.J.Davison,P.Kohli,J.Shotton,S.Hodges,and A.W.Fitzgibbon.Kinectfusion:Real-time dense surface mapping andtracking.In 10th IEEE International Symposium on Mixed and Augmented Reality,ISMAR 2011,Basel,Switzerland,October 26-29,2011,pages 127–136,2011]等提出了KinectFusion，使用基于体素的表示重建三维模型，通过ICP算法将每一帧对准到模型上，实验表明其在小尺度下有很好的重建质量。然而，受制于GPU内存的限制，KinectFusion不适用于大尺度的场景重建。同时，KinectFusion也不能处理漂移问题和回路闭合。许多工作对KinectFusion进行了改进，这些工作主要集中在使用更有效的数据结构实现大尺度场景的重建。例如，Kintinuous[T.Whelan,J.McDonald,M.Kaess,M.Fallon,H.Johannsson,andJ.Leonard.Kintinuous:Spatially extended Kinect-Fusion.In RSS Workshop on RGB-D:Advanced Reasoning with Depth Cameras,Sydney,Australia,Jul 2012.]使用移动体积的方法改进KinectFusion。Nieβner[M.Nieβner,M.S.Izadi,andM.Stamminger.Real-time 3d reconstruction at scale using voxel hashing.ACMTrans.Graph.,32(6):169:1–169:11,2013.]等提出用空间哈希的方法存储稀疏的体素信息。然而，这些方法都没有解决相机姿态估计的漂移问题和回路闭合所要求的模型调整。

无漂移的相机姿态估计和稀疏重建在视觉SLAM中得到了大量的研究。许多单目SLAM方法可以在小场景甚至街道大小的场景进行实时相机跟踪与稀疏三维重建。一些方法也可以处理重定位与回路闭合。然而，这些方法都不能产生稠密的三维模型。一些可以实时重建稠密三维模型的方法大都受制于小的场景或没有考虑无漂移的三维稠密重建。

一些离线方法可以通过回路闭合实现无漂移的稠密三维重建，然而其速度远达不到实时。最近，Whelan[T.Whelan,M.Kaess,H.Johannsson,M.F.Fallon,J.J.Leonard,andJ.McDonald.Real-time large-scale dense RGB-D SLAM with volumetricfusion.I.J.Robotics Res.,34(4-5):598–626,2015.]等提出基于体素融合的RGB-D SLAM***可以在大场景下检测到回路闭合，通过尽量刚性的模型表面变形纠正相机漂移带来的模型误差。ElasticFusion[T.Whelan,S.Leutenegger,R.F.Salas-Moreno,B.Glocker,andA.J.Davison.ElasticFusion:Dense SLAM without A pose graph.In Robotics:Scienceand Systems XI,Sapienza University of Rome,Rome,Italy,July 13-17,2015,2015.]使用面片来表示三维模型，在回路闭合的时同样采用尽量刚性的模型变换矫正三维模型。这些方法使用帧到模型的对准，对准时的误差会影响模型精度，而不精准的模型又会降低相机追踪的精度。在回路闭合时使用表面变形无法纠正模型错误，最终会导致在复杂场景下追踪失败。BundleFusion[A.Dai,M.Nieβner,M.S.Izadi,andC.Theobalt.BundleFusion:Real-time globally consistent 3d reconstruction usingonline surface reintegration.arXiv preprint arXiv:1604.01093,2016.]使用暴力匹配进行每一帧的对准，在调整姿态时重新集成，从而获得全局一致的模型。然而，BundleFusion保存所有的输入数据，限制了其在长序列数据上的运行。同时，即使用很强的GPU，在长序列输入时使用暴力匹配也是非常耗时的。本发明使用基于关键帧的表示和追踪框架，可以不断调整相机姿态并重新融合深度图。本发明的内存消耗是和关键帧的数据成线性的，因此在相当规模的场景下，由于关键帧的数目受限，可以无限长时间的运行。

发明内容

本发明针对上述技术的不足，提出了一种同时定位与稠密三维重建方法，能够可靠的处理强烈旋转和快速运动，支持重定位并能实时地构建与调整稠密三维模型。

本发明的同时定位与稠密三维重建方法包括如下步骤：

1)通过RGB-D对准估计相机初始姿态，使用基于单应性的特征匹配优化相机姿态；

2)运行“增量集束调整”算法，自适应地更新、优化关键帧的相机姿态；

3)将非关键深度图融合到关键帧，基于关键帧的深度图重建三维模型，通过重融合在相机姿态改变时更新三维模型。

通过RGB-D对准估计相机初始姿态，使用基于单应性的特征匹配优化相机姿态的具体步骤为：

对每个当前帧F_i，首先使用RGB-D对准估计相机从上一帧到当前帧的相对姿态T_(i-1)→i，这里只需要一个相对粗糙的估计，因此使用小模糊图(SBI)估计两帧相对姿态以在CPU上达到实时运行。其中参考文献列举的方法为可选用的现有方法。

精确的相机姿态是通过特征匹配获得的，为处理剧烈旋转和快速运动，本发明使用基于单应性的特征匹配方法求解相机姿态。在剧烈旋转和快速运动下，两帧之间的透视变形可能会很大，以至于基于特征的匹配不能鲁棒运行。单应性可以用来纠正图片块的透视变形，从而使零均值的SSD可行。单应性也可以用来决定一个较小的搜索区域避免沿着整个对极线搜索。在本发明中，由于有深度信息z_k以及使用RGB-D对准的相机姿态估计C_i＝T_(i-1)→iC_i-1，因此将搜索区域定为以及

在确定了相对姿态T_(i-1)→i以及3D-2D特征匹配的集合χ＝{(X_j,x_j)}后，通过最小化相对误差与重投影误差来估计相机姿态C_i：

这里，log(T)将3D刚性变换T∈SE(3)映射到se(3)，并返回R⁶中的向量表示。是标准马氏距离。是投影函数，K是已知并且固定的相机内参。z(X)从X中提取出z分量，z_j是x_j在当前帧的深度。||∈||_δ是Huber范数：

如果追踪失败，则重新跟踪特征。否则，则使用新特征的深度信息，用贝叶斯方法对特征的深度进行滤波。对关键帧F_k和当前帧F_i的每一对特征匹配(x_i,x_j)，获取x_k深度的倒数ρ_k：

这里，通过ρ_i→k，滤波器不断更新ρ_k和其内点的联合分布。

最后，本发明在每一新帧到来时决定是否选作关键帧。

所述的运行“增量集束调整”算法，自适应地更新、优化关键帧的相机姿态，其具体步骤为：

当有新的关键帧加入或者检测到闭合回路的时候，需要运行BA。在前一种情形下，由于只有局部地图调整，没有必要运行全局BA。然而，仅运行局部BA倾向于生成次优的解，尤其在局部地图有较大偏差的情况下。因此，需要在BA中引入更多的变量，否则误差无法得到完全消除。本发明提出的增量BA可以获得和全局BA相近的解，同时运行时间和BA中改变的变量数目线性相关，因此可以大幅减少运行时间。其具体方法如下：

将当前相机运动参数定义为C_i∈SE(3)，n_c是相机个数，将当前三维点位置定义为X_j∈R³，n_p为三维点的个数，将相机i可见的三维点的集合定义为将三维点j可见的相机集合定义为用高斯-牛顿算法迭代优化：

每次迭代，将当前相机运动参数定义为将当前三维点位置定义为将相机运动参数的增量定义为将三维点位置的增量定义为将e(C_i,X_j)在处线性展开：

其中，表示在上叠加一个对应的se(3)增量，定义法向方程：

U、V分别是n_c×n_c、n_p×n_p的对角矩阵块

对每个子矩阵U_i∈R^6×6，V_j∈R^3×3，W为n_c×n_p的块矩阵：

每个子矩阵W_ij∈R^6×3，法向方程右侧向量：

每个子向量u_i∈R⁶，v_j∈R³，定义舒尔补方程

其中，

S＝U-WV^-1W^T

g＝u-WV^-1v

S为n_c×n_c的块矩阵：

每个子矩阵S_ij∈R^6×6，舒尔补方程右侧向量

对每个子向量g_i∈R⁶，定义辅助向量 其中i＝1,…,n_c，定义辅助变量其中j＝1,…,n_p，初始时刻，令U＝0,V＝0,W＝0,u＝0,v＝0,S＝0,g＝0，每次迭代，运行以下步骤：

a)对每个三维点j及其可见的相机如果或上次迭代被更新，则计算 处的并由下式更新各变量：

f)对每个三维点j，如果V_j在本次迭代中被更新，则对每个三维点j可见的相机由下式更新各变量：

并对三维点j可见的相机对由下式更新各变量：

g)使用预处理共轭梯度算法求解上述的舒尔补方程，得到相机增量对每个相机i，如果则用更新

b)对每个三维点j，如果V_j本次迭代被更新，或相机中存在本次迭代被更新，则计算如果则用更新

c)如果本次迭代没有任何相机或三维点被更新，或达到最大迭代次数，则结束循环，否则，重复步骤a)。

本发明采用基于体素的三维模型表示，每个体素保存其截断的到最近表面带符号距离(TSDF)，为提高GPU显存利用率以及可重建场景尺度，采用空间哈希方法。

当相机姿态发生调整时，相应的，需要调整重建的三维模型。一种简单的方法是存储所有的深度图，并在每一深度图对应的相机姿态调整时重新融合进三维模型。然而，这种方法有着内存消耗高、计算量大的不足。为此，本发明提出基于关键帧的深度图表示和融合、反融合方法，有效的解决了这一问题。具体的：

如果当前帧F_i被选为关键帧，则直接将其对应的深度图D_i融合到模型上。对每个体素v，其截断带符号最近表面距离(TSDF)用D(v)表示，权重用W(v)表示，对F_i中的每个像素x，采用如下方法更新其对应的体素v：

其中w_i(x)是像素x的融合权重。

如果当前帧F_i未被选为关键帧，则首先寻找与F_i最匹配的关键帧，记为首先反融合关键帧所对应的深度图。反融合过程与融合过程相似，对每个像素x，采用如下方法更新其对应的体素v：

其中w_i(x)是像素x的融合权重。

随后，通过深度图D_i投影到将其融合到对F_i的每个像素x，用y表示其投影在的坐标。如果y对应的深度的倒数与x投影点深度的倒数(用表示)小于阈值τ_d(本发明设置为0.1)，则更新y的深度为：

对每个关键帧记录其被融合的次数当大于某一阈值时，停止融合。由于F_i与的视角重合较大，除了一些在视角之外的点，通常当前帧的大多数深度都能融合到上。因此该融合策略可以有效地减少深度的冗余性。如果当前帧融合后残余的深度点数目小于τ(例如2000)，则将这些点丢弃。否则，创建一个点云集V_i保存未融合的三维点并将其关联到之后，将更新后的关键帧深度图融合到模型上。如果V_i非空，则将V_i中的三维点投影到将其融合到模型。因此，每个非关键帧，将会执行两次融合与一次反融合。由于未融合的点数目相对很少，因此其需要的融合时间也较少。因此，本发明计算量基本上略大于通常的融合方法的两倍。如果较大并且关键帧的数目不再增加，这意味着RGB-D相机没有扫描新的信息，在这种情况下，简单的丢弃D_i。

为进一步减少深度数据的冗余，如果当前帧F_i被选为关键帧，则选择关联到相邻关键帧的若干点云集，融合到当前帧。如果融合后V_I剩余的点数小于τ(例如2000)，则将其从模型中反融合并丢弃。否则，将融合到当前帧的点从模型中反融合。

当关键帧的姿态被BA调整时，需要重新融合关键帧对应的深度图以及与其关联的点云集。然而，如果被调整的关键帧数目较大，重融合会耗费大量的时间而不能满足实时性的要求。因此，本发明限制每一帧重融合的次数，将所有重融合分摊到每一帧。特别的，姿态变化比较大的关键帧对应的深度图及其关联的点云集将会被首先重融合。此策略可以保证在BA发生时重建部分以恒定的速率运行。

为进一步加速，可以每两帧或更多融合一帧到模型，此方法可以极大地加速融合过程，在笔记本电脑上可以达到实时运行。

附图说明

图1是本发明的***框架。

图2是本发明基于关键帧的相机追踪框架。

图3是本发明提出的增量BA与标准BA的性能比较，可以看到，相对于标准BA，在保留相当精度的同时，本发明可以极大的加快BA速度。

图4是本发明在使用华硕Xtion Pro Live拍摄的数据集上运行所生成的三维模型。

具体实施方式

本发明的***架构如图1。***的相机追踪部分和三维模型重建部分并行运行。对每个包含RGB图像与深度图的输入帧，相机追踪部分首先结合RGB-D对准和基于单应性的特征追踪方法计算相机姿态，实现了鲁棒的相机跟踪。同时，提取关键帧的ORB特征并进行匹配以检测全局回路闭合。最后，周期性的在后台运行增量BA以优化相机姿态。重建部分在输入帧追踪相机姿态可靠时将输入深度融合到关键帧的深度图上。为在线调整稠密的三维表面，***会对关键帧进行反融合与在融合。此策略使得在相机姿态调整时，仅需要重融合关键帧的深度以及一些额外的点云集。由于在相当规模场景下的关键帧数目是有限的，本***可以无限长时间运行。

本发明同时结合图片强度和深度信息跟踪相机运动，结合基于稠密RGB-D和基于稀疏特征的方法使相机跟踪更为准确和鲁棒。其基本框架如附图2所示。对每个当前帧F_i，首先使用RGB-D对准[C.Kerl,J.Sturm,and D.Cremers.Dense visual SLAM for RGB-Dcameras.In 2013 IEEE/RSJ International Conference on Intelligent Robots andSystems,Tokyo,Japan,November 3-7,2013,pages 2100–2106,2013.]估计相机从上一帧到当前帧的相对姿态T_(i-1)→i，这里只需要一个相对粗糙的估计，因此我们使用小模糊图(SBI)[G.Klein and D.W.Murray.Improving the agility of keyframe-based SLAM.In10th European Conference on Computer Vision,Part II,pages 802–815.Springer,2008.]估计两帧相对姿态以在CPU上达到实时运行。

精确的相机姿态是通过特征匹配获得的，为处理剧烈旋转和快速运动，本发明使用基于单应性[H.Liu,G.Zhang,and H.Bao.Robust keyframe-based monocular SLAM foraugmented reality.In IEEE International Symposium on Mixed and AugmentedReality,ISMAR 2016.]的特征匹配方法求解相机姿态。在剧烈旋转和快速运动下，两帧之间的透视变形可能会很大，以至于基于特征的匹配不能鲁棒运行。单应性可以用来纠正图片块的透视变形，从而使零均值的SSD[G.Klein and D.W.Murray.Parallel tracking andmapping for small AR workspaces.In 6th IEEE/ACM International Symposium onMixed and Augmented Reality,pages 225–234,2007.]可行。单应性也可以用来决定一个较小的搜索区域避免沿着整个对极线搜索。在本发明中，由于有深度信息z_k以及使用RGB-D对准的相机姿态估计C_i＝T_(i-1)→iC_i-1，因此将搜索区域定为以及

在确定了相对姿态T_(i-1)→i以及3D-2D特征匹配的集合χ＝{(X_j,x_j)}后，通过最小化相对误差与重投影误差来估计相机姿态C_i：

这里，log(T)将3D刚性变换T∈SE(3)映射到se(3)，并返回R⁶中的向量表示。是标准马氏距离。是投影函数，K是已知并且固定的相机内参。z(X)从X中提取出z分量，z_j是x_j在当前帧的深度。||∈||_δ是Huber范数：

如果追踪失败，则使用[G.Klein and D.W.Murray.Improving the agility ofkeyframe-based SLAM.In 10th European Conference on Computer Vision,Part II,pages 802–815.Springer,2008.]所述的方法重新跟踪特征。否则，则使用新特征的深度信息，用贝叶斯方法[G.Vogiatzis and C.Hernández.Video-based,real-time multi-viewstereo.Image Vision Comput.,29(7):434–441,2011.]对特征的深度进行滤波。对关键帧F_k和当前帧F_i的每一对特征匹配(x_i,x_j)，获取x_k深度的倒数ρ_k：

这里，通过ρ_i→k，滤波器不断更新ρ_k和其内点的联合分布。

最后，本发明在每一新帧到来时决定是否选作关键帧。

当有新的关键帧加入或者检测到闭合回路的时候，需要运行BA。在前一种情形下，由于只有局部地图调整，没有必要运行全局BA。然而，仅运行局部BA倾向于生成次优的解，尤其在局部地图有较大偏差的情况下。因此，需要在BA中引入更多的变量，否则误差无法得到完全消除。本发明提出的增量BA可以获得和全局BA相近的解，同时运行时间和BA中改变的变量数目线性相关，因此可以大幅减少运行时间。其具体方法如下：

将当前相机运动参数定义为C_i∈SE(3)，n_c是相机个数，将当前三维点位置定义为X_j∈R³，n_p为三维点的个数，将相机i可见的三维点的集合定义为将三维点j可见的相机集合定义为用高斯-牛顿算法迭代优化：

每次迭代，将当前相机运动参数定义为将当前三维点位置定义为将相机运动参数的增量定义为将三维点位置的增量定义为将e(C_i,X_j)在处线性展开：

其中，表示在上叠加一个对应的se(3)增量，定义法向方程：

U、V分别是n_c×n_c、n_p×n_p的对角矩阵块

对每个子矩阵U_i∈R^6×6，V_j∈R^3×3，W为n_c×n_p的块矩阵：

每个子矩阵W_ij∈R^6×3，法向方程右侧向量：

每个子向量u_i∈R⁶，v_j∈R³，定义舒尔补方程

其中，

S＝U-WV^-1W^T

g＝u-WV^-1v

S为n_c×n_c的块矩阵：

每个子矩阵S_ij∈R^6×6，舒尔补方程右侧向量

对每个子向量g_i∈R⁶，定义辅助向量 其中i＝1,…,n_c，定义辅助变量其中j＝1,…,n_p，初始时刻，令U＝0,V＝0,W＝0,u＝0,v＝0,S＝0,g＝0,每次迭代，运行以下步骤：

a)对每个三维点j及其可见的相机如果或上次迭代被更新，则计算 处的并由下式更新各变量：

h)对每个三维点j，如果V_j在本次迭代中被更新，则对每个三维点j可见的相机由下式更新各变量：

并对三维点j可见的相机对由下式更新各变量：

i)使用预处理共轭梯度算法求解上述的舒尔补方程，得到相机增量对每个相机i，如果则用更新

b)对每个三维点j，如果V_j本次迭代被更新，或相机中存在本次迭代被更新，则计算如果则用更新

c)如果本次迭代没有任何相机或三维点被更新，或达到最大迭代次数，则结束循环，否则，重复步骤a)

同KinectFusion，本发明采用基于体素的三维模型表示，每个体素保存其截断的到最近表面带符号距离(TSDF)，为提高GPU显存利用率以及可重建场景尺度，采用了Nieβner[M.Nieβner,M.S.Izadi,and M.Stamminger.Real-time 3d reconstructionat scale using voxel hashing.ACM Trans.Graph.,32(6):169:1–169:11,2013.]等提出的空间哈希方法。

当相机姿态发生调整时，相应的，需要调整重建的三维模型。一种简单的方法是存储所有的深度图，并在每一深度图对应的相机姿态调整时重新融合进三维模型。然而，这种方法有着内存消耗高、计算量大的不足。为此，本发明提出基于关键帧的深度图表示和融合、反融合方法，有效地解决了这一问题。具体的：

如果当前帧F_i被选为关键帧，则直接将其对应的深度图D_i融合到模型上。对每个体素v，其截断带符号最近表面距离(TSDF)用D(v)表示，权重用W(v)表示，对F_i中的每个像素x，采用如下方法更新其对应的体素v：

其中w_i(x)是像素x的融合权重。

如果当前帧F_i未被选为关键帧，则首先寻找与F_i最匹配的关键帧，记为首先反融合关键帧所对应的深度图。反融合过程与融合过程相似，对每个像素x，采用如下方法更新其对应的体素v：

其中w_i(x)是像素x的融合权重。

随后，通过深度图D_i投影到将其融合到对F_i的每个像素x，用y表示其投影在的坐标。如果y对应的深度的倒数与x投影点深度的倒数(用表示)小于阈值τ_d(本发明设置为0.1)，则更新y的深度为：

对每个关键帧记录其被融合的次数当大于某一阈值时，停止融合。由于F_i与的视角重合较大，除了一些在视角之外的点，通常当前帧的大多数深度都能融合到上。因此该融合策略可以有效的减少深度的冗余性。如果当前帧融合后残余的深度点数目小于τ(例如2000)，则将这些点丢弃。否则，创建一个点云集V_i保存未融合的三维点并将其关联到之后，将更新后的关键帧深度图融合到模型上。如果V_i非空，则将V_i中的三维点投影到将其融合到模型。因此，每个非关键帧，将会执行两次融合与一次反融合。由于未融合的点数目相对很少，因此其需要的融合时间也较少。因此，本发明计算量基本上略大于通常的融合方法的两倍。如果较大并且关键帧的数目不再增加，这意味着RGB-D相机没有扫描新的信息，在这种情况下，我们简单的丢弃D_i。

为进一步减少深度数据的冗余，如果当前帧F_i被选为关键帧，则选择关联到相邻关键帧的若干点云集，融合到当前帧。如果融合后V_I剩余的点数小于τ(例如2000)，则将其从模型中反融合并丢弃。否则，将融合到当前帧的点从模型中反融合。考虑到实时计算的需求，本实施例只融合5个点云集。

当关键帧的姿态被BA调整时，需要重新融合关键帧对应的深度图以及与其关联的点云集。然而，如果被调整的关键帧数目较大，重融合会耗费大量的时间而不能满足实时性的要求。因此，本发明限制每一帧重融合的次数，将所有重融合分摊到每一帧。特别的，姿态变化比较大的关键帧对应的深度图及其关联的点云集将会被首先重融合。此策略可以保证在BA发生时重建部分以恒定的速率运行。

为进一步加速，可以每两帧或更多帧融合一帧到模型，此方法可以极大地加速融合过程，在笔记本电脑上可以达到实时运行。

在实验中测试了本发明的性能。在配置Intel i7 3.6GHz CPU，8GB内存，GTX960显卡的PC环境下，无GPU加速的相机跟踪可以达到10～25毫秒每帧，在融合每个输入帧的情况下稠密的重建部分需要50毫秒每帧，整个***运行速率大概位20fps。在配置为i7 2.6GHzCPU，16GB内存，GTX960M显卡的笔记本电脑上，如果每三帧融合一帧，运行速度可以达到20fps。

在用华硕Xtion Pro Live拍摄的多个数据集上测试了算法性能。如附图4所示，本发明可以构建出高质量的模型。

Claims (4)

1.一种同时定位与稠密三维重建方法,其特征在于包括如下步骤：

1)通过RGB-D对准估计相机初始姿态，使用基于单应性的特征匹配优化相机姿态；

2)运行“增量集束调整”算法，自适应地更新、优化关键帧的相机姿态；

3)将非关键深度图融合到关键帧，基于关键帧的深度图重建三维模型，通过重融合在相机姿态改变时更新三维模型。

2.如权利要求1所述的一种同时定位与稠密三维重建方法，其特征在于所述的步骤1)具体为：

对每个当前帧F_i，使用RGB-D对准在小模糊图上估计相机从上一帧到当前帧的相对姿态T_(i-1)→i，相应相机姿态估计为C_i＝T_(i-1)→iC_i-1；

使用单应性决定搜索区域同时用深度信息z_k以及相机姿态估计C_i决定搜索区域在搜索区域上寻找3D-2D特征匹配的集合χ＝{(X_j,x_j)}，其中X_j∈R³,x_j∈R²分别为三维点与二维点，K为相机内参；

在确定了相对姿态T_(i-1)→i以及3D-2D特征匹配的集合χ＝{(X_j,x_j)}后，通过最小化相对误差与重投影误差来优化相机姿态C_i：

$\begin{array}{c}{C}_i^{*}=\underset{C_i}{\arg \min }\left|\right|\log \left(C_i{C}_{i-1}^{-1}{T}_{\left(i-1\right)\→i}^{-1}\right)|{|}_{{\mathrm{\Σ}}_T}^2\\ +\underset{\left(X_j,x_j\right)\∈\mathrm{\χ}}{\mathrm{\Σ}}\left(\left|\right|\mathrm{\π}\left(K\left(C_i{X}_j\right)\right)-x_j|{|}_{{\mathrm{\δ}}_x}+||z\left(C_i{X}_j\right)-z_j|{|}_{{\mathrm{\δ}}_z}\right)\end{array}$

其中，log(T)将3D刚性变换T∈SE(3)映射到se(3)，并返回R⁶中的向量表示，是标准马氏距离，是投影函数，K是已知并且固定的相机内参，z(X)从X中提取出z分量，z_j是x_j在当前帧的深度，||∈||_δ是Huber范数：

如果追踪失败，则使用基于关键帧的重定位方法重新跟踪特征，否则，使用新特征的深度信息，用贝叶斯方法对特征的深度进行滤波；

对关键帧F_k和当前帧F_i的每一对特征匹配(x_i,x_j)，更新x_k深度的倒数ρ_k：

$\begin{array}{c}{\mathrm{\ρ}}_{i\→k}=\underset{\mathrm{\ρ}}{\min }\frac{1}{{\mathrm{\δ}}_x^2}\left|\right|\mathrm{\π}\left(K\left(C_i{\mathrm{\π}}^{-1}\left(C_k,x_k,{\mathrm{\ρ}}^{-1}\right)\right)\right)-x_i|{|}_2^2\\ +\frac{1}{{\mathrm{\δ}}_z^2}{\left(z\left(C_i{\mathrm{\π}}^{-1}\left(C_k,x_k,{\mathrm{\ρ}}^{-1}\right)\right)-z_i\right)}^2\end{array}$

这里，通过ρ_i→k，滤波器不断更新ρ_k和其内点的联合分布；

最后，在每一新帧到来时决定是否选作关键帧。

3.如权利1所述的一种同时定位与稠密三维重建方法，其特征在于所述的步骤2)具体为：

当有新的关键帧加入或者检测到闭合回路的时候，运行如下增量集束调整：

将当前相机运动参数定义为n_c是相机个数，将当前三维点位置定义为X_j∈R³，n_p为三维点的个数，将相机i可见的三维点的集合定义为V_i ^c，将三维点j可见的相机集合定义为用高斯-牛顿算法迭代优化：

$\underset{C,\mathrm{\χ}}{\mathrm{argmin}}\underset{i}{\Σ}\underset{j\∈V_i^C}{\Σ}\left|\right|e(C_i,X_j)|{|}_2^2$

每次迭代，将当前相机运动参数定义为将当前三维点位置定义为将相机运动参数的增量定义为将三维点位置的增量定义为将e(C_i,X_j)在处线性展开：

$e({\hat{C}}_i\⊕{\tilde{c}}_i,{\hat{X}}_j+{\tilde{x}}_j)\≈{\tilde{e}}_{ij}+{\hat{J}}_{ij}^c{\tilde{c}}_i+{\hat{J}}_{ij}^p{\tilde{x}}_j$

其中，表示在上叠加一个对应的se(3)增量，定义法向方程：

$\left[\begin{array}{cc}U& W\\ W^T& V\end{array}\right]\left[\begin{array}{c}\tilde{c}\\ \tilde{x}\end{array}\right]=\left[\begin{array}{c}u\\ v\end{array}\right]$

U、V分别是n_c×n_c、n_p×n_p的对角块矩阵

$U=diag(U_1,...,U_{n_c})$

$V=diag(V_1,...,V_{n_p})$

对每个子矩阵U_i∈R^6×6，V_j∈R^3×3，W为n_c×n_p的块矩阵：

每个子矩阵W_ij∈R^6×3，法向方程右侧向量：

$u={\[u_1^T,...,u_{n_c}^T\]}^T$

$v={\[v_1^T,...,v_{n_p}^T\]}^T$

每个子向量u_i∈R⁶，v_j∈R³，定义舒尔补方程

$S\tilde{c}=g$

其中，

S＝U-WV^-1W^T

g＝u-WV^-1v

S为n_c×n_c的块矩阵：

每个子矩阵S_ij∈R^6×6，舒尔补方程右侧向量

$g={\[g_1^T,...,g_{n_c}^T\]}^T$

对每个子向量g_i∈R⁶，定义辅助向量 其中i＝1,…,n_c，定义辅助变量其中j＝1,…,n_p，初始时刻，令U＝0,V＝0,W＝0,u＝0,v＝0,S＝0,g＝0,每次迭代，运行以下步骤：

a)对每个三维点j及其可见的相机如果或上次迭代被更新，则计算处的并由下式更新各变量：

$S_{ii}-=A_{ij}^U{A}_{ij}^U={\left({\hat{J}}_{ij}^c\right)}^T{\hat{J}}_{ij}^c{S}_{ii}+=A_{ij}^U$

$V_j-=A_{ij}^V{A}_{ij}^V={\left({\hat{J}}_{ij}^p\right)}^T{\hat{J}}_{ij}^p{V}_j+=A_{ij}^V$

$g_i-=b_{ij}^u{b}_{ij}^u=-{\left({\hat{J}}_{ij}^c\right)}^T{\hat{e}}_{ij}{g}_i+=b_{ij}^u$

$v_j-=b_{ij}^v{b}_{ij}^v=-{\left({\hat{J}}_{ij}^p\right)}^T{\hat{e}}_{ij}{v}_j+=b_{ij}^v$

$W_{ij}={\left({\hat{J}}_{ij}^c\right)}^T{\hat{J}}_{ij}^p$

b)对每个三维点j，如果V_j在本次迭代中被更新，则对每个三维点j可见的相机由下式更新各变量：

$g_i+=b_{ij}^q$

$Y_{ij}=W_{ij}{V}_j^{-1}$

$b_{ij}^g=Y_{ij}{v}_j$

$g-=b_{ij}^g$

并对三维点j可见的相机对由下式更新各变量：

$S_{i_1{i}_1}+=A_{i_1{i}_{1}j}^S{A}_{i_1{i}_{1}j}^S=Y_{i_{1}j}{W}_{i_{1}j}^T{S}_{i_1{i}_1}-=A_{i_1{i}_{1}j}^S$

$S_{i_1{i}_2}+=A_{i_1{i}_{2}j}^S{A}_{i_1{i}_{2}j}^S=Y_{1_{1}j}{W}_{i_{2}j}^T{S}_{1_1{i}_2}-=A_{i_1{i}_{2}j}^S$

$S_{i_2{i}_2}+=A_{i_2{i}_{2}j}^S{A}_{i_2{i}_{2}j}^S=Y_{i_{2}j}{W}_{i_{2}j}^T{S}_{i_2{i}_2}-=A_{i_2{i}_{2}j}^S$

c)使用预处理共轭梯度算法求解上述的舒尔补方程，得到相机增量对每个相机i，如果则用更新

d)对每个三维点j，如果V_j本次迭代被更新，或相机中存在本次迭代被更新，则计算如果则用更新

e)如果本次迭代没有任何相机或三维点被更新，或达到最大迭代次数，则结束循环，否则，重复步骤a)。

4.如权利1所述的一种同时定位与稠密三维重建方法，其特征在于所述的步骤3)具体为：

采用基于体素的三维模型表示，每个体素保存其截断的到最近表面带符号距离，采用空间哈希方法管理体素在GPU上的存储；

如果当前帧F_i被选为关键帧，则直接将其对应的深度图D_i融合到模型上，对每个体素v，其截断带符号最近表面距离用D(v)表示，权重用W(v)表示，对F_i中的每个像素x，采用如下方法更新其对应的体素v：

$D^{\′}\left(v\right)=\frac{D\left(v\right)W\left(v\right)+w_i\left(x\right)D_i\left(x\right)}{W\left(v\right)+w_i\left(x\right)},W^{\′}\left(v\right)=W\left(v\right)+w_i\left(x\right)$

其中w_i(x)是像素x的融合权重；

如果当前帧F_i未被选为关键帧，则首先寻找与F_i最匹配的关键帧，记为首先反融合关键帧所对应的深度图，对每个像素x，采用如下方法更新其对应的体素v：

$D^{\′}\left(v\right)=\frac{D\left(v\right)W\left(v\right)-w_i\left(x\right)D_i\left(x\right)}{W\left(v\right)-w_i\left(x\right)},W^{\′}\left(v\right)=W\left(v\right)-w_i\left(x\right)$

其中w_i(x)是像素x的融合权重；

随后，通过深度图D_i投影到将其融合到对F_i的每个像素x，用y表示其投影在的坐标，如果y对应的深度的倒数与x投影点深度的倒数小于阈值τ_d，则更新y的深度为：

$D^{\′}\left(y\right)=w_{K_i}\left(y\right)D_{K_i}\left(y\right)+w_i\left(x\right)z_x^{i\→K_i}$

$w_{K_i}=w_{K_i}\left(y\right)+w_i\left(x\right)$

对每个关键帧记录其被融合的次数当大于某一阈值时，停止融合；否则，创建一个点云集V_i保存未融合的三维点并将其关联到之后，将更新后的关键帧深度图融合到模型上；如果V_i非空，则将V_i中的三维点投影到将其融合到模型，如果较大并且关键帧的数目不再增加，则丢弃D_i；

如果当前帧F_i被选为关键帧，则选择关联到相邻关键帧的若干点云集，融合到当前帧，如果融合后V_I剩余的点数较少，则将其从模型中反融合并丢弃，否则，将融合到当前帧的点从模型中反融合；

最后，限制每一帧重融合的次数，将所有重融合操作分摊到每一帧。