技术领域
[0001] 本
发明涉及无人驾驶技术领域，尤其涉及一种基于ROS和Gazebo的旋转相机特征匹配算法。
背景技术
[0002] 环境
感知技术是智能无人驾驶车辆发展的前提。在环境感知领域，有许多技术手段来获取信息，例如毫米波雷达、
激光雷达、GPS和惯导、
超声波雷达和摄像头等等。在一般情况下，毫米波雷达和
超声波雷达获取定向目标距离信息；激光雷达获取大部分的三维环境感知信息；GPS和惯性导航获取
车身位置和
姿态信息；摄像头获取二维图像信息或者环境的深度信息。对比来讲，毫米波雷达适用于较短距离的目标
定位或者测距，GPS容易被大型障碍物遮挡造成信息缺失和断点，激光雷达造价过于昂贵。与这些技术手段相比较，摄像头价格更加亲民，拥有单目和多目等多种搭配方式可选，并且可以同时获取环境
颜色和深度信息，更好地支持人为调节。但基于相机的环境感知技术在控制相机旋转和特征点的匹配
精度上存在很大程度上的
缺陷。相机的旋转速度、图像的传输
帧率、信息过滤和
节点调用的速度、特征点匹配的精度等都是亟待解决的问题。
发明内容
[0003] 本发明旨在提供一种基于ROS和Gazebo的旋转相机特征匹配算法，很好的解决了上述问题，本发明搭建了ROS和Gazebo的仿真环境，通过程序来控制虚拟的相机旋转和获取环境信息的速率，进行特征点的匹配与双重优化，实现外点的滤除从而提高了匹配精度。
[0004] 本发明的技术方案是一种基于ROS和Gazebo的旋转相机特征匹配算法，发明中首先结合了
机器人操作系统ROS(Robot Operating System)和Gazebo环境进行了搭建了仿真平台。之后进行了控制和
优化算法的设计，算法中需要调用并订阅节点信息，实现目标图像的实时传输，进行信息过滤控制图像的传输帧率，同时进行特征点的检测、提取与匹配，在获取匹配好的特征点对之后，通过双重算法过滤外点从而达到提高匹配精度的目的。
[0005] 具体包括步骤：
[0006] a、基于
机器人操作系统ROS和Gazebo搭建仿真平台；
[0007] 所述仿真平台包括相机旋转平台、分别放置在相机旋转平台两侧的左相机和右相机、与左右相机连线垂直的中心线方向上的目标物体、周围环境散落的障碍物体。此仿真平台的主要作用在于
支撑所设计的算法实现相机的旋转控制，同时在两相机中心线方向上放置目标物体，当两相机法线与中心线的夹
角相同时，两相机的输出图像特征点匹配度应该是最高；当右相机静止不动，左相机以一定速度进行360°旋转时，会输入不同障碍物的图像信息，并进行特征匹配，此时输出的匹配率应该低于两相机处于中心线对称位置时的匹配率。
[0008] b、进行相机旋转的控制算法设计；
[0009] b1、初始化位置节点，等待set_link_state节点服务信息；
[0010] b2、定义节点服务信息并获取左相机的link位置，设置左相机的旋转速度为0.1；
[0011] b3、订阅节点位置信息，将原始位置信息的四元数组转换成欧拉角，同时进行左相机
连接线与法线夹角的计算，输出角度；
[0012] b4、使用信息
过滤器message_filters订阅左、右两侧相机的图像信息，并将其合成到一张图像的通道上进行输出。
[0013] c、进行特征点检测、提取、匹配的算法设计；
[0014] c1、将步骤b4中输出的图像信息进行格式转换，从BGR彩色图像改变成GRAY格式的灰度图像；
[0015] c2、获取图像尺寸信息，并定位法线方向上的感兴趣区域ROI的位置信息，在源图像上进行矩形框rectangle方式的
可视化，并创建ROI区域的图像信息对象；
[0016] c3、初始化SIFT特征检测器，创建检测器对象，进行ROI区域的关键点检测和描述符的计算；
[0017] c4、因为会出现“移位”现象，所以特征点会直接显示在图像的左上角，为了将特征点显示在图像原始的位置上，需要遍历感兴趣区域ROI的所有检测到的特征点，将其移动到图像法线ROI区域；
[0018] c5、创建并初始化暴
力匹配器对象，将上述SIFT特征检测到的特征点进行匹配，在匹配过程中设置右相机的一个特征点最多可以匹配左相机的两个特征点。
[0019] d、进行匹配的优化设计；
[0020] d1、创建一个掩膜，用来存储正确的匹配点；在步骤c5中时暴力匹配器的初始化阶段，设计右相机一个特征点可以匹配左侧旋转相机的两个特征点；在已经匹配的特征点对中进行比较，若与右相机同一个特征点匹配的左相机的两个特征点1、2之间的比值低于所设定的
阈值，则将特征点1加入到创建的掩膜中。
[0021] 此时掩膜中的匹配点是已经经过一次过滤之后的特征点，如果将这个特征点与左相机的特征点进行匹配和显示，通过可视化结果可以看出匹配的效果被大大地提高了。
[0022] d2、设置一定的阈值，当上述过程中所设定的掩膜长度大于这个阈值，进行如下的操作：获取源图像和目标图像的关键点坐标，应用OpenCV2里面的findHomography函数对两图像的坐标点进行仿射变换矩阵M的计算，同时设置特征点过滤优化算法，以及阈值，超过阈值的则被认定为外点，得到新的优化掩膜；
[0023] 获取源图像的高和宽，通过OpenCV2里的perspectiveTransform函数对源图像的四个角进行变换，获得在目标图像上对应的坐标，并在目标图像上进行显示。
[0024] e、最后进行信息的输出以及可视化。
[0025] e1、判断一次优化的掩膜尺寸是否大于设定的阈值，大于阈值则进行二次优化，输出源图像及目标图像的特征点数量信息；
[0026] e2、通过二次掩膜输出匹配点的数量；
[0027] e3、实时可视化匹配图像，包括左相机视图，右相机视图，左相机匹配特征点，右相机匹配特征点，匹配连线等；输出两张图像的匹配率，匹配率由掩膜的尺寸即匹配上的特征点数量和右相机源图像的特征点数量相比得出；
[0028] e4、如果一次掩膜的尺寸小于设定的阈值，则在输出两相机图像的特征点数量，一次优化过得匹配特征点数量之后，用一次掩膜进行可视化输出。
[0029] 本发明的有益效果是：
[0030] (1)在本发明的平台搭建阶段，创新性地使用机器人操作系统ROS和Gazebo仿真环境实现了双目相机的特征匹配平台的搭建，方便于控制相机进行旋转以及图像信息的订阅和可视化；
[0031] (2)在算法设计阶段，通过Python语句定义了服务类型的节点信息，同时控制左相机进行旋转，设置了旋转速度；同时订阅左右两侧相机的图像信息，并合成到一个通道进行可视化输出，创新性地实现了相机的自动旋转和图像实时输出的控制；
[0032] (3)在算法设计阶段，为了在相机旋转过程中实时的传输图像并克服图像尺度和旋转变换带来的匹配误差，通过Python语句应用尺度不变特征变换SIFT算法以及暴力匹配器BF实现了左右两侧相机图像的特征点检测、提取以及描述符的计算；
[0033] (4)在算法设计阶段，为了更优地表现两相机图像的特征匹配性能，设计了二次优化，第一次优化通过设定匹配点数量和阈值的方式来过滤掉一部分的外点，当第一次优化的掩膜尺寸足够大时，也就是匹配的特征点足够多时，使用RANSAC算法进行二次优化，得到二次优化的掩膜，达到更高的匹配精度；
[0034] (5)在可视化表达和最后结果的输出上，不管一次优化的匹配数量是否足够多，都将输出两相机的特征点检测数量，匹配数量以及可视化地实时输出两幅图像和相应匹配特征点的方向和大小，同时输出相机与法线之间的夹角。
附图说明
[0035] 图1为本发明基于ROS和Gazebo的仿真平台搭建的两个示意图；
[0036] 图2为本发明仿真平台内各部分的关系图；
[0038] 图4为本发明中特征检测、提取、匹配的优化流程图；
[0039] 图5为本发明两相机
位姿相对于中心线对称时的输出结果可视化图示；
[0040] 图6为本发明在左相机旋转期间法线方向为障碍物的特征匹配可视化输出。
[0041] 图中：1、相机旋转平台；2、右相机；3、左相机。
具体实施方式
[0042] 为使本发明
实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
[0043] 因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的
选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0044] 应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
[0045] 在本发明的描述中，需要说明的是，术语“上”、“下”、“内”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0046] 此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
[0047] 在本发明的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
[0048] 如图1所示，本发明提供了一种基于ROS和Gazebo的双目相机旋转控制和特征匹配的优化算法。如图1所示是基于机器人操作系统ROS和Gazebo环境建立的仿真环境。支撑两旋转相机的相机旋转平台1，尺寸为宽0.4m，长为0.8m，高为0.4m，放置在整个仿真环境的原点(0,0,0)位置上。相机的
坐标系原点设置为整个平台的质心位置，右相机2针对于相机坐标系的起始坐标为x＝0.2m，y＝-0.2m，z＝0.225m，roll＝0，pitch＝0，yaw＝0.0666rad；左相机3的起始坐标为x＝0.2m，y＝0.2m，z＝0.4m，roll＝0，pitch＝0，yaw＝-0.0666rad。在两相机基线的中心线延长线上，放置了一个球形目标物体，此目标物体坐标位置为x＝3m，y＝0m，z＝0.5m，roll＝0，pitch＝0，yaw＝0。此时两相机处于起始位置，状态静止，两相机的基线长度为0.4m，两相机基线与各自的法线夹角为86.1869°，弧度为1.504228，此时目标物体正好出现在两相机法线方向上的感兴趣区域ROI上，如图2所示。除了中心线上的目标物体外，为了验证相机在旋转过程中的匹配精度，在Gazebo环境周围放置了四个散落的障碍物体：两个正方体，一个圆柱体以及一个球体。将正方体1号放置在目标物体后侧，将正方体2号和球体仿照目标物体与正方体1号的摆放位置放置在右相机的后方，目的是增大障碍物与目标物体的匹配程度，验证算法的精准度，将剩下的圆柱体放置在目标物体的左侧。
[0049] 图3所示是整体的算法流程图，搭建好仿真环境之后，需要编写代码获取环境中两相机节点的相关信息，如图像信息，角度信息，坐标信息等，同时设置速度控制左相机旋转。为了使得红色左相机按照给定的速率进行旋转，需要通过ROS系统订阅左相机的节点信息，并发布这个话题。在控制面板中找到最初定义的左相机节点名称“/::camera_left”，设置对象并给予一个初始速率0.1，使其自动进行旋转。
[0050] 在相机可以按照给定的速率进行自动旋转之后，需要订阅两个相机的图像信息，获取图像的输入。首先需要初始化之后要订阅的节点信息，订阅左相机的位置信息，并通过函数将原本的四元数组转换为欧拉角度，进行换算，控制输出两相机法线与基线之间的夹角。之后通过消息过滤器订阅左右两侧相机的图像类型信息，并通过消息过滤器中的ApproximateTimeSynchronizer函数将两相机的图像信息合成为同一通道输出，方便之后进行特征点匹配的可视化。
[0051] 得到了左右两侧相机的图像输入之后，需要先进行预处理。分别将两幅图像的数据信息从ROS类型的图像数据转换成OpenCV类型的图像数据，使用OpenCV语言将其两相机的图像数据从BGR彩色图转换成GRAY灰度图。获取两幅源图像的尺寸信息，计算得到相机感兴趣区域的位置信息。
[0052] 使用尺度不变特征变换SIFT(Scale-invariant feature transform)进行源图像的特征点检测和提取，计算相关的描述符。SIFT会先进行尺度空间的检测，定位特征点的位置，之后会得到旋转参数，最后进行描述符的计算。由于“移位”现象的存在，检测到的法线感兴趣区域特征点会出现在源图像的左上角，为了将特征点显示坐标位置，需要遍历所有的特征点，使其在现有坐标的
基础之上增加最初感兴趣区域ROI的坐标，才可以正确显示在源图像上。
[0053] 在特征点的检测、提取与描述符计算完成之后，需要进行特征点的匹配。首先需要初始化匹配器对象，选择暴力匹配器BF(Brute-Force Matcher)进行特征点的匹配。暴力匹配器一般包含了两种匹配方法，BFMatcher.match()和BFMatcher.knnMatch()。其区别在于第一个方法会返回最佳匹配，第二个方法会为每个关键点返回k个最佳匹配，将参数k设置为2，目的是使右相机图像的一个特征点与左相机的两个特征点进行匹配。
[0054] 完成匹配之后开始进行优化。在上一步匹配的过程中参数k设置为2，因此每个特征点会存在多可匹配的对象，造成了匹配精度的下降。为了提高两图像之间特征点的匹配精度，需要创建一次掩膜，并设定阈值条件。比较和右相机特征点相匹配的左相机图像中的两个特征点，当点1与点2的比值小于设定的阈值时，将点1纳入一次掩膜之中。阈值代表着两个候选匹配点之间的重合度，只有重合度低于一定的阈值，才能算作正确的匹配点。这是一次优化过程。经过一次优化之后的匹配相较于没有优化的匹配，已经滤除了绝大部分外点的干扰。
[0055] 为了更好地提高匹配精度，需要进行二次优化。选用findHomography和perspectiveTransform进行二次优化。首先设置阈值，当一次掩膜的尺寸大于这个阈值，或者说经过一次优化之后的匹配特征点数量大于这个阈值时，再进行二次优化。获取源图像和目标图像的关键点坐标，使用findHomography函数确认两图像特征点坐标之间的变换矩阵。cv2 .findHomography ( )函数将两组特征点的位置信息传递给cv2.perspectiveTransform()函数，得到一个3*3的单应性变换矩阵以及二次掩膜，通过这个矩阵可以将源图像的特征点变换为目标图像的特征点。此函数的共提供了四种方法：
方法0、RANSAC算法、LEMDS算法以及RHO。默认0方法利用平面内部所有点以及最简单的最小二乘方案来计算初始的单应性矩阵，这种初始的估计效果较差，因此就会用到剩余的三种鲁棒性方法，基于RANSAC的鲁棒性方法，最小中值鲁棒性方法，以及基于PROSAC的鲁棒性方法。这三种方法都是通过不同点对的随机子集以及简单的最小二乘方法估计单应性矩阵，也就是前面所说的那个3*3的变换矩阵，区别在于矩阵的
质量评价方法的不同。RANSAC算法是通过内点的数量来进行矩阵的评价，LMEDS算法是通过中值重投影的误差来评价。通过这一评价指标来确定最佳子集、变换矩阵以及内外点的掩膜。选择RANSAC算法进行二次优化，设定阈值为5，超过阈值的则被认定为外点，小于阈值的则作为内点被纳入二次掩膜。获得了符合条件的内外点后，对源图像和变换矩阵M使用perspectiveTransform函数进行特征点的仿射变换，得到变换后的特征点坐标。经过了二次优化，匹配上的特征点精度更加准确。
[0056] 最后，需要进行匹配信息的输出以及可视化。输出左右两侧相机检测到的特征点数量，根据一次掩膜中匹配的特征点数量确认是否进行二次优化，将掩膜之中匹配好的特征点数量进行输出。通过匹配特征点数量以及源图像检测的特征点数量计算匹配率进行输出。最后输出左相机旋转过程中法线方向与基线之间的夹角。除了输出信息之外，还要进行匹配结果的可视化，通过drawMatches函数将匹配的特征点进行绘制，绘制方式有四种，选择DRAW_RICH_KEYPOINTS方式进行绘制，可以将特征点的方向和大小进行可视化。
[0057] 当然，本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员可根据本发明作出各种相应的改变和
变形，但这些相应的改变和变形都应属于本发明所附的
权利要求的保护范围。
