CN112634305A - 一种基于边缘特征匹配的红外视觉里程计实现方法 - Google Patents

Abstract

本发明公开了一种基于边缘特征匹配的红外视觉里程计实现方法，涉及机器视觉定位领域。该方案结合了传统特征点法和直接法的优势，相比直接采用传统方案，本方法都具有一定的优势，该发明通过对红外图像进行边缘提取，通过在连续红外图像帧间进行边缘配准，使用ICP匹配算法求解相机帧间运动估计，克服了机器人在光照动态变化、夜间、弱光照无光照、雾天等复杂环境中的定位问题。

Description

一种基于边缘特征匹配的红外视觉里程计实现方法

技术领域

本发明涉及机器视觉定位领域，具体涉及一种基于边缘特征匹配的红外视觉里程计实现方法。

背景技术

同步定位与地图构建(SimultaneousLocalizationandMapping)技术对于户外机器人及无人驾驶有着重要意义，其也对SLAM在复杂环境中的定位性能提出更高的要求。

在光照高动态变化、夜间黑暗环境、雾天等复杂环境中，现有基于可见光视觉的定位方案在该环境中容易发生传感器退化，无法提供准确的定位信息。

近年来，由于红外相机对可见光不敏感，在夜间、雾天环境中有着良好的成像，但红外相机的高温漂、弱纹理、低对比度等特性限制了红外相机在SLAM领域的进一步应用，现有红外SLAM采用传统基于可见光视觉的定位方案，但由于相机特性的不同，使得传统方案无法良好应用在红外相机上。

现有技术中，公布号为CN106846417A、名称为基于视觉里程计的单目红外视频三维重建方法的中国专利申请公开了一种直接法和稀疏法视觉里程计模型，通过红外图像帧间的像素匹配获得光度误差，借助高斯牛顿法进行迭代最小化光度误差从而求解红外热像仪的位姿，从而获得里程计信息。

该方案基于传统直接法，但由于红外图像存在温度漂移，无法保证直接法所依赖的灰度一致性假设，因而在室外长时间工作时，红外图像会发生较大的温度漂移，使得该方案容易失效。

如果进行红外相机的温度矫正，但又会因为温度矫正时引起的数据输出中断导致实时位姿估计的丢失，因而直接法无法良好适用于红外视觉里程计。

公布号为CN111882602A的中国专利申请公开了一种基于ORB特征点和GMS匹配过滤器的视觉里程计实现方法，对于每一帧图像，基于相机当前位姿，获取图像中的特征点(包括特征点对应角点的二维像素坐标、三维世界坐标和brief描述子)；对当前帧和上一帧之间的特征点结合GMS匹配过滤器进行匹配；根据对上一帧匹配到的特征点，求解PnP问题，得到当前帧的相机位姿；该方案为特征点法，依赖图像丰富的纹理信息，因此该视觉里程计实现方案在室外光照不稳定、夜间等环境中，由于图像无法提供丰富的纹理信息、导致描述子的区分度下降，特征点无法形成精确的匹配，从而在上述复杂环境中无法提供良好的定位信息。

因此，一种更加适用于红外相机的定位方案有待提出。

发明内容

为了解决上述技术问题，本发明的目的在于提供一种基于边缘特征匹配的红外视觉里程计实现方法，其结合了传统特征点法和直接法的优势，相比直接采用传统方案，本方法都具有一定的优势。该发明通过对红外图像进行边缘提取，通过在连续红外图像帧间进行边缘配准，使用最近点迭代匹配算法求解相机帧间运动估计，克服了机器人在光照动态变化、夜间、弱光照无光照、雾天等复杂环境中的定位问题。

为实现上述发明目的，本发明采取的技术方案如下：

一种基于边缘特征匹配的红外视觉里程计实现方法，包括以下步骤：

步骤S1：红外相机获取图像，对原始红外图像进行图像预处理，提高图像对比度，并进行自适应高斯差分边缘检测；

步骤S2：通过采用高斯差分平面拟合算法提取亚像素边缘位置；

