CN115588051B - 矿石加工环节的激光雷达和相机空间位置自动标定方法 - Google Patents

Abstract

本发明提出了一种矿石加工环节的激光雷达和相机空间位置自动标定方法，该方法包括：对相机传感器采集的包括矿石和皮带机的图像进行图像增强处理；利用图像的梯度幅值变化提取图像线特征数据；对激光雷达采集的点云数据利用点云数据的距离不连续性提取边界直线特征，经过滤得到更精确点云直线特征，并转换为深度图；对点云数据进行特征补全并分配特征权重；对深度图进行加权求和后投影到相机传感器的图像坐标系上，对当前帧的全部数据点灰度值及全部深度图数据点的最大灰度值分别求和，由两者比值得到投影得分；与设定阈值比较，分别使用不同的步长对深度图的灰度值进行迭代优化，直至达到精度要求，进行置信度评估得到标定结果。

Description

矿石加工环节的激光雷达和相机空间位置自动标定方法

技术领域

本发明涉及多传感器融合领域及煤矸识别领域，尤其涉及矿石加工环节的激光雷达和相机空间位置自动标定方法。

背景技术

多传感器数据融合是煤矸识别领域的研究重点和难点之一。

煤矸随主运输胶带升井后，作业人员需人工在地面分拣出大于200mm的大块矸石。这种方式分拣效率低，现场拣矸环境恶劣，粉尘大，作业人员劳动强度大，且矿石的体积和重量难以有效统计。目前国内外的主流方案是使用激光雷达和相机分别去采集皮带机上的数据，用图像数据识别区分矿石和石头，用激光雷达采集的点云数据完成针对矿石的外轮廓坐标点采集，并进一步进行矿石的体积和重量估算。

两传感器数据是否可以有效融合直接决定了最后矿石体积和重量估计的准确性。激光雷达与相机的数据格式、存储内容各不相同。

发明内容

本发明的目的在于提供矿石加工环节的激光雷达和相机空间位置自动标定方法，可以实时判断当前激光雷达和相机的空间位置是否准确，并且在准确度低的情况下在线纠正激光雷达和相机的空间位置误差，以实现激光雷达和相机传感器数据的有效融合。

为了实现上述目的，本发明提出了一种矿石加工环节的激光雷达和相机空间位置自动标定方法，所述方法包括：

步骤1)对相机传感器采集的包括矿石和皮带机的图像进行包括去尘和轮廓细节保留的图像增强处理；

步骤2)对图像增强处理后的图像数据利用图像的梯度幅值变化提取图像线特征数据；

步骤3)对激光雷达采集的点云数据利用点云数据的距离不连续性提取边界直线特征，经过滤得到更精确的点云直线特征，并转换为深度图；

步骤4)对点云数据进行特征补全并分配特征权重；

步骤5)根据特征权重对深度图进行加权求和后投影到相机传感器的图像坐标系上，对当前帧的全部数据点灰度值累计求和，对全部深度图数据点的最大灰度值累计求和，并计算两者的比值得到投影得分；

步骤6)根据投影得分与设定阈值的比较，分别使用不同的步长对深度图的灰度值进行迭代优化，直至达到精度要求，进行置信度评估得到标定结果。

作为上述方法的一种改进，所述步骤1)包括：

步骤1-1)使用改进的Retinex算法，从相机传感器采集的图像I(x,y)中去除照射图L(x,y)，获得去尘后的图像R_L(x,y)：

lgR_L(x,y)＝lgI(x,y)-lgL(x,y)

步骤1-2)利用以下公式将两种不同的高斯核分别与去尘后的图像R_L进行卷积，得到两种不同尺度的模糊图像

和/>

其中，g₁和g₂分别表示标准差为1和4的高斯核，以对图像中不同细节的图像予以保留；

再通过减法运算提取精细细节和大体外观；

对卷积后的模糊图像

和/>

进行叠加，提高矿石边缘细节的区分度。

作为上述方法的一种改进，所述步骤2)包括：

将图像增强处理后的图像数据转换为灰度图，并进行高斯滤波；对高斯滤波后的灰度图进行梯度幅值的提取，得到梯度幅值图像；

以进行分水岭变换的方法提取矿石和皮带机的轮廓边缘信息；

对梯度幅值图进行非极大值抑制，以增强矿石细节的边缘信息；

