一种基于深度信息融合的室内场景三维点云重建方法和系统

技术领域

本发明涉及计算机图形学、计算机视觉、机器人技术领域，更具体的涉及一种基于深度信息融合的室内场景三维点云重建方法和系统。

背景技术

近年来，同步定位与地图构建(SLAM)问题再次成为计算机视觉和机器人研究领域的热点话题。SLAM可以被分为两部分讨论，即定位与建图，定位是指对机器人自身位置及姿态的描述，而建图则是对机器人周围环境信息的描述，当机器人知道自身的位置以及周围的环境信息后，机器人就可以帮助人类完成许多的工作。随着计算机视觉技术的快速发展，例如摄像头、激光雷达之类原先价格昂贵的产品，现在成本降低了许多，这使得SLAM在AR、机器人、无人机、无人驾驶等方面被广泛运用，推进了这些方面研究的进展，大大加速了计算机视觉的发展历程，提高了SLAM的技术价值。

对于基于特征点法的视觉SLAM系统，一个高精度的前端是非常有必要的。目前传统的视觉里程计采用ORB，SIFT，SURF或其变种等特征点方法，在特征提取的过程中考虑了光照、旋转，尺度等特性，但是这些方法并未考虑到像素深度值这一重要特性。

发明内容

本发明实施例提供一种基于深度信息融合的室内场景三维点云重建方法和系统，用以解决上述背景技术中提出的问题。

本发明实施例提供一种基于深度信息融合的室内场景三维点云重建方法，包括：采用深度相机，获取场景的彩色图像和深度图像；

融合彩色图像和深度图像，并进行特征提取、匹配，获得特征点匹配信息；

根据特征点匹配信息，确定深度相机的位姿，并筛选出关键帧；

根据关键帧所包含的彩色图像和深度图像，生成每个关键帧的三维点云；

根据深度相机的位姿，逐帧拼接每个关键帧的三维点云，获得全局一致的场景三维点云模型。

进一步地，所述融合彩色图像和深度图像，并进行特征提取、匹配，获得特征点匹配信息，具体包括：

对彩色图像进行特征提取；

对每个特征点，计算基于深度值的BRIEF描述子；

确定BRIEF描述子间的汉明距离；

根据汉明距离，采用双向FLANN特征匹配算法进行特征匹配，获得特征点匹配信息。

进一步地，所述对每个特征点，计算基于深度值的BRIEF描述子，具体包括：

对每个特征点定义图像邻域；

在特征点的图像邻域中选取测试点对；

分别比较测试点对的灰度值和深度值大小，获得基于深度值的BRIEF描述子。

进一步地，所述在特征点的图像邻域中选取测试点对，具体包括：

将测试点对旋转角度，得到新的点对；并根据特征点方向对应的旋转矩阵，构造出测试点对的校正矩阵；

使用图像邻域中某个点的5×5邻域灰度平均值代替某个点对的灰度值，及使用图像邻域中某个点的5×5邻域深度平均值代替某个点对的深度值；

从测试点对集合中获取256个测试点对。

进一步地，所述确定深度相机的位姿，具体包括：

基于RANSAC算法和ICP算法，估计深度相机的位姿。

进一步地，所述筛选出关键帧，具体包括：

遍历每一帧图像，根据帧的内点数，判断该帧图像是否作为关键帧；

设置关键帧间距最小阈值和最大阈值；当距离上一关键帧至少为最小阈值时，插入关键帧；当距离上一关键帧为最大阈值时，必须插入关键帧；局部建图线程中，关键帧队列的关键帧数量不超过3个时，插入关键帧。

进一步地，本发明实施例提供的基于深度信息融合的室内场景三维点云重建方法，还包括：

采用图优化理论，对深度相机的位姿进行局部图优化；

采用词袋模型，对局部图优化后的位姿进行回环检测和全局优化。

进一步地，所述采用图优化理论，对深度相机的位姿进行局部图优化；具体包括：

确定待优化的局部关键帧和局部地图点；所述待优化的局部关键帧包括：当前关键帧、与当前关键帧存在15个以上重复观测到的地图点的共视关键帧、及能观测到局部地图点却不属于当前关键帧和共视关键帧的关键帧；

将局部关键帧顶点、局部地图点顶点、及每一对关联的局部地图点和局部关键帧构建的边加入到位姿图中；

