基于二次曲面初始化和联合数据关联的物体感知SLAM算法

基于二次曲面初始化和联合数据关联的物体感知slam算法
技术领域
1.本发明涉及物体感知算法领域，具体涉及一种基于二次曲面初始化和联合数据关联的物体感知slam算法。


背景技术：

2.随着移动机器人和无人驾驶的应用越来越广泛，语义slam(同时定位和建图)在室内移动机器人上越来越成为一种重要的技术，因为它可以帮助机器人充分地感知周围环境并和环境进行交互，为机器人在室内导航，交互提供先验的几何和语义信息。物体级slam是语义slam的一种方案，主要是将场景中的物体建模为立方体和二次曲面，由于二次曲面有着更紧凑的数学表达，同时二次曲面对物体有着更好的包络表达，所以出现了很多优秀的二次曲面级的物体slam算法。
3.文献《ieee/rsj international conference on robotics and automation(icra),7123-7129,2019》提出构建包含平面和二次曲面的几何语义地图，首次提出利用平面和二次曲面之间的相切关系来优化二次曲面参数。文献《ieee/rsjrobotics and automation,4712-4719,2021》提出一种相机向前运动时的二次曲面快速初始化算法，首次提出引入额外点云约束来降低了小视角观测时的二次曲面初始化难度。
4.文献《ieee/rsj international conference on robotics and automation(icra),7123-7129,2019》中二次曲面初始化方法是基于目标检测的，这种方法需要多帧大视角的观测数据才可以完成物体二次曲面参数的初始化，但是并不是所有物体都可以有多个视角观测的，所以导致二次曲面初始化的成功率特别低。而文献《ieee/rsjrobotics and automation,4712-4719,2021》虽然引入额外约束降低了二次曲面初始化的难度，但是没有对二次曲面做数据关联来进行进一步的优化工作，因此构建的物体的二次曲面参数精度并不高。


技术实现要素：

