CN109544456B - 基于二维图像和三维点云数据融合的全景环境感知方法 - Google Patents

Abstract

本发明公开了一种基于二维图像和三维点云数据融合的全景环境感知方法，应用样本一致性初始对准(SAC‑IA)算法与迭代最近点(ICP)算法将多帧点云数据统一到世界坐标系；对这些点云数据对应的灰度图像进行相似特征匹配与图像拼接，对拼接图像采用像素加权的方法消除图像拼接区域的裂缝，平滑拼接区域像素点的亮度值，得到优化的全景图像；应用深度信息对世界坐标系中所有点云数据进行分割，得到不同深度范围中的点云数据，将这些点云数据分别投影为二维图像并二值化；将生成的二值图像与全景图像进行信息融合，实现对不同深度的环境信息全景视觉感知。本发明能满足背景复杂、存在较强干扰的检测场景快速全景信息重构的技术需求，可有效降低后续场景理解、路径规划等工作的难度。

Description

基于二维图像和三维点云数据融合的全景环境感知方法

技术领域

本发明涉及及三维计算机视觉技术领域,特别是一种基于二维图像和三维点云数据融合的全景环境感知方法。

背景技术

点云是大多数三维信息获取设备的原始输出数据，与二维图像相比，点云数据能提供对真实世界的场景更全面的描述。近年来，随着计算机硬件性能的提升和点云数据获取设备的普及，点云数据利用技术在智能机器人、无人驾驶、先进制造、虚拟现实和增强现实等领域的应用越来越多。

然而，点云数据获取设备一次只能获取单一视角的点云，无法反应真实世界的全貌；点云数据通常质量较差，存在无效数据、噪声、孔洞等缺陷。点云数据难以直接被利用。

因此，为有效的利用点云数据，国内外科技人员开展了大量的工作，这些工作大都遵循以下技术框架：首先，通过数据预处理方法消除点云数据的一些缺陷；然后，采用点云数据配准的方法将多帧不同视角的点云数据统一到世界坐标系；最后，获取配准点云数据的法向和曲率等几何参数，通过插值或者拟合实现对真实世界的重建。

上述工作已能较好的满足干扰较少、对环境感知精确度和效率要求不高的应用场合技术需求。但是，现有技术采集的点云数据在复杂环境下存在较多的缺陷，对于这些缺陷造成的环境信息缺失，现有方法大多是通过拟合、插值的方法进行补充的，得到的信息只是对环境信息的估计，而不是真实准确的信息。现有点云数据利用技术不能满足复杂条件下环境实时精确感知的要求。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种基于二维图像和三维点云数据融合的全景环境感知方法，通过点云数据和(采集点云数据时同时采集的)质量相对较高的图像数据进行信息融合，实现对于背景复杂、存在较强干扰的检测场景快速全景信息重构，降低后续场景理解、路径规划等工作的难度。为解决上述技术问题，本发明所采用的技术方案是：一种基于二维图像和三维点云数据融合的全景环境感知方法，包括以下步骤：

1)信息获取：从TOF相机中提取待检测的第一帧点云数据C_g1与其对应的灰度图像I_g1，将拍摄位置水平方向移动距离为0-15cm或将拍摄位置旋转0-60°提取待检测的第二帧点云数据C_g2与其对应的灰度图像I_g2，按照上述方式经过N次拍摄得到N帧不同场景的点云数据和其对应的灰度图像。

2)点云数据配准：应用SAC-IA算法与ICP算法将步骤1)中的N帧点云数据全部变换到第一帧点云数据C_g1的坐标系空间中；

3)点云数据对应的灰度图像配准：对步骤1)拍摄的第N帧图像与第N-1帧图像进行融合，生成两帧图像的融合图像，并利用像素加权的方法优化融合图像的拼接区域，得到优化的全景子图像P_N-1；将P_N-1与第N-2帧图像进行融合优化得到全景子图像P_N-2，以此类推，最后将P₂与第一帧图像融合优化得到全景子图像P₁，即为全景图像P_map。

