CN112197773B - 基于平面信息的视觉和激光定位建图方法 - Google Patents

基于平面信息的视觉和激光定位建图方法 Download PDF

Info

Publication number
CN112197773B
CN112197773B CN202011065887.4A CN202011065887A CN112197773B CN 112197773 B CN112197773 B CN 112197773B CN 202011065887 A CN202011065887 A CN 202011065887A CN 112197773 B CN112197773 B CN 112197773B
Authority
CN
China
Prior art keywords
plane
points
point
coordinate system
moment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011065887.4A
Other languages
English (en)
Other versions
CN112197773A (zh
Inventor
王庆
冯悠扬
严超
许九靖
刘鹏飞
张波
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu Jicui Future City Application Technology Research Institute Co ltd
Original Assignee
Jiangsu Jicui Future City Application Technology Research Institute Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu Jicui Future City Application Technology Research Institute Co ltd filed Critical Jiangsu Jicui Future City Application Technology Research Institute Co ltd
Priority to CN202011065887.4A priority Critical patent/CN112197773B/zh
Publication of CN112197773A publication Critical patent/CN112197773A/zh
Application granted granted Critical
Publication of CN112197773B publication Critical patent/CN112197773B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/213—Feature extraction, e.g. by transforming the feature space; Summarisation; Mappings, e.g. subspace methods
    • G06F18/2135—Feature extraction, e.g. by transforming the feature space; Summarisation; Mappings, e.g. subspace methods based on approximation criteria, e.g. principal component analysis
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
    • G06V10/267—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion by performing operations on regions, e.g. growing, shrinking or watersheds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Multimedia (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Processing (AREA)

Abstract

本发明提出了一种基于平面信息的视觉和激光定位建图方法，在激光传感器方面创新性地提出了具有几何解释意义的针对激光传感器的曲率计算方法，基于三次增长的平面检测，通过平面匹配方法实现位姿估计和全局参数更新。采用本申请的技术方案，可以有效提高平面检测和匹配的准确性；充分利用空间中的平面信息，将视觉和激光传感器进行了紧耦合，克服了单一传感器的缺点。特别是将空间中的平面信息的引入，提高了特征匹配的稳定性和建图的稠密度；同时设计了一种地图点不参与优化的定位方法，提高了地图点估计的一致性，进而提高了定位的精度。

Description

基于平面信息的视觉和激光定位建图方法
技术领域：
本发明涉及视觉传感器和激光传感器领域，尤其是涉及一种基于平面信息的定位和建图方法。
技术背景：
目前，逆向工程分为两类：高精度的三维重建和以定位为目标的SLAM算法。三维重建使用视觉传感器通过对图像上特征点的匹配获取得到各个时刻的位姿，并对图像进行三角化面片分割和贴图的工作，完成对空间精确的描述。但此方法计算量大，后期数据的维护需要专业人员。SLAM算法是由三维重建衍生出来的实时定位方法，SLAM方法将三维重建中整体优化模式调整为增量优化模式以保证定位数据的实时性。而SLAM方法其构建的地图主要是用于定位，常由稀疏的点云构成。虽然以直接法为代表的DSO算法能够构建稠密点云地图，但是存在可视化程度低和空间一致性较差的问题。
以巡检机器人场景为例，巡检机器人巡检开始前工程师需要根据厂区每个表的位置和地图的坐标设定机器人与环境交互任务。以超过200亩的1000千伏特变电站为例，现有的巡检系统配置完成需要耗费一个月的时间，大大地降低了智能化系统的自动化水平和效率。其主要原因在于现有的巡检机器人无法快速地对厂区的仪表仪器进行位置建模。现有的三维重建技术因其耗时长、专业性要求强和人力成本高的原因无法使用在巡检机器人的应用中。鉴于此种情况，提出了一种基于视觉和激光融合的快速可视化方法。该方法能够对空间的平面进行快速的分割、提取和匹配，利用空间的平面语义信息进行定位和建图。
参考文献：
王泽华.视觉融合SLAM技术及其在移动机器人上的应用研究[D].宁波大学,2019.
欧阳毅.基于激光雷达与视觉融合的环境感知与自主定位系统[D].哈尔滨工业大学,2019.
刘毅.基于单目视觉的实时高精度定位方法研究[D].华中科技大学,2018.
发明内容：
本发明针对现有激光和视觉融合的算法不足提出了激光和视觉紧耦合的定位和建图方法。在激光传感器数据处理方面，使用三维激光的分辨率构建稀疏点云的体素，基于体素的连续性实现了平面的快速检测。为了保证平面融合时参数更新的实时性，提出了基于平面参数的增量式更新方法。视觉部分使用双目相机实现了基于滑动窗口的SLAM方法。同时，使用空间中平面点云来估计图像中处于同一平面像素的深度，进而对空间中的点云进行上色得到易于人机交互的三维稠密地图。
一种基于平面信息的视觉和激光定位建图方法，包括以下步骤：
A.通过激光传感器、视觉传感器进行数据采集并进行数据同步；
B.根据公式(1)计算当前时刻所有点曲率，
c＝180*acos(|norm(P(i-5,j,k)-P(i,j,k))·norm((P(i+5,j,k)-P(i,j,k)))|)/π (1)
其中，P(i,j,k)为当前点三维坐标，P(i-5,j,k)为当前点左侧第五个相邻点的三维坐标，P(i+5,j,k)为当前点右侧第五个相邻点的三维坐标，norm函数表示将输入的向量进行单位化；符号“·”表示将两个向量点乘，将整个点云分为有曲率点和无曲率点，然后进行区域增长，将平面点、无曲率点与平面点融合；
C.已知i时刻的平面点集合
Figure GDA0003821073640000021
和当前时刻j的平面点集合
Figure GDA0003821073640000022
其中i时刻一共存在n个平面，j时刻一共存在m个平面，将i时刻下的坐标系移动到j时刻坐标系下的位姿矩阵为[Rj,i tj,i]，已知在平面上的点满足Ax+By+Cz+D＝0约束方程，根据公式(3)将j时刻下的所有平面参数变换到i时刻坐标系下得到(Ai(j),Bi(j),Ci(j),Di(j))，通过公式(4)计算与i时刻平面的两个参数Δr、Δd，当夹角Δr小于30度并且距离Δd小于0.5米，则将i时刻平面加入到j时刻匹配的候选平面中；
Figure GDA0003821073640000023
其中i时刻第k个平面和j时刻下第l个平面对应空间中一个平面，
Figure GDA0003821073640000024
和
Figure GDA0003821073640000025
分别为在两个坐标系下拟合得到的平面参数；上标i表示此平面坐标为i时刻下的平面参数，括号中的j表示此平面参数由j时刻下的平面参数变换得到，
Figure GDA0003821073640000026
其中，Δr用于衡量两个待匹配平面的法线方向是否一致；Δd用于衡量i时刻坐标系下坐标原点到平面的距离是否一致；
D.根据图像的单应性矩阵性质并与空间三维平面点相融合形成稠密地图。
进一步地，所述步骤B中，先通过第一次区域增长将空间点分为有效平面点，无效平面点，无曲率点和角点，然后进行第二次增长，所述第二次增长为遍历空间中所有的无曲率点和角点，判断其周围是否存在有效平面点，当存在则判断到有效平面的距离是否小于5cm，如果小于5cm则向其他的无曲率点和角点增长，直到停止。
进一步地，所述步骤C中，当存在多个候选平面时，通过多边形覆盖度的方法来选择最优的候选平面。
进一步地，所述步骤C中，所述步骤C中，对于超过15个关键帧没有被观测到的地图点和空间平面，使用以往观测到这个地图点的所有关键帧对其在世界坐标系下的坐标和平面参数进行拟合,通过代价函数实现帧间的位姿估计和滑动窗口的位姿估计，代价函数为；
用so(3)李代数来表示旋转矩阵R，由此可以计算得到四个代价函数的雅克比矩阵：
Figure GDA0003821073640000027
Figure GDA0003821073640000031
Figure GDA0003821073640000032
Figure GDA0003821073640000033
上式中
Figure GDA0003821073640000034
内容如下：
Figure GDA0003821073640000035
其中，SO(3)流形对应的so(3)李代数为φ；
Figure GDA0003821073640000036
i时刻在世界坐标系下的位姿ti,w和j时刻在世界坐标系下的位姿tj,w，[Ri,j ti,j]为i帧和j帧的相对位姿变换矩阵；K为相机的内参，fx、fy为相机内参；pi为i帧载体坐标系下的三维点坐标；[uivi 1]为i帧下特征点坐标与j帧下的特征点[uj vj 1]匹配；[u'i v'i 1]和[u'j v'j 1]为归一化像素坐标；[Ai Bi Ci Di]和[Aj Bj Cj Dj]分别对应空间同一平面在i时刻和j时刻各自载体坐标系下的平面参数；z'j为特征点在j时刻坐标系下的深度；运算符“^”表示向量的反对称矩阵；残差e1对应着ICP算法问题，即在两个坐标系下的三维坐标已知估计两个坐标系的位姿变换矩阵；残差e2对应着PnP算法问题，即已知空间三位点的坐标和其图像中的对应坐标，计算相机的位姿；残差e3对应对极几何问题，即已知两张图像对应的特征点计算本质矩阵，e4为位姿状态估计时构建的代价函数的第四项残差。
进一步地，所述步骤D中，对于激光测量到的平面上的点，通过公式(19)直接将三维激光点变换到相机坐标系并通过相机的图像得到激光点的颜色；
Figure GDA0003821073640000037
其中，[Rl→c,tl→c]能够将激光坐标系下的点变换到相机坐标系下；
Figure GDA0003821073640000038
为激光坐标系下第i个坐标点；
Figure GDA0003821073640000039
计算得到的是在图像坐标系下的坐标；K为已知的相机内参3×3矩阵；z'为激光点在相机坐标系下的深度坐标可以通过
Figure GDA00038210736400000310
求解得到；I为图像灰度函数，输入为图像坐标，输出为在此像素上的灰度值；
Figure GDA00038210736400000311
为激光点对应的灰度值；
根据公式(6-33)计算得到对应的相机坐标系下的坐标，通过空间中的平面帮助图像恢复深度
使用空间中的平面帮助图像恢复深度：
Figure GDA0003821073640000041
其中，[u,v]为图像中的像素坐标；[A,B,C,D]为像素坐标[u,v]对应的平面参数；内参矩阵
Figure GDA0003821073640000042
[xl yl zl]为激光坐标系下点的坐标；[xc yc zc]为相机坐标系下点的坐标。
采用本发明的技术方案，具有以下技术效果：
本技术方案提出了一种基于激光和视觉紧耦合定位和建图方法。在激光传感器方面创新性地提出了具有几何解释意义的针对激光传感器的曲率计算方法。相比于现有的PCA曲率和LOAM曲率，提出的曲率计算方法使用同一线上左右相邻的夹角，根据实验表明对不同环境条件都拥有较高的一致性描述，不需要根据环境的变换而改变曲率阈值的设定，能够更好地对空间中的角点和平面点进行分类。其次，在激光传感器数据处理方面，提出了一种更加稳定的基于三次增长的平面检测方法。在此检测方法中针对激光传感器的稀疏特性，建立了基于俯仰角、导航角度和测量深度所划分的Voxel最小单元，并使用广度优先搜索方法对空间中的平面点进行聚类和分割。相比于现有的Hough变换方法和PCL中的平面增长方法，该方法在平面分类的准确度上有明显的提高。为了能够将空间中的平面用于位姿估计，基于以上两点提出了更加快速稳定地平面匹配方法。平面匹配的难点在于平面融合后全局参数的更新，本文没有使用增量或者全局式的平面参数更新方法，而是将图像处理中的膨胀/腐蚀的引入到离散点集的凹凸点计算中，并使用提取到的凹凸点更新平面的全局参数，大大降低了全局平面参数的估计时间。
在位姿估计方面没有将地图点和世界坐标系下的平面视为待优化参数，而是将共视点和共视平面作为相对位姿优化的测量值引入到位姿优化中。在整体的优化框架中只有两帧之间的位姿作为待优化参数，这种方式大大降低了地图点初始值不准确对位姿精度的影响。然后，给出了四种残差函数的解析雅可比推导结果。本申请结合精确的位姿估算结果、激光的平面深度信息和视觉的像素信息，得到了空间中平面的稠密颜色点云建图结果。相比于现有的激光点云上色方法，本文提出的方法将空间中的平面几何信息纳入到建图和位姿优化的过程中提高了位姿优化的精度和建图的空间一致性。
本申请的方案提高了平面检测和匹配的准确性；充分利用空间中的平面信息，将视觉和激光传感器进行了紧耦合，克服了单一传感器的缺点。特别是将空间中的平面信息的引入，提高了特征匹配的稳定性和建图的稠密度；同时设计了一种地图点不参与优化的定位方法，提高了地图点估计的一致性，进而提高了定位的精度。
附图说明：
图1为相机与激光的滑动窗口示意图。
图2为实施例一的流程图。
图3为区域增长的示意图。
图4为实施例一的图像处理流程图。
图5为存在多个候选平面的示意图。
图6为平面旋转的示意图。
图7为空间平面投影结构示意图。
图8为硬件设备结构示意图。
第一平面1、第二平面2、点11、点12、点13，第一圆圈31、第二圆圈32、方框33、激光传感器5、视觉传感器6。
具体实施方式：
下面通过具体实例进一步描述本发明。不过这些实例仅仅是范例性的，并不对本发明的保护范围构成任何限制。
实施例一：
(1)首先需要将视觉传传感器内参进行标定，激光-视觉外参进行标定。
(2)然后将激光传感器5、视觉传感器6与核心处理器相连，并接通核心处理和传感器的电源，如图8所示。
(3)开始进行数据的采集和处理。
本实施例的总体的算法分为四个步骤：点云曲率计算阶段；多边形边界和平面匹配；视觉和激光紧耦合位姿估计阶段；基于平面投影可视化阶段。
(1)点云曲率计算阶段。提出了一种具有几何意义的曲率计算方法：
c＝180*acos(|norm(P(i-5,j,k)-P(i,j,k))·norm((P(i+5,j,k)-P(i,j,k)))|)/π (1)
其中，P(i,j,k)为当前点三维坐标，P(i-5,j,k)为当前点左侧第五个相邻点的三维坐标，P(i+5,j,k)为当前点右侧第五个相邻点的三维坐标；norm函数表示将输入的向量进行单位化；符号“·”表示将两个向量点乘。
利用最远端的两个与当前点构成的夹角作为曲率计算值。该方法的优势是无论使用何种数据集和三维激光雷达，提取角点的曲率阈值不需要发生更改。在实验过程中统一设置曲率阈值为45度，大于45度的为角点，小于45度的为平面点。相比于LOAM使用的曲率计算方法，提出的计算方法只取用了最远的两个相邻点。距离当前点越远，则测量深度造成的误差对于曲率的影响就越小。
首先，根据上式计算所有点曲率。对于处在边缘或者离群的点是无法使用上式计算得到曲率的，将此类点划分为无曲率点。此时整个点云被划分两类点：有曲率点和无曲率点。进行第一次区域增长的操作后，空间中平面点已经被大致的聚类完毕。但是有些缓慢变化的曲面也会被归类为平面点，此时需要对归类的平面点进行筛选。首先对于同一类的平面点使用RANSAC算法拟合出平面参数(A,B,C,D)。然后使用RANSAC筛选出来内置点投影到平面上，并计算平面的多边形外角点由此计算平面的面积。再使用PCA算法计算点云的三个主方向，如果平面的面积大于0.06平方米并且inliner百分比大于80％则将属于此平面的点归类为有效平面点，将剩余的点归类为无效平面点。此时空间中的点被划归为了四类：有效平面点，无效平面点，无曲率点和角点。但是平面边缘点无论是角点或者无曲率点，仍旧属于平面。为了将这类点与现有的平面进行聚类，执行第二次的区域增长，如图3所示。第二次区域增长是遍历空间中所有的无曲率点和角点，判断其周围是否存在有效平面点，如果存在则判断到有效平面的距离是否超过5cm，如果没有超过5cm则向其他的无曲率点和角点增长，直到停止。第二次增长的算法过程如图3所示，蓝色方框为属于同一平面的有效平面点，黑色和红色方框为无曲率点和角点，红色的方框为当前被遍历的点。绿色的方框为与周围平面融合的点。黄色箭头为区域增长的方向。经过第二次区域增长后，可以将原本属于一个平面的角点和无曲率点与现有的平面点融合。
(2)多边形边界估计和平面匹配。
1)平面匹配
平面匹配总体流程如图4所示。在帧与帧的相对位姿计算和滑动窗优化时都需要进行平面匹配，用于建立帧间的相对位姿约束关系的代价函数。在进行平面匹配时帧间的相对位姿已经根据双目视觉计算得到。已知i时刻的平面点集合
Figure GDA0003821073640000061
和j时刻的平面点集合
Figure GDA0003821073640000062
其中i时刻一共存在n个平面，j时刻一共存在m个平面。将i时刻下的坐标系移动到j时刻坐标系下的位姿矩阵为[Rj,i tj,i]。已知在平面上的点满足Ax+By+Cz+D＝0约束方程。假设i时刻第k个平面和j时刻下第l个平面对应空间中一个平面，根据几何关系这两个平面参数与帧间的相对位姿有如下约束关系：
Figure GDA0003821073640000063
其中
Figure GDA0003821073640000064
和
Figure GDA0003821073640000065
分别为在两个坐标系下拟合得到的平面参数；上标i表示此平面坐标为i时刻下的平面参数，括号中的j表示此平面参数由j时刻下的平面参数变换得到。
将j时刻下的所有平面参数变换到i时刻坐标系下得到(Ai(j),Bi(j),Ci(j),Di(j))，并计算与i时刻平面的如下两个参数：
Figure GDA0003821073640000066
其中，Δr用于衡量两个待匹配平面的法线方向是否一致；Δd用于衡量i时刻坐标系下坐标原点到平面的距离是否一致。
如果法线夹角小于30度，并且距离小于0.5米则将i时刻平面加入到j时刻匹配的候选平面中。空间是无限延展的聚类集合，即使两个点云没有重合这两个平面的参数也会有可能极度地接近，如图5所示。在实际测量的室内环境中，黑板所在的第一平面1由虚线标出，虚线标出的第二平面2与第一平面1很显然是同一个平面，因此其平面参数会非常的接近，这就会导致在i时刻有两个候选平面计算得到的Δr和Δd非常地接近。因此只依靠法方向和距离一致是不足以进行平面匹配的。
为了解决这个问题，使用基于多边形覆盖度的方法来判断两个平面是否匹配。需要将i时刻某个平面的点云投影到拟合得到的平面上。为了保证激光测量值的有效性，不可以直接将点垂直投影到平面中得到投影点，因为激光的导航角和俯仰角是准确的，而测量的误差主要来源于深度。在做平面投影时需要计算激光光束与此平面相交的点即为投影点。如图6所示，红色点P2为实际的测量点其坐标为p2，绿色的点P1为激光光束与平面的交点其坐标为p1。已知i时刻的平面模型和P2点在i时刻坐标系下的坐标，由此可以计算得到P2到平面的距离d2，P0点到平面的距离为d3，则P1的坐标可以计算得到：
Figure GDA0003821073640000071
将点投影到平面后，接下来需要提取平面点所构成的多边形凸点。为了能够像平面图像中一样提取二维的多边形凸点，需要将平面中点的z轴坐标变为相等。因此接下来将j时刻的平面坐标变换到j时刻坐标系的X-Y平面，在图6中对应从点P1变换到点P3。需要注意的是这里进行的不是垂直投影，因为垂直投影会改变原始平面的面积和凸点的几何关系，需要将j时刻某个平面的点变换到X-Y平面，保证点云上的Z轴坐标相同，如图6示。na向量为X-Y平面的法向量等于(0，0，1)，nb为j时刻某个平面的法向量。θ为两个平面的所成角度。nb向量与na向量所成的夹角即为θ。则可以得到进行平面旋转的姿态矩阵为：
Figure GDA0003821073640000072
其中，
Figure GDA0003821073640000073
和
Figure GDA0003821073640000074
向量来自于图6中na和nb向量。
如果na和nb的夹角大于90度则将na向量乘以-1，则得到了方向归一化之后的
Figure GDA0003821073640000075
和
Figure GDA0003821073640000076
向量。上式中通过
Figure GDA0003821073640000077
和
Figure GDA0003821073640000078
向量的叉乘得到了旋转轴，再对叉乘得到的向量进行单位化则得到了旋转轴向量φ。两个平面的夹角可以通过两个向量的点乘得到，最终通过旋转轴和旋转角度构造出了位姿变换矩阵Rp。则在X-Y平面的p3坐标可以通过下式计算得到:
Figure GDA0003821073640000079
根据在i时刻X-Y平面上的二维坐标点，提取每个平面的多边形的凸/凹包点。i时刻每个平面的凸/凹包点对应图5中点11、12。对于j时刻坐标系下的平面点将其投影到拟合的平面上，以纠正深度测量的误差。然后再将点通过相对位姿变换矩阵[Ri,j ti,j]得到i时刻坐标系下的坐标点。通过第一步已经能够得到与j时刻平面所匹配的i时刻的候选平面，如果候选平面超过两个，则需要将来自于j时刻的坐标点投影到X-Y平面。计算落在i时刻不同凸包轮廓内的数量，选择包含最多j时刻投影点的凸/凹包所在候选平面为最优匹配平面。整个匹配过程如图5所示，i时刻各个平面对应的凸包多边形已经建立，对应第一平面1和第二平面2。点11、12为建立多边形的凸/凹包点。j时刻某个平面的点云经过位置修正和帧间位姿变化投影到了i时刻，在图5对应点13。由于第一平面和第二平面基本平行因此其曲面的参数非常接近，这两个平面都可作为j时刻的候选平面。但是j时刻平面的点落在第二平面所构成的凸包中，则j时刻的最佳平面为虚线所框出的第二平面。通过凸包的重合率可以非常有效地与空间中参数相似度高但是不连续的平面进行匹配。
2)多边形边界估计
在判断某个点是否在一个多边形内，需要求解得到多边形的边界点。笔者尝试了现有的凹凸点计算方法但是效果都很不理想，其主要原因在于三维点投影到图像后上下间距远远大于左右相邻的间距，并且随着越来越靠近图像的两端，像素点的间距越来越大，阈值在每帧图像上都需要进行调整。为了解决这个问题，提出了一种在激光体素空间的腐蚀和膨胀方法。
Figure GDA0003821073640000081
现有的空间平面将XYZ坐标系下的点投影到体素空间，由于已经确定了是一个平面的点，因此这里不用计算深度空间d的坐标，而是需要计算得到上式中的j和k。相比于稀疏的XYZ空间，在体素空间中各个有效单元相聚的较为紧密。然后使用图像处理中膨胀和腐蚀函数对平面的体素空间进行操作，保证体素空间中不存在无效点。本文中将膨胀和腐蚀的j维度的值设置为5，k维度的值设置为2。然后寻找边缘点，得到最终的结果。在实际膨胀的过程中可能会出现边缘点不存在三维点与之对应的情况，为了解决这个问题，将有效体素空间存储到八叉树中，得到边缘点后搜索与之最相近的八叉树中的元素。最后将八叉树中元素对应的三维点取出，即为边界点。
3)视觉和激光紧耦合位姿阶段。现有的位姿估计方法都是将当前帧与世界地图点匹配进行位姿优化，将地图点作为一个待优化的参数进行求解。但是实际上地图点只是表征了两帧之间的共视关系，仅为帧间相对位姿提供了一个约束方程，不需要得到地图点在世界坐标系下的坐标，现有的视觉和激光里程计方法之所以这么做是因为其优化的位姿在世界坐标系下，并且期望能够实时看到地图的更新。这种方法的缺点是将共视地图点作为一个待优化的参数加入到了优化函数中，而在优化过程中又无法对地图点坐标进行全局优化而仅仅是增量式的优化导致地图点坐标精度降低，从而影响后续的位姿估计的结果。而本章提出的位姿估计方法中将共视的平面和特征点作为测量值带入到代价函数中，这样做的优势是不用对地图点的世界坐标进行优化。对于超过15个关键帧没有被观测到的地图点和空间平面，使用以往观测到这个地图点的所有关键帧对其在世界坐标系下的坐标和平面参数进行拟合。本文将代价函数分为两种：视觉代价函数和激光代价函数。视觉代价函数根据匹配特征点是否存在深度信息又被分为三种。具体的代价函数如下所示：
Ri,jpi+ti,j-pj＝e1 (9)
Figure GDA0003821073640000082
Figure GDA0003821073640000083
Figure GDA0003821073640000084
其中，[Ri,j ti,j]为i帧和j帧的相对位姿变换矩阵；K为相机的内参；pi为i帧载体坐标系下的三维点坐标；[ui vi 1]为i帧下特征点坐标与j帧下的的特征点[uj vj 1]匹配；[u'i v'i 1]和[u'j v'j 1]为归一化像素坐标；[Ai Bi Ci Di]和[Aj Bj Cj Dj]分别对应空间同一平面在i时刻和j时刻各自载体坐标系下的平面参数；z'j为特征点在j时刻坐标系下的深度；运算符“^”表示向量的反对称矩阵。
式(9)-式(11)对应视觉的代价函数，式(12)对应激光的代价函数。在视觉代价函数中分为三种情况：i时刻和j时刻匹配的特征点在各自的坐标系下其深度已知；i时刻和j时刻匹配的特征点仅知道在某个时刻坐标系下的位姿；i时刻和j时刻匹配的特征点无法得到在各自坐标系下的深度。当使用双目相机进行特征点匹配时，会同时与右目相机进行极线匹配得到特征点在相机坐标系下的坐标。如果右相机没有观测到左相机的地图点那么此特征点的深度便无法计算得到。残差e1对应着ICP算法问题，即在两个坐标系下的三维坐标已知估计两个坐标系的位姿变换矩阵。残差e2对应着PnP算法问题，即已知空间三位点的坐标和其图像中的对应坐标，计算相机的位姿。残差e3对应对极几何问题，即已知两张图像对应的特征点计算本质矩阵；e4为位姿状态估计时构建的代价函数的第四项残差。
对于激光的代价函数本文使用空间中的平面对位姿进行拟合，根据空间同一平面在两个坐标系下的参数将两个坐标系之间的位姿变换构建联系。平面的匹配精度要远远高于现有的点-点或者点-线匹配。
上述的四个残差函数中需要优化的参数为i时刻在世界坐标系下的位姿ti,w和j时刻在世界坐标系下的位姿tj,w，与上式中[Ri,j ti,j]变量相关：
Figure GDA0003821073640000091
用so(3)李代数来表示旋转矩阵R，由此可以计算得到四个代价函数的雅克比矩阵：
Figure GDA0003821073640000092
Figure GDA0003821073640000093
Figure GDA0003821073640000094
Figure GDA0003821073640000095
上式中
Figure GDA0003821073640000096
内容如下：
Figure GDA0003821073640000101
其中，SO(3)流形对应的so(3)李代数为φ；
Figure GDA0003821073640000102
fx、fy为相机内参。
使用上述的代价函数可以实现帧间的位姿估计和滑动窗的位姿估计，并且可以大大减少非线性优化过程中Hessian矩阵的大小减少求解线性方程的运算时间。
(4)基于平面投影可视化阶段。
由于视觉在纹理较少的环境中无法进行特征点匹配，从而无法得到三维点在相机坐标系下的深度。激光虽然能够非常直接地测量到深度，但是无法捕捉空间中的纹理信息。本节中针对视觉和激光各自的优缺点，对空间中的平面进行稠密重建。提出的可视化方法没有使用三角化+面片的贴图技术方案，而是在有限的硬件资源下基于正方形体素进行点云上色，空间中的最小单位为体素而非三维重建中的面片。用于稠密化平面的点分为两类：通过三次区域增长得到的激光点和图像中与平面对应的像素点。对于激光点需要将其投影到图像中获取灰度值。而对于图像的像素点需要首先与激光坐标系下的平面进行匹配，然后计算与平面匹配成功的像素坐标深度。下面针对这三个问题进行详细的说明和公式推导。
1)对于激光测量到的平面上的点，可以直接将三维激光点变换到相机坐标系并通过相机的图像得到激光点的颜色，其具体过程如下公式所示：
Figure GDA0003821073640000103
其中，[Rl→c,tl→c]能够将激光坐标系下的点变换到相机坐标系下；
Figure GDA0003821073640000104
为激光坐标系下第i个坐标点；
Figure GDA0003821073640000105
计算得到的是在图像坐标系下的坐标；K为已知的相机内参3×3矩阵；z'为激光点在相机坐标系下的深度坐标可以通过
Figure GDA0003821073640000106
求解得到；I为图像灰度函数，输入为图像坐标，输出为在此像素上的灰度值；
Figure GDA0003821073640000107
为激光点对应的灰度值。
2)因为三维激光在垂直方向上角度分辨率较低，如果仅仅使用三维激光生成带有灰度的地图点，在平面距离三维激光较远的情况下会导致平面上的地图点过于稀疏，无法将平面中的纹理信息显示完全。为了解决地图点稀疏的问题，使用空间中的平面帮助图像恢复深度：
Figure GDA0003821073640000108
其中，[u,v]为图像中的像素坐标；[A,B,C,D]为像素坐标[u,v]对应的平面参数；内参矩阵
Figure GDA0003821073640000109
[xl yl zl]为激光坐标系下点的坐标；[xc yc zc]为相机坐标系下点的坐标。
根据上式的关系可以计算得到对应的相机坐标系下坐标[xc yc zc]为：
Figure GDA0003821073640000111
根据上式可知，通过像素坐标值和激光坐标系下的平面参数可以计算得到像素坐标对应的相机坐标系下坐标。此点的灰度值即为像素坐标[u,v]在图像I中的灰度。
3)前文已经可以在像素坐标与平面匹配已知的情况下，计算得到像素坐标的深度值。使用图7详细说明像素坐标与平面的匹配方法：
首先，计算得到每个平面的凸/凹包点，对应图7中的第一圆圈31。然后，根据激光坐标系和相机坐标系的固定位姿变换关系，将激光坐标系下的凸/凹包点变换到图像中，得到图像中对应的凸/凹包点，对应图7中的第二圆圈32。最后，根据图像中的凸/凹包点在图像坐标系下构建多边形，对应图7中的方框33。将虚线方框33内的所有像素坐标与空间中的平面建立匹配关系。当通过以上的三个步骤计算得到空间平面的稠密带有灰度的点云之后，需要将平面中的点云进行降采样。其降采样的方法与PCL中常用的降采样方法不相同，不使用每个体素内的重心来近似体素中的其他点，这样做会导致重心点的灰度值需要使用体素中的其他点进行拟合，使得边缘灰度的不确定度增加。使用的降采样方法与第一小节介绍的区域增长法类似，随机选择平面中的任意一点作为种子节点并以此开始BFS搜索。当相邻点距离种子节点大于1cm时，才将此相邻点加入到种子队列中。然后依次从种子队列中取出种子点并继续执行BFS搜索直到种子队列为空。需要注意的是当滑动窗口完成优化并且地图中的平面参数被更新后，激光点投影到图像上的坐标和像素对应的相机坐标系坐标并不会发生改变。因此地图点的灰度值完全由当前帧的测量值决定，一旦赋值便不再改变。
应该理解，虽然本发明结合实例进行了详细的说明，但上述说明旨在举例说明，而不以任何方式限制其发明内容。对本领域技术人来说，基于本文的说明可以最大程度地利用本发明，并可在没有脱离本发明的权利要求范围或精神内进行多种修改或修饰。本申请所引用的各个参考文献，在此全文引入作为参考。