利用灰度的值的变化梯度提取轮廓的初步直线特征，运用双阈值算法检测和连接边缘，得到直线特征；

对直线特征进行过滤，去除过短的直线特征，得到图像线特征数据T_t。

作为上述方法的一种改进，所述步骤3)包括：

运用正态分布变化方法计算当前帧与前一帧间的变换矩阵，利用点坐标的距离不连续性计算点云中物体的边缘特征，再利用边缘特征的连续性及梯度变化过滤掉弧度大于阈值的边缘，得到剩余边缘即为直线特征；

通过比较当前点与相邻点的距离，去除孤立点，得到更精确的直线特征；

将提取过滤后的点云图像转化为深度图。

作为上述方法的一种改进，所述步骤4)包括：

利用正态分布变化方法，使用局部映射的方式，将前一帧深度图像通过求得的变换矩阵变换到当前帧实现特征补全；

基于Hough变换，构建特征金字塔，提取出水平方向直线F_h与竖直方向的直线F_v，并赋值为α和1-α，对水平线特征赋予较高权重，对竖直线特征赋予较低权重。

作为上述方法的一种改进，所述步骤5)包括：

根据特征权重，对提取过滤后的深度图数据进行加权赋值后，投影到相机传感器的像素坐标系上，并结合步骤2)得到的图像线特征数据T_t，计算投影后的符合直线特征的一个数据点的灰度值，t时刻当前帧的全部数据点灰度值累计求和记为A_t；t时刻当前帧的全部数据点的最大灰度值累计求和记为B_t；分别满足下式：

其中，t表示t时刻，n表示所有点云直线特征的总点数，；p_t表示点云直线特征

计算两者的比值得到t时刻投影得分S_t＝A_t/B_t。

作为上述方法的一种改进，所述步骤6)包括：

步骤6-1)判断投影得分A_t/B_t是否大于第一阈值，判断为是，则当前激光雷达和相机传感器的空间位置准确，转至步骤6-3)；否则，当前激光雷达和相机传感器的空间位置不准确，转至步骤6-2)；

步骤6-2)判断投影得分A_t/B_t是否大于第二阈值，判断为是，使用小步长和大灰度值变化的图像进行灰度变换，否则，使用大步长和小灰度值变化的图像进行灰度变换；继续优化，直至投影得分A_t/B_t大于第一阈值，转至步骤6-3)；

步骤6-3)从时域关系和空间位置联合进行置信度评估，得到精确标定结果。

作为上述方法的一种改进，所述步骤6-3)包括：

根据下式得到num帧累计的激光雷达和相机传感器数据重合度均值Saverage：

其中，S_i为在第i时刻的投影得分；

当Saverage大于0.8，且S_i大于0.6时，则认定激光雷达和相机传感器之间的位置参数可靠，进而得到精确标定结果。

另一方面，本发明提出了一种矿石加工环节的激光雷达和相机空间位置自动标定系统，所述系统包括：图像增强模块、图像直线特征提取模块、点云直线特征提取模块、特征补全及权重分配模块、投影及投影得分计算模块和置信度评估模块；其中，

所述图像增强模块，用于对相机传感器采集的包括矿石和皮带机的图像进行包括去尘和轮廓细节保留的图像增强处理

所述图像直线特征提取模块，用于对图像增强处理后的图像数据使用分水岭变换的方式提取边缘信息，经处理得到图像线特征数据；

所述点云直线特征提取模块，用于对激光雷达采集的点云数据提取边界直线特征，经过滤得到更精确的点云直线特征，并转换为深度图；

所述特征补全及权重分配模块，用于对点云数据进行特征补全并分配特征权重；

所述投影及投影得分计算模块，用于根据特征权重对深度图进行加权求和后投影到相机传感器的图像坐标系上，对当前帧的全部数据点灰度值累计求和，对全部深度图数据点的最大灰度值累计求和，并计算两者的比值得到投影得分；

所述置信度评估模块，用于根据投影得分与设定阈值的比较，分别使用不同的步长对深度图的灰度值进行迭代优化，直至达到精度要求，进行置信度评估得到标定结果。

与现有技术相比，本发明的优势在于：

1、本发明的方法可以利用矿石加工环节中矿石边缘的线性特征及传输机边缘的线性特征来实时判断当前激光雷达和相机的空间位置是否准确，并且在准确度低的情况下自动纠正激光雷达和相机的空间位置误差，以达到视觉传感器和激光雷达传感器参数自动优化目的；