步骤S3：构建稠密深度图，进行图像边缘点深度值恢复，获得红外视觉里程计的尺度信息；

步骤S4：计算图像边缘点的距离变换图变体，加快位姿求解速度；

步骤S5：使用边缘点邻域的温度信息对边缘点进行描述，创建温度邻域描述子；

步骤S6：进行帧间的三维与二维边缘最近点迭代匹配求解，得到位姿估计。

作为优选，步骤S1中所述的图像预处理过程包括：

步骤S1.1：对原始图像进行0°～30°范围的直方图均衡化，与原图进行加权平均，并进行高斯滤波；

步骤S1.2：使用自适应高斯差分算法进行红外边缘提取并过滤掉短边缘；

其中，短边缘是指边缘点连接后，数量少于20个边缘点的边缘；

中长边缘是指数量大于等于40的为长边缘；

步骤S1.1和步骤S1.2中还分别包括权值计算和阈值计算：

权值计算：计算当前边缘提取的噪声短边缘占比，调整下次与原图加权时的权值，从而减少直方图均衡化引入的噪声短边缘数量；

阈值计算：计算当前边缘提取的中长边缘占比，调整下次高斯边缘检测的阈值，从而获得预先设定的中长边缘点的数量。

作为优选，所述步骤S1.2采用的高斯差分算法将高斯函数转换成分离形式，使用分离式滤波器进行加速检测。

作为优选，所述步骤S2中所述亚像素边缘提取的过程包括：

步骤S2.1：通过边缘梯度阈值化剔除响应噪声点；

步骤S2.2：保留的边缘点进行[3×3]邻域的边缘点搜索并连接，形成边缘轮廓。

作为优选，所述步骤S2.2中，对保留的边缘点进行领域高斯差分值平面拟合，包括以下步骤：

步骤S2.2.1：假设邻域为[3×3]的窗口，同时假设该高斯差分平面为一简单线性模型，如公式(1)：

公式(1)：ax+by+cz＝d；

其中，a、b、c、d为三维平面方程的系数，为待求量；

(x，y)为边缘点及其邻域点的像素坐标值，z为对应高斯差分值；

步骤S2.2.2：通过边缘点及其邻域点拟合该高斯差分平面，进一步获得与图像平面相交的高斯零值线，将当前边缘像素中心点垂直投影至该高斯零值线上获得亚像素边缘点位置，归纳为通过公式(2)(3)(4)求解，进而使用该亚像素边缘点进行红外图像位姿求解：

公式(2)：

其中，A为(x，y，z)组合成的3×9的矩阵；

n为平面系数；

公式(3)：n＝(A^TA)^-1A^Tb；

公式(4)：

其中，其中p为边缘点亚像素图像位置。

作为优选，所述步骤S6包括：

步骤S6.1：假设

为红外图像第k帧I^k所提取的边缘点集，同时

为第I^k-1帧图像边缘点；

其中，i用于索引当前帧提取的边缘点的序号，每帧提取步骤S1.2中阈值计算得到的预设数量的中长边缘点；I^k定位为图像(I)第k帧；

步骤S6.2：获取边缘点

对应的深度值z_i；

步骤S6.3：将图像二维边缘点及其对应的深度值投影至相机坐标系中，如公式(5)：

公式(5)：

其中，π^-1为反投影变换；C^k-1表示k-1时刻下的相机坐标系；

为三维边缘点；

为三维边缘点集；

步骤S6.4：通过坐标变换矩阵，可以将第k-1帧的三维边缘点变换到第k帧的相机坐标系下，进而转化到图像平面上，获得对应的像素坐标，如公式(6)：

公式(6)：

其中，π为投影变换；

为图像帧间位姿估计待求量，即分别旋转矩阵、平移向量；

为经过坐标变化后的图像边缘点集；

步骤S6.5：对于任意经过公式(6)获得的边缘点

在

边缘点集中进行搜索最近的边缘点，形成对应的匹配关系，并将其之间的欧式距离值作为误差项进行优化从而求解相对位姿；