Claims (5)

1.一种基于平面信息的视觉和激光定位建图方法，其特征在于，包括以下步骤：
A.通过激光传感器、视觉传感器进行数据采集并进行数据同步；
B.根据公式(1)计算当前时刻所有点曲率，
c＝180*acos(|norm(P(i-5,j,k)-P(i,j,k))·norm((P(i+5,j,k)-P(i,j,k)))|)/π(1)
其中，P(i,j,k)为当前点三维坐标，P(i-5,j,k)为当前点左侧第五个相邻点的三维坐标，P(i+5,j,k)为当前点右侧第五个相邻点的三维坐标，norm函数表示将输入的向量进行单位化；符号“·”表示将两个向量点乘，将整个点云分为有曲率点和无曲率点，然后进行区域增长，将平面点、无曲率点与平面点融合；
C.已知i时刻的平面点集合
Figure FDA0003762134350000011
和当前时刻j的平面点集合
Figure FDA0003762134350000012
其中i时刻一共存在n个平面，j时刻一共存在m个平面，将i时刻下的坐标系移动到j时刻坐标系下的位姿矩阵为[Rj,i tj,i]，已知在平面上的点满足Ax+By+Cz+D＝0约束方程,A、B、C、D为同一类平面点使用PANSAC算法拟合出的平面参数，根据公式(3)将j时刻下的所有平面参数变换到i时刻坐标系下得到(Ai(j),Bi(j),Ci(j),Di(j))，通过公式(4)计算与i时刻平面的两个参数Δr、Δd，当夹角Δr小于30度并且距离Δd小于0.5米，则将i时刻平面加入到j时刻匹配的候选平面中；
Figure FDA0003762134350000013
其中i时刻第k个平面和j时刻下第l个平面对应空间中一个平面，
Figure FDA0003762134350000014
和
Figure FDA0003762134350000015
分别为在两个坐标系下拟合得到的平面参数；上标i表示此平面坐标为i时刻下的平面参数，括号中的j表示此平面参数由j时刻下的平面参数变换得到，
Figure FDA0003762134350000016
其中，Δr用于衡量两个待匹配平面的法线方向是否一致；Δd用于衡量i时刻坐标系下坐标原点到平面的距离是否一致；
根据公式(6)构造位姿变换矩阵Rp，然后根据位姿变换矩阵Rp将i时刻平面的凸/凹包点投影到j时刻的平面
Figure FDA0003762134350000017
na向量为j时刻的法向量等于(0，0，1)，nb为j时刻某个平面的法向量，θ为两个平面的所成角度，nb向量与na向量所成的夹角即为θ，
Figure FDA0003762134350000018
和
Figure FDA0003762134350000019
为方向归一化之后向量；
D.根据图像的单应性矩阵性质并与空间三维平面点相融合形成稠密地图。
2.根据权利要求1所述的基于平面信息的视觉和激光定位建图方法，其特征在于，所述步骤B中，先通过第一次区域增长将空间点分为有效平面点，无效平面点，无曲率点和角点，然后进行第二次增长，所述第二次增长为遍历空间中所有的无曲率点和角点，判断其周围是否存在有效平面点，当存在则判断到有效平面的距离是否小于5cm，如果小于5cm则向其他的无曲率点和角点增长，直到停止。
3.根据权利要求1所述的基于平面信息的视觉和激光定位建图方法，其特征在于，所述步骤C中，当存在多个候选平面时，通过位姿变换矩阵Rp进行投影，然后通过多边形覆盖度的方法来选择凸/凹包重合率最大的平面作为最优的候选平面。
4.根据权利要求1所述的基于平面信息的视觉和激光定位建图方法，其特征在于，所述步骤C中，对于超过15个关键帧没有被观测到的地图点和空间平面，使用以往观测到这个地图点的所有关键帧对其在世界坐标系下的坐标和平面参数进行拟合,通过代价函数实现帧间的位姿估计和滑动窗口的位姿估计，代价函数为：
Figure FDA0003762134350000021
Figure FDA0003762134350000022
Figure FDA0003762134350000023
Figure FDA0003762134350000024
Figure FDA0003762134350000025
上式中
Figure FDA0003762134350000026
内容如下：
Figure FDA0003762134350000027
其中，
Figure FDA0003762134350000028
i时刻在世界坐标系下的位姿为ti,w，j时刻在世界坐标系下的位姿为tj,w，[Ri,j ti,j]为i帧和j帧的相对位姿变换矩阵；fx、fy为相机内参；pi为i帧载体坐标系下的三维点坐标；[ui vi 1]为i帧下特征点坐标与j帧下的特征点[uj vj1]匹配；[u′i v′i 1]和[u′j v′j 1]为归一化像素坐标；[Ai Bi Ci Di]和[Aj Bj Cj Dj]分别对应空间同一平面在i时刻和j时刻各自载体坐标系下的平面参数；z′j为特征点在j时刻坐标系下的深度；运算符“^”表示向量的反对称矩阵，残差e1对应着ICP算法问题，即在两个坐标系下的三维坐标已知估计两个坐标系的位姿变换矩阵；残差e2对应着PnP算法问题，即已知空间三位点的坐标和其图像中的对应坐标，计算相机的位姿；残差e3对应对极几何问题，即已知两张图像对应的特征点计算本质矩阵，e4为位姿状态估计时构建的代价函数的第四项残差。
5.根据权利要求1所述的基于平面信息的视觉和激光定位建图方法，其特征在于，所述步骤D具体包括，对于激光测量到的平面上的点，通过公式(19)直接将三维激光点变换到相机坐标系并通过相机的图像得到激光点的颜色；
Figure FDA0003762134350000031
其中，[Rl→c,tl→c]能够将激光坐标系下的点变换到相机坐标系下；
Figure FDA0003762134350000032
为激光坐标系下第i个坐标点；
Figure FDA0003762134350000033
计算得到的是在图像坐标系下的坐标；K为已知的相机内参3×3矩阵；z′为激光点在相机坐标系下的深度坐标通过
Figure FDA0003762134350000034
求解得到；I为图像灰度函数，输入为图像坐标，输出为在此像素上的灰度值；
Figure FDA0003762134350000035
为激光点对应的灰度值；
根据公式(6-33)计算得到对应的相机坐标系下的坐标，通过空间中的平面帮助图像恢复深度
使用空间中的平面帮助图像恢复深度：
Figure FDA0003762134350000036
其中，[u,v]为图像中的像素坐标；[A,B,C,D]为像素坐标[u,v]对应的平面参数；内参矩阵
Figure FDA0003762134350000037
[xl yl zl]为激光坐标系下点的坐标；[xc yc zc]为相机坐标系下点的坐标。
CN202011065887.4A 2020-09-30 2020-09-30 基于平面信息的视觉和激光定位建图方法 Active CN112197773B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011065887.4A CN112197773B (zh) 2020-09-30 2020-09-30 基于平面信息的视觉和激光定位建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011065887.4A CN112197773B (zh) 2020-09-30 2020-09-30 基于平面信息的视觉和激光定位建图方法