4)点云和图像配准信息的融合感知：应用深度信息对步骤2)中C_g1坐标系空间中的所有点云数据进行分割，得到D个深度范围的点云数据，将不同深度范围的点云数据分别投影为二维图像，结合步骤3)生成的全景图像P_map对生成的图像进行图像增强处理，实现对环境中不同深度的全景视觉感知。

所述步骤2)中，点云数据配准具体实现过程包括：

1)对点云数据C_gN和C_gN-1去除无效点，得到点云数据C_vN和C_vN-1，对C_nN和C_nN-1进行下采样滤波。得到滤波后的点云数据C_fN和C_fN-1。

2)计算C_fN和C_fN-1的法向量n_N和n_N-1，利用n_N和n_N-1计算点云数据的点特征直方图Hist_N和Hist_N-1。通过Hist_N和Hist_N-1对C_fN和C_Fn--1进行SAC-IA粗配准，将C_vN变换成C_vN-1坐标系空间的粗配准点云数据C_vN ¹；

3)对C_vN-1和C_vN ¹进行ICP精配准，将C_vN ¹变换成C_vN-1坐标系空间的精配准点云数据C_vN ²。将C_vN ²和C_vN-1进行拼接得到拼接点云数据C_mN-1，计算公式为

点云数据配准的步骤1)中，对点云数据进行下采样滤波，首先根据点云数据创建一个最小三维体素栅格，然后计算出小立方体栅格边长L，将点云数据划分到m×n×k个小立方栅格中，同时删除不包含数据点的小栅格，在其他栅格中保留离栅格重心最近的数据点并删除其他数据点。其中，

L_x表示三维体素栅格X轴方向边长，L_y表示三维体素栅格Y轴方向边长，L_z表示三维体素栅格Z轴方向边长。a为比例因子，用来调节用来调节小立方体栅格的边长。s是比例系数，N表示点云数据中点的总数。L_x，L_y，L_z取值范围均为(0，5)，单位为m。

点云数据配准的步骤2)中，采用KdTree搜索点云数据法向量，点云数据法向量中求解协方差矩阵的特征向量

和特征值λ_j的公式为：

其中，H_C为协方差矩阵，pi是点云数据中一点，k是点pi邻近点的数目,

表示最近邻元素的三维质心，λ_j是协方差矩阵的第j个特征值，

是第j个特征向量。点云数据配准的步骤2)中，使用FPFH的方式来计算点特征直方图，搜索半径取值范围为(0，0.01)，单位为m，FPFH的计算公式为：

α＝v·n_t，

θ＝arctan(w·n_t,u·n_t)；

u＝n_s，v＝(p_t-p_s)×u，w＝u×v，

其中，FPFH(p)为p点的快速点特征直方图，p_s为查询点，p_t为p_s的k邻域内一个邻近点，w_t为p_s与p_t间的距离；SPFH(p)为p点的简化点特征直方图，通过统计p点与它的k邻域内每一点之间的α，φ，θ三个特征值求出；在Ouvw坐标系中求这三个特征值，n_s和n_t分别为p_s和p_t的估计法线，(x_s,y_s)为p_s的坐标，(x_t,y_t)为p_t的坐标；Ouvw是以O为原点，u、v、w为uvw轴的坐标系，设定p_s为O点，p_s的法向量为u轴，以过原点分别垂直于p_t-p_s和u轴的向量为v轴，p_t-p_s为p_s到p_t的向量，以过原点分别垂直于v轴和u轴的向量为w轴。

点云数据配准的步骤2)中，点云数据SAC-IA配准时，配准次数取值范围为(50，150)，单位为次。

点云数据配准的步骤3)中，点云数据ICP配准时，设定最大均方差误差阈值取值范围为(0，0.01)，单位为m，均方差误差公式为：

其中，d_k为均方差误差，

Q(∑_P，X)的最大特征值对应的特征向量即为最佳旋转向量R，Δ＝[A₂₃ A₃₁ A₁₂]^T，

