[发明专利]一种视觉SLAM的认知点云地图创建系统

权利要求书

1.一种视觉SLAM的认知点云地图创建系统，其特征在于，包括：

系统启动模块，负责选取第一个关键帧；

逆深度图估计模块，负责估计关键帧的逆深度图以及选取新的关键帧；

地图模块，负责维护创建的认知点云地图，并在新关键帧中识别感兴趣物体，同时负责地图创建的可视化显示；

其中，在逆深度图估计模块中，估计关键帧的逆深度图，包括逆深度图更新和逆深度图传递，包括以下步骤：

2.1)将关键帧中的像素的逆深度估计表示为一种高斯分布假设下的深度滤波器：

id～N(μ,σ²)

式中，id表示像素的逆深度，μ表示当前像素逆深度的估计值，σ²表示当前逆深度估计值的误差方差；

2.2)读取下一帧图像及其对应的位姿，将其作为参考帧；

2.3)生成参考帧的图像金字塔，对于关键帧的图像金字塔的每一层并行地执行步骤2.4)-2.6)；图像金字塔使用降采样的方式生成，下层图像中每个2×2的正方形中的4个像素合并为上层图像中的一个像素，最终生成一个三层图像金字塔：

式中，为图像金字塔的第k层图像的第i行第j列像素的灰度值；

2.4)根据地图点提取策略，在关键帧图像金字塔每一层图像上提取地图点；

2.5)对于每一个地图点，在参考帧图像金字塔对应层搜索最佳匹配点，并计算地图点逆深度和误差方差的观测值；

2.6)判断地图点是否存在先验逆深度和误差方差，如果存在，则使用观测值更新先验逆深度和误差方差：

式中，id_update表示更新得到的逆深度，表示更新得到的误差方差，id_prior表示先验逆深度，表示先验误差方差，id_obs表示地图点的逆深度观测值，表示地图点的误差方差观测值；

否则，使用本次更新的观测值初始化地图点的逆深度和误差方差：

式中，id_init表示初始化得到的逆深度，表示初始化得到的误差方差；

地图点更新或初始化得到的逆深度和误差方差作为下一次更新时该地图点的先验逆深度和误差方差；

2.7)将关键帧图像金字塔的每一层的逆深度图自上而下传递到金字塔最底层的原始关键帧图像；

2.8)根据关键帧选取策略，判断是否需要将参考帧选取为新的关键帧，如果不需要，则跳转到步骤2.2)；否则，将当前关键帧的逆深度图传递给新关键帧，并跳转到步骤2.2)，读取下一帧图像用于更新新关键帧的逆深度图。

2.根据权利要求1所述的一种视觉SLAM的认知点云地图创建系统，其特征在于：在系统启动模块中，根据相机的运动幅度选取第一个关键帧，相机的运动幅度使用李代数se(3)表示的两帧图像之间的位姿变换来描述，包括以下步骤：

1.1)读取第一帧图像，将其作为第一个关键帧候选，只有当其满足一定的条件时才能真正地成为第一个关键帧；

1.2)读取下一帧图像，判断其与关键帧候选之间的运动幅度是否较小，具体操作如下：

设ξ∈se(3)表示位姿变换，其中ξ的前三维表示平移，记为ρ＝(ρ₁,ρ₂,ρ₃)^T，后三维表示旋转，记为φ＝(φ₁,φ₂,φ₃)^T，相机的运动幅度包括旋转幅度和平移幅度：

式中，rot表示相机的旋转幅度，trans表示相机的平移幅度；

相机的运动幅度大可能是旋转幅度大也可能是平移幅度大，只有当旋转幅度和平移幅度都小于设定值时才认为相机的运动幅度小；

1.3)如果运动幅度小，则将候选关键帧选取为第一个关键帧；否则，将当前帧作为第一个关键帧候选，并重复步骤1.2)直到选取到第一个关键帧。