一种分步边缘化的点线视觉惯性SLAM方法及系统

本发明公开一种分步边缘化的点线视觉惯性SLAM方法及系统。首先，获取环境图像和IMU数据并进行预处理；提取图像中的点线特征，保留图像金字塔各层中长度大于最小线段长度阈值的线特征，计算任意两个被保留线特征的方向夹角和中心点欧式距离；若两个线特征的方向夹角小于等于角度阈值且中心点欧式距离小于等于距离阈值，则将两个线特征合并为一个新线特征；接着，对视觉惯性坐标系进行对齐，并估计初始状态；最后，使用基于分步边缘化策略的滑动窗口进行局部视觉惯性优化及建图，得到先验误差；同时，进行回环检测，将回环误差和先验误差加入到下一轮滑动窗口优化中进行紧耦合状态估计。该方法具有较高的鲁棒性与一致性，同时保证了实时性。