优化时，预先计算图像所有位置的最近邻边缘点，保存最近点的图像坐标值，形成距离变化图变体函数，即公式(7)：

公式(7)：

步骤S6.6：将误差项沿边缘梯度方向进行投影，最终构建优化函数如公式(8)，通过优化公式(8)求得红外图像帧间位姿估计：

公式(8)：

其中，δ_H为损失核函数；g为获取边缘点梯度向量函数。

作为优选，所述步骤S5包括：

步骤S5.1：以固定长度的向量与边缘点梯度向量形成给定的夹角，选中四个邻域点，提取其温度值T_j；

步骤S5.2：求取邻域点温度值与边缘点温度值的差值，即温度差。

步骤S5.3：进行归一化，构成一个四位的邻域温度描述子，如公式(9)所示：

公式(9)：

其中，j表示在i边缘点附近索引到的4个边缘点的索引。

相对于现有技术，本发明取得了有益的技术效果：

本发明的方案先对原始红外图像进行图像预处理，提高图像对比度，进而进行高斯差分边缘检测；通过采用高斯差分(DoG)平面算法提取亚像素边缘位置；为了进一步提高边缘点的在局部范围内的区分度，因距离相近的边缘点差异性很小，无法良好区分，本发明设计一种边缘温度描述子，对边缘点进行简单高效的描述，进而提高后续边缘点匹配的准确度，进而提高3D-2D边缘ICP(三维与二维边缘最近点迭代)匹配求解的精度；

由于单目视觉里程计缺乏尺度信息，因此可以通过激光雷达点云构建稠密的深度图，辅助视觉里程计恢复绝对尺度信息，但不局限于其他能够进行图像点深度值恢复的技术；

相比于传统特征点法，本方案能够在低对比度和弱纹理的红外图像上提取半稠密的边缘点数量，且无需进行复杂精确的描述子提取和匹配；

相比于直接法，本方案能够在高温漂的红外图像上避免依赖灰度一致性假设，通过边缘匹配之间的欧式距离作为误差，替换直接法中采用的光度误差，从而使得本方案在长时间的复杂环境中依然能够达到稳定的定位效果；

且由于红外图像的弱纹理特性，红外图像无法对树叶、草等进行良好的边缘轮廓成像，从而在成像时能够避免在自然特征中提取不稳定的边缘点(例如树叶、草丛)，因此边缘特征匹配更加适用于红外视觉里程计。

附图说明

图1是本发明实施例中红外视觉里程计实现方法整体流程图；

图2是本发明实施例中红外图像预处理流程图；

图3是本发明实施例中高斯差分(DoG)平面拟合获取亚像素边缘位置示意图；

图4是本发明实施例中创建边缘温度描述子示意图；

图5是本发明实施例中红外图像边缘检测效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例对本发明进行进一步详细说明，但本发明要求保护的范围并不局限于下述具体实施例。

实施例

参考图1，本实施例公开了一种基于边缘特征匹配的红外视觉里程计实现方法，包括以下步骤：

步骤S1：红外相机获取图像，对原始红外图像进行图像预处理，提高图像对比度，并进行自适应高斯差分边缘检测；

步骤S2：通过采用高斯差分平面拟合算法提取亚像素边缘位置；

步骤S3：构建稠密深度图，进行图像边缘点深度值恢复，获得红外视觉里程计尺度信息；

步骤S4：计算图像边缘点的距离变换图变体，加快位姿求解速度；

步骤S5：使用边缘点邻域的温度信息对边缘点进行描述，创建温度邻域描述子；

步骤S6：进行帧间的三维与二维边缘最近点迭代匹配求解，得到位姿估计。

红外图像的一个主要特性为纹理较弱，由于其图像强度值反映物体温度信息，即无法感知物体的纹理信息，造成图像纹理缺失，直接对原始图像进行边缘提取，难以保证边缘提取的数量与稳定性，其稳定性影响后续的边缘点的跟踪。

红外图像预处理过程为：首先，对原始红外图像进行预处理，增强图像对比度，然后再进行边缘提取。

具体流程如图2所示，步骤S1中的红外图像预处理包括：

