[发明专利]基于自适应鲁棒核的单目视觉惯性SLAM方法与系统

权利要求书

1.一种基于自适应鲁棒核的单目视觉惯性SLAM方法，其特征在于，包括：

在单目初始化时对当前帧与参考帧进行初始特征匹配，并行计算单应矩阵和基础矩阵两个运动模型，并通过几何鲁棒信息含量选择单应矩阵或者基本矩阵，进行模型分解以及光束法平差BA优化；

在局部建图线程构建最小化重投影误差的成本函数，采用自适应鲁棒核应用于BA优化；

在回环检测线程利用自适应鲁棒核减少回环校正过程中的累积误差。

2.根据权利要求1所述的基于自适应鲁棒核的单目视觉惯性SLAM方法，其特征在于，通过几何鲁棒信息含量的模型选择，表示为：

其中，G为几何鲁棒信息含量，k表示关键点序列号，L为稳健函数，e表示重投影误差，用于限制重投影误差的大小，β表示模型的大小，其中β＝2为单应矩阵，β＝3为基本矩阵，n为两个关键帧之间跟踪的地图点数量，表示模型中的自由度数，其中表示基本矩阵，表示单应矩阵，θ²表示关键点定位误差的协方差，λ表示数据的维数，两帧之间的二维对应λ＝4。

3.根据权利要求1所述的基于自适应鲁棒核的单目视觉惯性SLAM方法，其特征在于，自适应鲁棒核的核函数ρ表示为：

其中，μ,ω,c为参数，常数c0用于约束μ＝0附近二次损失区域规模。

4.根据权利要求3所述的基于自适应鲁棒核的单目视觉惯性SLAM方法，其特征在于，在局部建图线程，最小化重投影误差的成本函数，表示为：

其中，R为旋转矩阵，t为平移向量，为所有匹配特征点的集合，ρ为鲁棒核函数，为投影坐标，X^α为投影点在世界坐标系中的坐标，表示重投影误差的二范数平方和。

5.根据权利要求3所述的基于自适应鲁棒核的单目视觉惯性SLAM方法，其特征在于，在回环检测线程，自适应鲁棒核函数应用于Sim(3)求解，表示为：

其中，S₁₂为变换矩阵，p_1,i为关键帧1中的关键点i，p_1,j为关键帧2中与i匹配的关键点j，P_1,i为关键点i在世界坐标系的3D坐标，P_2,j为与关键点i匹配的关键点j在世界坐标系的3D坐标，N为匹配的关键点数，π_i(i＝1,2)表示投影函数。

6.一种基于自适应鲁棒核的单目视觉惯性SLAM系统，其特征在于，包括：

单目初始化模块，用于对当前帧与参考帧进行初始特征匹配，并行计算单应矩阵和基础矩阵两个运动模型，并通过几何鲁棒信息含量选择单应矩阵H或者基本矩阵F，进行模型分解以及光束法平差BA优化；

局部建图模块，用于构建最小化重投影误差的成本函数，采用自适应鲁棒核应用于BA优化；

以及，回环检测模块，用于利用自适应鲁棒核减少回环校正过程中的累积误差。

7.根据权利要求6所述的基于自适应鲁棒核的单目视觉惯性SLAM系统，其特征在于，所述单目初始化模块中，通过几何鲁棒信息含量的模型选择，表示为：

其中，G为几何鲁棒信息含量，k表示关键点序列号，L为稳健函数，e表示重投影误差，用于限制重投影误差的大小，β表示模型的大小，其中β＝2为单应矩阵，β＝3为基本矩阵，n为两个关键帧之间跟踪的地图点数量，表示模型中的自由度数，其中表示基本矩阵，表示单应矩阵，θ²表示关键点定位误差的协方差，λ表示数据的维数，两帧之间的二维对应λ＝4。

8.根据权利要求6所述的基于自适应鲁棒核的单目视觉惯性SLAM系统，其特征在于，自适应鲁棒核的核函数ρ表示为：

其中，μ,ω,c为参数，常数c0用于约束μ＝0附近二次损失区域规模。