[发明专利]基于稠密视觉SLAM的无人机三维地图快速重建方法

权利要求书

1.一种基于稠密视觉SLAM的无人机三维地图快速重建方法，其特征在于：包括无人机平台三维地图快速重建、无人机三维地图快速重建算法、无人机三维地图快速重建系统、基于ROS的视觉SLAM节点通信系统和基于ROS的视觉SLAM节点通信算法；

所述无人机平台三维地图快速重建：

基于ROS的系统硬件底层抽象方法：

接受使用sensor_msgs/Image消息类型的传感器数据，该消息类型包含RAW图像和压缩图像两种，RAW图像就是CMOS或者CCD图像感应器将捕捉到的光源信号转化为数字信号的原始数据；

在成功获取传感器数据后，ROS在图像管道提供了相机标定、扭曲校正、颜色解码，ROS图像管道通过image_proc功能包运行，提供用于从摄像头采集的RAW图像中获取单色和彩色的转换功能，在标定完摄像头，图像管道就会提取CameraInfo消息并修正图像的径向与切向畸变，即在获取数据的同时能够完成预处理；

对于复杂的图像处理任务，使用OpenCV、cv_bridge和image_transport库对消息进行转换连接，对订阅和发布的图像Topic进行处理，另一方面，OpenCV进行图像处理时，使用cv_bridge将其转换为ROSImage消息发布；

在ROS中将执行计算的进程称为节点，为了在节点中使用OpenCV进行图像处理，需要安装独立的OpenCV库，然后必须在工作空间下的package.xml文件中指定编译和运行需要的opencv2依赖包；然后，对于每个使用OpenCV的节点，必须在target_link_libraries中加OpenCV的lib文件；最后，在节点的cpp文件中，加入所需的OpenCV头文件；

通过上述过程与步骤，实现对硬件的抽象，即通过传感器获取数据，将其发布至ROS网络中，并通过订阅图像的RAW消息，使用OpenCV来进行图像处理；

算法解耦与分布式计算：

三维重建算法运行时，将运行多个节点，每个节点对应实现各自的功能，节点间通过ROS消息进行通信，各个节点可在不同设备上运行，通过ROS构建的网络，实现算法功能的耦合；

基于CUDA并行计算的算法加速方法：

选择NVIDIA Jetson TX2嵌入式开发模块作为实验的处理平台，在基于TX2开发模块的并行程序运行时，每32个并行线程被叫做一个Wrap，GPU中线程的调度执行是以一个Wrap为一个调度单位进行的，每一个SM内有2个线程束调度器，每个线程束调度器内有32个CUDA核，该GPU共256个CUDA核；

基于Topic的消息传递与通信：

(1)启动Talker，Talker通过端口将其信息注册到Master端，其中包括Talker所发布消息的话题名、节点地址信息等；Master将这些信息加入到一个注册列表中；

(2)启动Listener，Listener向Master端注册，注册其所需订阅的话题以及Listener自己的地址信息；

(3)Master对发布者与订阅者进行信息匹配，如果二者发布/订阅同一Message则帮其建立连接；如果没有匹配的则继续等待；若找到匹配的，且此时Talker在发布Message，便会把Talker的地址发送给Listener；

(4)Listener接收到Master发送的Talker地址后，向Talker发送连接请求，同时将Listener要订阅的话题名和完成通讯所需的通信协议全发给Talker；

(5)Talker接收到Listener请求后，返还一个确认连接的信息，包括其自身的TCP地址；

(6)Listener接收到Talker的TCP地址后，通过TCP与Talker建立网络连接；

(7)Talker通过建立的TCP网络连接将数据发送给Listener；

所述无人机三维地图快速重建系统：包括视觉传感器、无人机、嵌入式处理器、GPU，软件实现视觉SLAM的核心算法；软硬件通过分布式框架及通信协议相互联系，组成完整的软硬件系统；其流程如下：

(1)无人机机载嵌入式搭载视觉传感器，获取实时图像序列；

(2)图像序列分别传输至前端与后端；

(3)前端通过特征提取匹配、关键帧提取、位姿估计为后端提供位姿初值；

(4)后端对(2)中接收的图像进行闭环检测，并对(3)中获取的位姿进行优化；

(5)后端基于位姿与图像进行稠密重建；

(6)由观测端通过ROS可视化工具进行实时监测；

所述无人机三维地图快速重建算法：

视觉SLAM前端算法的流程如下：

(1)SLAM系统提供接口，接收由传感器所获取的连续的图像序列，将图像序列传入VO端；

(2)判断SLAM系统是否已初始化完成，若未完成，则进行第(3)步，若已完成初始化，则进行第(4)步；

(3)初始化第一个关键帧，并初始化位姿T0；

(4)持续对图像序列进行ORB特征提取，选取下一个关键帧F_i(i＞＝1)；

(5)将F_i与F_i-1的ORB特征进行匹配，并筛选出可靠的匹配；

(6)基于可靠的特征点对，通过对极约束求解帧间的运动，计算出该关键帧的位姿T_i；

(7)若帧流结束则停止，否则返回第4步；

重建部分算法的流程如下：

(1)选取需要计算深度的目标像素点p；