步骤S1.1：对原始图像进行0°～30°范围的直方图均衡化，与原图进行加权平均，从而在增强图像细节的同时，将噪声维持在较低的水平，并进行高斯滤波；

步骤S1.2：使用自适应高斯差分算法进行边缘检测，并过滤掉短边缘。

本实施例定义：边缘点连接后，数量少于20个边缘点的为短边缘，数量大于等于40个边缘点的为长边缘。

由于直方图均衡化增强对比度同时也引入噪声，因此进行权值计算，即图2中预处理图像与原图相加的权值，主要计算当前边缘提取的噪声短边缘占比，进而调整下次与原图加权时的权值，从而减少直方图均衡化引入的噪声短边缘数量。

高斯差分边缘检测后，根据边缘点梯度大小进行边缘点剔除，梯度大小的划分为阈值，阈值计算主要计算当前边缘提取的中长边缘占比，调整下次高斯边缘检测的阈值，从而获得预先设定的中长边缘点数量，用于后续的位姿求解，本实施例设定6000个中长边缘。

对于边缘检测，由于可见光图像相比红外图像有较强的辨识度，能够保证良好的边缘点数量，现有研究采用Canny和Sobel等方法进行边缘提取，这些方法依赖较大的边缘灰度变化，但是由于红外图像的低对比度特性，这些检测算法无法适用在红外图像上。

因此，针对视觉SLAM对特征点的要求，连续帧中边缘点的重复性稳定提取至关重要；再者是精确的边缘点位置计算，影响后续的ICP(最近点迭代)算法的优化；最后在满足上述两点的同时保证较高的运行速率。

为此本实施例使用过零点的DoG(高斯差分)算法进行红外边缘提取，其具有较好的重复性检测能力与边缘提取精度。

边缘提取的重复性是高斯查分边缘检测的主要特性之一，而边缘位置获取的精度可以通过调整高斯差分的参数σ来缩小边缘的宽度，从而获得更加精确的图像边缘点位置。

为了进一步提高高斯差分边缘检测的速度，也即缩短边缘检测的时间，本实施例，步骤S1.2通过将高斯函数转化成分离形式，使用分离式滤波器进行加速检测。

可分离滤波器，即将二维卷积转化为一维卷积，对应于高斯差分中的高斯滤波，首先对x轴进行一维卷积，然后再进行y轴方向的一维卷积，将原始的复杂度O(MMNN)降O(MMN)，M、N为图像和滤波器的窗口大小。

通过对图像进行高斯差分边缘检测后，效果如图5所示，将会存在大量的检测边缘点。

因此，步骤S2中亚像素边缘提取的过程包括：

S2.1：通过边缘梯度阈值化剔除响应较小或较大的边缘点，这些边缘点可认为是噪声点。

步骤S2.2：对保留的边缘点进行[3×3]邻域的边缘点搜索并连接，形成边缘轮廓。

由于边缘点的图像2D坐标的精度影响后续相对位姿求解的精度，为了进一步提高边缘点的图像位置精度，即边缘点在图像上的坐标，本发明对保留的边缘点进行邻域高斯差分值平面拟合，如图3所示。

步骤S2.2包括：

步骤S2.2.1：假设邻域为[3×3]的窗口，同时假设该高斯差分平面为一简单线性模型，如公式(1)。

步骤S2.2.2：通过边缘点及其邻域点拟合该高斯差分平面，进一步获得与图像平面相交的高斯零值线，将当前边缘像素中心点垂直投影至该高斯零值线上获得亚像素边缘点位置。

可归纳为通过公式(2)(3)(4)求解，进而使用该亚像素边缘点进行红外图像位姿求解。

公式(1)：ax+by+cz＝d；

其中，a、b、c、d均为三维平面方程的系数，为待求量；

公式(2)：

其中，A就是(x，y，z)组合成的3×9的矩阵，n为平面系数；

公式(3)：n＝（A^TA）^-1A^Tb；

公式(4)：

其中，p为边缘点亚像素图像位置。