I₃为3×3单位矩阵，tr(Σ_P,X)为矩阵Σ_P,X的迹。最佳平移向量T公式为T＝μ_X-Rμ_P，N_P,N_X分别为点云数据P与点云数据X中点的个数，p_i为点云数据P中的点，x_i为点云数据X中与p_i配准的点。

所述步骤3)中，点云数据对应的灰度图像配准具体实现过程包括：

1)读取TOF相机拍摄的灰度图像I_gN和I_gN-1；应用中值滤波法对I_gN和I_gN-1进行滤波得到I_fN和I_fN-1；

2)使用SURF算法分别对I_fN和I_fN-1进行特征提取，得到I_fN的特征点集K_N和描述符集D_N，得到I_fN-1的特征点集K_N-1和描述符集D_N-1；将D_N作为查询集，D_N-1作为训练集。使用Brute-Force匹配算法对D_N和D_N-1中的描述符进行欧几里德距离测试，K_N中任一特征点与K_N-1中所有特征点中欧几里德距离最近的一点作为K_N中该特征点在K_N-1中的最佳匹配点，K_N中任一特征点与K_N-1中所有特征点中欧几里德距离次近的一点作为K_N中该特征点在K_N-1中的次最佳匹配点。得到I_fN和I_fN-1特征点之间的M个匹配，使用比率测试的方法对所有匹配进行误匹配判定，通过判定结果消除M个匹配中的误匹配得到M’个匹配，通过设定最大匹配距离阈值消除M’匹配中匹配距离大于阈值的匹配，最终得到Q个匹配；

3)依据上一步骤中求出的Q个匹配生成I_fN和I_fN-1的单应性变换矩阵H，通过H对I_fN-1做透视变换投影到I_fN的图像坐标系中，得到I_fN-1的变换图像I_fN-1’。将I_fN与I_fN-1’进行拼接得到融合图像I_mN-1，使用像素加权的方法对I_mN-1的拼接区域进行裂缝消除和亮度平滑处理，生成全景子图像P_N-1。

点云数据对应的灰度图像配准的步骤2)中，使用SURF算法对图像进行特征提取时，Hessian矩阵的响应公式为：

其中，为尺度参数，(x,y)为像素点A的坐标，g(σ)是高斯-拉普拉斯算子中的高斯扩展函数，T_H为Hessian响应阈值，T_H的取值范围为(0，1000)。

点云数据对应的灰度图像配准的步骤2)中，其特征在于，比率测试公式为：

D＝{d₁,d₁＞a·d₂}；

其中，D为正确匹配，A(x₁,y₁)为K_N中的点，B(x₂,y₂)为A点在K_N-1中的最佳匹配点，C(x₃,y₃)为A点在K_N-1中的次最佳匹配点，d₁为最佳匹配距离，d₂为次最佳匹配距离，a为比例因子，a的取值范围为(0.5，5)。

点云数据对应的灰度图像配准的步骤2)中，其特征在于，消除距离值大于阈值的匹配公式为：

其中，D_o为优化匹配，d为阈值，d₁为最佳匹配距离，d_min为所有最佳匹配中距离值最小的最佳匹配距离。A(x_i,y_i)为K_N中的点，B(x_p,y_p)为A点在K_N-1中的最佳匹配点，d_i为A点到B点的欧几里得距离，b为比例因子，b的取值范围为(2，10)。

点云数据对应的灰度图像配准的步骤3)中，I_fN-1生成I_fN-1’时I_fN-1四个顶点坐标的变换公式为：

Q_i(x',y',z')＝V_i(x_i,y_i,z_i)×H_T,i＝1,2,3,4；

V_i'(x_i',y_i',1)＝Q_i(x',y',z')/z',i＝1,2,3,4；

x_min'＝min(x₁',x₂',x₃',x₄')，y_min'＝min(y₁',y₂',y₃',y₄')；