2、本发明的方法不需要人为设计标定物体，自动采集图像和点云中本身存在的矿石和传输机边缘的直线特征，利用两个传感器之间特征的对应关系来自动标定激光雷达和相机空间位置；

3、相比于其他的自动标定方法，本发明的方法针对矿石加工场景下高粉尘和需要24小时连续作业的特性，预先对图像数据进行数据去尘和增强，保证了高粉尘或光照不明环境下图像中直线特征能够被稳定提取。

附图说明

图1是本发明矿石加工环节的激光雷达和相机空间位置自动标定方法的流程图。

具体实施方式

为了将两组异构传感器数据有效融合，我们利用图像和点云数据中共同存在的矿石直线特征，通过二者之间特征的对应关系，计算和自动校正两个传感器之间的空间位置关系。并且在皮带机长期运行过程中，激光雷达和相机的空间位置关系容易产生偏移，传感器的相对位置发生偏移也为两传感器数据的融合结果的可靠与否带来极大的不确定性。因此需要一种可以实时根据激光雷达与相机的空间位置关系变化进行动态调控，并能自动校正累计误差的自动标定方法。

下面结合附图和实施例对本发明的技术方案进行详细的说明。

实施例1

如图1所示，本发明的实施例1提出了一种矿石加工环节的激光雷达和相机空间位置自动标定方法。包括以下步骤：

1.图像增强

1)使用改进的Retinex算法对图像进行去尘

Retinex算法具有提高图像亮度的特点，因此被广泛应用于图像的增强和去尘环节。基于Retinex算法对于低光环境(如采矿和选矿工作环境)中进行去尘、增强，是一种适合且有效的方法。

通过从原图像中去掉对应的照射图，获得去尘后的图像，也被称为反射图，达到图像增强的目的。Retinex算法认为原图像是照射图L和反射图R_L相乘而得：

I(x,y)＝R_L(x,y)×L(x,y)

其中，I(x,y)是输入的原图像；R_L(x,y)决定该图像的内在性质；L(x,y)是照射图，代表图像像素值的动态范围。但是考虑到乘法的计算复杂度，为了提高运算速度，对算法进行对数变换得：

logR_L(x,y)＝logI(x,y)-logL(x,y)

2)考虑到直线特征容易受到粉尘和光照等影响，本方法使用多尺度卷积增强图像，以保留、改善图像中物体的轮廓细节信息

首先，利用下方公式将两种不同的高斯核分别与输入图像进行卷积，得到三种不同尺度的模糊图像：

其中g₁ g₂分别表示标准差为1,和4的高斯核，以对图像中不同细节的图像予以保留。然后，通过减法运算提取精细细节

大体外观/>

然后对卷积后的图像进行叠加，以提高矿石边缘细节的区分度。

2.图像线特征提取、过滤

将图像增强处理后的图像数据转换为灰度图，并进行高斯滤波。接下来，对灰度图的梯度幅值进行提取，得到梯度复制图像。以进行分水岭变换的方法提取矿石和皮带机的轮廓边缘信息。对梯度幅值图进行非极大值抑制，以增强矿石细节的边缘信息。然后利用灰度的值的变化梯度提取轮廓的初步直线特征。运用双阈值算法检测和连接边缘，得到直线特征；

对直线特征进行过滤，去除过短的直线特征，得到图像线特征数据T_t。

3.激光雷达点云直线特征提取

1)提取边界直线特征：运用正态分布变化(NDT)方法计算当前帧与前一帧间的变换矩阵。然后利用点坐标的距离不连续性计算点云中物体的边缘特征。再利用边缘特征的连续性及梯度变化过滤掉弧度较大的边缘，剩余边缘被认为是本方法提取的直线特征。

2)过滤点云线特征：通过比较当前点与相邻点的距离，去除相距过于的孤立点，提取更为精确的直线特征。

3)转换为深度图：将提取过滤后的点云图像转化为深度图，从而将三维数据转换为二维数据，降低后续点云直线特征计算的复杂度。

4.点云直线特征增强：

1)特征补全：利用正态分布变化(NDT)方法，使用局部映射的方式，将前一帧深度图像通过求得的变换矩阵变换到当前帧，即每帧深度数据都包含了当前帧的数据和前一帧的数据。以增加点云数据的线特征密度，补全特征。