(2)基于相机运动，即相机光心的平移向量，以及所求关键帧相机光心与p的连线的向量，构成的平面交新关键帧于极线，计算极线位置；

(3)遍历所求得的极线，搜索与p匹配的点；

(4)通过三角测量计算出p点实际的空间位置；

(5)更新该像素的深度信息；

SLAM前端特征提取与匹配方法：首先检测Oriented FAST角点位置，然后基于角点位置，计算其BRIEF描述子；对两幅图像中BRIEF描述子进行匹配，记录其汉明距离，即二进制字符串不同位数的个数；记录所有匹配之中的最小值d_min，若描述子之间汉明距离大于2倍d_min，则删除该匹配，反之保留该匹配；

关键帧提取方法：首先处理第一张图像，先检测FAST特征点和边缘特征，如果图像中间的特征点数量超过设定的阈值，就把这张图像作为第一个关键帧，然后处理第一张之后的连续图像，持续跟踪特征点；当匹配点的数量大于阈值，如果视差的中位数大于阈值，选择计算本质矩阵E，否则选择计算单应矩阵H；如果计算完H或E后，还有足够的内点，就将其作为关键帧；

相机运动估计：相机的运动估计即计算出相邻关键帧之间的平移向量t和旋转矩阵R，而t与R可通过分解本质矩阵E来获得，而在关键帧相机只有旋转而无平移的时候，两视图的对极约束不成立，基础矩阵F为零矩阵，这时候需要分解单应矩阵H；分解方法为奇异值(SVD)分解：

tR＝E＝UDV^T (4-1)

其中，U、V均为三阶正交矩阵，D为对角阵D＝diag(r,s,t)；分解可得对角矩阵的特征值，该3个特征值中将会有两个特征值为正，其值相等，另一个则为0；在分解过程中会获得四组不同的解，分别对应关键帧的四种相对位姿；

稠密重建：以相邻两幅图像中估计任一像素的深度来说，对于某一个像素p₁对应的空间点P，相邻两帧的相机光心与其连线O₁P、O₂P以及O₁O₂共面，其中O₁P交第一帧于p₁，O₂P交第二帧于p₂，则p₂必然落于面O₁O₂P与第二帧交线上，即p₁在第二帧中的位置在极线上；

由于单个像素的亮度没有区分性，则考虑对像素块相似性进行比较，即在像素点p₁周围取一块w*w的小块，然后遍历极线上各点，与其周围的w*w的小块进行匹配，则算法的假设由像素的灰度不变性转为图像块的灰度不变性；

对于两个小块的匹配，计算其归一化互相关(Normalized Cross Correlation，NCC)：

在极线上计算每一个相似性度量之后，将会得到一个沿着极线的NCC分布，即p1像素在第二帧中出现的概率分布，NCC值越高，则该点为p1像素的同名点概率越高；然而仅靠两张图像计算某像素的深度值存在一定的偶然性，因此需要多张图像进行估计；我们假设某个像素点的深度d服从高斯分布：

P(d)＝N(μ,σ²) (4-3)

每当新的数据到来时，观测其深度也将会是一个高斯分布：

则更新观测点p1的深度变成一个信息融合问题，而融合后的深度则满足：

经过多次迭代，则p1在第二帧极线上的概率分布峰值越接近其真实位置；由此，对于稠密重建其完整步骤如下：

(1)假设所有像素的深度满足某个初始的高斯分布；

(2)当新数据产生时，通过极线搜索与块匹配确定投影点位置；

(3)根据几何关系计算三角化后的深度及不确定性；

(4)将当前观测融合进上一次的估计中，若收敛则停止计算，否则返回第2步；

基于ROS的视觉SLAM节点通信系统：采用ROS分布式计算架构，以降低系统各模块之间的耦合，提高系统在复杂环境中的可用性与可靠性；将视觉SLAM重建过程中的数据获取、位姿估计、稠密重建、可视化分别由四个计算节点运行，每个节点各司其职，节点间通信采用Topic订阅模式；系统工作时共4个节点参与通信，其中，传感器捕捉数据，其原始数据被发布成话题/sensor_msgs，由视觉传感器与Jetson TX2通过USB或CSI接口直连，将流数据转换为图像序列数据消息，重新发布成话题/usb_cam；

VO节点订阅/usb_cam，进行位姿估计，并将结果发布至成位姿数据，重建节点同时订阅/usb_cam与位姿数据，并将处理后得到的深度图数据与点云数据发布至ROS网络中，由可视化节点订阅，实现系统的增量地图构建的可视化功能；

所述基于ROS的视觉SLAM节点通信算法：SLAM前端，即VO节点，与后端重建节点同时订阅话题/usb_cam；VO节点将/usb_cam节点所发布的数据通过SLAM前端库的接口进行位姿解算，然后，将其以消息的形式进行发布，由SLAM后端的重建节点进行订阅，即重建节点同时订阅帧数据以及位姿数据；为了能够实时地对三维地图重建的结果进行可视化，采用的方法流程如下：

(1)当选取的参照帧其深度图收敛时，将深度图结果发布至话题；

(2)可视化节点根据参照帧及其时间戳，获取由VO节点发布的位姿/pose；

(3)基于初始化结果，计算出该参照帧光心的空间位置以及该帧的法线方向；

(4)基于深度图逐像素构造点云。