融合点云和图像的交互式三维测距算法的制作方法

1.本发明属于视觉定位领域，尤其是一种电网背景下的融合点云和图像的交互式三维测距算法。


背景技术：

2.为了对接近户外电网的危险移动物体提出警告，需要获取场景中物体的高度信息。在行业中，常使用人工的方式肉眼估计场景中移动物体的高度来检测威胁。但是，这种方式需要消耗大量的人力资源，且人工检测难以适应多场景下的预警工作。因此，实现场景中物体高度的自动检测成为经济有效的解决办法。
3.实现场景中的物体高度的自动检测，本质上是通过两个像素间的距离获取其在点云中的实际距离。其实现思路有以下两种：第一种方法是基于深度估计来实现高度检测。其本质是估计二维图片像素的深度信息，直接获取其三维坐标来计算真实距离。但是，这种思路容易受到场景遮挡物的影响。第二种方法则是基于像素-三维点密集对应的方法。这种方法同时需要图片和预先建立的场景点云，通过获取图片和点云之间的投影关系对应来获取像素的三维信息来计算高度。这种方法由于使用了预先建立的点云，所以可以规避场景遮挡物的问题。因此，本文解决三维测距问题的方法是通过视觉定位与相机投影获取从点到像素的密集对应。目前，在尚未找到同时使用图像和场景点云实现场景中物体高度测量的方法和系统。
4.结合上述描述的解决方案，存在如下缺点和不足：
5.1)由于利用的是已经架设好的单目摄像头，其内参数难以获取。
6.2)计算点云到图像的投影时，由于点云的稀疏性、误差、舍入等问题，会出现不存在对应3d点的像素(成为空像素)，对测距过程造成影响。


技术实现要素：