2)特征权重分配：基于Hough变换，构建特征金字塔。提取出水平方向与竖直方向的直线，分别记为F_h、F_v，并赋值为α和1-α。在矸石识别这一特定场景中，皮带是稳定出现的水平线特征，并且这一水平线特征显著。为了更有效地提取线性特征，本场景下对水平线特征赋予更高权重，相对应的，赋予竖直线特征更低权重。

5.深度图与图像匹配

将提取过滤后的深度图数据加权赋值后，投影到相机传感器的像素坐标系上，计算投影后的符合直线特征的一个数据点的灰度值,对当前帧的全部数据点灰度值累计求和，记为A_t；对全部深度图数据点的最大灰度值累计求和，记为B_t；计算A_t/B_t得到精度值，当所有数据点灰度值全部匹配时，精度值为1，即A_t＝B_t，当所有数据点灰度值无法不匹配时，即两个传感器之间的数据点基本没有重合，精度值为0。

计算两者的比值得到投影得分S_t＝A_t/B_t

6.标定结果优化

判断由A_t/B_t得到的精度值是否大于0.9，如果为是，则当前激光雷达和相机传感器的空间位置准确，否则，当前激光雷达和相机传感器的空间位置不准确。

对图像进行灰度变换：在精度高于阈值0.7时使用小步长和大灰度值变化的图像，在精度低于阈值0.7时，使用大步长和小灰度值变化的图像。迭代优化，最终得到精确标定结果。

7.置信度评估

提出一种从时域关系和空间位置联合评估两传感器标定结果的方法

为了确保激光雷达-视觉传感器数据融合结果的可靠性和稳定性，提出从时域关系和空间位置联合评估两传感器标定结果的方法,确保结果两传感器空间位置误差最小化，并且在一段连续时间内标定结果稳定。

S_t也是两传感器数据在同一时刻t的重合度，同一时刻激光雷达-视觉传感器标定结果重合度公式为：

当传感器不发生移动，即稳定放置时，同一传感器数据在其前后几帧，即T时刻，T-1时刻和T-2时刻的位置基本一致。相应的，当T时刻的标定结果可靠时，T-1时刻和T-2时刻的另一传感器位置关系应该也基本一致。当T时刻的标定结果可靠，但T-1时刻或T-2时刻标定结果不可靠时说明传感器标定结果或者传感器位置存在不确定性。所以除了需要评估两传感器同一时刻的标定准确性，还需保证前几帧同样标定结果可靠。

根据下式得到num帧累计的激光雷达和相机传感器数据重合度均值Saverage：

其中，S_i为在第i时刻的投影得分；

当Saverage大于0.8，且S_i大于0.6，即若干帧数据重合度均值高于百分之八十，同时每一帧数据重合度都大于百分之60时，认定两传感器之间的位置参数可靠。

实施例2

本发明的实施例2提出了一种矿石加工环节的激光雷达和相机空间位置自动标定系统，基于实施例1的方法实现，系统包括：图像增强模块、图像直线特征提取模块、点云直线特征提取模块、特征补全及权重分配模块、投影及投影得分计算模块和置信度评估模块；其中，

所述图像增强模块，用于对相机传感器采集的包括矿石和皮带机的图像进行包括去尘和轮廓细节保留的图像增强处理

所述图像直线特征提取模块，用于对图像增强处理后的图像数据使用分水岭变换的方式提取边缘信息，经处理得到图像线特征数据；

所述点云直线特征提取模块，用于对激光雷达采集的点云数据提取边界直线特征，经过滤得到更精确的点云直线特征，并转换为深度图；

所述特征补全及权重分配模块，用于对点云数据进行特征补全并分配特征权重；

所述投影及投影得分计算模块，用于根据特征权重对深度图进行加权求和后投影到相机传感器的图像坐标系上，对当前帧的全部数据点灰度值累计求和，对全部深度图数据点的最大灰度值累计求和，并计算两者的比值得到投影得分；

所述置信度评估模块，用于根据投影得分与设定阈值的比较，分别使用不同的步长对深度图的灰度值进行迭代优化，直至达到精度要求，进行置信度评估得到标定结果。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (4)

1.一种矿石加工环节的激光雷达和相机空间位置自动标定方法，所述方法包括：