更新局部关键帧的位姿、局部地图点的3D位置、平均观测方向，完成局部图优化。

进一步地，所述采用词袋模型，对局部图优化后的位姿进行回环检测和全局优化，具体包括：

构建字典，并确定帧对应的词袋向量；

将当前关键帧与地图中的既往关键帧进行随机匹配，通过词袋向量中单词对应的权重，分析两帧之间的差异；如果两帧之间的差异小于设定阈值，则两帧之间构成回环；

根据检测到回环的帧数，遍历所有关键帧；并根据阈值minWords和最低相似度评分minScore，确定候选关键帧、及将候选关键帧分组；

对每组候选关键帧进行时间一致性检验、几何一致性检验，获得最终候选关键帧；

对最终候选关键帧，通过词袋模型进行匹配，确定候选关键帧与当前帧构成的回环；

根据候选关键帧与当前帧构成的回环，进行位姿校正，实现在产生回环后，对全局所有关键帧位姿及地图点坐标的优化。

进一步地，本发明实施例提供的基于深度信息融合的室内场景三维点云重建方法，还包括：当位姿发生丢失时，重新定位、重新找回当前帧的位姿，其具体包括：

从关键帧库中选取候选关键帧；

匹配当前帧与候选关键帧；

采用EPNP算法估计位姿，估计成功后，更新当前帧的特征匹配；同时采用PoseOptimization方法优化当前帧的位姿，剔除特征匹配的外点；利用SearchByProjection方法对没有匹配的特征点匹配当前帧和关键帧，以此扩充匹配个数，若匹配个数大于50且优化后内点数目大于50，则重定位成功。

本发明实施例提供一种基于深度信息融合的室内场景三维点云重建方法和系统，与现有技术相比，其有益效果如下：

本发明将深度信息融合与SLAM系统进行了结合，实现了在室内场景点云三维重建。使用深度相机KinectV2获取场景彩色图像和深度图像，并通过图像金字塔生成三层降采样图像，以使得图像特征匹配保持尺度不变性；在对ORB特征提取的基础上加入了深度约束，之后对提取出来的特征点进行均匀化，控制特征点的数量；之后使用快速近似最近邻(FLANN)算法进行图像匹配，利用RANSAC方法剔除误匹配，并且为ICP算法提供一个初始值。即本发明通过在特征提取中同时使用图像亮度信息和深度信息评判一个点是否为特征点，使得提取出来的特征点具有高度代表性，同时对提取出来的特征点进行均匀化操作，保证特征点的数量在合理范围内，有效地控制系统的计算量，提高了前端的精度，增强了前端的鲁棒性，使得后端优化的结果更加准确或者减少了后端优化的迭代次数。

附图说明

图1为本发明实施例提供的一种基于深度信息融合的室内场景三维点云重建方法基本流程；

图2为本发明实施例提供的特征提取及描述子计算基本流程；

图3为本发明实施例提供的RGBD-ICP算法的流程示意图；

图4为本发明实施例提供的在普通卧室内采集的单帧场景点云；

图5为本发明实施例提供的系统后端优化框架图；

图6为本发明实施例提供的整个办公室的全局场景三维点云模型。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参见图1，本发明实施例提供一种基于深度信息融合的室内场景三维点云重建方法，该方法具体包括：

步骤1：用户在ROS环境下安装KinectV2的驱动，利用libfreenect2和iai-kinect2驱动KinectV2，保证设备的正常运行。

步骤2：利用棋盘格标定法对深度相机的参数进行标定，将标定后的参数保存为相机参数文件供系统读取并运行。

步骤2.1：用户应首先准备一个已知尺寸的标定板，在本系统中使用的是8×6，边长为108mm的棋盘格标定板。在标定时应确保标定环境拥有一个5m×5m的无遮挡环境。

步骤2.2：运行ROS自带的标定节点开始进行设备的标定工作。

步骤2.3：为了达到良好的标定效果，本发明要求在相机周围移动标定板，并完成以下基本需求。

步骤2.3.1：移动标定板到标定窗口的最左、最右、最上、最下方。

步骤2.3.2：移动标定板到视野的最近和最远处。

步骤2.3.3：移动标定板使其充满整个标定窗口。

步骤2.3.4：保持标定板倾斜状态并使其移动到标定窗口的最左、最右、最上、最下方。

步骤2.4：当标定窗口中的CALIBRATE按钮亮起时，表明已有足够的数据进行相机标定，此时按下CALIBRATE按钮并等待一分钟左右。