当获得亚像素边缘点后，相比利用直接法优化特征点之前的光度误差，本文采用ICP(最近点迭代)匹配的方案更加适用于红外图像高温漂的特性。

步骤S6包括：

步骤S6.1：假设

为红外图像第k帧I^k所提取的边缘点集，同时

为第I^k-1帧图像边缘点。

其中，i用于索引当前帧提取的边缘点的序号，前文提到每帧提取6000个边缘点；I^k定位为图像(I)第k帧。

步骤S6.2：对于边缘点

可以通过点云深度图获取其对应的深度值z_i。

但不局限于激光雷达构建深度图，也可双目相机获取边缘深度，双目相机通过视差也可计算边缘点深度，则可以将图像二维边缘点及其对应深度值投影至相机坐标系；

步骤S6.2：将图像二维边缘点及其对应的深度值投影至相机坐标系中，如下：

公式(5)：

其中，π^-1为反投影变换，C^k-1表示k-1时刻下的相机坐标系，z_i为边缘点

的深度值，

为三维边缘点，

为三维边缘点集。

通过反投影变换获得三维边缘点后，进行步S6.4；

步骤S6.4：通过坐标变换矩阵，可以将第k-1帧的三维边缘点变换到第k帧的相机坐标系下，进而转化到图像平面上，获得对应的像素坐标，如下：

公式(6)：

其中，π为投影变换，

为图像帧间位姿估计待求量，即旋转矩阵和平移向量，

为经过坐标变化后的图像边缘点集。

步骤S6.5：对于任意经过公式(6)获得的边缘点

在

边缘点集中进行搜索最近的边缘点，形成对应的匹配关系，并将其之间的欧式距离值作为误差项进行优化从而求解相对位姿，然而在该优化过程中，需要进行重复的最近邻点搜索匹配，这严重限制了计算的实时性。

现有方案中，参考文献“Schenk F,Fraundorfer F.RESLAM:A real-time robustedge-based SLAM system[C]//2019International Conference on Robotics andAutomation(ICRA).IEEE,2019:154-160”，通过引入距离变化图加快这一过程，即通过预先计算并保存图像所有位置到最近边缘点的欧式距离值，进而通过距离变化图函数

实现快速返回误差项值，加快求解的同时该函数不可微的性质使得优化函数无法进行迭代求解。

因此，本发明通过将距离变化图进行改进，即预先计算图像所有位置的最近邻边缘点，保存最近点的图像坐标值，形成距离变化图变体函数：

公式(7)：

该距离变化图变体在保证计算的实时性的同时，将最终的优化问题转化为PnP问题，进而能够进行高斯牛顿迭代求解。

为了加快最终的优化的收敛，进行步骤S6.6；

步骤S6.6：将误差项沿边缘梯度方向进行投影，最终构建优化函数如下：

公式(8)：

其中，δ_H为损失核函数，g为获取边缘点梯度向量函数，通过优化公式(8)求得红外图像帧间位姿估计。

然而仅仅通过边缘点最近邻搜索和梯度向量进行匹配，并不能获得精确的边缘匹配。

由于边缘属于弱特征，即在局部范围内存在较为相似的边缘，无法形成强匹配关系。

为了进一步提高边缘匹配的准确性，采用了特征点描述子的方案，但摒弃了较为复杂的描述子构建，结合红外图像的特征，本发明使用边缘点邻域的温度信息对边缘点进行描述，创建温度邻域描述子，如图4所示，Pi为当前所要进行构建描述子的边缘点，以边缘的梯度方向为主方向，保证了后续创建描述子的旋转不变性。

步骤S5包括：

步骤5.1：以固定长度的向量与边缘点梯度向量形成给定的夹角，如图4，以边缘点梯度向量为主方向，然后旋转45°，135°、-45°、-135°，分别与梯度方向进行设定的角度，固定长度指2个像素长度，选中四个邻域点，提取其温度值T_j。

步骤5.2：求取邻域点温度值与边缘点温度值的差值，即温度差。

步骤5.3：进行归一化，构成一个四位的邻域温度描述子，如公式(9)所示。