其中，V₁,V₂,V₃,V₄为I_fN-1的四个顶点，V₁’,V₂’,V₃’,V₄’为I_fN-1’的四个顶点，H_T为I_fN-1变换到I_fN-1’的单应性变换矩阵。

点云数据对应的灰度图像配准的步骤3)中，优化两帧图像拼接区域的公式为：

d(j)＝p(j)×s+t(j)×(1-s)；

其中，j点为I_fN和I_fN-1’在I_mN-1的重合点，x_j,y_j为I_mN-1拼接区域j点的x和y坐标，x_min为I_mN-1拼接区域左边界上x坐标最小值，w为拼接区域宽度。d(j)为P_N-1上j点的像素值，p(j)为I_fN上j点的像素值，t(j)为I_fN-1’上j点的像素值。

所述步骤4)中，点云和图像配准信息的融合感知具体实现过程包括：

1)以c为切割长度对C_vN ²和C_vN-1基于深度值切割，c的取值范围为(1，10)，单位为dm。分别得到X组深度范围中的点云数据集C_cN(X)和C_cN-1(X),将带有典型环境信息的点云数据C_cN ⁱ和C_cN-1 ⁱ投影变换生成灰度图像I_iN和I_{i N-1}，i为整数，取值范围为[1，X]。

2)对I_jN和I_jN-1进行二值化处理得到I_bN和I_bN-1，计算公式为

其中，

分别为I_jN和I_jN-1在i行j列像素点的灰度值，

分别为I_bN和I_bN-1在i行j列像素点的灰度值。将I_bN和I_bN-1进行形态学腐蚀运算得到前景图像I_front1和I_front2，计算公式为I＝XΘS。将I_bN和I_bN-1进行形态学膨胀运算得到I_middle1和I_middle2，计算公式为

将I_middle1和I_middle2进行尺度二值化得到背景图像I_back1和I_back2。将对应前景图像与背景图像相加得到标记图像I_mark1和I_mark2，计算公式为I_marki＝I_backi+I_fronti,i＝1,2，通过SURF算法对I_mark1和I_mark2进行融合，生成融合标记图像I_marks，对I_marks的拼接区域进行裂缝消除和亮度值平滑，得到优化标记图像I_markso，利用I_markso在全景子图像P_N-1上进行分水岭分割得到连通域图像I_wN-1。

3)对I_wN-1进行二值化处理得到环境信息掩膜图像I_rN-1，计算公式为

其中，G_ij为I_wN-1在i行j列像素点的灰度值，B_ij为I_rN-1在i行j列像素点的灰度值。将I_rN-1与全景子图像P_N-1进行与运算得到环境信息全景提取结果P_rN-1，运算公式为P_rN-1＝I_rN-1∧P_N-1。

点云和图像配准信息的融合感知的步骤1)中，分割点云数据的公式为：

其中，C_cut(i)为第i帧分割点云数据的点集，C_n为被切割点云数据的点集，z_Cn为C_n中点的z坐标值，(z_j,z_k)为第i帧点云数据的深度范围分割区间。

点云和图像配准信息的融合感知的步骤1)中，点云数据投影生成灰度图像公式为：

x_I＝x_C·fx/z_C+cx；

y_I＝y_C·fy/z_C+cy；

其中，(x_I,y_I)为二维点坐标，V(x_I,y_I)为二维点对应像素值，w为灰度图像宽度。(x_C,y_C,z_C)为三维点坐标，V(x_C,y_C,z_C)为三维点对应像素值。fx,fy为相机Y轴与X轴焦距，cx,cy为相机光心位置坐标。

点云和图像配准信息的融合感知的步骤2)中，对I_middle1和I_middle2进行尺度二值化的运算公式为：

其中，

分别为I_middle1和I_middle2在i行j列像素点的灰度值，

分别为I_back1和I_back2在i行j列像素点的灰度值。

