融合MSCKF和图优化的视觉SLAM定位方法及装置与流程

技术特征：
1.一种融合msckf和图优化的视觉slam定位方法，其特征在于，包括：实时获取当前的图像以及惯性数据；每获取到一帧当前图像，则基于当前的所述惯性数据，跟踪得到所述当前图像上的多个特征点；将所述多个特征点的信息以及所述惯性数据输入滤波器中，进行滤波器预测和更新，并输出当前位姿、关键帧的信息以及所述多个特征点的信息；其中，所述关键帧的信息在所述当前图像被确定为关键帧时输出；从关键帧队列中提取出目标关键帧；其中，所述关键帧队列包括所有待处理的关键帧；所述目标关键帧为所述关键帧队列中获取时间距离当前最长的关键帧；将所述关键帧队列中的多帧关键帧中与所述目标关键帧共视的特征点，还原到所述关键帧列表中获取时间最新的关键帧中；从所述关键帧队列的各个关键帧上的特征点中筛选出多个地图点；针对目标关键帧进行回环检测；若回环检测失败，则将所述多个地图点、所述目标关键帧及其共视关系，更新到地图库中；若回环检测成功，则将相关关键帧的信息输入全局光束平差法模型中，通过全局光束平差法模型对所述相关关键帧以及回环地图点进行优化；其中，所述相关关键帧包括回环检测到的回环帧以及所述目标关键帧；所述回环地图点为回环检测到的所述地图点；将优化后的所述相关关键帧以及所述回环地图点更新至所述地图库，以及反馈给所述滤波器进行状态更新。2.根据权利要求1所述的方法，其特征在于，所述将所述多个特征点的信息以及所述惯性数据输入滤波器中，进行滤波器预测和更新，并输出位姿、关键帧信息以及所述多个特征点的信息，包括：将所述多个特征点的信息以及所述惯性数据输入滤波器中，进行滤波器预测和更新，得到当前位姿；根据跟踪帧数是否小于预设帧数，将所述多个特征点划分为两个类别，以及基于所述当前图像的多个预设类型的信息，确定所述当前图像是否为关键帧；若所述当前图像不属于关键帧，则输出所述当前位姿；若所述当前图像属于关键帧，则输出所述当前位姿、所述关键帧的信息以及两类所述特征点的信息。3.根据权利2所述的方法，其特征在于，所述从所述关键帧队列的各个关键帧上的特征点中筛选出多个地图点，包括：对于所述关键帧队列的任一关键帧上的任一特征点，若所述任一特征点属于第一类别，且至少在所述关键帧队列中的两帧关键帧上，或所述任一特征点属于第二类别，且具有新的共视关系，则将所述任一特征点确定为地图点；其中，所述第一类别的特征点指代跟踪帧数不小于预设帧数的特征点，所述第二列表指代跟踪帧数小于预设帧数的特征点。4.根据权利要求1所述的方法，其特征在于，所述将优化后的所述相关关键帧以及所述回环地图点更新至所述地图库，以及反馈给所述滤波器进行状态更新，包括：将优化后的所述相关关键帧以及所述回环地图点更新至所述地图库中；
判断所述滤波器的滑动窗口中是否存在所述相关关键帧以及所述回环地图点；若判断出所述滤波器的滑动窗口中存在所述相关关键帧或所述回环地图点，则将所述滤波器中存在的所述相关关键帧以及所述回环地图点，相应地更新为优化后的所述相关关键帧或优化后的所述回环地图点；若判断出所述滤波器的滑动窗口中不存在所述相关关键帧或所述回环地图点，则将当前的惯性数据更新至所述滤波器中。5.一种融合msckf和图优化的视觉slam定位装置，其特征在于，包括：采集单元，用于实时获取当前的图像以及惯性数据；跟踪单元，用于每获取到一帧当前图像，则基于当前的所述惯性数据，跟踪得到所述当前图像上的多个特征点；定位单元，用于将所述多个特征点的信息以及所述惯性数据输入滤波器中，进行滤波器预测和更新，并输出当前位姿、关键帧的信息以及所述多个特征点的信息；其中，所述关键帧的信息在所述当前图像被确定为关键帧时输出；提取单元，用于从关键帧队列中提取出目标关键帧；其中，所述关键帧队列包括所有待处理的关键帧；所述目标关键帧为所述关键帧队列中获取时间距离当前最长的关键帧；还原单元，用于将所述关键帧队列中的多帧关键帧中与所述目标关键帧共视的特征点，还原到所述关键帧列表中获取时间最新的关键帧中；筛选单元，用于从所述关键帧队列的各个关键帧上的特征点中筛选出多个地图点；回环检测单元，用于针对目标关键帧进行回环检测；第一更新单元，用于在回环检测失败时，将所述多个地图点、所述目标关键帧及其共视关系，更新到地图库中；优化单元，用于在回环检测成功时，将相关关键帧的信息输入全局光束平差法模型中，通过全局光束平差法模型对所述相关关键帧以及回环地图点进行优化；其中，所述相关关键帧包括回环检测到的回环帧以及所述目标关键帧；所述回环地图点为回环检测到的所述地图点；第二更新单元，用于将优化后的所述相关关键帧以及所述回环地图点更新至所述地图库，以及反馈给所述滤波器进行状态更新。6.根据权利要求5所述的装置，其特征在于，定位单元，包括：状态更新单元，用于将所述多个特征点的信息以及所述惯性数据输入滤波器中，进行滤波器预测和更新，得到当前位姿；匹配单元，用于根据跟踪帧数是否小于预设帧数，将所述多个特征点划分为两个类别，以及基于所述当前图像的多个预设类型的信息，确定所述当前图像是否为关键帧；第一输出单元，用于在所述当前图像不属于关键帧时，输出所述当前位姿；第二输出单元，用于在所述当前图像属于关键帧时，输出所述当前位姿、所述关键帧的信息以及两类所述特征点的信息。7.根据权利6所述的装置，其特征在于，所述筛选单元，包括：筛选子单元，用于对于所述关键帧队列的任一关键帧上的任一特征点，若所述任一特征点属于第一类别，且至少在所述关键帧队列中的两帧关键帧上，或所述任一特征点属于第二类别，且具有新的共视关系，则将所述任一特征点确定为地图点；其中，所述第一类别
的特征点指代跟踪帧数不小于预设帧数的特征点，所述第二列表指代跟踪帧数小于预设帧数的特征点。8.根据权利要求5所述的装置，其特征在于，所述第二更新单元，包括：地图更新单元，用于将优化后的所述相关关键帧以及所述回环地图点更新至所述地图库中；判断单元，用于判断所述滤波器的滑动窗口中是否存在所述相关关键帧以及所述回环地图点；第一前端更新单元，用于在所述判断单元判断出所述滤波器的滑动窗口中存在所述相关关键帧或所述回环地图点时，将所述滤波器中存在的所述相关关键帧以及所述回环地图点，相应地更新为优化后的所述相关关键帧或优化后的所述回环地图点；第二前端更新单元，用于在所述判断单元判断出所述滤波器的滑动窗口中不存在所述相关关键帧或所述回环地图点时，将当前的惯性数据更新至所述滤波器中。

技术总结
本申请公开了一种融合MSCKF和图优化的视觉SLAM定位方法及装置，所述方法主要包括：实时获取当前的图像以及惯性数据，并基于当前的惯性数据，跟踪得当前图像上的多个特征点，然后将多个特征点的信息以及惯性数据输入滤波器中，进行滤波器预测和更新，并输出当前位姿、关键帧的信息以及所述多个特征点的信息。同时从关键帧队列中提取出目标关键帧，并对该关键帧进行处理，然后针对目标关键帧进行回环检测；若回环检测失败，则将多个地图点、目标关键帧及其共视关系，更新到地图库中；若回环检测成功，则利用全局光束平差法，对关键帧位姿和地图点进行全局优化，将优化结果反馈给MSCKF进行状态融合，更新滑窗中关键帧的位姿和地图点。点。点。


技术研发人员：高洪臣 李骊
受保护的技术使用者：北京华捷艾米科技有限公司
技术研发日：2021.04.23
技术公布日：2021/10/19