该温度描述子即能够反应边缘点局部的温度信息与温度差异方向，同时保证高效的计算效率。

公式(9)：

其中，i还是表示边缘点的索引；j表示在i边缘点附近索引到的4个边缘点的索引；T_i为当前进行描述子创建的边缘点的温度值；T_j为T_i周围选中的邻域点的温度值。

上述的边缘匹配过程为：当对通过高斯差分检测并通过梯度阈值保留的边缘点，首选通过距离变换图变体在上一帧图像中搜索最近邻的边缘点，当返回的边缘点之间的欧式距离小于一个距离阈值时，则通过距离阈值筛选，此距离阈值设定为设置为8个图像单位长度；

然后通过边缘点的梯度向量的方向进行匹配，当进行匹配的两个边缘点的梯度向量夹角小于一定角度阈值时，则认为该边缘点对之间形成弱匹配，即能够认为在同一物体边缘上所提取的边缘点，此角度阈值设定为10°；

最后，在该匹配的边缘点的附近寻找5个最近邻的边缘点，并通过边缘点的温度描述子通过现有的余弦距离公式进行余弦距离计算，在这五个边缘点中寻找余弦距离最低的边缘点形成最终的边缘点匹配；

最终对所有通过上述匹配流程的边缘点对集合进行3D-2D的ICP匹配进行两帧之间的相对位姿变换。

本发明方案面向室外复杂光照、弱光照、夜间等室外环境定位问题，以及现有采用传统方案的红外视觉里程计方法局限性，本发明创新性的提出一种基于3D-2D边缘ICP匹配的红外视觉里程计算法，该方案更加适用于红外图像特性，克服了红外图像弱纹理、低对比度、高温漂等限制SLAM应用的劣势特性，实现了在室外复杂环境中的实时定位问题，避免了长时间工作引发的红外相机退化。

根据上述说明书的揭示和教导，本发明所属领域的技术人员还可以对上述实施方式进行变更和修改。因此，本发明并不局限于上面揭示和描述的具体实施方式，对发明的一些修改和变更也应当落入本发明的权利要求的保护范围内。此外，尽管本说明书中使用了一些特定的术语，但这些术语只是为了方便说明，并不对发明构成任何限制。

Claims (7)

1.一种基于边缘特征匹配的红外视觉里程计实现方法，其特征在于，包括以下步骤：

步骤S1：红外相机获取图像，对原始红外图像进行图像预处理，提高图像对比度，并进行自适应高斯差分边缘检测；

步骤S2：通过采用高斯差分平面拟合算法提取亚像素边缘位置；

步骤S3：构建稠密深度图，进行图像边缘点深度值恢复，获得红外视觉里程计尺度信息；

步骤S4：计算图像边缘点的距离变换图变体，加快位姿求解速度；

步骤S5：使用边缘点邻域的温度信息对边缘点进行描述，创建温度邻域描述子；

步骤S6：进行帧间的三维与二维边缘最近点迭代匹配求解，得到位姿估计。

2.根据权利要求1所述的基于边缘特征匹配的红外视觉里程计实现方法，其特征在于，步骤S1中所述的图像预处理过程包括：

步骤S1.1：对原始图像进行0°～30°范围的直方图均衡化，与原图进行加权平均，并进行高斯滤波；

步骤S1.2：使用自适应高斯差分算法进行红外边缘提取并过滤掉短边缘；

其中，短边缘是指边缘点连接后，数量少于20个边缘点的边缘；

中长边缘是指数量大于等于40个边缘点的边缘；

步骤S1.1和步骤S1.2中还分别包括权值计算和阈值计算：

权值计算：计算当前边缘提取的噪声短边缘占比，调整下次与原图加权时的权值，从而减少直方图均衡化引入的噪声短边缘数量；

阈值计算：计算当前边缘提取的中长边缘占比，调整下次高斯边缘检测的阈值，从而获得预先设定的中长边缘点的数量。

3.根据权利要求2所述的基于边缘特征匹配的红外视觉里程计实现方法，其特征在于，