与现有技术相比，本发明所具有的有益效果为：本发明通过将点云数据与质量相对较高的图像数据信息全景融合，可实现在复杂环境下，采集的点云数据存在较多缺陷时实现场景环境的精确重构；对场景环境信息重构是依据不同的深度范围分别进行的，得到的重构数据复杂度低，降低了后续场景理解、路径规划等工作的难度；采用的图像配准、点云配准算法及算法参数的选取经过了优化，在复杂环境下具有较强的鲁棒性；本发明对于需感知环境三维信息的应用场合具有广泛的适用性。

附图说明

图1为本发明一实施例方法流程图；图中，1、信息获取与点云数据配准，2、点云数据对应的灰度图像配准，3、点云和图像配准信息的融合感知；

图2为本发明待识别的两个场景的灰度图像；图2a)为待检测的第一帧图像，图2b)为水平方向相距8cm拍摄的待检测的第二帧图像；

图3为本发明图2两帧灰度图像对应的点云数据示意图；图3a)为待检测的第一帧点云数据，图3b)为待检测的第二帧点云数据；

图4为本发明图3两帧点云数据变换到一个基准坐标系空间的结果；

图5为本发明图2两帧图像的匹配结果；

图6为本发明图2两帧图像的融合结果；

图7为本发明在图6中提取的三组基于深度信息的全景环境信息结果，图7a)为距离相机镜头1m的环境信息掩膜图像，图7b)为距离1m的环境信息提取结果，图7c)为距离相机镜头0.8m的环境信息掩膜图像，图7d)为距离相机镜头0.8m的环境信息提取结果,图7e)为距离相机镜头0.5m的环境信息掩膜图像，图7f)为距离相机镜头0.5m的环境信息提取结果。

具体实施方式

如图1所示，本发明一实施例方法步骤为：

1)信息获取与点云数据配准：从TOF相机中提取待检测的第一帧点云数据C_g1与其对应的灰度图像I_g1，将拍摄位置水平方向移动距离为0-15cm或将拍摄位置旋转0-60°提取待检测的第二帧点云数据C_g2与其对应的灰度图像I_g2，按照上述方式经过N次拍摄得到N帧不同场景的点云数据和其对应的灰度图像。应用SAC-IA算法与ICP算法将N帧点云数据全部变换到第一帧点云数据C_g1的坐标系空间中；

2)点云数据对应的灰度图像配准：对步骤1)拍摄的第N帧图像与第N-1帧图像进行融合，生成两帧图像的融合图像，并利用像素加权的方法优化融合图像的拼接区域，得到优化的全景子图像P_N-1；将P_N-1与第N-2帧图像进行融合优化得到全景子图像P_N-2，以此类推，最后将P₂与第一帧图像融合优化得到全景子图像P₁，即为全景图像P_map。

3)点云和图像配准信息的融合感知：应用深度信息对步骤1)中C_g1坐标系空间中的所有点云数据进行分割，得到D个深度范围的点云数据，将不同深度范围的点云数据分别投影为二维图像，结合步骤2)生成的全景图像P_map对生成的图像进行图像增强处理，实现对环境中不同深度的全景视觉感知。

如图1所示，本发明方法感知全景环境信息过程如下，

图2是两个待识别的场景，由图2可知，该待识别图像背景较复杂，环境信息在图像中的显著性不强，且由于相机拍摄的位置与角度差异，环境信息出现在图像中的位置会有所差异，环境信息形状与尺度大小也会有所差异；并且在环境信息复杂的场景中，环境信息之间难以区分，现有二维图像处理方法无法识别场景中不同深度范围的环境信息。

本发明方法首先通过TOF相机读取水平方向相距8cm的两帧大小分别为214306和223298个三维点的待识别点云数据，参见附图3。对两帧点云数据进行下采样，采样体素大小为2×2×2，单位为m。接着，分别计算两帧点云数据的法线和点特征直方图，使用FPFH的方式计算点特征直方图时，搜索半径设定为5cm。进行SAC-IA点云数据粗配准时，Hessian响应阈值设定为10，配准次数为100次，通过粗配准矩阵将两帧矩阵粗变换到同一坐标系空间，粗配准矩阵为：