步骤2.5：标定完成后，将终端输出的标定结果保存为参数文件kinect2.yaml。

用户在按本发明的方式配置好KinectV2相关的驱动后，可以手持KinectV2进行缓慢的移动，在此过程中，本发明程序自动采集彩色图和深度图，用于三维点信息的计算。

采用上述拓展和改进方案的有益效果：使用标定后的相机参数，可以确保系统可以更加精确地处理图像数据，完成图像匹配及生成点云等工作。

步骤3：使用KinectV2以30FPS的速度同步采集场景彩色图像和深度图像，作为系统的输入数据。

步骤4：根据深度相机KinectV2拍摄的彩色和深度图像，本发明对彩色和深度图像分别做平滑等预处理，提高系统的抗噪性，将彩色图像转为灰度图像，完成对灰度图像特征的快速提取，通过深度图像获得深度信息，结合深度信息计算具有旋转不变性、相关性小的特征点描述子，通过双向近似快速最近邻搜索算法对图像特征进行数据关联。

步骤4.1：本发明采用双重FAST检测算法提取特征，要求至少提取300个特征点，建立特征点测试集。

步骤4.2：对步骤4.1中提取出的每个特征点计算基于深度值的BRIEF描述子，使得计算出的描述子在具有较强旋转不变性的同时具备相关性小，抗噪性高的特点，同时保证计算过程快速精确，以确保系统的精确性及实时性。

步骤4.2.1：通过RGB颜色模型将彩色图转换为灰度图，对彩色图和深度图分别进行预处理。每个测试点使用31×31像素邻域中的5×5子窗口，再采用积分图像加速计算实现灰度图预处理。采用联合双边滤波去除深度图像噪声，以确保良好的边缘保持效果。

步骤4.2.2：对步骤4.1中提取的每个特征点定义图像邻域T，T为a×a大小，以便后续在该邻域内选择n个测试点对，(u,v)为一个测试点对，通过比较测试点对大小得出描述子。

步骤4.2.3：在特征点的图像邻域中按照一定方法选取测试点对p，将选取出的点对旋转角度θ，得到新的点对，使用步骤4.1得出的特征点方向对应的旋转矩阵构造出矩阵p的校正版本p

步骤4.2.4：选取测试点对时使用邻域中的某个点的5×5邻域灰度平均值来代替某个点对的灰度值，使用邻域中的某个点的5×5邻域深度平均值来代替某个点对的深度值。为了加快计算速度，使用积分图像快速求取5×5邻域灰度平均值和深度平均值。

步骤4.2.5：增大描述子间的可区分性。步骤4.2.3在增加描述子旋转不变性的同时也降低了描述子的可区分性。描述子是特征点性质的描述，表达了特征点不同于其他特征点的区别，故描述子要尽量的表达特征点的独特性。如果不同特征点描述子的相关性较大，匹配时很难找到对应的匹配点，容易造成误匹配。本发明通过统计学习的方法来选择得到点对集合，并采用点对集合来获取256个测试点，以此增强特征描述子可区分性。

步骤4.2.6：计算具备深度信息的BRIEF描述子。本方法利用KinectV2拍摄获得的灰度和深度信息，分别比较256个测试点对灰度值t

步骤4.3：计算描述子间的汉明距离。描述子是特征点性质的描述，两特征点描述子间的相似性可用于数据关联。描述子间的汉明距离越小，说明描述子间相似度越高，描述子对应的特征点匹配的可能性越高。本发明通过异或操作快速计算描述子间汉明距离。

步骤4.4：对图像特征快速匹配。FLANN特征匹配算法匹配速度快，但精确度较低。由于本发明对匹配速度和精度有较高要求，与FLANN特征匹配算法相比，本发明使用双向FLANN特征匹配算法进行匹配，通过牺牲少量时间提高匹配正确率，得到可靠准确的匹配点。

对上述步骤4的进一步说明：

