[发明专利]一种基于RGBD相机的弱纹理及动态场景视觉SLAM定位方法

权利要求书

1.一种基于RGBD相机的弱纹理及动态场景视觉SLAM定位方法，其特征在于，包括以下步骤：

步骤1，通过张正友相机标定法对相机主点、焦距、畸变参数进行标定，得到标定后相机主点、标定后焦距、标定后畸变参数；进一步设置尺度因子、预设特征点数量、滑动窗口的长度；

步骤2，通过相机实时采集多帧图像，根据当前帧图像的序号与滑动窗口的长度进行比较，进一步计算当前帧图像的网格尺寸；

步骤3，初始化当前帧相机位姿，根据前一帧图像特征点集合构建多个像素块，结合当前帧初始相机位姿构建所有像素块依次向当前帧投影，计算前一帧图像中各像素块与其在当前帧图像中对应的投影像素块的灰度差，结合灰度差去除前一帧图像特征点集合中动态特征点得到前一帧图像静态特征点集合；

步骤4，基于前一帧图像静态特征点集合、当前帧初始相机位姿构建位姿求解目标函数，对当前帧相机位姿进行第一阶段优化求解，进一步基于关键帧共视图对当前帧相机位姿进行进一步优化求解，得到优化后求解后当前帧相机位姿；

步骤1所述尺度因子为：空间点到相机中心的物理距离与该点在深度图像中深度值的比例；

步骤1所述预设特征点数量为：用户预设的希望在每帧图像上提取到的特征点数量，定义为N；

步骤1所述滑动窗口的长度定义为：L；

所述步骤2具体为：

设当前帧序号为k，若k＝1，步骤2所述计算当前帧图像的网格尺寸具体为：

根据当前帧图像的尺寸、步骤1所述预设特征点数量计算当前帧图像的网格尺寸，具体计算公式为：

其中，G_k表示第k帧图像的网格尺寸，N表示步骤1设定的预设特征点数量，w表示第k帧图像的长，h表示第k帧图像的宽；

若1k≤L，步骤2所述计算当前帧图像的网格尺寸具体为：

将第k-1帧图像根据第k-1帧图像的网格尺寸进行均匀划分得到第k-1帧图像中多个网格，定义第k-1帧的所有网格集合为Gr_k-1

对第k-1帧图像进行FAST特征提取得到第k-1帧图像的多个特征点；

基于Shi-Tomasi算法计算第k-1帧图像的每个特征点得分，选取第k-1帧图像中每个网格得分最高的特征点作为第k-1帧图像中该网格的主选特征点，选取第k-1帧图像中每个网格得分次高的特征点作为第k-1帧图像中该网格的备选特征点；

第k-1帧图像中网格j的主选特征点，定义为：

第k-1帧图像中网格j的备选特征点，定义为：

统计第k-1帧图像中所有网格的主选特征点数量，定义为：N_k-1；

将第k-1帧图像中所有网格的主选特征点数量与预设特征点数量N进行比较：

当N_k-1≥N时，第k-1帧图像的特征点由所有网格的主选特征点构成，否则，第k-1帧图像的特征点由所有网格的主选特征点和备选特征点共同构成；

若N_k-1＜N时则当前帧图像即第k帧图像的网格尺寸为G_k-1-θ，若N_k-1≥N时则当前帧图像即第k帧图像的网格尺寸为G_k-1+θ，当前帧图像即第k帧图像的网格尺寸定义为：G_k；

当k＞L，步骤2所述计算当前帧图像的网格尺寸具体为：

计算当前帧图像的网格尺寸，具体计算公式为：

其中，G_k表示第k帧图像的网格尺寸，N表示步骤1设定的预设特征点数量，L表示滑动窗口的长度，i∈[1,L]，N_k,pre表示第k帧图像中主选特征点数量加权值，G_k,pre表示第k帧图像中网格尺寸加权值，p表示加权比例系数，σ表示网格尺寸调整系数，G_k-i表示第k-i帧图像的网格尺寸，N_k-i表示第k-i帧图像中所有网格的主选特征点数量，m表示权重和；

第k帧图像的主选特征点集合，定义为：

第k帧图像的备选特征点集合，定义为：

第k帧图像的特征点集合，定义为：

其中，num_k为第k帧中的网格数量，j为网格序号。