对两帧点云数据继续进行ICP精配准，精配准最大均方差误差阈值设定为10^(-6)m，精配准最大变换矩阵误差阈值设定为10^(-10)m，通过精配准矩阵将两帧点云数据精变换到同一坐标系空间，对两帧点云数据进行拼接显示在同一坐标系空间中，精配准矩阵为：

精配准后x轴配准误差为0.0468869mm，y轴配准误差为0.200064mm，z轴配准误差为0.465643mm，同一坐标系空间点云数据结果见附图4。

由TOF相机读入与上述两帧点云对应的灰度图像，对两帧图像进行中值滤波。用SURF算法分别对两帧图像进行特征提取，使用Brute-Force匹配算法对两帧图像的特征描述符进行匹配。得到两帧图像特征点之间的所有匹配，使用比率测试的方法去除这些匹配中的误匹配，通过设定最大匹配距离阈值消除剩余匹配中匹配距离大于阈值的匹配，比率测试中，比例因子a设定为3。去除距离值大于阈值的匹配中，比例因子b设定为5。最终得到优化后的匹配，参见附图5。依据优化后的匹配生成两帧图像的单应性变换矩阵H，H如下所示：

通过H将两帧图像变换到同一像素坐标系中，将两帧图像进行融合，再使用像素加权的方法对融合图像的拼接区域进行裂缝消除和亮度平滑处理，生成全景图像，参见附图6。

对精配准变换后的两帧点云数据基于深度值进行切割，切割单位长度为1.2dm。对三组包含典型环境信息的分割结果进行投影变换，得到不同深度范围中的典型环境信息图像。使用SURF算法对同组图像进行匹配，利用匹配结果融合同组图像，对融合后的二值图像消除裂缝得到优化的标记图像，利用标记图像在全景图像上进行分水岭分割，得到环境信息的连通域集合，对连通域图像进行二值化处理得到掩膜图像。将掩膜图像与全景图像进行与运算，得到三组深度范围的环境信息提取输出，参见附图7。由图7可知，本发明方法准确的实现了空间环境信息的识别与提取。

本发明通过将点云数据与质量相对较高的图像数据信息全景融合，可实现在复杂环境下，采集的点云数据存在较多缺陷时实现场景环境的精确重构；对场景环境信息重构是依据不同的深度范围分别进行的，得到的重构数据复杂度低，降低了后续场景理解、路径规划等工作的难度；采用的图像配准、点云配准算法及算法参数的选取经过了优化，在复杂环境下具有较强的鲁棒性；本发明对于需感知环境三维信息的应用场合具有广泛的适用性。

Claims (10)

1.一种基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，包括以下步骤：

1)从飞行时间相机中提取待检测的第一帧点云数据C_g1与其对应的灰度图像I_g1，将拍摄位置水平方向移动距离0-15cm或将拍摄位置旋转0-60°提取待检测的第二帧点云数据C_g2与其对应的灰度图像I_g2，按照上述方式经过N次拍摄得到N帧不同场景的点云数据和其对应的灰度图像；

2)应用SAC-IA算法与ICP算法将步骤1)中的N帧点云数据全部变换到第一帧点云数据C_g1的坐标系空间中；

3)对步骤1)拍摄的第N帧图像与第N-1帧图像进行融合，生成两帧图像的融合图像，并利用像素加权的方法优化融合图像的拼接区域，得到优化的全景子图像P_N-1；将P_N-1与第N-2帧图像进行融合优化得到全景子图像P_N-2，以此类推，最后将P₂与第一帧图像融合优化得到全景子图像P₁，即为全景图像P_map；

4)应用深度信息对步骤2)中C_g1坐标系空间中的所有点云数据进行分割，得到D个深度范围的点云数据，将不同深度范围的点云数据分别投影为二维图像，结合步骤3)生成的全景图像P_map对生成的图像进行图像增强处理，实现对环境中不同深度的全景视觉感知。

