所筛选出的像素点深度值;并将所筛选出的带有深 度值的像素点加入地图中。
[0072] 由于目前整套系统前后依赖性强，为了减小来自传感器的误差对最终结果的影 响，矫正每一步的位姿，这里采用图优化方法对第i帧对应的相机在局部坐标下的位置，以 及所筛选出的带有深度值的像素点位置进行优化，将优化后的带有深度值的像素点加入地 图中。公知的图优化方法公式为：
[0073]
[0074] Xk表示前面定义的节点（也可以理解为状态），Zk表示边(也可以理解为约束），ek表 示这些节点满足约束的状态(如果没有噪声及e3k=〇)，Qk表示引入的信息矩阵，及约束的置 信度，如果误差大则它对应的置信度就小。定义完变量之后，我们要做的就是使整个误差函 数降到最小，以达到全局最优的目的。在本方法中我们将地图点的三维位置和无人机的位 姿定义为节点，将从图像到地图点的投影关系和相邻两帧之间的SE(3)的变化定义为边，信 息矩阵包含两方面:地图点的被关键帧观测次数和图像中点的灰度梯度。
[0075] 此外在步骤2.4中，将所筛选出的带有深度值的像素点加入地图的过程中，若某一 像素点反投影后，在地图中对应的三维点的邻域(设定的某一小值）内，已存在有地图三维 点，则将该像素点反投影后在地图中对应的三维点，以及三维点邻域内的已存在的地图三 维点去除，并将该像素点反投影后在地图中对应的三维点，与三维点邻域内的已存在的地 图三维点的加权平均点加入地图中。
[0076] 步骤2.5:若步骤2.2得到的第i帧到当前关键帧的位姿变化大于设定的位姿变化 阈值，则用第i帧代替当前关键帧作为新的关键帧。本实施例，为了提高运算速率，降低存储 的数据量，这里要求若步骤2.2得到的第i帧到当前关键帧的位姿变化大于设定的位姿变化 阈值，且第i帧与当前关键帧的帧数差不小于15帧，则用第i帧代替当前关键帧作为新的关 键帧。
[0077] 关键帧的设立是由于其具有的位姿相对于前一个关键帧变化较大，其探测到的三 维环境信息相比较于之前的关键帧有较大不同，因此将其设为一个标尺，用于扩展全局地 图和检测后续的帧是否有较大的位姿变化。
[0078] 在跟踪过程中，如果产生"丢帧"现象(可能的原因有：相机移动过快，导致当前帧 与当前的关键帧的"差距"过大，无法在当前的关键帧上进行跟踪，如果这时不进行处理将 导致前后创建的两个地图间没有一个紧密的"联系"，让之前的所有工作失去意义），所以判 断若步骤2.2中图像对齐操作无法实现，则进行失败重建：
[0079]提取去畸变后的当前帧图像的特征点，将当前帧的特征点与存储的每一个关键帧 图像中的特征点进行匹配，寻找成功匹配特征点个数最多的关键帧，若该关键帧中成功匹 配特征点的个数占该关键帧中特征点总数的比例不大于40%，则以当前帧作为第一帧，返 回步骤1;否则以该关键帧作为为基准，进行去畸变后的当前帧图像与基准的图像对齐操 作，得到当前帧到当前关键帧的位姿变化；
[0080]根据基准对应的相机姿态，以及当前帧到当前关键帧的位姿变化，得到当前帧对 应的相机姿态；
[0081 ]根据设定的灰度梯度阈值，筛选出去畸变后的当前帧图像中灰度梯度大于灰度梯 度阈值的像素点，并根据相机参数和当前帧对应的相机姿态，将筛选出的像素点反投影回 三维环境，得到所筛选出的像素点深度值;并将所筛选出的带有深度值的像素点加入地图 中；而后继续按照步骤2进行。
[0082]由于上述重建出来的三维环境是基于局部坐标系下的，与真实三维环境在统一的 尺度上不匹配，为了能够更好的应用重建地图，所以下面基于卫星定位信号进行拟合，将重 建出来的三维环境在统一的尺度下匹配到真实三维环境中。
[0083]在无人机实时地图重建过程中，通过卫星定位信号获得每一帧时刻下，无人机在 世界坐标系下的轨迹信息Xn，n表示的是第η帧;并在无人机实时地图重建过程中，得到每一 帧对应的相机在局部坐标系下的位置χ η:通过优化函数
[0084]
[0085] 得到优化函数取最小值对应的变换矩阵δ，其中Ν为无人机实时地图重建过程中的 总帧数，Τ(χη，δ)表示从局部坐标系到世界坐标系的投影转换函数，δ为从局部坐标系到世 界坐标系的变换矩阵;根据得到的变换矩阵S对应的投影转换函数，将重建得到的地图转换 到世界坐标系下。
[0086] 另外，卫星定位信号频率往往小于帧频，而且卫星定位信号的采集时刻与图像采 集时刻不严格对齐，此时，对于每一个卫星定位信号采集时刻^，得到无人机在世界坐标系 下的轨迹信息Xn;并用采集时刻t n前后各一帧对应的相机在局部坐标系下的位置插值得到 采集时刻^下相机在局部坐标系下的位置Xn;通过优化函数
[0087]
[0088]得到优化函数取最小值对应的变换矩阵δ，其中N为卫星定位信号的总采集点数，T (Xn，S)表示从局部坐标系到世界坐标系的投影转换函数J为从局部坐标系到世界坐标系 的变换矩阵;根据得到的变换矩阵δ对应的投影转换函数，将重建得到的地图转换到世界坐 标系下。
[0089] 下面给出重建的结果，包括实时和离线数据的测试结果，并与当前一些主流的视 觉SLAM方法作比较，来评价系统性能。以下所有的测试都在mint 17(基于Ubuntu 14.04， 64b i t)上进行，8G RAM，不用GPU，主频3 · 50GHz。
[0090] 表1:RGB-D基准测试结果比较
[0091]
[0092] 表2:绝对跟踪误差测试结果比较(单位cm)
[0093]
[0094] 注：[1 ]方法为：Dense RGB-D Odometry; [2]方法为：PTAM(keypoint_based); [3] 方法为：semi-densemono-VO; [4]方法为：keypoint-based mono-SLAM; [5]方法为：Direct RGB-D SLAM; [6]方法为:keypoint-based RGB-D SLAM;-表示没有查到该方法的数据，X表 示追踪失败(Tracking Lost);用作比较的数据来自Computer Vision Group;通过比较可 以发现:本专利的方法无论是位置漂移量、角度漂移量还是绝对误差都处于中上水平，相比 较其他的方法，我们的方法获取环境较多信息，同时与全稠密的方法，我们实验精度依然能 够达到使用标准，而且能够直接在CHJ上运行，不用要求GPU。
[0095] 此外本方法也在无人机上验证了实时地图构建，如图8和图9所示。本系统中无人 机上搭载了一台高清摄像机，通过高清图传将图像传输到地面的电脑，利用电脑运行的本 系统软件能够实时计算得到大地的稀疏点云，利用解算的飞机位姿、点云、特征点将拍摄的 图像实时拼接得到二维地图。通过本实验能够证明本发明方法能够在以实时重建地图。 [0096] 尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例 性的，不能理解为对本发明的限制，本