7.为了解决前述问题：难以确定内参数；空像素。本发明提供了一种点云和图像融合的交互式三维测距流程和方法，解决上述存在的几个问题。
8.技术方案：
9.本发明首先公开了一种融合点云和图像的交互式三维测距算法，它基于激光雷达扫描获得的三维点云数据和拍摄对应的二维图像数据实现三维测距，具体包括以下步骤：
10.s1、电线数据分割标注，二维-三维点对数据匹配标注，获得带标注的场景数据；
11.s2、初始位姿计算；
12.s3、判断初始位姿是否满足要求，即步骤s1中的二维-三维点对是否已经重合，是则进行s4；否则进行位姿手动对齐后进行s4；
13.s4、点云像素预处理；
14.s5、三维测距。
15.优选的，所述s1中，电线数据分割标注具体包括以下步骤：
16.s1-1-1、导入三维点云数据至软件cloudcompare，标注点云数据的电线数据区域；
17.s1-1-2、分割标注的电线数据；
18.s1-1-3、判断是否全部标注，是则进行s1-1-4，否则返回s1-1-2；
19.s1-1-4、电线数据添加label；
20.s1-1-5、导出电线点云数据；
21.s1-1-6、合并电线与三维点云数据获得带标注的场景数据。
22.优选的，所述s1中，二维-三维点对数据匹配标注具体包括：
23.s1-2-1、选取与带标注的场景数据对应的二维图像数据；
24.s1-2-2、对二维图像数据中关键二维点标注；对带标注的场景数据中的对应点对照标注；
25.s1-2-3、判断是否全部标注，是则进行s1-2-4，否则返回步骤s1-2-2；
26.s1-2-4、将标注好二维-三维点对数据保存。
27.优选的，所述s2中初始位姿计算采用优化算法，具体包括：
28.s2-1、根据全局搜索求解相机内参，得到相机内参初始值为[α、β、γ、u0、v0、k1、k2、k3、p1、p2]，结合pnp方法求解相机外参旋转矩阵r和平移矩阵t，根据得到的内参初始值，随机生成d个不同的粒子，并初始化粒子的位置和速度；
[0029]
s2-2、计算每个粒子的适应度值，并将每个粒子的当前位置作为个体最优位置，将其对应的适应度值作为当前粒子的个体最优值；同时，对所有粒子的适应度值取最小值，作为全局最优值，并将其对应的粒子位置作为全局最优位置；
[0030]
s2-3、将每个粒子当前的适应度值与其个体最优值进行比较，若小于个体最优值，则将当前适应度值和对应的粒子位置作为该粒子更新后的个体最优值；计算所有粒子个体最优值的最小值，并与全局最优值进行比较，若小于全局最优值，则更新全局最优值；
[0031]
s2-4、判断算法是否达到迭代完成条件，若未完成，返回执行；若完成，算法执行结束，输出结果，将结果保存为位姿文件。
[0032]
优选的，所述s3中，位姿手动对齐的步骤为：
[0033]
s3-1、根据计算得到的相机外参和内参，准备参数的文件；同时，准备带标注的场景数据文件与背景图片文件；
[0034]
s3-2、程序读去相机内参，外参，带标注的场景数据和背景图片，在可视化窗口中显示带标注的场景数据作为前景、图片作为背景；
[0035]
s3-3、用户通过鼠标和键盘交互式的调整带标注的场景数据，直到带标注的场景数据和作为背景图片重合；
[0036]
s3-4、当带标注的场景数据和背景图片重合后，输出mvp矩阵，替换步骤s2生成的位姿文件。
[0037]
优选的，所述s4中，点云像素预处理具体包括以下步骤：
[0038]
s4-1、读取带标注的场景数据，位姿和图像分辨率信息文件；其中带标注的场景数据是通过步骤s1-1获得，位姿文件通过步骤s2和s3获得，图像分辨率是二维图像的自带信息；
[0039]
s4-2、计算像素与点云的对应匹配；
[0040]
s4-2-1、投影匹配：将点云根据位姿信息投影到图像上，对每个点云投影到的像素
点，将该像素点的点云匹配信息同样赋值给周围临近像素点，扩充匹配范围；
[0041]
s4-2-2、按行填充插值：按行遍历图像每个像素点，对于每一行像素，当像素之间无对应像素小于阈值，则插值填充；插值方法使用两端非空像素对应的点云密集采样，再将采样投影到无对应图像像素上，完成插值；
[0042]
s4-2-3、按列填充插值：按列遍历图像每个像素点，对于每一列像素，当像素之间无对应像素小于阈值，则插值填充；插值方法使用两端非空像素对应的点云密集采样，再将采样投影到无对应图像像素上，完成插值；
[0043]
s4-3、输出像素与点云的匹配对应关系文件到指定文件夹。
[0044]
优选的，所述s5中，三维测距的具体步骤包括：
[0045]
s5-1、计算物体左部距离电网的最短距离；
[0046]
s5-1-1、获取左下角像素对应的三维点坐标，然后增加该三维坐标的z轴直到增加后的三维坐标的投影像素坐标的垂直坐标超过左上角的垂直坐标；该步骤最终获取物体的高度和其左最高点的坐标p1；
[0047]
s5-1-2、遍历line.dat中的所有点，计算其与p1的欧式距离，取欧式距离的最小值d1作为物体左部距离电网的最短距离；
[0048]
s5-2、计算物体右部距离电网的最短距离；
[0049]
s5-1-1、获取右下角像素对应的三维点坐标，然后增加该三维坐标的z轴直到增加后的三维坐标的投影像素坐标的垂直坐标超过右上角的垂直坐标；该步骤最终获取物体的高度和其右最高点的坐标p2；
[0050]
s5-1-2、遍历line.dat中的所有点，计算其与p2的欧式距离，取欧式距离的最小值d2作为物体右部距离电网的最短距离；
[0051]
s5-3、比较d1和d2的大小，如果d1小于d2，则返回物体左侧的距离作为物体到电网的最短距离；如果d2小于d1，则返回物体右侧的距离作为物体到电网的最短距离。
[0052]
本发明还公开了一种融合点云和图像的交互式三维测距系统，用于实现本发明所述的方法，它包括：
[0053]-标注子模块：用于电线数据分割标注，二维-三维点对数据匹配标注；
[0054]-初始位姿计算子模块：用于计算初始位姿；
[0055]-初始位姿准确性判断子模块：用于判断初始位姿是否准确；
[0056]-手动对齐子模块：用于交互式矫正位姿；
[0057]-点云像素对应预处理子模块：用于实现像素与点云的对应匹配，以及像素插值避免采样投影无对应图像像素；
[0058]-三维测距网络接口子模块：实现数据的传输及测距计算。
[0059]
本发明的有益效果
[0060]
本发明使用点云和图像融合的方法实现了三维测距。
[0061]
本发明通过pso过程同时优化相机内参和相机外参。通过使得投影误差尽可能小的方式，迭代地优化相机内参和相机外参，最终得到一个使得点云投影尽可能接近原本图像的相机内参和相机外参。
[0062]
对于空像素等问题，本项目采用插值算法填充由点云的稀疏性产生的空白(忽略由未建模对象和天空产生的空像素)。
附图说明
[0063]
图1为本发明的流程示意图
[0064]
图2为分割出的电线点云数据示意图
[0065]
图3为分割后的点云场景数据示意图
[0066]
图4为电线点云数据标注流程图
[0067]
图5为带标注的场景数据示意图
[0068]
图6为2d-3d点对匹配标注流程图
[0069]
图7为坐标系变换示意图
[0070]
图8为位姿手动对齐示意图
[0071]
图9为点云像素对应模块流程图
具体实施方式
[0072]
下面结合实施例对本发明作进一步说明，但本发明的保护范围不限于此：
[0073]
结合图1，本发明的关键流程如下：
[0074]
1电线数据分割标注及2d-3d点对匹配标注
[0075]
1.1框选并分割标注电线点云数据
[0076]
在电线点云数据的标注子模块，首先导入数据至软件cloudcompare，然后框选标注区域，即点云数据的电线区域。点击选中电线数据，绿色框选中电线数据区域，并旋转、移动依次框选。选中电线点云数据区域后，分割出标注的电线点云数据区域，保存电线数据，依次框选后并保存。分割出的电线点云数据和点云场景数据效果如图2和图3所示。
[0077]
1.2电线点云数据添加标签及保存
[0078]
首先点击选中电线点云数据，然后为电线点云数据添加label，此处定义电线点云对象对应的label id为5，故填入5。全部完成后进行保存预处理操作，将处理后的点云数据(电线点云文件)再次导入软件，不同的点云数据则所带属性列数会不一样，而此处只需要坐标[x,y,z]三列和标注的[label]列，对于其他点云数据所带的属性列选择“忽略”。最终，导出电线点云数据，保存为txt格式文件用于后续的距离计算模块。此时，文件中每一行代表一个点数据，最后一列label为5表示该点为电线数据，数据格式只有4列为[x,y,z,label]。整体标注流程如图4所示，效果图如图5所示。
[0079]
1.3 2d-3d点对数据匹配标注
[0080]
将与点云场景对应的图像数据放入子模块的image文件夹，可以同时放入多张图像文件，在系统界面中标注点像素坐标，系统界面右侧和下侧点击滑动，依次标注2d点。标注时寻找关键点，如路牌顶点、路牌角点、电线杆顶点和房屋顶点等，且标注点尽量分布于不同区域。按空格键可进入下一张图像文件，依次标注完全部的点对应。完成后的2d-3d点对数据存入txt文件中，文件中的每一行代表着一个2d-3d点对匹配的数据，且2d点和3d点之间以分号分隔。整体标注流程如图6所示。
[0081]
2基于改进优化算法的初始位姿计算
[0082]
2.1坐标系变换及相机成像
[0083]
世界坐标系中的一个点，在相机坐标系中的点坐标为(x_c,y_c,z_c)，投影到图像坐标系中的点坐标为(x,y)，转换到像素坐标系中的坐标值为(u,v)。则结合图7，定义像素
坐标系到世界坐标系的映射关系如下式：
[0084][0085]
在理想成像模式下可以得到:
[0086][0087]
式中：r,t分别为旋转矩阵和平移向量；s为尺度因子；μ，v为二维像平面像素坐标；α,β分别为x，y轴方向上的等效焦距；μ0，v0为光轴与图像平面的交点坐标；参数γ是x，y像素坐标轴之间夹角的扭曲系数。
[0088]
实际中，相机制造、安装等过程中会造成各种各样的误差，导致相机通常会存在多种非线性畸变，应该考虑加入相机的非线性畸变因素。相机镜头畸变主要可以分为径向畸变和切向畸变，则考虑加入径向畸变和切向畸变的影响后的相机成像表达式为：
[0089][0090]
由上可以看出，相机模型的建立，共涉及了8个内部参数以及2个外部参数，是一个多参数优化问题。
[0091]
2.2改进的粒子群算法
[0092]
粒子群算法pso本质是一种全局搜索算法，对非线性、多目标等优化问题具有较强的全局搜索能力。pso初始化为一群随机粒子，然后通过迭代找到最优解，在每次迭代期间，粒子通过跟踪两个“极值”来更新自身。一种称为粒子个体极值，是自身找到的最优解；另一种称为粒子全局极值，是全局粒子群的最优解。
[0093]
在pso算法的基础上，根据算法运行时粒子目标值的分散程度来调整惯性权重，将惯性权重系数设置为动态惯性权重系数，如下式所示。
[0094][0095]
一般适应度函数由目标函数变换而来，为了使所获得的相机参数能够尽可能接近拍摄时候的实际参数。也就是说，通过理论模型计算得到的投影点坐标，跟实际投影点坐标尽可能重合，最终建立目标函数如下：
[0096][0097]
其中，(ui，vi)是由世界坐标点通过相机成像模型计算得到；(ui，vi)为实际投影点的像素坐标，θ＝(f
x
，fy，uo，r0，k1，k2，k3，p2，p1)
t
。
[0098]
在pso算法中，学习因子决定着本身经验和群体经验对粒子运动轨迹的影响，反映了粒子间的信息交流。考虑单个粒子当前状态与群体全局最优位置间的距离，则距离表达式如下：
[0099][0100]
当单个粒子当前状态与种群全局最优位置间的距离较小时，那么种群全局最优位置对该粒子的当前状态影响较小；当单个粒子与全局最优位置的距离较大时，其影响也较大，此时全局学习因子应该取较大值。对于全局学习因子，设定为根据下式进行自主调整:
[0101][0102]
2.3初始位姿计算
[0103]
(1)根据全局搜索求解相机内参，得到相机内参初始值为[α β γ u
0 v
0 k
1 k
2 k
3 p
1 p2]，并结合pnp方法求解相机外参旋转矩阵r和平移矩阵t。根据得到的内参初始值，随机生成d个不同的粒子，并初始化粒子的位置和速度
[0104]
(2)计算每个粒子的适应度值，并将每个粒子的当前位置作为个体最优位置，将其对应的适应度值作为当前粒子的个体最优值。同时，对所有粒子的适应度值取最小值，作为全局最优值，并将其对应的粒子位置作为全局最优位置。
[0105]
(3)将每个粒子当前的适应度值与其个体最优值进行比较，若小于个体最优值，则将当前适应度值和对应的粒子位置作为该粒子更新后的个体最优值。计算所有粒子个体最优值的最小值，并与全局最优值进行比较，若小于全局最优值，则更新全局最优值。
[0106]
(4)判断算法是否达到迭代完成条件，若未完成，返回执行。若完成，算法执行结束，输出结果。
[0107]
设置粒子种群数量d＝600，迭代次数设置为300次，将搜索求解出的内参初始值作为基准值，并为粒子的各个维度设置浮动区间，进而确定粒子位置的取值区间。
[0108]
改进后的粒子群算法优化后的适应度值较低，结果更优，同时，算法的收敛速度较快。
[0109]
算法迭代300次之后，得到优化后的内参结果，将点云数据文件放入投影验证子模块的程序中，得到相机的重投影效果图(通过和图片对比验证相机外参的效果)。
[0110]
2位姿手动对齐模块
[0111]
算法计算的相机外参有时会不够准确，这时，可以通过位姿手动对齐模块交互式地校正位姿。
[0112]
其流程如下：
[0113]
1.1)根据之前模块计算得到的相机外参和内参，准备参数的文件。同时，也准备场景点云文件与背景图片文件。
[0114]
1.2)程序读去相机内参，外参，场景点云和背景图片，在可视化窗口中显示点云作为前景、图片作为背景。
[0115]
1.3)用户可以通过鼠标和键盘交互式的调整点云，直到点云和作为背景图片重合。
[0116]
1.4)当点云和背景图片重合后，本模块输出mvp矩阵。
[0117]
其可视部分如图8所示。
[0118]
3点云像素对应预处理模块部分
[0119]
结合图9，包括如下步骤：
[0120]
1.1)读取点云，位姿和图像分辨率信息文件。
[0121]
1.2)计算像素与点云的对应匹配。
[0122]
1.2.1)投影匹配：将点云根据位姿信息投影到图像上，对每个点云投影到的像素点，将该像素点的点云匹配信息同样赋值给周围临近像素点，扩充匹配范围。
[0123]
1.2.2)按行填充插值：按行遍历图像每个像素点，对于每一行像素，当像素之间无对应像素小于阈值(可设置为100像素)，则插值填充。插值方法使用两端非空像素对应的点云密集采样，再将采样投影到无对应图像像素上，完成插值。
[0124]
1.2.3)按列填充插值：按列遍历图像每个像素点，对于每一列像素，当像素之间无对应像素小于阈值(可设置为100像素)，则插值填充。插值方法使用两端非空像素对应的点云密集采样，再将采样投影到无对应图像像素上，完成插值。
[0125]
1.3)输出像素与点云的匹配对应关系文件到指定文件夹。
[0126]
4三维测距网络接口模块
[0127]
包括如下部分：
[0128]
1.1)从指定文件夹中获取所有通道场景信息，并读取所有通道的点对应文件(2d3dpairs.dat)、电网点云文件(line.dat)、mvp或rt矩阵文件(pose.dat)和场景设置设置文件(setting.json)。
[0129]
1.2)启动网络监听
[0130]
1.3)接收到客户端的请求json之后，开始执行测距程序：
[0131]
1.3)1.对于目标检测框的左下角，执行如下步骤：
[0132]
1.3)1.1.执行高度检测方法，其原理是获取左下角像素对应的3d点坐标，然后增加该3d坐标的z轴直到增加后的3d坐标的投影像素坐标的垂直坐标超过左上角的垂直坐标。该步骤最终获取物体的高度和其左最高点的坐标p1。
[0133]
1.3)1.2.遍历line.dat中的所有点，计算其与p1的欧式距离，取欧式距离的最小值d1作为物体左部距离电网的最短距离。
[0134]
1.3)2.对于目标检测框的右下角，执行与左下角类似的操作
[0135]
1.3)2.1.执行高度检测方法，其原理是获取左下角像素对应的3d点坐标，然后增加该3d坐标的z轴直到增加后的3d坐标的投影像素坐标的垂直坐标超过左上角的垂直坐标。该步骤最终获取物体的高度和其左最高点的坐标p2。
[0136]
1.3)2.2.遍历line.dat中的所有点，计算其与p2的欧式距离，取欧式距离的最小值d2作为物体右部距离电网的最短距离。
[0137]
1.3)3.比较d1和d2的大小，如果d1小于d2，则返回物体左侧的距离作为物体到电
网的最短距离。如果d2小于d1，则返回物体右侧的距离作为物体到电网的最短距离。
[0138]
描述：由激光雷达扫描获得的杆塔等3d点云数据和拍摄对应的2d图像数据，输入到基于cloudcompare的电线点云与2d-3d点对的数据标注模块。首先框选点云数据的电线区域，然后将其分割出来标注并添加标签后保存，同时利用opencv在和3d点云对应的2d图像文件上依次标注2d点坐标，并将2d-3d点对数据存入txt。
[0139]
将标注后的2d-3d点对匹配数据，输入到基于改进优化算法的初始位姿计算模块。首先根据坐标系变换及相机成像的映射关系，利用2d-3d点对数据确定相机内参的大致范围，然后初始化粒子的位置和速度，并利用改进的粒子群算法迭代优化输出初始位姿信息，即相机的9个内部参数与2个外部参数，同时利用投影程序来重投影并验证位姿效果。之后，如果位姿效果不足够好，则可以通过位姿手动校正模块交互式地校正位姿。根据得到的位姿，遍历点云建立投影匹配，并对无对应的像素点插值对应，最后输出匹配信息。而三维测距网络接口模块接收这些信息，在收到远程客户端请求时，计算障碍物的高度和其到电网的最近距离，并返回。
[0140]
本提案的三维测距算法方案，实现了几个方面的突破：
[0141]
1)设计并实现了通过通过改进粒子群算法优化相机内参和外参的技术。
[0142]
2)设计并实现了使用点云和图像融合的方法实现三维测距的技术。
[0143]
本文中所描述的具体实施例仅仅是对本发明精神做举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。