2.根据权利要求1所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，步骤2)的具体实现过程包括：

1)对点云数据C_gN和C_gN-1去除无效点，得到点云数据C_vN和C_vN-1，对C_nN和C_nN-1进行下采样滤波，得到滤波后的点云数据C_fN和C_fN-1；

2)计算C_fN和C_fN-1的法向量n_N和n_N-1，利用n_N和n_N-1计算点云数据的点特征直方图Hist_N和Hist_N-1；通过Hist_N和Hist_N-1对C_fN和C_Fn--1进行SAC-IA粗配准，将C_vN变换成C_vN-1坐标系空间的粗配准点云数据C_vN ¹；

3)对C_vN-1和C_vN ¹进行ICP精配准，将C_vN ¹变换成C_vN-1坐标系空间的精配准点云数据C_vN ²；将C_vN ²和C_vN-1进行拼接得到拼接点云数据C_mN-1，计算公式为

3.根据权利要求2所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，对C_nN和C_nN-1进行下采样滤波的具体实现过程包括：根据点云数据大小创建一个最小三维体素栅格，然后计算出小立方体栅格边长L，将点云数据划分到m×n×k个小立方栅格中，同时删除不包含数据点的小栅格，在其他栅格中保留离栅格重心最近的数据点并删除其他数据点；其中，

L_x表示三维体素栅格X轴方向边长，L_y表示三维体素栅格Y轴方向边长，L_z表示三维体素栅格Z轴方向边长；a为比例因子，用来调节用来调节小立方体栅格的边长；s是比例系数，N表示点云数据中点的总数；L_x，L_y，L_z取值范围均为(0，5)，单位为m。

4.根据权利要求2所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，采用KdTree搜索点云数据法向量，点云数据法向量中求解协方差矩阵的特征向量

和特征值λ_j的公式为：

其中，H_C为协方差矩阵，p_i是点云数据中一点，k是点p_i邻近点的数目,

表示最近邻元素的三维质心，λ_j是协方差矩阵的第j个特征值，

是第j个特征向量。

5.根据权利要求2所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，使用FPFH的方式来计算点特征直方图，搜索半径取值范围为(0，0.01)，单位为m，FPFH的计算公式为：

α＝v·n_t，

θ＝arctan(w·n_t,u·n_t)；u＝n_s，v＝(p_t-p_s)×u，w＝u×v，

其中，FPFH(p)为p点的快速点特征直方图，p_s为查询点，p_t为p_s的k邻域内一个邻近点，w_t为p_s与p_t间的距离；u,v,w构成p_s和p_t之间的局部坐标系；SPFH(p)为p点的简化点特征直方图，通过统计p点与它的k邻域内每一点之间的α，φ，θ三个特征值求出；在Ouvw坐标系中求这三个特征值，n_s和n_t分别为p_s和p_t的估计法线，(x_s,y_s)为p_s的坐标，(x_t,y_t)为p_t的坐标；Ouvw是以O为原点，u、v、w为uvw轴的坐标系，设定p_s为O点，p_s的法向量为u轴，以过原点分别垂直于p_t-p_s和u轴的向量为v轴，p_t-p_s为p_s到p_t的向量，以过原点分别垂直于v轴和u轴的向量为w轴。

6.根据权利要求2所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，精配准次数取值范围为(50，150)。

7.根据权利要求1所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，步骤3)的具体实现过程包括：

1)读取TOF相机拍摄的灰度图像I_gN和I_gN-1；应用中值滤波法对I_gN和I_gN-1进行滤波得到I_fN和I_fN-1；