步骤1)对相机传感器采集的包括矿石和皮带机的图像进行包括去尘和轮廓细节保留的图像增强处理；

步骤2)对图像增强处理后的图像数据利用图像的梯度幅值变化提取图像线特征数据；

步骤3)对激光雷达采集的点云数据利用点云数据的距离不连续性提取边界直线特征，经过滤得到更精确的点云直线特征，并转换为深度图；

步骤4)对点云数据进行特征补全并分配特征权重；

步骤5)根据特征权重对深度图进行加权求和后投影到相机传感器的图像坐标系上，对当前帧的全部数据点灰度值累计求和，对全部深度图数据点的最大灰度值累计求和，并计算两者的比值得到投影得分；

步骤6)根据投影得分与设定阈值的比较，分别使用不同的步长对深度图的灰度值进行迭代优化，直至达到精度要求，进行置信度评估得到标定结果；

所述步骤3)包括：

运用正态分布变化方法计算当前帧与前一帧间的变换矩阵，利用点坐标的距离不连续性计算点云中物体的边缘特征，再利用边缘特征的连续性及梯度变化过滤掉弧度大于阈值的边缘，得到剩余边缘即为直线特征；

通过比较当前点与相邻点的距离，去除孤立点，得到更精确的直线特征；

将提取过滤后的点云图像转化为深度图；

所述步骤6)包括：

步骤6-1)判断投影得分A_t/B_t是否大于第一阈值，判断为是，则当前激光雷达和相机传感器的空间位置准确，转至步骤6-3)；否则，当前激光雷达和相机传感器的空间位置不准确，转至步骤6-2)；

步骤6-2)判断投影得分A_t/B_t是否大于第二阈值，判断为是，使用小步长和大灰度值变化的图像进行灰度变换，否则，使用大步长和小灰度值变化的图像进行灰度变换；继续优化，直至投影得分A_t/B_t大于第一阈值，转至步骤6-3)；

步骤6-3)从时域关系和空间位置联合进行置信度评估，得到精确标定结果；

所述步骤6-3)包括：

根据下式得到num帧累计的激光雷达和相机传感器数据重合度均值Saverage：

其中，S_i为在第i时刻的投影得分；

当Saverage大于0.8，且S_i大于0.6时，则认定激光雷达和相机传感器之间的位置参数可靠，进而得到精确标定结果；

所述步骤4)包括：

利用正态分布变化方法，使用局部映射的方式，将前一帧深度图像通过求得的变换矩阵变换到当前帧实现特征补全；

基于Hough变换，构建特征金字塔，提取出水平方向直线F_h与竖直方向的直线F_v，并赋值为α和1-α，对水平线特征赋予较高权重，对竖直线特征赋予较低权重；

所述步骤5)包括：

根据特征权重，对提取过滤后的深度图数据进行加权赋值后，投影到相机传感器的像素坐标系上，并结合步骤2)得到的图像线特征数据T_t，计算投影后的符合直线特征的一个数据点的灰度值，t时刻当前帧的全部数据点灰度值累计求和记为A_t；t时刻当前帧的全部数据点的最大灰度值累计求和记为B_t；分别满足下式：

其中，t表示t时刻，n表示所有点云直线特征的总点数；p_t表示点云直线特征；

计算两者的比值得到t时刻投影得分S_t＝A_t/B_t。

2.根据权利要求1所述的矿石加工环节的激光雷达和相机空间位置自动标定方法，其特征在于，所述步骤1)包括：

步骤1-1)使用改进的Retinex算法，从相机传感器采集的图像I(x,y)中去除照射图L(x,y)，获得去尘后的图像R_L(x,y)：

lgR_L(x,y)＝lgI(x,y)-lgL(x,y)

步骤1-2)利用以下公式将两种不同的高斯核分别与去尘后的图像R_L进行卷积，得到两种不同尺度的模糊图像

和/>

其中，g₁和g₂分别表示标准差为1和4的高斯核，以对图像中不同细节的图像予以保留；

再通过减法运算提取精细细节和外观；

对卷积后的模糊图像

和/>

进行叠加，提高矿石边缘细节的区分度。

3.根据权利要求2所述的矿石加工环节的激光雷达和相机空间位置自动标定方法，其特征在于，所述步骤2)包括：