Publications (2)

Publication Number Publication Date
CN112197773A CN112197773A (zh) 2021-01-08
CN112197773B true CN112197773B (zh) 2022-11-11

Family

ID=74013706

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011065887.4A Active CN112197773B (zh) 2020-09-30 2020-09-30 基于平面信息的视觉和激光定位建图方法

Country Status (1)

Country Link
CN (1) CN112197773B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124880B (zh) * 2021-05-18 2023-06-13 杭州迦智科技有限公司 一种基于两种传感器数据融合的建图及定位方法、装置
CN117007061A (zh) * 2023-08-07 2023-11-07 重庆大学 一种用于无人驾驶平台的基于地标的激光slam方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11210805B2 (en) * 2017-01-13 2021-12-28 Zhejiang University Simultaneous localization and dense three-dimensional reconstruction method
CN107292925A (zh) * 2017-06-06 2017-10-24 哈尔滨工业大学深圳研究生院 基于Kinect深度相机测量方法
US10782137B2 (en) * 2019-01-28 2020-09-22 Qfeeltech (Beijing) Co., Ltd. Methods, apparatus, and systems for localization and mapping
CN110261870B (zh) * 2019-04-15 2021-04-06 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN111076733B (zh) * 2019-12-10 2022-06-14 亿嘉和科技股份有限公司 一种基于视觉与激光slam的机器人室内建图方法及系统
CN111595333B (zh) * 2020-04-26 2023-07-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及系统