对彩色图像和深度图像进行预处理。通过对图像块进行预平滑处理，提高BRIEF描述子的抗噪性。鉴于在在空间域和频率域的有效性，本发明采用高斯滤波器对灰度图像的随机噪声和高斯噪声进行预处理。每个测试点使用31×31像素邻域中的5×5子窗口，再采用积分图像加速计算实现灰度图预处理。由于主动光设备拍摄的深度图存在边缘不匹配等问题，本发明对深度图像采用联合双边滤波去除噪声。与线性滤波器相比，使用联合双边滤波器处理过的图像的边缘仍然可以很好的被保持。若选取测试点对时只考虑图像邻域的单个像素，抗噪性会降低，为提高抗噪性，BRIEF描述子采取的措施是：每个测试点采用31×31像素邻域中的5×5子窗口灰度平均值来代替该点对的值。

计算联合深度信息的BRIEF描述子。定义图像邻域T，T为a×a大小，(u,v)为一个测试点对，t(u)是图像邻域T在点u处灰度值，在特征点的邻域T内，选择n对像素点(u

f

增强BRIEF描述子的旋转不变性。在旋转角度较小的图像帧中，用BRIEF生成的描述子匹配质量较高。但在旋转大于30°后，BRIEF的匹配率极速降低。为使ORB描述子具有旋转不变性，本发明将特征点方向信息引入描述子。对在特征点a×a邻域内选取的n对点集Q，可以用的2×n矩阵表示。使用特征点方向θ使描述子中包含有特征点的方向信息，以此使BRIEF描述子具有旋转不变性。对于提取出的特征点，本发明选取特征点a×a邻域内的n对测试点对，并将选取出的点对旋转角度θ，得到新的点对，对新的点对的大小进行比较形成n维向量形式的描述子，使用特征点方向对应的旋转矩阵，可以构造出矩阵p的校正版本p

g

约束测试点对相关性。加入旋转不变性的特征描述子具有旋转不变性，但与传统的BRIEF描述子相比，相关性有所降低。描述子表达了特征点不同于其他特征点的区别，是特征点性质的描述，我们计算的描述子要尽量的表达特征点的独特性。如果不同特征点的描述子的相关性较大，匹配时容易出现误匹配。为了降低BRIEF描述子的相关性，本发明使用统计学习的方法重新选择点对集合，利用贪婪式搜索来提高BRIEF描述子的可区分性。

采用双向近似快速最近邻搜索FLANN特征匹配算法进行数据关联。由于BRIEF描述子是由一系列的1和0组成二进制码串，本发明采用汉明距离计算描述子之间的距离，然后通过异或运算快速计算描述子间距。256位BRIEF描述子的汉明距离最大值和最小值分别为256比特和0比特。通常来说，非匹配点的汉明距离呈现出均值为128比特的高斯分布。对于ORB特征向量，以往算法采用基于最近邻汉明距离的匹配算法，其匹配过程计算量太大。通过利用FLANN算法快速性，本发明采用双向近似快速最近邻搜索FLANN特征匹配算法，通过牺牲少量时间提高了匹配的正确率，在确保速度的前提下找到可靠准确的匹配点。

采用上述拓展和改进方案的有益效果：使用联合深度信息的特征点描述子和可以确保特征点匹配的高精度和实时性。

步骤5：采用ICP算法，实时估计相机位姿。ICP算法中，源点云p

步骤5.1：使用非线性优化方法以迭代的方法估计相机位姿，本发明将第一帧图像所对应的相机位姿作为世界坐标系的原点，定义为a

步骤5.2：定义图像特征点匹配集合L，利用深度图得出特征点的三维坐标L

步骤5.3：使用李代数扰动模型，不断迭代，最终得出最优值，估计出相机位姿：

τ＝argmin(E)

步骤5.4：根据匹配信息判断关键帧。若所有图像不经筛选全部插入点云地图将占据大量的内存空间，造成信息冗余。保存冗余信息的系统的效率和精度急速降低，因此SLAM点云地图不会将每一帧图像保存，为使系统具有更高的精度和实时性，本发明将根据一定条件对图像进行筛选，选择满足一定条件的帧，将该帧定义为关键帧。

步骤5.5：遍历每一帧图像，判断该图像是否可作为关键帧，若该帧的内点数较多，不小于设定的最小阈值且内点重叠度不大，该帧可作为关键帧。以上操作在保证关键帧质量的同时尽量少的引入冗余信息。

步骤5.6：设置关键帧间距最小阈值MIN和关键帧间距最大阈值MAX，设置关键帧间距最小阈值MIN和关键帧间距最大阈值MAX，为减小冗余，应减少关键帧数量，距离上一关键帧至少MIN帧才可插入关键帧，为防止因舍弃太多帧而跟丢，距离上一关键帧至多MAX帧插入关键帧。

对上述步骤5的进一步说明：

将源点云p

基于稀疏特征点匹配的图像对齐。特征点匹配的关键优势在于它们可以在不需要进行初始化的情况下进行匹配。但使用特征点和描述子匹配可能会产生误匹配，使得计算获取的位姿精度低，最终位姿估计值不正确。针对上述问题，本发明基于RANSAC算法策略，用ICP算法求解位姿。该算法可以保证既获取较多的内点数又可以计算出最优估计值。我们的RGBD-ICP算法使用视觉特征及其相关的深度值来获得初始对齐，然后共同针对稀疏特征匹配和密集点云对齐进行优化。

筛选关键帧。若所有图像不经筛选全部插入点云地图将占据大量的内存空间，造成信息冗余。保存冗余信息的系统的效率和精度急速降低，因此SLAM点云地图不会将每一帧图像保存，为使系统具有更高的精度和实时性，本发明将根据一定条件对图像进行筛选,选择满足一定条件的帧，将该帧定义为关键帧。

判断内点数。遍历每一帧图像，判断该图像是否可作为关键帧，若该帧的内点数较多，不小于设定的最小阈值且内点重叠度较小，该帧可作为关键帧。以上操作在保证关键帧质量的同尽量少的引入冗余信息。

插入关键帧。设置关键帧间距最小阈值MIN和关键帧间距最大阈值MAX，为尽量减小冗余，距离上一关键帧至少MIN帧才可插入关键帧，为防止因舍弃太多帧而跟丢，距离上一关键帧MAX帧插入关键帧，局部建图线程中关键帧队列的关键帧数量不超过3个才可插入关键帧。

采用上述拓展和改进方案的有益效果：使用ICP算法估计相机位姿确保位姿估计的高精度，根据匹配条件判断关键帧确保系统的高效性。

步骤6：通过KinectV2相机获取深度图像，从关键帧的深度图像获取深度信息，通过深度信息及步骤5估计出的相机位姿重建关键帧中图像像素点所对应的空间三维点坐标，得到三维点云地图。

步骤6.1：读取彩色和深度图像对和位姿信息。

步骤6.2：计算像素点在相机坐标系下的位置，像素点在图像中的位置是(u,v)，像素点的深度为c。然后根据外参把相机坐标系下的位置变换到世界坐标下。相机坐标e

根据上述关系推导出x，y，z：

z＝C

步骤6.2：计算每一关键帧的点云，图4为生成的单个帧的场景点云。

采用上述拓展和改进方案的有益效果：使用KinectV2相机获取的深度图中的深度信息可以确保系统精确且高效的生成点云。

步骤7：本发明基于图优化以及词袋模型算法，对地图中关键帧的位姿进行优化，后端优化结构图如图5所示。

步骤7.1：局部图优化。

步骤7.1.1：初始化优化列表，将当前关键帧及其共视关键帧加入局部关键帧列表lLocalKeyFrames中；遍历局部关键帧列表，将每个关键帧所观测到的地图点加入局部地图点列表lLocalMapPoints中；将不在局部关键帧列表，却能观测到局部地图点的关键帧加入固定关键帧列表lFixedCameras中。

步骤7.1.2：将局部关键帧列表和固定关键帧列表中的所有关键帧定义为g2o::VertexSE3Expmap*类型的顶点，将局部地图点中的所有地图点定义为g2o::g2o::VertexSBAPointXYZ*类型的顶点，最终采用addVertex方法将上述顶点添加入位姿图中。

步骤7.1.3：采用g2o::EdgeSE3ProjectXYZ*类型定义连接每一对关联的地图点和关键帧的边，并采用addEdge方法将上述边添加入位姿图中。

步骤7.1.4：先采用optimize方法进行5次的初步迭代优化，对优化后的顶点观测值进行检测，剔除异常点，再采用optimize方法进行10次的迭代优化。

步骤7.1.5：遍历优化后的位姿图中的所有边，通过chi2方法计算出其连接偏差，并在每一个关键帧中剔除其连偏差大于阈值的地图点，在每一个地图点中剔除其连接偏差大于阈值的关键帧。

步骤7.1.6：更新局部关键帧的位姿以及局部地图点的3D位置、平均观测方向等属性，最终完成局部图优化。

步骤7.2：在进行局部图优化后，需要根据词袋模型进行回环检测并进行全局优化，因此首先需要构建字典，本发明采用离线构建字典，以此提高整个系统运行的效率。

步骤7.2.1：采用K-means聚类算法构建字典，在字典中随机选取k个中心点，之后通过计算每一个样本点与中心点的距离，选取其中最小的点进行聚类。此后继续重新计算每个类里的中心点，对中心点进行分析，如果中心点的变化很小，说明算法收敛，构建完成，否则继续迭代查找。

步骤7.2.2：通过确定一帧中有哪些单词，由此形成相应的词袋向量，其公式如下所示：

F＝1·w

步骤7.2.3：将当前关键帧与地图中的既往关键帧进行随机匹配，并分析两帧之间的差异，通常使用TF-IDF方案对词的重复程度进行评估，借此来分析两帧之间的差异性，计算每个单词的权重，组成对应的词袋，其公式如下：

A＝{(w

步骤7.2.4：通过A、b两幅图像的v

步骤7.3：实现回环检测与全局优化

步骤7.3.1：本发明判断是否进行检测，若距离上次检测到回环的帧数大于10帧，且地图中关键帧的数量大于10帧，则开始进行检测。

步骤7.3.2：遍历所有关键帧，找出其中与当前关键帧存在相同单词的关键帧，并将最大数目的80％保存为阈值minWords；同时找出当前关键帧与其共视关键帧的最低相似度评分minScore。

步骤7.3.3：筛选出所有与当前关键帧存在不少于minWords个相同单词，并且相似度评分大于minScore的关键帧，作为候选关键帧。

步骤7.3.4：对候选关键帧进行分组，采用GetBestCovisibilityKeyFrames方法得到与其共视关系最强的前10个共视关键帧。计算每组的相似度总评分bestAccScore以及确定每组中评分最高的关键帧。

步骤7.3.5：进行时间一致性检验，以总评分bestAccScore的0.75作为阈值，筛选出高于此阈值的所有组中得分最高的关键帧，作为候选关键帧。

步骤7.3.6：进行几何一致性检验，将当前帧及其相邻帧与候选关键帧及其相邻帧进行匹配，判断是否有相同的相邻关键帧，如果数量大于3则认为候选关键帧通过此检测。

步骤7.3.7：对于上述得到的最终的候选关键帧，通过词袋模型进行匹配，若得到的匹配点对数小于20，则视为不合格进行剔除。

步骤7.3.8：对剩下的候选关键帧进行Sim3求解，检测内点，若其数量大于阈值则进一步通过上述Sim3对词袋模型进行的匹配进行优化，再计算出大致的匹配区域后进行Sim3优化，先进行5次迭代，然后剔除误差值过大的边，对剩余的边继续进行优化。

步骤7.3.9：对优化后的内点数量再次判断，若不少于20个，则继续将此候选关键帧与其相邻关键帧的地图点投影到当前帧，根据Sim3确定大致区域，在其周围进行搜索匹配，若匹配点数目超过40个，则匹配成功，候选关键帧与当前帧构成回环。

步骤7.3.10：基于当前帧求得的相机位姿，对所有与其相邻的关键帧进行校正。

步骤7.3.11：所有的闭环关键帧观察到的地图点通过映射在一定范围内，在对其进行近邻匹配，从而对所有匹配到的地图点进行融合。

步骤7.3.12：根据共视关系，重新对各个关键帧的连接关系进行更新，并对冲突的地图点进行替补与优化。最终对图进行更新，得到产生闭环后的图。

步骤7.3.13：采用OptimizeEssentialGraph方法与GlobalBundleAdjustemnt方法执行对本征图的优化以及全局优化，从而实现在产生回环后，对全局所有关键帧位姿以及地图点坐标的优化。

步骤74：当在前端中发生丢失时，进行重定位，以重新找回当前帧的位姿。

步骤7.4.1：采用DetectRelocalizationCandidates方法进行候选关键帧的选取。

步骤7.4.2：匹配当前帧与关键帧，使用SearchByBow匹配当前帧和该关键帧，将成功个数少于15的筛除。

步骤7.4.3：更新mvMapPoints列表，同时使用上述两帧的特征匹配关系对PnPSolver进行初始化。

步骤7.4.4：使用EPNP估计位姿，估计成功后，更新当前帧的特征匹配，同时调用PoseOptimization优化当前帧的位姿，剔除特征匹配的外点，若优化后内点数量过少，使用SearchByProjection对没有匹配的特征点匹配当前帧和关键帧，以此扩充匹配个数，若匹配个数大于50且优化后内点大于50，则重定位成功。

对上述步骤7的进一步说明：

确定待优化的关键帧与地图点。将当前关键帧，以及与当前关键帧存在15个以上重复观测到的地图点的共视关键帧，加入到局部关键帧列表lLocalKeyFrames中。再遍历lLocalKeyFrames中关键帧，将每个关键帧所能观测到的所有的地图点加入到局部地图点列表lLocalMapPoints中。此外，本发明还会将能观测到局部地图点，却不属于局部关键帧列表的关键帧加入固定关键帧列表lFixedCameras，以提供额外的约束条件，进一步提高图优化的效果。

构建位姿图。利用g2o::VertexSE3Expmap定义的顶点，设置局部关键帧以及固定关键帧顶点，再利用g2o::g2o::VertexSBAPointXYZ定义的顶点，设置局部地图点顶点，最终通过g2o::SparseOptimizer优化器的addVertex方法将上述顶点加入到位姿图中。在进行局部地图点顶点添加的过程中，每添加一个地图点，将采用g2o::EdgeSE3ProjectXYZ类型对每一对关联的地图点和关键帧构建边，并通过g2o::SparseOptimizer优化器的addEdge方法将上述边加入到位姿图中。在所有顶点和边添加完毕后，完成位姿图的构建。

执行局部图优化。先利用optimize方法进行5次的初步迭代优化，再对初步优化后的顶点的观测值进行检测，将异常点排除不再进行优化。所有外点排除完毕后，再次采用optimize方法进行10次的迭代优化。

检查优化结果。遍历位姿图中所有的边，计算出每一条边的连接偏差，并针对每一个关键帧，剔除其连接偏差大于阈值的地图点；针对每一个地图点，剔除其连接偏差大于阈值的关键帧。

同步优化结果。更新局部关键帧的位姿以及局部地图点的位置、平均观测方向等属性。

在进行局部图优化后，需要根据词袋模型进行回环检测并进行全局优化，因此首先需要构建字典，本发明中采用的字典是离线构建的字典，以此提高整个系统运行的效率。构建字典的聚类算法有很多种，本发明中采用了K-means算法，该算法首先在字典中随机选取k个中心点：c

形成词袋向量。通过确定一帧中具有哪些单词，以此来形成词袋向量。

F＝1·w

分析两帧之间的差异。通常使用TF-IDF方案对词的重复程度进行评估。计算每个单词的权重η

A＝{(w

并通过A、B两幅图像的v

在进行回环检测时，需要先判断是否进行检测，如果距离上次检测到回环的帧数大于10帧，并且地图中关键帧数量大于10帧，则进行回环检测，若上述条件有一个不满足，则不进行回环检测。

确定回环候选关键帧，遍历所有的关键帧，找出与当前关键帧具有相同单词的关键帧，统计出这些关键帧中相同单词的最大数量，以其的80％作为阈值对这些关键帧进行筛选，同时确定当前关键帧与其共视关键帧的最低相似度评分minScore，最终筛选出所有单词数量超过阈值，且相似度评分大于minScore的所有关键帧作为候选关键帧。

对候选关键帧进行筛选，将上述候选关键帧与其共视关系最强的前10个关键帧分为一组，计算每组的相似度总评分以及确定每组相似度得分最高的关键帧。此后，将对每一组分别进行时间一致性检验，以组得分的0.75作为阈值，筛选出相似度评分高于此阈值的所有组中得分最高的关键帧，作为候选关键帧。再进行几何一致性检验，将当前帧及其相邻帧与候选关键帧及其相邻帧进行匹配，判断是否有相同的相邻关键帧，如果数量大于3则认为候选关键帧通过此检测。

使用三对匹配点求解相似变换，进而得出两个坐标系之间的旋转矩阵、平移向量以及尺度因子。对于上述得到的每一个候选关键帧，首先通过词袋模型进行匹配，如果匹配得到的特征点对数小于20，则视为不合格删除该帧，否则则求解其Sim3变换，即使用3对匹配点进行相似变换的求解，进而解出两帧之间的旋转矩阵、平移向量以及尺度因子。求解完毕后进行内点检测，最多迭代5次，若检测合格则进一步通过上述的Sim3变换对词袋模型进行的匹配进行优化，再计算出大致的匹配区域后则进行Sim3变换的优化，先进行5次迭代，然后剔除误差值过大的边，对剩余的边继续进行优化，若优化后的内点数量不少于20个，则继续将此候选关键帧及其相邻关键帧的地图点投影到当前帧，根据Sim3变换确定大致区域，在其附近区域进行搜索匹配，若匹配点数目超过40个，则认为匹配成功，候选关键帧与当前帧构成回环。

根据回环进行位姿校正。基于当前帧求得的相机位姿，对所有与其相邻的关键帧进行校正。所有的闭环关键帧观察到的地图点通过映射在一定范围内，在对其进行近邻匹配，从而对所有匹配到的地图点进行融合。在完成地图点的融合后，再根据共视关系，重新对各个关键帧的连接关系进行更新，并对冲突的地图点进行替补与优化。最终对图进行更新，得到产生闭环后的图。

采用OptimizeEssentialGraph方法与GlobalBundleAdjustemnt方法执行对本征图的优化以及全局优化，从而实现在产生回环后，对全局所有关键帧位姿以及地图点坐标的优化。

若在前端中发生了丢失，那么还将进行重定位，以重新找回当前帧的位姿。在进行重定位时，首先需要选取候选关键帧,采用DetectRelocalizationCandidates方法实现对候选关键帧的选取。

匹配当前帧与关键帧。采用SearchByBow方法匹配当前帧和该关键帧，将成功个数少于15的筛除，更新mvMapPoints列表，同时使用上述两帧的特征匹配关系初始化PnPSolver。

使用EPNP估计位姿，估计成功后，更新当前帧的特征匹配，同时采用PoseOptimization方法优化当前帧的位姿，剔除特征匹配的外点，若优化后内点数量过少，利用SearchByProjection方法对没有匹配的特征点匹配当前帧和关键帧，以此扩充匹配个数，若匹配个数大于50且优化后内点大于50，则重定位成功。

采用上述拓展和改进方案的有益效果：采用基于图优化的后端优化方法，可以实现对局部或全局的所有位姿及地图点数据进行优化，从而最终得到全局一致的轨迹和地图。基于词袋模型的重定位技术则可以实现在跟踪丢失的情况下，快速找回系统当前的位姿。

步骤8：使用优化后的相机位姿，逐帧拼接每个关键帧的三维点云，可以得到全局一致的场景三维点云模型。

步骤8.1：对于每个关键帧生成的点云，根据相机位姿的变换矩阵对其作变换。使用PCL提供的点云变换函数transformPointCloud(input，output，T)移动该帧点云。

步骤8.2：关键帧的点云经过变换后，将其合并到整个场景点云模型中，得到全局一致的场景三维点云模型。

步骤8.3：本系统在对场景进行三维重建时，并不需要场景点云模型具有过高的分辨率，因为这会降低系统运行速度。我们使用PCL自带的Voxel功能，将点云画在网格中，每个网格保留一个点，这样相当于我们对点云做了一个下采样。如图6所示，最终我们得到一个稀疏的场景三维点云模型。

采用上述拓展和改进方案的有益效果：使用优化后的相机位姿可以实现点云的精准拼接，提高整个场景点云模型的质量，对点云进行体素滤波可提高点云的质量和限制点云的大小。

综上所述，本发明涉及一种基于KinectV2的场景实时点云重建系统，在系统前端部分，通过用户手持深度相机对场景进行拍摄，得到场景的彩色图像和深度图像，并融合彩色图像和深度图像信息进行特征提取及匹配以计算相机位姿。后端通过图优化、词袋模型等技术对不同帧之间的相机位姿进行优化。在点云建图阶段，利用优化后的位姿进行点云拼接，最终得到场景的三维点云模型。本发明在重建中同步设计的显示模块包括：图像流特征提取实时展示，相机位姿实时展示以及场景点云模型实时展示。即通过手持KinectV2设备采集室内场景彩色图像和深度图像并提取点云信息，并在扫描过程中实时跟踪相机位姿，根据相机位姿对单帧点云进行拼接，得到全局一致的场景点云，同时在扫描过程中实时展示相机位姿以及场景点云。

以上公开的仅为本发明的几个具体实施例，本领域的技术人员可以对本发明实施例进行各种改动和变型而不脱离本发明的精神和范围，但是，本发明实施例并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围内。