将图像增强处理后的图像数据转换为灰度图，并进行高斯滤波；对高斯滤波后的灰度图进行梯度幅值的提取，得到梯度幅值图像；

以进行分水岭变换的方法提取矿石和皮带机的轮廓边缘信息；

对梯度幅值图进行非极大值抑制，以增强矿石细节的边缘信息；

利用灰度的值的变化梯度提取轮廓的初步直线特征，运用双阈值算法检测和连接边缘，得到直线特征；

对直线特征进行过滤，去除过短的直线特征，得到图像线特征数据T_t。

4.一种矿石加工环节的激光雷达和相机空间位置自动标定系统，其特征在于，所述系统包括：图像增强模块、图像直线特征提取模块、点云直线特征提取模块、特征补全及权重分配模块、投影及投影得分计算模块和置信度评估模块；其中，

所述图像增强模块，用于对相机传感器采集的包括矿石和皮带机的图像进行包括去尘和轮廓细节保留的图像增强处理；

所述图像直线特征提取模块，用于对图像增强处理后的图像数据使用分水岭变换的方式提取边缘信息，经处理得到图像线特征数据；

所述点云直线特征提取模块，用于对激光雷达采集的点云数据提取边界直线特征，经过滤得到更精确的点云直线特征，并转换为深度图；

所述特征补全及权重分配模块，用于对点云数据进行特征补全并分配特征权重；

所述投影及投影得分计算模块，用于根据特征权重对深度图进行加权求和后投影到相机传感器的图像坐标系上，对当前帧的全部数据点灰度值累计求和，对全部深度图数据点的最大灰度值累计求和，并计算两者的比值得到投影得分；

所述置信度评估模块，用于根据投影得分与设定阈值的比较，分别使用不同的步长对深度图的灰度值进行迭代优化，直至达到精度要求，进行置信度评估得到标定结果；

所述点云直线特征提取模块的处理过程包括：

运用正态分布变化方法计算当前帧与前一帧间的变换矩阵，利用点坐标的距离不连续性计算点云中物体的边缘特征，再利用边缘特征的连续性及梯度变化过滤掉弧度大于阈值的边缘，得到剩余边缘即为直线特征；

通过比较当前点与相邻点的距离，去除孤立点，得到更精确的直线特征；

将提取过滤后的点云图像转化为深度图；

所述置信度评估模块的处理过程包括：

步骤6-1)判断投影得分A_t/B_t是否大于第一阈值，判断为是，则当前激光雷达和相机传感器的空间位置准确，转至步骤6-3)；否则，当前激光雷达和相机传感器的空间位置不准确，转至步骤6-2)；

步骤6-2)判断投影得分A_t/B_t是否大于第二阈值，判断为是，使用小步长和大灰度值变化的图像进行灰度变换，否则，使用大步长和小灰度值变化的图像进行灰度变换；继续优化，直至投影得分A_t/B_t大于第一阈值，转至步骤6-3)；

步骤6-3)从时域关系和空间位置联合进行置信度评估，得到精确标定结果；

所述步骤6-3)包括：

根据下式得到num帧累计的激光雷达和相机传感器数据重合度均值Saverage：

其中，S_i为在第i时刻的投影得分；

当Saverage大于0.8，且S_i大于0.6时，则认定激光雷达和相机传感器之间的位置参数可靠，进而得到精确标定结果；

所述特征补全及权重分配模块的处理过程包括：

利用正态分布变化方法，使用局部映射的方式，将前一帧深度图像通过求得的变换矩阵变换到当前帧实现特征补全；

基于Hough变换，构建特征金字塔，提取出水平方向直线F_h与竖直方向的直线F_v，并赋值为α和1-α，对水平线特征赋予较高权重，对竖直线特征赋予较低权重；

所述投影及投影得分计算模块的处理过程包括：

根据特征权重，对提取过滤后的深度图数据进行加权赋值后，投影到相机传感器的像素坐标系上，并结合步骤2)得到的图像线特征数据T_t，计算投影后的符合直线特征的一个数据点的灰度值，t时刻当前帧的全部数据点灰度值累计求和记为A_t；t时刻当前帧的全部数据点的最大灰度值累计求和记为B_t；分别满足下式：

其中，t表示t时刻，n表示所有点云直线特征的总点数；p_t表示点云直线特征；

计算两者的比值得到t时刻投影得分S_t＝A_t/B_t。

Images