本发明公开了一种视觉SLAM的认知点云地图创建系统，利用视觉SLAM获取的图像位姿信息，不仅能估计出环境的较稠密的逆深度信息，而且能识别出环境中的感兴趣物体，创建出环境的认知点云地图。包括三个模块：系统启动模块，负责选取第一个关键帧；逆深度图估计模块，负责估计关键帧的逆深度图以及选取新的关键帧；地图模块，负责维护创建的认知点云地图，并在新关键帧加入地图中时识别关键帧中的感兴趣物体，同时负责地图创建的可视化显示。本发明利用图像信息创建环境的较稠密的认知点云地图，该地图可以实现移动机器人的定位、避障、自主导航以及三维环境重建，地图中的认知信息甚至可以用于实现移动机器人与人、移动机器人与环境的交互。
1.一种视觉SLAM的认知点云地图创建系统，其特征在于，包括：系统启动模块，负责选取第一个关键帧；
逆深度图估计模块，负责估计关键帧的逆深度图以及选取新的关键帧；
地图模块，负责维护创建的认知点云地图，并在新关键帧中识别感兴趣物体，同时负责地图创建的可视化显示；
其中，在逆深度图估计模块中，估计关键帧的逆深度图，包括逆深度图更新和逆深度图传递，包括以下步骤：
2.1)将关键帧中的像素的逆深度估计表示为一种高斯分布假设下的深度滤波器：id～N(μ,σ2)
式中，id表示像素的逆深度，μ表示当前像素逆深度的估计值，σ2表示当前逆深度估计值的误差方差；
2.2)读取下一帧图像及其对应的位姿，将其作为参考帧；
2.3)生成参考帧的图像金字塔，对于关键帧的图像金字塔的每一层并行地执行步骤
2.4)-2.6)；图像金字塔使用降采样的方式生成，下层图像中每个2×2的正方形中的4个像素合并为上层图像中的一个像素，最终生成一个三层图像金字塔：式中， 为图像金字塔的第k层图像的第i行第j列像素的灰度值；
2.4)根据地图点提取策略，在关键帧图像金字塔每一层图像上提取地图点；
2.5)对于每一个地图点，在参考帧图像金字塔对应层搜索最佳匹配点，并计算地图点逆深度和误差方差的观测值；
2.6)判断地图点是否存在先验逆深度和误差方差，如果存在，则使用观测值更新先验逆深度和误差方差：式中，idupdate表示更新得到的逆深度， 表示更新得到的误差方差，idprior表示先验逆深度， 表示先验误差方差，idobs表示地图点的逆深度观测值， 表示地图点的误差方差观测值；
否则，使用本次更新的观测值初始化地图点的逆深度和误差方差：式中，idinit表示初始化得到的逆深度， 表示初始化得到的误差方差；
地图点更新或初始化得到的逆深度和误差方差作为下一次更新时该地图点的先验逆深度和误差方差；
2.7)将关键帧图像金字塔的每一层的逆深度图自上而下传递到金字塔最底层的原始关键帧图像；
2.8)根据关键帧选取策略，判断是否需要将参考帧选取为新的关键帧，如果不需要，则跳转到步骤2.2)；否则，将当前关键帧的逆深度图传递给新关键帧，并跳转到步骤2.2)，读取下一帧图像用于更新新关键帧的逆深度图。
2.根据权利要求1所述的一种视觉SLAM的认知点云地图创建系统，其特征在于：在系统启动模块中，根据相机的运动幅度选取第一个关键帧，相机的运动幅度使用李代数se(3)表示的两帧图像之间的位姿变换来描述，包括以下步骤：
1.1)读取第一帧图像，将其作为第一个关键帧候选，只有当其满足一定的条件时才能真正地成为第一个关键帧；
1.2)读取下一帧图像，判断其与关键帧候选之间的运动幅度是否较小，具体操作如下：T
设ξ∈se(3)表示位姿变换，其中ξ的前三维表示平移，记为ρ＝(ρ1,ρ2,ρ3) ，后三维表示旋转，记为φ＝(φ1,φ2,φ3)T，相机的运动幅度包括旋转幅度和平移幅度：式中，rot表示相机的旋转幅度，trans表示相机的平移幅度；
相机的运动幅度大可能是旋转幅度大也可能是平移幅度大，只有当旋转幅度和平移幅度都小于设定值时才认为相机的运动幅度小；
1.3)如果运动幅度小，则将候选关键帧选取为第一个关键帧；否则，将当前帧作为第一个关键帧候选，并重复步骤1.2)直到选取到第一个关键帧。
3.根据权利要求1所述的一种视觉SLAM的认知点云地图创建系统，其特征在于：在步骤
式中，Guv表示像素的梯度， 表示像素的绝对梯度值，Iuv表示图像第v行第u列的像素灰度值；
2.4.2)极线和梯度的夹角的余弦值平方不能小于设定值，或者梯度在极线上的投影模长不能短于设定值：式中，θ表示梯度与极线的夹角，len表示梯度在极线上的投影模长，(u,v)表示地图点的像素坐标，(gxuv,gyuv)表示地图点的梯度，(epxuv,epyuv)表示地图点的极线；
2.4.3)地图点与关键帧的极点需要保持设定的距离，即关键帧极线的长度不能短于设定值：式中，epuv表示关键帧极线， 表示关键帧极线的长度，(u,v)表示地图点在关键帧中的像素坐标，(u0,v0)表示参考帧的相机光心在关键帧上的投影坐标；
上述3个地图点提取策略必须同时满足才能表明当前像素是一个好的地图点，它们之间是一种“且”的关系。
4.根据权利要求1所述的一种视觉SLAM的认知点云地图创建系统，其特征在于：在步骤
2.5)中，对于关键帧图像金字塔每一层图像中的地图点，在参考帧图像金字塔对应层图像中搜索地图点的最佳匹配点，同时根据最佳匹配点计算该地图点的逆深度和误差方差的观测值，包括以下步骤：
2.5.1)以单位长度为步长遍历参考帧极线，以误差平方和作为匹配标准；选取参考帧极线上的5个等距离像素以及关键帧上对应的5个等距离像素，计算它们的误差平方和，选择遍历过程中误差平方和最小的像素作为当前最佳匹配点，同时记录前一次遍历和后一次遍历的误差平方和；
2.5.2)对当前最佳匹配点的误差平方和以及其前一次遍历和后一次遍历的误差平方和进行抛物线插值，获得一个误差平方和最小的像素，将该像素作为当前最佳匹配点；
2.5.3)在当前最佳匹配点周围选取一个固定大小的窗口作为最佳匹配的搜索范围，遍历窗口内的每一个像素，以归一化积相关作为匹配标准；在地图点和当前遍历的像素周围选取相同固定大小的窗口内的所有像素，计算它们的归一化积相关，选择遍历过程中归一化积相关最大的像素作为最终的最佳匹配点；
2.5.4)根据最佳匹配点计算地图点的逆深度和误差方差的观测值；设地图点的归一化相机坐标为 最佳匹配点的归一化相机坐标为 关键帧到参考帧的旋转矩阵为R，关键帧到参考帧的平移向量为t；
式中，idobs表示地图点的逆深度观测值，R0为R的第一行，R1为R的第二行，R2为R的第三行，t0为t的第一维，t1为t的第二维行，t2为t的第三维；
地图点的误差方差的观测值采用LSD-SLAM中的计算方法：式中， 表示地图点的误差方差观测值， 表示几何视差误差方差， 表示光度视差误差方差；α＝δd/δλ表示比例系数，δd表示逆深度间隔的长度，δλ表示极线步进的长度。
5.根据权利要求1所述的一种视觉SLAM的认知点云地图创建系统，其特征在于：在步骤
2.7)中，经过一次更新后，得到关键帧图像金字塔每一层的逆深度图，将图像金字塔的每一层的逆深度图自上而下传递到金字塔最底层的原始关键帧图像，包括以下步骤：
2.7.1)对于上层逆深度图中的每一个地图点，遍历其在下层图像中对应的四个像素；
2.7.2)判断下层图像中对应的像素是否存在逆深度估计，如果对应像素不存在逆深度估计，则使用地图点的逆深度和误差方差初始化对应像素的逆深度和误差方差；
2.7.3)如果对应像素存在逆深度估计，则判断地图点的误差方差是否小于对应像素的误差方差，即地图点的逆深度估计是否比对应像素的逆深度估计准确；
2.7.4)如果地图点的误差方差小于对应像素的误差方差，则判断地图点与对应像素的逆深度估计是否相似：|id1-id2|＜σ1+σ2
式中，id1和id2分别表示两个逆深度估计的逆深度，σ1和σ2分别是它们对应的误差方差开根号；
2.7.5)如果地图点和对应像素的逆深度估计相似，则使用地图点的逆深度估计更新对应像素的逆深度估计；
2.7.6)如果地图点的误差方差大于对应像素的误差方差或者地图点和对应像素的逆深度估计不相似，则跳过该像素，遍历下一个对应像素。
6.根据权利要求1所述的一种视觉SLAM的认知点云地图创建系统，其特征在于：在步骤
2.8.1)如果关键帧的平均逆深度误差没有降低到τ，但相机的旋转幅度大于α1或者平移幅度大于β1，那么选取新的关键帧；
2.8.2)如果关键帧的平均逆深度误差降低到τ，且相机的旋转幅度大于α2或者平移幅度大于β2，那么选取新的关键帧；
其中，τ、α1、β1、α2、β2均为设定的阈值，且α1大于α2、β1大于β2；关键帧的平均逆深度误差为：式中，mean表示关键帧的平均逆深度误差，n表示所有逆深度估计的像素个数，σi表示逆深度估计的误差方差开根号；
上述2个关键帧选取策略，只要满足其中一个就表明需要选取新的关键帧，它们之间是一种“或”的关系。
7.根据权利要求1所述的一种视觉SLAM的认知点云地图创建系统，其特征在于：在地图模块中，维护关键帧的认知点云地图，并识别关键帧中的感兴趣物体作为环境的认知信息，包括以下内容：
3.1)用地图表示方法描述所有关键帧，包括关键帧的位姿信息、图像信息、逆深度信息、误差方差信息和物体识别信息，其中位姿信息使用位姿变换矩阵表示，图像信息是相机拍摄到的且经过矫正的图像，逆深度信息、误差方差信息和物体识别信息分别被表示为一种与关键帧图像大小相同且像素一一对应的图像形式；
3.2)使用MS COCO数据集训练得到的Mask R-CNN模型对关键帧中的感兴趣物体进行识别，识别出感兴趣物体的类别并分割出感兴趣物体在图像中的区域，将识别结果转换为一种特定形式的图像作为环境的认知信息，识别结果的转换方法如下：对于MS COCO中的80种不同的物体类别，使用数字1到80进行编号，并使用数字0表示背景类型，使用一个3位的五进制数表示0到80的所有数字；设0到80的任意一个整数n的五进制表示形式为xyz，则有：n＝25·x+5·y+z
根据整数n的五进制表示形式，计算其对应的物体类别在转换得到的特定形式的图像中的颜色：式中，R、G和B分别表示红色、绿色和蓝色三通道的数值，通过RGB三通道的数值能够得到最终的颜色；
3.3)可视化显示地图创建的过程，一方面显示当前关键帧图像金字塔每一层的逆深度图的更新状态，另一方面将已加入地图中的关键帧转换为认知点云，方便用户查看地图创建的好坏，及时地发现地图创建过程中的异常。
一种视觉SLAM的认知点云地图创建系统
技术领域
[0001] 本发明涉及视觉SLAM、逆深度估计、物体识别以及地图创建领域，尤其是指一种视觉SLAM的认知点云地图创建系统。
背景技术
[0002] 同时定位与地图创建(Simultaneous Localization and Mapping，SLAM)是实现机器人智能感知的一种重要方式，1986年在IEEE机器人和自动化会议上被首次提出。在未知环境中，移动机器人通过自身搭载的传感器获取环境信息，并在运动过程中不断估计自身的位姿，同时对其周围环境创建地图。SLAM主要用于解决移动机器人的“定位”和“地图创建”，移动机器人一方面需要知道自己当前在地图的什么位置(定位)，另一方面又需要创建周围环境的地图(地图创建)。只通过相机获取环境信息的SLAM被称为视觉SLAM。由于相机价格低、功耗低、重量轻、体积小、图像信息丰富、存在于很多手持设备中，且技术难点大，所以视觉SLAM受到了国内外广大学者和专家的研究。
[0003] 三维地图创建是视觉SLAM中的一个研究重点，地图能帮助移动机器人理解环境和执行任务，甚至能提供给人类，帮助人类认识和理解未知环境，这对视觉SLAM创建的地图提出了更高的要求。视觉SLAM主要包括定位和地图创建两部分，当前研究主要集中在如何提高定位准确度。当前视觉SLAM创建的地图还不能很好地用于移动机器人理解环境和执行任务，更不用说帮助人类理解未知环境。视觉SLAM创建的三维地图的最基本的功能应该是机器人的导航以及在机器人执行任务时用于路径规划和障碍躲避；其次，三维地图应该更进一步描述环境，具备一定的认知能力，用于提高机器人的智能化水平，如倒水、取物体以及更高水平的自主导航，认知地图在一定程度上也能帮助人类理解环境。
发明内容
[0004] 本发明的目的在于克服现有技术的缺点与不足，提出了一种准确性和实用性较强的视觉SLAM的认知点云地图创建系统，利用视觉SLAM获取的图像的位姿信息，实现环境的认知点云地图的创建，能够用于实现移动机器人的定位、避障、自主导航以及三维重建和人机交互等。
[0005] 为了实现上述目的，本发明所提供的技术方案为：一种视觉SLAM的认知点云地图创建系统，包括模块：
[0006] 系统启动模块，负责选取第一个关键帧。
[0007] 逆深度图估计模块，负责估计关键帧的逆深度图以及选取新的关键帧；
[0008] 地图模块，负责维护创建的认知点云地图，并在新关键帧中识别感兴趣物体，同时负责地图创建的可视化显示。
[0009] 在系统启动模块中，根据相机的运动幅度选取第一个关键帧，相机的运动幅度使用李代数se(3)表示的两帧图像之间的位姿变换来描述，包括以下步骤：
[0010] 1.1)读取第一帧图像，将其作为第一个关键帧候选，只有当其满足一定的条件时才能真正地成为第一个关键帧；
[0011] 1.2)读取下一帧图像，判断其与关键帧候选之间的运动幅度是否较小，具体操作如下：
[0012] 设ξ∈se(3)表示位姿变换，其中ξ的前三维表示平移，记为ρ＝(ρ1,ρ2,ρ3)T，后三维表示旋转，记为φ＝(φ1,φ2,φ3)T。相机的运动幅度包括旋转幅度和平移幅度，计算公式如下：
[0013]
[0014] 式中，rot表示相机的旋转幅度，trans表示相机的平移幅度。
[0015] 相机的运动幅度过大可能是旋转幅度过大也可能是平移幅度过大，只有当旋转幅度和平移幅度都较小时才认为相机的运动幅度较小。
[0016] 1.3)如果运动幅度较小，则将候选关键帧选取为第一个关键帧；否则，将当前帧作为第一个关键帧候选，并重复步骤1.2)直到选取到第一个关键帧。
[0017] 在逆深度图估计模块中，根据读入的图像估计关键帧的逆深度图，主要包括逆深度图更新和逆深度图传递，包括以下步骤：
[0018] 2.1)将关键帧中的像素的逆深度估计表示为一种高斯分布假设下的深度滤波器：
[0019] id～N(μ,σ2)
[0020] 式中，id表示像素的逆深度，μ表示当前像素逆深度的估计值，σ2表示当前逆深度估计值的误差方差。
[0021] 2.2)读取下一帧图像及其对应的位姿，将其作为参考帧。
[0022] 2.3)生成参考帧的图像金字塔，对于关键帧的图像金字塔的每一层并行地执行步骤2.4)-2.6)；图像金字塔使用降采样的方式生成，下层图像中每个2×2的正方形中的4个像素合并为上层图像中的一个像素，最终生成一个三层图像金字塔：
[0023]
[0024] 式中， 为图像金字塔的第k层图像的第i行第j列像素的灰度值。
[0025] 2.4)根据地图点提取策略，在关键帧图像金字塔每一层图像上提取地图点。
[0026] 地图点的提取策略为：
[0027] 2.4.1)地图点必须具有较明显的梯度：
[0028]
[0029] 式中，Guv表示像素的梯度， 表示像素的绝对梯度值，Iuv表示图像第v行第u列的像素灰度值。
[0030] 2.4.2)极线和梯度的夹角的余弦值平方不能太小，或者梯度在极线上的投影模长不能太短：
[0031]
[0032]
[0033] 式中，θ表示梯度与极线的夹角，len表示梯度在极线上的投影模长，(u,v)表示地图点的像素坐标，(gxuv,gyuv)表示地图点的梯度，(epxuv,epyuv)表示地图点的极线。
[0034] 2.4.3)地图点与关键帧的极点需要保持一定的距离，即关键帧极线的长度不能太短：
[0035]
[0036] 式中，epuv表示关键帧极线， 表示关键帧极线的长度，(u,v)表示地图点在关键帧中的像素坐标，(u0,v0)表示参考帧的相机光心在关键帧上的投影坐标。
[0037] 上述3个地图点提取策略必须同时满足才能表明当前像素是一个好的地图点，它们之间是一种“且”的关系。
[0038] 2.5)对于每一个地图点，在参考帧图像金字塔对应层搜索最佳匹配点，并计算地图点逆深度和误差方差的观测值，包括以下步骤：
[0039] 2.5.1)以单位长度为步长遍历参考帧极线，以误差平方和作为匹配标准。选取参考帧极线上的5个等距离像素以及关键帧上对应的5个等距离像素，计算它们的误差平方和，选择遍历过程中误差平方和最小的像素作为当前最佳匹配点，同时记录前一次遍历和后一次遍历的误差平方和。
[0040] 2.5.2)对当前最佳匹配点的误差平方和以及其前一次遍历和后一次遍历的误差平方和进行抛物线插值，获得一个误差平方和更小的像素，将该像素作为当前最佳匹配点。
[0041] 2.5.3)在当前最佳匹配点周围选取一个固定大小的窗口作为最佳匹配的搜索范围，遍历窗口内的每一个像素，以归一化积相关作为匹配标准。在地图点和当前遍历的像素周围选取相同固定大小的窗口内的所有像素，计算它们的归一化积相关，选择遍历过程中归一化积相关最大的像素作为最终的最佳匹配点。
[0042] 2.5.4)根据最佳匹配点计算地图点的逆深度和误差方差的观测值。设地图点的归一化相机坐标为 最佳匹配点的归一化相机坐标为 关键帧到参考帧的旋转矩阵为R，关键帧到参考帧的平移向量为t。
[0043] 地图点的逆深度观测为：
[0044] 或
[0045] 式中，idobs表示地图点的逆深度观测值，R0为R的第一行，R1为R的第二行，R2为R的第三行，t0为t的第一维，t1为t的第二维行，t2为t的第三维。
[0046] 地图点的误差方差的观测值采用LSD-SLAM中的计算方法：
[0047]
[0048] 式中， 表示地图点的误差方差观测值， 表示几何视差误差方差， 表示光度视差误差方差；α＝δd/δλ表示比例系数，δd表示逆深度间隔的长度，δλ表示极线步进的长度。
[0049] 2.6)判断地图点是否存在先验逆深度和误差方差，如果存在，则使用观测值更新先验逆深度和误差方差：
[0050]
[0051] 式中，idupdate表示更新得到的逆深度， 表示更新得到的误差方差，idprior表示先验逆深度， 表示先验误差方差，idobs表示地图点的逆深度观测值， 表示地图点的误差方差观测值。
[0052] 否则，使用本次更新的观测值初始化地图点的逆深度和误差方差：
[0053]
[0054] 式中，idinit表示初始化得到的逆深度， 表示初始化得到的误差方差。
[0055] 地图点更新或初始化得到的逆深度和误差方差作为下一次更新时该地图点的先验逆深度和误差方差。
[0056] 2.7)将关键帧图像金字塔的每一层的逆深度图自上而下传递到金字塔最底层的原始关键帧图像，包括以下步骤：
[0057] 2.7.1)对于上层逆深度图中的每一个地图点，遍历其在下层图像中对应的四个像素。
[0058] 2.7.2)判断下层图像中对应的像素是否存在逆深度估计，如果对应像素不存在逆深度估计，则使用地图点的逆深度和误差方差初始化对应像素的逆深度和误差方差。
[0059] 2.7.3)如果对应像素存在逆深度估计，则判断地图点的误差方差是否小于对应像素的误差方差，即地图点的逆深度估计是否比对应像素的逆深度估计准确。
[0060] 2.7.4)如果地图点的误差方差小于对应像素的误差方差，则判断地图点与对应像素的逆深度估计是否相似：
[0061] |id1-id2|＜σ1+σ2
[0062] 式中，id1和id2分别表示两个逆深度估计的逆深度，σ1和σ2分别是它们对应的误差方差开根号。
[0063] 2.7.5)如果地图点和对应像素的逆深度估计相似，则使用地图点的逆深度估计更新对应像素的逆深度估计。
[0064] 2.7.6)如果地图点的误差方差大于对应像素的误差方差或者地图点和对应像素的逆深度估计不相似，则跳过该像素，遍历下一个对应像素。
[0065] 2.8)根据关键帧选取策略，判断是否需要将参考帧选取为新的关键帧，如果不需要，则跳转到步骤2.2)；否则，将当前关键帧的逆深度图传递给新关键帧，并跳转到步骤2.2)，读取下一帧图像用于更新新关键帧的逆深度图。关键帧选取策略为：
[0066] 2.8.1)如果关键帧的平均逆深度误差没有降低到τ，但相机的旋转幅度大于α1或者平移幅度大于β1，那么选取新的关键帧。
[0067] 2.8.2)如果关键帧的平均逆深度误差降低到τ，且相机的旋转幅度大于α2或者平移幅度大于β2，那么选取新的关键帧。
[0068] 其中，τ、α1、β1、α2、β2均为设定的阈值，α1大于α2，β1大于β2。关键帧的平均逆深度误差为：
[0069]
[0070] 式中，mean表示关键帧的平均逆深度误差，n表示所有逆深度估计的像素个数，σi表示逆深度估计的误差方差开根号。
[0071] 上述2个关键帧选取策略，只要满足其中一个就表明需要选取新的关键帧，它们之间是一种“或”的关系。
[0072] 在地图模块中，维护关键帧的认知点云地图，并识别关键帧中的感兴趣物体作为环境的认知信息，包括以下内容：
[0073] 3.1)使用一种新颖的地图表示方法描述所有关键帧，主要包括关键帧的位姿信息、图像信息、逆深度信息、误差方差信息和物体识别信息，其中位姿信息使用位姿变换矩阵表示，图像信息是对相机拍摄到的图像矫正后的图像，逆深度信息、误差方差信息和物体识别信息分别被表示为一种与关键帧图像大小相同且像素一一对应的图像形式。
[0074] 3.2)使用MS COCO数据集训练得到的Mask R-CNN模型对关键帧中的感兴趣物体进行识别，识别出感兴趣物体的类别并分割出感兴趣物体在图像中的区域，将识别结果转换为一种特定形式的图像作为环境的认知信息。识别结果的转换方法如下：
[0075] 对于MS COCO中的80种不同的物体类别，使用数字1到80进行编号，并使用数字0表示背景类型，使用一个3位的五进制数表示0到80的所有数字。设0到80的任意一个整数n的五进制表示形式为xyz，则有：
[0076] n＝25·x+5·y+z
[0077] 根据整数n的五进制表示形式，计算其对应的物体类别在转换得到的特定形式的图像中的颜色：
[0078]
[0079] 式中，R、G和B分别表示红色(red)、绿色(green)和蓝色(blue)三通道的数值，通过RGB三通道的数值可以得到最终的颜色。
[0080] 3.3)可视化显示地图创建的过程，一方面显示当前关键帧图像金字塔每一层的逆深度图的更新状态，另一方面将已加入地图中的关键帧转换为认知点云，方便用户查看地图创建的好坏，及时地发现地图创建过程中的异常。
[0081] 本发明相对于现有技术具有如下的优点及效果：
[0082] 1、本发明是对视觉SLAM的补充和完善，能够创建出环境的认知点云地图，可以用于实现移动机器人的定位、避障、自主导航以及三维重建和人机交互。
[0083] 2、本发明使用一种关键帧选取策略，保证关键帧的选取能够提高逆深度估计的准确度和三维物体识别的完整度。
[0084] 3、本发明使用一种地图点立体匹配方式，在保证地图点立体匹配效率的同时能够达到较准确的匹配效果。
[0085] 4、本发明使用一种图像金字塔传递的方式估计关键帧的逆深度图，能够提高关键帧逆深度图的稠密度并加快逆深度图的收敛速度。
[0086] 5、本发明能够识别环境中的感兴趣物体，并在点云地图中进行标识，实现认知点云地图的创建，有助于实现移动机器人与人、移动机器人与环境的交互。
附图说明
[0087] 图1是认知点云地图创建的流程图。
[0088] 图2是基于图像金字塔的逆深度图估计的流程图。
[0089] 图3是图像金字塔传递的流程图。
[0090] 图4是图像金字塔的示意图。
[0091] 图5是地图种类及其适用场景。
具体实施方式
[0092] 下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
[0093] 参见图1所示，本实施例所提供的视觉SLAM的认知点云地图创建系统，包括模块：
[0094] 系统启动模块，根据相机的运动幅度选取第一个关键帧，相机的运动幅度使用李代数se(3)表示的两帧图像之间的位姿变换来描述，包括以下步骤：
[0095] 1.1)读取第一帧图像，将其作为第一个关键帧候选，只有当其满足一定的条件时才能真正地成为第一个关键帧；
[0096] 1.2)读取下一帧图像，判断其与关键帧候选之间的运动幅度是否较小，具体操作如下：
[0097] 设ξ∈se(3)表示位姿变换，其中ξ的前三维表示平移，记为ρ＝(ρ1,ρ2,ρ3)T，后三维表示旋转，记为φ＝(φ1,φ2,φ3)T。相机的运动幅度包括旋转幅度和平移幅度，计算公式如下：
[0098]
[0099] 式中，rot表示相机的旋转幅度，trans表示相机的平移幅度。
[0100] 相机的运动幅度过大可能是旋转幅度过大也可能是平移幅度过大，只有当旋转幅度和平移幅度都较小时才认为相机的运动幅度较小。
[0101] 1.3)如果运动幅度较小，则将候选关键帧选取为第一个关键帧；否则，将当前帧作为第一个关键帧候选，并重复步骤1.2直到选取到第一个关键帧。
[0102] 逆深度图估计模块，根据读入的图像估计关键帧的逆深度图，主要包括逆深度图更新和逆深度图传递，包括以下步骤：
[0103] 2.1)将关键帧中的像素的逆深度估计表示为一种高斯分布假设下的深度滤波器：
[0104] id～N(μ,σ2)
[0105] 式中，id表示像素的逆深度，μ表示当前像素逆深度的估计值，σ2表示当前逆深度估计值的误差方差。
[0106] 2.2)读取下一帧图像及其对应的位姿，将其作为参考帧。
[0107] 2.3)生成参考帧的图像金字塔，对于关键帧的图像金字塔的每一层并行地执行步骤2.4)-2.6)；图像金字塔使用降采样的方式生成，下层图像中每个2×2的正方形中的4个像素合并为上层图像中的一个像素，最终生成一个三层图像金字塔(如图4所示)：
[0108]
[0109] 式中， 为图像金字塔的第k层图像的第i行第j列像素的灰度值。
[0110] 2.4)根据地图点提取策略，在关键帧图像金字塔每一层图像上提取地图点。
[0111] 地图点的提取策略为：
[0112] 2.4.1)地图点必须具有较明显的梯度：
[0113]
[0114] 式中，Guv表示像素的梯度， 表示像素的绝对梯度值，Iuv表示图像第v行第u列的像素灰度值。
[0115] 2.4.2)极线和梯度的夹角的余弦值平方不能太小，或者梯度在极线上的投影模长不能太短：
[0116]
[0117]
[0118] 式中，θ表示梯度与极线的夹角，len表示梯度在极线上的投影模长，(u,v)表示地图点的像素坐标，(gxuv,gyuv)表示地图点的梯度，(epxuv,epyuv)表示地图点的极线。
[0119] 2.4.3)地图点与关键帧的极点需要保持一定的距离，即关键帧极线的长度不能太短：
[0120]
[0121] 式中，epuv表示关键帧极线， 表示关键帧极线的长度，(u,v)表示地图点在关键帧中的像素坐标，(u0,v0)表示参考帧的相机光心在关键帧上的投影坐标。
[0122] 上述3个地图点提取策略必须同时满足才能表明当前像素是一个好的地图点，它们之间是一种“且”的关系。
[0123] 2.5)对于每一个地图点，在参考帧图像金字塔对应层搜索最佳匹配点，并计算地图点逆深度和误差方差的观测值，包括以下步骤：
[0124] 2.5.1)以单位长度为步长遍历参考帧极线，以误差平方和作为匹配标准。选取参考帧极线上的5个等距离像素以及关键帧上对应的5个等距离像素，计算它们的误差平方和，选择遍历过程中误差平方和最小的像素作为当前最佳匹配点，同时记录前一次遍历和后一次遍历的误差平方和。
[0125] 2.5.2)对当前最佳匹配点的误差平方和以及其前一次遍历和后一次遍历的误差平方和进行抛物线插值，获得一个误差平方和更小的像素，将该像素作为当前最佳匹配点。
[0126] 2.5.3)在当前最佳匹配点周围选取一个固定大小的窗口作为最佳匹配的搜索范围，遍历窗口内的每一个像素，以归一化积相关作为匹配标准。在地图点和当前遍历的像素周围选取相同固定大小的窗口内的所有像素，计算它们的归一化积相关，选择遍历过程中归一化积相关最大的像素作为最终的最佳匹配点。
[0127] 2.5.4)根据最佳匹配点计算地图点的逆深度和误差方差的观测值。设地图点的归一化相机坐标为 最佳匹配点的归一化相机坐标为 关键帧到参考帧的旋转矩阵为R，关键帧到参考帧的平移向量为t。
[0128] 地图点的逆深度观测为：
[0129] 或
[0130] 式中，idobs表示地图点的逆深度观测值，R0为R的第一行，R1为R的第二行，R2为R的第三行，t0为t的第一维，t1为t的第二维行，t2为t的第三维。
[0131] 地图点的误差方差的观测值采用LSD-SLAM中的计算方法：
[0132]
[0133] 式中， 表示地图点的误差方差观测值， 表示几何视差误差方差， 表示光度视差误差方差；α＝δd/δλ表示比例系数，δd表示逆深度间隔的长度，δλ表示极线步进的长度。
[0134] 2.6)判断地图点是否存在先验逆深度和误差方差，如果存在，则使用观测值更新先验逆深度和误差方差：
[0135]
[0136] 式中，idupdate表示更新得到的逆深度， 表示更新得到的误差方差，idprior表示先验逆深度， 表示先验误差方差。
[0137] 否则，使用本次更新的观测值初始化地图点的逆深度和误差方差：
[0138]
[0139] 式中，idinit表示初始化得到的逆深度， 表示初始化得到的误差方差。
[0140] 地图点更新或初始化得到的逆深度和误差方差作为下一次更新时该地图点的先验逆深度和误差方差。
[0141] 2.7)将关键帧图像金字塔的每一层的逆深度图自上而下传递到金字塔最底层的原始关键帧图像。
[0142] 2.8)根据关键帧选取策略，判断是否需要将参考帧选取为新的关键帧，如果不需要，则跳转到步骤2.2)；否则，将当前关键帧的逆深度图传递给新关键帧，并跳转到步骤2.2)，读取下一帧图像用于更新新关键帧的逆深度图。关键帧选取策略为：
[0143] 2.8.1)如果关键帧的平均逆深度误差没有降低到τ，但相机的旋转幅度大于α1或者平移幅度大于β1，那么选取新的关键帧。
[0144] 2.8.2)如果关键帧的平均逆深度误差降低到τ，且相机的旋转幅度大于α2或者平移幅度大于β2，那么选取新的关键帧。
[0145] 其中，τ、α1、β1、α2、β2均为设定的阈值，α1大于α2，β1大于β2。关键帧的平均逆深度误差为：
[0146]
[0147] 式中，mean表示关键帧的平均逆深度误差，n表示所有逆深度估计的像素个数，σi表示逆深度估计的误差方差开根号。
[0148] 上述2个关键帧选取策略，只要满足其中一个就表明需要选取新的关键帧，它们之间是一种“或”的关系。
[0149] 地图模块，维护关键帧的认知点云地图，并识别关键帧中的感兴趣物体作为环境的认知信息，包括以下内容：
[0150] 3.1)使用一种新颖的地图表示方法描述所有关键帧，主要包括关键帧的位姿信息、图像信息、逆深度信息、误差方差信息和物体识别信息，其中位姿信息使用位姿变换矩阵表示，图像信息是对相机拍摄到的图像矫正后的图像，逆深度信息、误差方差信息和物体识别信息分别被表示为一种与关键帧图像大小相同且像素一一对应的图像形式。
[0151] 3.2)使用MS COCO数据集训练得到的Mask R-CNN模型对关键帧中的感兴趣物体进行识别，识别出感兴趣物体的类别并分割出感兴趣物体在图像中的区域，将识别结果转换为一种特定形式的图像作为环境的认知信息。识别结果的转换方法如下：
[0152] 对于MS COCO中的80种不同的物体类别，使用数字1到80进行编号，并使用数字0表示背景类型，使用一个3位的五进制数表示0到80的所有数字。设0到80的任意一个整数n的五进制表示形式为xyz，则有：
[0153] n＝25·x+5·y+z
[0154] 根据整数n的五进制表示形式，计算其对应的物体类别在转换得到的特定形式的图像中的颜色：
[0155]
[0156] 式中，R、G和B分别表示红色(red)、绿色(green)和蓝色(blue)三通道的数值，通过RGB三通道的数值可以得到最终的颜色。
[0157] 3.3)可视化显示地图创建的过程，一方面显示当前关键帧图像金字塔每一层的逆深度图的更新状态，另一方面将已加入地图中的关键帧转换为认知点云，方便用户查看地图创建的好坏，及时地发现地图创建过程中的异常。
[0158] 其中，地图种类及其适用场景如图5所示。
[0159] 参见图2所示，本实施例所述的基于图像金字塔的逆深度图估计方法，包括以下步骤：
[0160] 1)读取下一帧图像及其对应的位姿，将其作为参考帧。
[0161] 2)生成参考帧的图像金字塔，对于关键帧的图像金字塔的每一层并行地执行步骤3)-5)。
[0162] 3)根据地图点提取策略，在关键帧图像金字塔每一层图像上提取地图点。
[0163] 4)对于每一个地图点，在参考帧图像金字塔对应层搜索最佳匹配点，并计算地图点逆深度和误差方差的观测值。
[0164] 5)判断地图点是否存在先验逆深度和误差方差，如果存在，则使用观测值更新先验逆深度和误差方差；否则，使用本次更新的观测值初始化地图点的逆深度和误差方差。地图点更新或初始化得到的逆深度和误差方差作为下一次更新时该地图点的先验逆深度和误差方差。
[0165] 6)将关键帧图像金字塔的每一层的逆深度图自上而下传递到金字塔最底层的原始关键帧图像。
[0166] 7)根据关键帧选取策略，判断是否需要将参考帧选取为新的关键帧，如果不需要，则跳转到步骤1)；否则，将当前关键帧的逆深度图传递给新关键帧，并跳转到步骤1)，读取下一帧图像用于更新新关键帧的逆深度图。
[0167] 参见图3所示，本实施例所述的图像金字塔传递方法，包括以下步骤：
[0168] 1)对于上层逆深度图中的每一个地图点，遍历其在下层图像中对应的四个像素。
[0169] 2)判断下层图像中对应的像素是否存在逆深度估计，如果对应像素不存在逆深度估计，则使用地图点的逆深度和误差方差初始化对应像素的逆深度和误差方差。
[0170] 3)如果对应像素存在逆深度估计，则判断地图点的误差方差是否小于对应像素的误差方差，即地图点的逆深度估计是否比对应像素的逆深度估计准确。
[0171] 4)如果地图点的误差方差小于对应像素的误差方差，则判断地图点与对应像素的逆深度估计是否相似：
[0172] |id1-id2|＜σ1+σ2
[0173] 式中，id1和id2分别表示两个逆深度估计的逆深度，σ1和σ2分别是它们对应的误差方差开根号。
[0174] 5)如果地图点和对应像素的逆深度估计相似，则使用地图点的逆深度估计更新对应像素的逆深度估计。
[0175] 6)如果地图点的误差方差大于对应像素的误差方差或者地图点和对应像素的逆深度估计不相似，则跳过该像素，遍历下一个对应像素。
[0176] 综上所述，本发明补充和完善了视觉SLAM，利用视觉SLAM获得的图像位姿信息，不仅能估计出环境的较稠密的逆深度信息，而且能识别出环境中的感兴趣物体，创建得到的认知点云地图能够用于实现移动机器人的定位、避障、自主导航以及三维重建和人机交互等，扩展了视觉SLAM的使用场景。
[0177] 上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