2)使用SURF算法分别对I_fN和I_fN-1进行特征提取，得到I_fN的特征点集K_N和描述符集D_N，得到I_fN-1的特征点集K_N-1和描述符集D_N-1；将D_N作为查询集，D_N-1作为训练集；使用Brute-Force匹配算法对D_N和D_N-1中的描述符进行欧几里德距离测试，K_N中任一特征点与K_N-1中所有特征点中欧几里德距离最近的一点作为K_N中该特征点在K_N-1中的最佳匹配点，K_N中任一特征点与K_N-1中所有特征点中欧几里德距离次近的一点作为K_N中该特征点在K_N-1中的次最佳匹配点；得到I_fN和I_fN-1特征点之间的M个匹配，使用比率测试的方法对所有匹配进行误匹配判定，通过判定结果消除M个匹配中的误匹配得到M’个匹配，通过设定最大匹配距离阈值消除M’匹配中匹配距离大于阈值的匹配，最终得到Q个匹配；

3)依据上一步骤中求出的Q个匹配生成I_fN和I_fN-1的单应性变换矩阵H，通过H对I_fN-1做透视变换投影到I_fN的图像坐标系中，得到I_fN-1的变换图像I_fN-1’， 将I_fN与I_fN-1’进行拼接得到融合图像I_mN-1，使用像素加权的方法对I_mN-1的拼接区域进行裂缝消除和亮度平滑处理，生成全景子图像P_N-1。

8.根据权利要求7所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，使用SURF算法对图像进行特征提取时，比率测试公式为：D＝{d₁,d₁＞a·d₂}；

其中，D为正确匹配，A(x₁,y₁)为K_N中的点，B(x₂,y₂)为A点在K_N-1中的最佳匹配点，C(x₃,y₃)为A点在K_N-1中的次最佳匹配点，d₁为最佳匹配距离，d₂为次最佳匹配距离，a为比例因子，a的取值范围为(0.5，5)。

9.根据权利要求7所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，消除距离值大于阈值的匹配公式为：

其中，D_o为优化匹配，d为阈值，d₁为最佳匹配距离，d_min为所有最佳匹配中距离值最小的最佳匹配距离；A(x_i,y_i)为K_N中的点，B(x_p,y_p)为A点在K_N-1中的最佳匹配点，d_i为A点到B点的欧几里得距离，b为比例因子，b的取值范围为(2，10)。

10.根据权利要求1所述的基于二维图像和三维点云数据融合的全景环境感知方法，其特征在于，步骤4)具体实现过程包括：

1)以c为切割长度对C_vN ²和C_vN-1基于深度值切割，c的取值范围为(1，10)，单位为dm，分别得到X组深度范围中的点云数据集C_cN(X)和C_cN-1(X),将带有典型环境信息的点云数据C_cN ⁱ和C_cN-1 ⁱ投影变换生成灰度图像I_iN和I_iN-1，i为整数，取值范围为[1，X]；

2)对I_jN和I_jN-1进行二值化处理得到I_bN和I_bN-1，计算公式为

其中，

分别为I_jN和I_jN-1在i行j列像素点的灰度值，

分别为I_bN和I_bN-1在i行j列像素点的灰度值，将I_bN和I_bN-1进行形态学腐蚀运算得到前景图像I_front1和I_front2，计算公式为I＝XΘS，将I_bN和I_bN-1进行形态学膨胀运算得到I_middle1和I_middle2，计算公式为

将I_middle1和I_middle2进行尺度二值化得到背景图像I_back1和I_back2；将对应前景图像与背景图像相加得到标记图像I_mark1和I_mark2，计算公式为I_marki＝I_backi+I_fronti,i＝1,2，通过SURF算法对I_mark1和I_mark2进行融合，生成融合标记图像I_marks，对I_marks的拼接区域进行裂缝消除和亮度值平滑，得到优化标记图像I_markso，利用I_markso在全景子图像P_N-1上进行分水岭分割得到连通域图像I_wN-1；

3)对I_wN-1进行二值化处理得到环境信息掩膜图像I_rN-1，计算公式为

其中，G_ij为I_wN-1在i行j列像素点的灰度值，B_ij为I_rN-1在i行j列像素点的灰度值；将I_rN-1与全景子图像P_N-1进行与运算得到环境信息全景提取结果P_rN-1，运算公式为P_rN-1＝I_rN-1∧P_N-1。

Images