5.本发明的目的就是提出一种基于二次曲面初始化和联合数据关联的物体感知slam算法，在提高物体建模和相机定位精度的同时，构建环境的几何语义地图。
6.本发明的技术方案：一种基于高效二次曲面初始化和联合数据关联的物体感知slam算法，首先提取图像中的特征点、平面和物体信息；然后根据特征点和平面信息计算相机位姿；然后对图像的物体信息和地图中的物体信息进行联合数据关联，进而完成物体二次曲面参数的高效初始化；然后联合优化相机位姿和地图中构建的物体二次曲面，最终构建出完整的几何语义地图；
7.包括步骤如下：
8.步骤1：输入rgb图像和深度图像；
9.步骤2：特征提取；
10.从rgb图像上提取特征点信息和语义物体信息；从深度图像上提取几何平面信息；
语义物体信息包括物体目标检测结果和物体表面稀疏特征点；
11.步骤3：位姿估计；
12.将当前帧数据提取到的特征点信息和几何平面信息与包含点云、平面和二次曲面的地图中已经构建的数据进行联合数据关联，使用关联后的信息对相机位姿进行基于3d信息到2d信息的最小重投影误差的求解；
13.步骤4：高效二次曲面初始化；
14.从步骤2中得到的物体目标检测结果和物体表面稀疏特征点出发构建物体表面的切平面，即物体的包络曲面；
15.每个包络平面为物体二次曲面参数求解提供一个线性约束，基于得到的所有包络平面，构建出对应的线性方程组：
16.a(π)q
*
＝0 (1)
17.其中a(π)∈r
nobs
×
10
表示平面包络构成的系数矩阵，nobs是构建的包络平面个数，q
*
∈r
10
×1表示待求解的二次曲面参数；求解问题转换为线性最小二乘问题；
[0018][0019]
步骤5：基于g2o技术的多约束联合优化因子图；结合几何平面信息和语义物体信息，利用地图中构建的点云、平面和二次曲面在图像平面上投影的特征点、区域和二次曲线的误差对相机位姿以及构建的物体二次曲面进行基于g2o非线性优化技术的联合优化；
[0020]
步骤6：算法输出；输出包含平面和二次曲面的几何语义地图。
[0021]
从几何和概率的双角度出发，采用四种关联距离进行鲁棒关联当前帧检测的物体和地图已经构建的物体；
[0022]
a.图像2d-iou关联距离
[0023]
2d图像平面的iou是做数据关联常用的方法，因为在slam这种相邻帧之间没有太大视角变化的条件下，使用2d的iou可以很容易且有效地关联两帧之间的目标检测框，从而根据上一帧所关联的二次曲面得到当前帧目标检测与二次曲面的关联结果；记为第i个检测框和第j个二次曲面的2d图像平面iou关联距离检测框和第j个二次曲面的2d图像平面iou关联距离表示当前帧第i个检测框的边界框，为第j个二次曲面对应的上一帧中的检测框的边界框；
[0024][0025]
b.二次曲面投影iou关联距离
[0026]
仅仅依靠2d图像平面的iou是远远不够的，2d图像平面的iou方法受目标检测漏检测的影响还是很大的。因此，将地图中已经构建的二次曲面投影到当前帧，得到投影的检测框，然后和当前帧的检测框计算iou；记为第i个检测框和第j个二次曲面的投影iou关联距离距离表示当前帧第i个检测框的边界框，为第j个二次曲面投影到当前帧的检测框的边界框；
[0027][0028]
c.地图点投影频数关联距离
[0029]
使用投影iou方法的前提是物体已经初始化为二次曲面，但是在系统运行的开始阶段，物体需要足够的数据来进行合适的初始化，所以一开始是没有物体的二次曲面参数使用的。因此，针对这种情况，虽然没有物体的二次曲面参数可以使用，但是可以使用物体所关联的地图点，投影不同的物体所关联的地图点到当前帧的图像平面上，计算和当前帧每一个检测框所包含的物体地图点频数；记为第i个检测框和第j个二次曲面的投影地图点频数关联距离num
ij
表示当前帧第i个检测框所包含的第j个二次曲面关联的地图点投影的频数；
[0030][0031]
d.非参数概率关联距离
[0032]
上述设计的关联距离在设计过程中仅仅考虑了几何层面的因素，而且只计算同类别的目标检测和物体之间的关联距离，但是目标检测是存在误检测的情况，很可能由于误检测导致有些物体的关联中断，不利于物体地图的不重复性和一致性。因此，从概率分布的角度出发，得出目标检测结果和地图中路标的数据关联的后验概率正比于狄利克雷过程先验概率、目标检测类别测量似然概率以及路标测量似然概率的乘积；
[0033]
记为第i个检测框和第j个二次曲面的非参数概率关联距离dp_prior(oj)表示属于第j个二次曲面的狄利克雷过程先验概率，label_likelihood(di)表示第i个检测框的类别似然概率，pos_likelihood(di,oj)表示第i个检测框和第j个二次曲面的位置似然概率，则
[0034][0035]
基于以上四种关联距离，设定当前帧存在m个目标检测框，地图中存在n个二次曲面，构建矩阵am×n＝[a
ij
]m×n来表示当前帧目标检测框与地图中二次曲面的数据关联距离矩阵，其中a
ij
表示第i个检测框和第j个物体的关联距离(0≤a
ij
≤1)，采用上述不同层面的关联距离加权求和来计算矩阵a的值；
[0036][0037]
其中，kq kw ke kr＝1，是各项关联距离的权重值，在我们的系统中取值为kq＝0.2，kw＝0.2，ke＝0.3，kr＝0.3。
[0038]
本发明的有益效果：本发明提出的一种基于二次曲面初始化和联合数据关联的物体感知slam算法，解决了以往二次曲面级slam算法中存在的二次曲面初始化困难、数据关联不鲁棒等问题；提高了物体建模精度和相机定位精度。
附图说明
[0039]
图1为基于二次曲面初始化和联合数据关联的物体感知slam算法的流程图。
具体实施方式
[0040]
图1是本发明技术方案的主流程图。如图1所示，本发明提出了一种基于二次曲面初始化和联合数据关联的物体感知slam算法包括下列步骤：
[0041]
1)系统输入：本发明以rgb图像和深度图像为输入；
[0042]
2)特征提取：本发明首先从rgb图像上提取特征点信息和语义物体信息，然后从深度图上提取几何平面信息；
[0043]
3)位姿估计：本发明将当前帧数据提取到的特征点信息和几何平面信息和地图中已经构建的数据进行数据关联，使用关联后的信息进行相机位姿的求解；
[0044]
本发明从几何和概率的角度出发，提出了四种关联距离来鲁棒的关联当前帧检测的物体和地图已经构建的物体：
[0045]
a.图像2d-iou关联距离
[0046]
2d图像平面的iou是做数据关联常用的方法，因为在slam这种相邻帧之间没有太大视角变化的条件下，使用2d的iou可以很容易且有效地关联两帧之间的目标检测框，从而根据上一帧所关联的二次曲面得到当前帧目标检测与二次曲面的关联结果。我们记为第i个检测框和第j个二次曲面的2d图像平面iou关联距离i个检测框和第j个二次曲面的2d图像平面iou关联距离表示当前帧第i个检测框的bounding box，为第j个二次曲面对应的上一帧中的检测框的bounding box，则：
[0047][0048]
b.二次曲面投影iou关联距离
[0049]
仅仅依靠2d图像平面的iou是远远不够的，2d图像平面的iou方法受目标检测漏检测的影响还是很大的。因此，我们根据将地图中已经构建的二次曲面投影到当前帧，得到投影的检测框，然后和当前帧的检测框计算iou。我们记为第i个检测框和第j个二次曲面的投影iou关联距离投影iou关联距离表示当前帧第i个检测框的bounding box，为第j个二次曲面投影到当前帧的检测框的bounding box，则
[0050][0051]
c.地图点投影频数关联距离
[0052]
使用投影iou方法的前提是物体已经初始化为二次曲面，但是在系统运行的开始阶段，物体需要足够的数据来进行合适的初始化，所以一开始是没有物体的二次曲面参数使用的。因此，针对这种情况，虽然没有物体的二次曲面参数可以使用，但是可以使用物体所关联的地图点，我们投影这些不同的物体所关联的地图点到当前帧的图像平面上，然后
计算和当前帧每一个检测框所包含的物体地图点频数。我们记为第i个检测框和第j个二次曲面的投影地图点频数关联距离num
ij
表示当前帧第i个检测框所包含的第j个二次曲面关联的地图点投影的频数，则
[0053][0054]
d.非参数概率关联距离
[0055]
上述设计的关联距离在设计过程中仅仅考虑了几何层面的因素，而且只计算同类别的目标检测和物体之间的关联距离，但是目标检测是存在误检测的情况，很可能由于误检测导致有些物体的关联中断，不利于物体地图的不重复性和一致性。因此，我们从概率分布的角度出发，可以得出目标检测结果和地图中路标的数据关联的后验概率正比于狄利克雷过程先验概率、目标检测类别测量似然概率以及路标测量似然概率的乘积。
[0056]
参考这个思路，我们记为第i个检测框和第j个二次曲面的非参数概率关联距离dp_prior(oj)表示属于第j二次曲面的狄利克雷过程先验概率，label_likelihood(di)表示第i个检测框的类别似然概率，pos_likelihood(di,oj)表示第i个检测框和第j个二次曲面的位置似然概率，则
[0057][0058]
基于以上设计的四种关联距离，我们假设当前帧存在m个目标检测框，地图中存在n个二次曲面，可以构建矩阵am×n＝[a
ij
]m×n来表示当前帧目标检测框与地图中二次曲面的数据关联距离矩阵，其中a
ij
表示第i个检测框和第j个物体的关联距离(0≤a
ij
≤1)，然后采用上述不同层面的关联距离加权求和来计算矩阵a的值。
[0059][0060]
4)高效二次曲面初始化：本发明从物体目标检测结果和物体表面稀疏特征点出发构建物体表面的切平面，即物体的包络平面；
[0061]
每个包络平面为物体二次曲面参数求解提供一个线性约束，基于得到的所有包络平面，构建出对应的线性方程组；
[0062]
a(π)q
*
＝0
[0063]
其中a(π)∈r
nobs
×
10
表示包络平面构成的系数矩阵，nobs是构建的包络平面个数，q
*
∈r
10
×1表示待求解对偶二次曲面参数。一般来说，nobs是要远大于对偶二次曲面的参数的，所以式是超定方程组，是无解的，但是存在最小二乘解，因此求解式的问题很容易转换为一个线性最小二乘问题：
[0064][0065]
5)多约束联合优化因子图：本发明结合几何平面信息和语义物体信息，提出一种多约束的优化因子图，利用地图中构建的点云、平面和二次曲面在图像平面上投影的特征点、区域和二次曲线的误差对相机位姿以及构建的物体二次曲面进行联合优化；
[0066]
6)算法输出：本发明以包含平面和二次曲面的几何语义地图为输出；
[0067]
为了验证算法在提升二次曲面建模精度和相机定位精度方面的有效性，本发明在室内tum数据集的8个rgbd序列上进行了测试。根据实验结果，本发明提出的系统在所有序列上平均的建模完整率为77％,初始化物体的二次曲面参数所需要的平均帧数为2.99，物体的二次曲面参数的平均建模精度为0.78，二次曲面平均初始化成功率为76％，相机的定位精度的误差在1cm左右。