所述步骤S1.2采用的高斯差分算法将高斯函数转换成分离形式，使用分离式滤波器进行加速检测。

4.根据权利要求2或3所述的基于边缘特征匹配的红外视觉里程计实现方法，其特征在于，所述步骤S2中所述亚像素边缘提取的过程包括：

步骤S2.1：通过边缘梯度阈值化剔除响应噪声点；

步骤S2.2：保留的边缘点进行[3×3]邻域的边缘点搜索并连接，形成边缘轮廓。

5.根据权利要求4所述的基于边缘特征匹配的红外视觉里程计实现方法，其特征在于，

所述步骤S2.2中，对保留的边缘点进行领域高斯差分值平面拟合，包括以下步骤：

步骤S2.2.1：假设邻域为[3×3]的窗口，同时假设该高斯差分平面为一简单线性模型，如公式(1)：

公式(1)：ax+by+cz＝d；

其中，a、b、c、d为三维平面方程的系数，为待求量；

(x，y)为边缘点及其邻域点的像素坐标值，z为对应高斯差分值；

步骤S2.2.2：通过边缘点及其邻域点拟合该高斯差分平面，进一步获得与图像平面相交的高斯零值线，将当前边缘像素中心点垂直投影至该高斯零值线上获得亚像素边缘点位置，归纳为通过公式(2)(3)(4)求解，进而使用该亚像素边缘点进行红外图像位姿求解：

公式(2)：

其中，A为(x，y，z)组合成的3×9的矩阵；

n为平面系数；

公式(3)：n＝(A^TA)^-1A^Tb；

公式(4)：

其中，其中p为边缘点亚像素图像位置。

6.根据权利要求2、3或5所述的基于边缘特征匹配的红外视觉里程计实现方法，其特征在于，所述步骤S6包括：

步骤S6.1：假设

为红外图像第k帧I^k所提取的边缘点集，同时

为第I^k-1帧图像边缘点；

其中，i用于索引当前帧提取的边缘点的序号，每帧提取步骤S1.2中阈值计算得到的预设数量的中长边缘点；I^k定位为图像(I)第k帧；

步骤S6.2：获取边缘点

对应的深度值z_i；

步骤S6.3：将图像二维边缘点及其对应的深度值投影至相机坐标系中，如公式(5)：

公式(5)：

其中，π^-1为反投影变换；C^k-1表示k-1时刻下的相机坐标系；

为三维边缘点；

为三维边缘点集；

步骤S6.4：通过坐标变换矩阵，可以将第k-1帧的三维边缘点变换到第k帧的相机坐标系下，进而转化到图像平面上，获得对应的像素坐标，如公式(6)：

公式(6)：

其中，π为投影变换；

为图像帧间位姿估计待求量，即分别旋转矩阵、平移向量；

为经过坐标变化后的图像边缘点集；

步骤S6.5：对于任意经过公式(6)获得的边缘点

在

边缘点集中进行搜索最近的边缘点，形成对应的匹配关系，并将其之间的欧式距离值作为误差项进行优化从而求解相对位姿；

优化时，预先计算图像所有位置的最近邻边缘点，保存最近点的图像坐标值，形成距离变化图变体函数，即公式(7)：

公式(7)：

步骤S6.6：将误差项沿边缘梯度方向进行投影，最终构建优化函数如公式(8)，通过优化公式(8)求得红外图像帧间位姿估计：

公式(8)：

其中，δ_H为损失核函数；g为获取边缘点梯度向量函数。

7.根据权利要求6所述的基于边缘特征匹配的红外视觉里程计实现方法，其特征在于，所述步骤S5包括：

步骤S5.1：以固定长度的向量与边缘点梯度向量形成给定的夹角，选中四个邻域点，提取其温度值T_j；

步骤S5.2：求取邻域点温度值与边缘点温度值的差值，即温度差；

步骤S5.3：进行归一化，构成一个四位的邻域温度描述子，如公式(9)所示：

公式(9)：

其中，j表示在i边缘点附近索引到的4个边缘点的索引。

Images