点击下方卡片，关注「3D视觉工坊」公众号
选择星标，干货第一时间送达
来源：3D视觉工坊
星球内有20多门3D视觉系统课程、3DGS独家系列视频教程、顶会论文最新解读、海量3D视觉行业源码、项目承接、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎加入！
摘要：针对动态结构化环境下视觉与IMU融合时，低纹理造成匹配精度下降、动态物体干扰造成特征误匹配等影响定位精度的问题，提出一种面向动态结构化空间的视觉-IMU融合定位方法。利用IMU信息预积分预测特征位置，通过设置点特征掩码、合并线特征，并结合光流法跟踪，构建点线特征匹配算法，显著提高了特征匹配的成功率并降低了耗时。利用边缘约束的深度补全算法生成稠密深度掩码，结合漏检补偿机制消除动态物体误匹配的影响，以实现动态特征剔除。实验结果表明：在结构化低纹理场景下，特征匹配成功率提升12.7% ，动态特征误匹配率降低68.3%，该方法可应用于电力系统等领域。
1 测量模型
基于传感器物理特性建立精确的观测模型，主要包括RGB-D相机模型和IMU运动学模型，构建多传感器的坐标转换关系，实现观测数据空间对齐。
1.1 RGB-D相机模型
RGB-D摄像头可以快速稳定地获得深度图，结构光法通过发射红外光到物体表面，然后通过接收器采集被测物体的结构光图像，将数据发送到计算单元，解算后获取位置和深度信息，从而实现3D重建。
RGB-D相机模型如图1所示。
1.2 IMU测量模型
IMU可以测得无人设备的加速度和角速度，然而IMU测量值会受到各种噪声影响，容易发散。为方便建立测量模型，假设陀螺仪噪声 和加速度计噪声 为高斯噪声：
同理可得，陀螺仪和加速度计的偏置参数 和 为：
因此，IMU数学模型建模为：
式中： ， 为角速度和加速度测量值； ， 为角速度和加速度真值； 为世界坐标系到载体坐标系的旋转矩阵； 为世界坐标系下的重力加速度。
1.3 坐标系变换
将无人设备建模为刚体并将其运动视为刚体运动，主要涉及的坐标系为世界坐标系、IMU坐标系、相机坐标系、图像坐标系及像素坐标系，通过欧拉角、旋转矩阵、四元数等实现坐标系转换，统一多传感器数据。
（1）欧拉角
利用绕三个相互垂直轴的旋转表示主体旋转，具体分解如下：横滚角 为刚体绕 轴旋转，俯仰角 为刚体绕 轴旋转，偏航角 为刚体绕 轴旋转。
（2）旋转矩阵
在已知旋转变换欧拉角的情况下，旋转矩阵可由下式计算：
且需要满足：
（3）四元数
设无人设备绕 旋转了 ，四元数表示为：
其对应的旋转矩阵 为：
除旋转外，平移变换可通过向量 表示，平移加旋转则可表示整个变换过程：
齐次坐标表示为：
变换矩阵 表示载体坐标系到世界坐标系的转换关系。
2 方法
本文提出一种视觉SLAM特征提取优化方法，通过引入高实时性、高精度的点线特征提取算法，并在特征匹配阶段使用IMU数据预测特征运动，从而提供更准确的特征初始位置估计并提高匹配准确率。
2.1 IMU辅助的特征提取
首先，采用Shi-Tomasi算法进行点特征提取，并结合IMU辅助的KLT光流法实现特征匹配，该方法可以在保证光流法实时性的同时，减少光流金字塔层数，从而显著提升特征匹配的精度和鲁棒性。然后，通过锚点主动分组的线特征提取算法Ag3line，结合IMU数据提供更好的特征初始位置估计，从而更加准确地跟踪线特征，有效剔除不稳定的特征，增强线特征匹配的鲁棒性并提高线特征跟踪的效率。
2.1.1 点特征提取与匹配
为计算两个图像在时刻 和 之间的相对姿态变化，首先将IMU缓冲区的数据存储在队列中，接着将队列中时间戳小于等于 的IMU数据移除，直到找到大于 的数据为止，再找到首个大于 的数据，从而保证处理的IMU数据处于 和 之间。
IMU与视觉传感器对齐如图2所示。
通过对 和 间的IMU信息进行积分，获得IMU坐标系下两帧之间的变换矩阵 ，利用IMU坐标系和相机坐标系之间的外参矩阵 得到相机坐标系下两帧之间的变换矩阵 ：
接着，进行点特征预测，具体流程如下：
（1）上一帧点特征的坐标 除以 得到其在归一化平面上的坐标 ，简写为 。
（2）其次，计算归一化平面上点特征的径向与切向畸变：
式中： 为畸变参数。
（3）将IMU积分得到的旋转量及位移量，与去畸变的归一化坐标相乘得到点特征在下一帧的预测值 。
（4）最后，将畸变后的点通过内参矩阵投影到图像平面上，得到精确的点特征投影坐标 ：
特征检测如图3所示。
在上一帧中可以稳定跟踪的点特征用红色表示，新检测到的点特征用蓝色表示，不稳定点特征用紫色表示。当新一帧到达时，使用当前帧和前一帧之间的IMU测量值预测前一帧点特征在当前帧的像素位置，并用绿色表示，光流法使用预测的特征位置作为初始位置，在当前中帧寻找匹配特征，成功跟踪的特征被标记为红色，未被跟踪的特征被标记为紫色。
在点特征提取以及光流跟踪过程中，点特征聚集会导致大量的误匹配，通过设置点特征掩码以避免上述情况。掩码设置如图4所示，设置以稳定点特征为中心的黄色圆形掩码，以及以不稳定点特征为中心的紫红色区域掩码，掩码范围内不再提取新点特征。上述方法可以获得均匀分布的点特征，避免在室内结构化空间中低纹理的区域重复提取不稳定的特征。
2.1.2 线特征提取与匹配
本文采用Agline算法提取线特征，通过主轴分组减少断裂和误检，结合IMU信息与光流法实现线特征匹配，提升长线特征的跟踪稳定性。
（1）线特征提取
首先，对图像进行灰度化，经高斯滤波抑制噪声后输出平滑图像，计算图像梯度：
式中： 为像素值； 为梯度幅值； 为水平线角度。
然后，采用非极大值抑制选择梯度较大的像素点，设定锚点阈值来选择高质量锚点，以提高边缘检测质量，验证通过非极大值抑制获得的锚点，将邻域范围内梯度方向差值小于阈值的像素点定为有效锚点。
最后，经主轴拟合形成线特征，具体步骤为：
首先，定义主轴的方向为 ，已分组锚点的坐标定义为 ，计算候选锚点：
其次，验证候选对象。候选锚点与已分组锚点之间的角度应小于 。
最后，更新主轴，重复上述步骤。
（2）IMU辅助KLT光流法
对于连续两帧图像 ，线特征提取数量分别为 ，线特征分别表示为 ， ， 为线特征起止点。对于前一帧 的线特征，通过 计算得到方向变量。沿线特征方向变量等间距提取三个点，连同起止点在内的5个点构成该线特征的“点特征集”，表示为 。通过IMU提供两帧之间位姿变换信息预测点集 在下一帧中的预测点集 。使用光流法判断预测点集 中点特征对应状态是否不为0且误差是否小于阈值error。对于通过验证的点集，通过最短距离寻找最近的线特征。当最短距离小于阈值时，构建预测线 ，通过距离、角度阈值排除错误匹配的线特征。
2.2 深度图与目标检测算法的动态特征检测
本文提出一种基于深度图与目标检测融合的方法，通过分割动态对象规避动态特征。首先通过边缘深度信息补全深度图，之后联合目标检测与深度图像，估算动态物体深度以分割动态对象，保留静态点线特征。
2.2.1 稀疏深度图补全
深度图空缺处的深度信息往往与其相邻的深度信息有关，通过边缘深度信息补全深度图像，在高帧率且不对硬件额外限制的情况下，得出较好的深度信息。由于有效深度附近的空洞可能共享相似的深度值，因此本文方法按从小到大的顺序进行空洞填充。这允许每个有效深度的影响区域缓慢增加，同时仍保留局部结构。最后的模糊步骤用于降低输出噪声和平滑局部平面。对于稀疏图像的处理机制是应用较大的深度值覆盖近距离较小的深度值。在深度图补全中首先填充最接近有效深度值的空值，在这一步采用的是 菱形内核，使得最可能具有相同深度值的空洞被填补。
菱形内核及空洞填补如图5所示。
然后，利用 矩形内核进行闭运算，连接相邻的深度值并填补小孔，扩散相邻深度连接以形成物体的边缘深度。 矩形内核及小孔填补如图6所示。
最后利用 矩形内核来进行膨胀操作，将仍为空值的深度值填充起来，其他位置有效深度值不变。
2.2.2 YOLO与深度图的动态检测
本文通过深度融合目标检测信息、深度信息以及点线特征，在保证运行速度的情况下，有效剔除动态点线特征，具体步骤为：
首先，获取YOLO检测框的4个角点深度 ，代表物体的背景深度，取得YOLO检测框中心深度 ，代表动态物体深度，计算得到 ：
距离动态物体深度差距较大， 差距较小，根据相关特征计算深度范围：
式中： 代表动态物体近处与远处深度值，深度位于两者之间的特征被认为是动态特征， 为设定阈值。
3 实验
本文算法利用EuRoC、OpenLORIS数据集进行验证，并与算法VINS-Mono、PL-VIO、UV-SLAM进行对比，证明本文算法在定位精度以及鲁棒性方面的优越性。
3.1 实验环境
EuRoC数据集包含多个场景下的视觉和惯性数据，由视觉传感器提供 的双目图像，IMU传感器提供 的IMU测量数据。
OpenLORIS数据集利用小车对办公室、咖啡厅、市场等5个场景采集了22个序列，数据集包括D435i摄像机提供的 RGB图像、深度图像，以及IMU测量数据。
本实验在RYZEN7-5800H处理器、16GB运行内存的计算机上进行。
3.2 实验结果与分析
3.2.1 点特征匹配
本文将改进KLT光流法与KLT光流法进行仿真对比，选取OpenLORIS数据集进行实验。
（1）首先，验证添加掩码均分点特征的算法，掩码设置效果如图7所示，可以看出与原算法相比，改进算法能够获得更加均匀的特征表示。
（2）然后，选取有代表性的三类场景以验证IMU辅助光流匹配算法的有效性，如图8~10所示，原算法如左图所示，改进算法如右图所示。
如图8所示，在正常光照丰富特征匀速移动的场景中，可以提取并稳定跟踪大量的特征，只有极少部分特征属于不稳定特征。在图9的正常光照丰富特征快速移动室内场景中，无人设备在快速向左旋转，由IMU数据提供的绿色预测点特征，与实际提取特征点的距离从右向左逐渐增加，符合现实情况，也证明了本文算法在快速旋转运动下的鲁棒性。图10展示了低光照低纹理匀速移动室内场景，IMU提供的信息仍然可以有效辅助跟踪。通过三个场景验证，加入IMU数据进行预测并且设置掩码后，光流法的跟踪具有更好的实时性和鲁棒性。
3.2.2 线特征匹配
本文使用EuRoC数据集评估线特征匹配性能。选取其中具有代表性的5个场景，并与PL-VINS中线特征匹配算法进行对比，匹配率结果如表1所示。
可以看出，本文方法在所有场景下都取得了更高的匹配成功率，相较于LBD描述子算法，其特征匹配成功率平均提升了 。为公平验证匹配算法的实时性，实验统一使用Ag3line作为线特征提取算法，其结果如表2所示。左侧采用现有描述子匹配算法，右侧则使用本文的光流+IMU匹配算法。
选取同一场景稳定运行时的连续6帧数据进行比对，可见本文算法虽然在复杂场景耗时有所上升，然而多数场景仍维持在 左右，而现有算法耗时呈现明显上升趋势。可见，本文算法相比现有算法在实时性方面有较大改进。
3.2.3 动态特征检测
采用OpenLORIS动态数据集验证算法性能，通过快速提取动态对象深度掩码，分离出动态特征以避免其对定位精度的影响。
如图11所示，在只含有单个动态物体的场景中，可以精确地分割出动态物体。虽然附近墙壁因深度值处于动态物体深度范围内而被错误分类为动态物体，但占比较小。
如图12所示，在富含动态物体的复杂场景中，本文算法仍然可以对多个动态物体进行分割。RGB图像中的多个人物姿势及距离关系都可以在右图反映出来，体现了该算法面向复杂场景的优越性。
图13展示了利用深度掩码排除动态特征。绿色方框为YOLO检测框，红点为被剔除的动态点，蓝线为初始提取到的线特征，黄线是被保留下的线特征。图13a为原始线特征提取图，图13b为排除动态特征后的图像。可以看到，在第一列图像中位于检测框内的部分背景线特征被正确保留，第二列图像可以在剔除动态特征时保留静态特征，最后一列则显示中近距离背景线特征被保留而动态线特征被剔除，通过计算，动态特征被剔除后的误匹配率降低了 ，显著提高了特征匹配的成功率，证明了本文算法的有效性以及面向复杂场景的鲁棒性。
4 结束语
本文提出一种面向动态结构化空间的IMU辅助视觉SLAM方法，通过系统性地解决低纹理环境特征匹配与动态物体干扰两大关键问题，显著提升了系统的鲁棒性与精度。该方法首先利用IMU预积分信息为点线特征匹配提供运动先验信息，结合光流法跟踪，在结构化低纹理场景下将特征匹配成功率提升了12.7% ；通过融合边缘约束的深度补全与目标检测，并引入漏检补偿机制，有效剔除了动态物体上的误匹配特征，使动态特征误匹配率降低了68.3% 。实验结果表明，本文方法有效克服了传统方法的缺陷，为电力巡检等实际应用场景下的高精度、高可靠定位提供了可行的技术方案。
本文仅做学术分享，如有侵权，请联系删文。
。
添加微信：cv3d001，备注：姓名+方向+单位，邀请入群。