Also Published As

Publication number Publication date
CN112197773A (zh) 2021-01-08

Similar Documents

Publication Publication Date Title
Ma et al. A review of 3D reconstruction techniques in civil engineering and their applications
CN112396650B (zh) 一种基于图像和激光雷达融合的目标测距系统及方法
Wei et al. A non-contact measurement method of ship block using image-based 3D reconstruction technology
Xu et al. Reconstruction of scaffolds from a photogrammetric point cloud of construction sites using a novel 3D local feature descriptor
Zhang et al. A 3D reconstruction method for pipeline inspection based on multi-vision
Oelsch et al. R-LOAM: Improving LiDAR odometry and mapping with point-to-mesh features of a known 3D reference object
CN112197773B (zh) 基于平面信息的视觉和激光定位建图方法
CN115372989A (zh) 基于激光雷达的越野自动小车长距离实时定位系统及方法
Tao et al. Stereo priori RCNN based car detection on point level for autonomous driving
Hu et al. An automatic 3D registration method for rock mass point clouds based on plane detection and polygon matching
CN103994755A (zh) 一种基于模型的空间非合作目标位姿测量方法
Xu et al. A 3D reconstruction method for buildings based on monocular vision
CN115222884A (zh) 一种基于人工智能的空间对象分析及建模优化方法
Xiong et al. Road-Model-Based road boundary extraction for high definition map via LIDAR
Manivannan et al. Vision based intelligent vehicle steering control using single camera for automated highway system
Shang et al. Single-pass inline pipeline 3D reconstruction using depth camera array
Lu et al. A lightweight real-time 3D LiDAR SLAM for autonomous vehicles in large-scale urban environment
CN114299242A (zh) 高精地图中图像处理方法、装置、设备以及存储介质
Zhang et al. Accurate real-time SLAM based on two-step registration and multimodal loop detection
CN114648639B (zh) 一种目标车辆的检测方法、系统及装置
CN115937449A (zh) 高精地图生成方法、装置、电子设备和存储介质
CN115457130A (zh) 一种基于深度关键点回归的电动汽车充电口检测定位方法
Zhao et al. Design of 3D reconstruction system on quadrotor Fusing LiDAR and camera
Salvago et al. Robust and efficient pose estimation of pipes for contact inspection using aerial robots
Zhao et al. A method of measuring stacked objects volume based on laser sensing

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant