1.本发明涉及三维地图构建技术领域，特别涉及一种八叉树地图构建方法。
背景技术：2.三维地图更能够详细的描述三维空间环境，在对无人机、机器人提供定位、导航、避障等服务功能上相对于二维地图更有优势。现有技术中构建三维地图的方式包括构建三维点云地图和构建八叉树地图。但由于三维点云地图是稀疏的，无法准确清晰看出原三维场景的结构，区分可行区域和障碍物，故不可直接用于导航，而八叉树地图则可克服三维点云地图的缺陷。
3.但现有技术在构建八叉树地图时，由于在根据相邻帧图像进行位姿估计时存在累计误差，建立的地图容易出现参考平面不平、角度与实际不一致的问题，导致建图准确度降低。
4.并且现有技术在将三维点云转换为八叉树地图时，由于三维点云数据量大，转换过程耗时长，很难保证算法的实时性。
技术实现要素：5.有鉴于此，本发明的目的是提供一种基于闭环轨迹的八叉树地图构建方法，以解决现有技术在构建八叉树地图时，因根据相邻帧图像进行位姿估计存在累计误差会使建立的地图出现参考平面不平、角度与实际不一致的问题，导致所建地图准确度降低的技术问题；以及解决现有技术在将三维点云转换为八叉树地图过程中耗时长较难保证算法实时性的技术问题。
6.本发明基于闭环轨迹的八叉树地图构建方法包括步骤：
7.1)提取图像中的orb特征点，根据相邻两帧图像提取出的orb特征点进行特征点的匹配，根据匹配后的orb特征点进行位姿估计和优化，确定是否生成关键帧；
8.2)根据新加入关键帧进行地图点的生成；
9.3)进行闭环检测和闭环校正，优化步骤1)和步骤2)根据orb特征点生成的位姿和地图点，生成闭环轨迹；
10.4)保存图像文件和闭环轨迹文件：将每个关键帧对应的rgb图像保存在rgb文件夹下，rgbd图像保存在depth文件夹下，以图像对应的时间戳命名文件，形成图像文件；将每个关键帧对应的位姿存入轨迹文件，轨迹文件内容格式为时间戳和其对应的位姿；
11.5)读取轨迹文件，获取每个关键帧对应的时间戳和位姿；
12.6)读取图像文件，根据时间戳获取每个关键帧对应的rgbd图像，从rbgd图像获取地图点的三维点坐标，坐标转换公式如下，生成三维点云；
[0013][0014]
其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从rgbd图像中获取的深度；c
x
图像采集相机的光轴在图像坐标x轴方向上的偏移量，cy图像采集相机的光轴在图像坐标y轴方向上的偏移量，f
x
图像采集相机在x轴方向上的焦距，fy图像采集相机在y轴方向上的焦距；
[0015]
7)对三维点云进行滤波处理，其又包括以下步骤：
[0016]
7.1)对三维点云进行z方向的直通滤波，其包括步骤：
[0017]
a)将三维点云中所有地图点投影到世界坐标系的z轴，统计每个z坐标出现的次数和z坐标出现的总次数c；
[0018]
b)选取z坐标的最大值z
max
、最小值z
min
为初始值，以k为步长，进行迭代，统计(z
bottom
,z
top
)内的点数c
ik
，直到点数c
ik
的占比q大于98％，其中i为迭代变量，初始值为0；
[0019][0020]
c)以最后一次迭代的z
bottom
为直通滤波下限值，z
top
为直通滤波上限值，进行直通滤波；
[0021]
7.2)在三维点云中按设定大小划分三维体素栅格，每个体素栅格内的所有点都由该体素栅格内所有点的重心来显示；
[0022]
8)将三维点云转为八叉树地图；
[0023]
9)对新生成的八叉树地图按高度进行染色，同一高度对应同一颜色。
[0024]
进一步，所述步骤1)中提取图像中的orb特征点又包括步骤：
[0025]
i)提取出图像中的线段特征；
[0026]
ii)沿第i条线段的长度方向对第i条线段进行等距的预选点划分，确定划分距离的公式为：
[0027][0028]
其中，di为第i条线段对应的划分距离；n为要提取的特征点数；li为第i条线段的长度，lj为第j条线段的长度；m为此幅图像中线段的总数；
[0029]
iii)对每个预选点进行orb特征提取的自适应阈值计算，阈值计算公式为：
[0030][0031]
其中，λ为每个特征点对应的阈值，λ
max
为设置的阈值最大值，λ
min
为设置的阈值最小值，x为特征点距线段左端点的距离；
[0032]
iv)根据自适应阈值采用orbslam中的特征点提取算法对每个预选点进行orb特征提取，得到orb特征点。
[0033]
本发明的有益效果：
[0034]
1、本发明基于闭环轨迹的八叉树地图构建方法，对生成的位姿和地图点进行了闭环检测和闭环校正，降低了位姿估计累计误差，使得所建立的地图与实际更一致，提高了建图准确度。
[0035]
2、本发明基于闭环轨迹的八叉树地图构建方法，其通过对三维点云进行z轴方向的直通滤波和对三维点云进行体素滤波，去除了噪声点，减少了三维点云的数据量，缩短了八叉树地图的转化时间，保证了算法的实时性。
[0036]
3、本发明基于闭环轨迹的八叉树地图构建方法中提出的基于直线引导的自适应阈值orb特征点提取方法，相比于现有orbslam中的特征点提取算法能在线段中部、低纹理区域提取出更多的特征点，使得特征点数量更多且分布更合理，通过该方法得到的orb特征点能让相邻两帧图像的位姿估计误差降低，提高闭环轨迹的准确度，能进一步提高所建三维地图的准确度。
附图说明
[0037]
图1为闭环轨迹图；
[0038]
图2为由orbslam建立的稀疏点地图；
[0039]
图3为未进行直通滤波的三维点云图；
[0040]
图4为经过直通滤波处理后的三维点云图；
[0041]
图5为现有方法构建的八叉树地图，从图中可以看出其存在参考平面不平的问题；
[0042]
图6为现有方法构建的八叉树地图，从图中可以看出其存在角度与实际不一致的问题；
[0043]
图7为实施例中公开的方法构建的八叉树地图；
[0044]
图8为沿线段的长度等距划分预选点的效果图；
[0045]
图9为自适应阈值λ与特征点距线段左端点的距离x的关系图；
[0046]
图10为采用现有orbslam中的特征点提取算法提取图像a中特征点的效果图；
[0047]
图11为采用实施例中方法提取图像a中特征点的效果图；
[0048]
图12为采用现有orbslam中的特征点提取算法提取图像b中特征点的效果图；
[0049]
图13为采用实施例中方法提取图像b中特征点的效果图；
[0050]
图14为基于闭环轨迹的八叉树地图构建方法的流程图。
具体实施方式
[0051]
下面结合附图和实施例对本发明作进一步描述。
[0052]
本实施例基于闭环轨迹的八叉树地图构建方法包括步骤：
[0053]
1)提取图像中的orb特征点，根据相邻两帧图像提取出的orb特征点进行特征点的匹配，根据匹配后的orb特征点进行位姿估计和优化，确定是否生成关键帧。
[0054]
2)根据新加入关键帧进行地图点的生成。本步骤采用orbslam处理关键帧图像建立地图点。
[0055]
3)进行闭环检测和闭环校正，优化步骤1)和步骤2)根据orb特征点生成的位姿和地图点，生成闭环轨迹；
[0056]
闭环轨迹如图1所示，如图2所示orbslam建立的稀疏点地图无法准确清晰看出原三维场景的结构，区分可行区域和障碍物，故不可直接用于导航。
[0057]
4)保存图像文件和闭环轨迹文件：将每个关键帧对应的rgb图像保存在rgb文件夹下，rgbd图像保存在depth文件夹下，以图像对应的时间戳命名文件，形成图像文件；将每个关键帧对应的位姿存入轨迹文件，轨迹文件内容格式为时间戳和其对应的位姿。
[0058]
5)读取轨迹文件，获取每个关键帧对应的时间戳和位姿。
[0059]
6)读取图像文件，根据时间戳获取每个关键帧对应的rgbd图像，从rbgd图像获取地图点的三维点坐标，坐标转换公式如下，生成三维点云；
[0060][0061]
其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从rgbd图像中获取的深度；c
x
图像采集相机的光轴在图像坐标x轴方向上的偏移量，cy图像采集相机的光轴在图像坐标y轴方向上的偏移量，f
x
图像采集相机在x轴方向上的焦距，fy图像采集相机在y轴方向上的焦距。
[0062]
7)对三维点云进行滤波处理，其又包括以下步骤：
[0063]
7.1)对三维点云进行z方向的直通滤波，其包括步骤：
[0064]
a)将三维点云中所有地图点投影到世界坐标系的z轴，统计每个z坐标出现的次数和z坐标出现的总次数c；
[0065]
b)选取z坐标的最大值z
max
、最小值z
min
为初始值，以k＝0.05m为步长，进行迭代，当然步长还可设定为其它值，统计(z
bottom
,z
top
)内的点数c
ik
，直到点数c
ik
的占比q大于98％，其中i为迭代变量，初始值为0；
[0066][0067]
c)以最后一次迭代的z
bottom
为直通滤波下限值，z
top
为直通滤波上限值，进行直通滤波。
[0068]
对于室内场景，z方向的高度通常在一有限范围内，使用直通滤波器设置一个范围，可较快剪除z方向的离群点。对比图3和图4可知，本实施例中通过对三维点云进行z方向的直通滤波，剪除了z方向的离群点。
[0069]
7.2)对三维点云进行体素滤波。由于前述步骤生成的三维点云较为密集，在后续转为八叉树地图中会占据较多内存，花费较多时间，难以保证算法实时性。为了保证建图的实时性，本步骤在三维点云中按一定大小划分三维体素栅格，可以把体素栅格视为微小的空间三维立方体的集合，然后单个体素栅格(即三维立方体)内所有点都由该体素栅格中所有点的重心来显示。如此，通过三维点云进行体素滤波减小了地图点的数量，进而在后续转为八叉树地图中时耗时更短，能更好的保证算法实时性。
[0070]
经多次实验，体素栅格大小为3cmx3cmx3cm时效果最好，在保持三维点云的形状特征的同时减少了点云数据，提高了转化速度，保证算法实时性。以i5-7200u,8gb电脑实测将一副为640x480像素的rgbd图像转为点云，结果如下：
[0071] 所占内存大小(kb)生成点云耗时(ms)未体素滤波的点云340139体素滤波后的点云3012
[0072]
8)将三维点云转为八叉树地图。利用八叉树地图库octomap，创建一颗octree,调用octree的updatenode方法，将三维点云中的点插入octree中。
[0073]
9)对新生成的八叉树地图按高度进行染色，同一高度对应同一颜色。
[0074]
生成的八叉树地图如图7所示，通过图7与图5和图6对比可知，本实施例中方法构建的八叉树地图与实际更相符，误差更小，避免了采用现有方法构建八叉树地图存在的参考平面不平、角度与实际不一致的技术问题。
[0075]
作为对上述实施例的改进，所述步骤1)中提取图像中的orb特征点又包括步骤：
[0076]
i)提取出图像中的线段特征；具体实施中可采用edlines算法提取出图像中的线段特征。
[0077]
ii)沿第i条线段的长度方向对第i条线段进行等距的预选点划分，确定划分距离的公式为：
[0078][0079]
其中，di为第i条线段对应的划分距离；n为要提取的特征点数；li为第i条线段的长度，lj为第j条线段的长度；m为此幅图像中线段的总数；
[0080]
iii)对每个预选点进行orb特征提取的自适应阈值计算，阈值计算公式为：
[0081][0082]
其中，λ为每个特征点对应的阈值，λ
max
为设置的阈值最大值，λ
min
为设置的阈值最小值，x为特征点距线段左端点的距离。
[0083]
从图9自适应阈值λ与特征点距线段左端点的距离x的关系图可知，靠近线段两端区域的特征点不仅数量少且容易随着相机运动在下一帧中消失，而靠近线段中间区域的特征点数量较多且在相机运动中较为稳定，因此希望能提出更多靠近线段中间区域的特征点。而阈值越低，特征点提取条件就越宽松，从而将提取出更多的特征点，因此，通过自适应降低靠近线段中间区域的特征点阈值，就可以达到从一条线段中更均衡化地提取orb特征的目的。
[0084]
iv)根据自适应阈值采用orbslam中的特征点提取算法对每个预选点进行orb特征提取，得到orb特征点。
[0085]
本改进中提出的基于直线引导的自适应阈值orb特征点提取方法，相比于现有orbslam中的特征点提取算法能在线段中部、低纹理区域提取出更多的特征点，使得特征点数量更多且分布更合理，通过该方法得到的orb特征点能让相邻两帧图像的位姿估计误差降低，提高闭环轨迹的准确度，能进一步提高所建三维地图的准确度。
[0086]
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，则均应涵盖在本发明的权利要求范围当中。
技术特征：1.一种基于闭环轨迹的八叉树地图构建方法，其特征在于，包括步骤：1)提取图像中的orb特征点，根据相邻两帧图像提取出的orb特征点进行特征点的匹配，根据匹配后的orb特征点进行位姿估计和优化，确定是否生成关键帧；2)根据新加入关键帧进行地图点的生成；3)进行闭环检测和闭环校正，优化步骤1)和步骤2)根据orb特征点生成的位姿和地图点，生成闭环轨迹；4)保存图像文件和闭环轨迹文件：将每个关键帧对应的rgb图像保存在rgb文件夹下，rgbd图像保存在depth文件夹下，以图像对应的时间戳命名文件，形成图像文件；将每个关键帧对应的位姿存入轨迹文件，轨迹文件内容格式为时间戳和其对应的位姿；5)读取轨迹文件，获取每个关键帧对应的时间戳和位姿；6)读取图像文件，根据时间戳获取每个关键帧对应的rgbd图像，从rbgd图像获取地图点的三维点坐标，坐标转换公式如下，生成三维点云；其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从rgbd图像中获取的深度；c
x
图像采集相机的光轴在图像坐标x轴方向上的偏移量，c
y
图像采集相机的光轴在图像坐标y轴方向上的偏移量，f
x
图像采集相机在x轴方向上的焦距，f
y
图像采集相机在y轴方向上的焦距。7)对三维点云进行滤波处理，其又包括以下步骤：7.1)对三维点云进行z方向的直通滤波，其包括步骤：a)将三维点云中所有地图点投影到世界坐标系的z轴，统计每个z坐标出现的次数和z坐标出现的总次数c；b)选取z坐标的最大值z
max
、最小值z
min
为初始值，以k为步长，进行迭代，统计(z
bottom
,z
top
)内的点数c
ik
，直到点数c
ik
的占比q大于98％，其中i为迭代变量，初始值为0；c)以最后一次迭代的z
bottom
为直通滤波下限值，z
top
为直通滤波上限值，进行直通滤波；7.2)对三维点云进行体素滤波：在三维点云中按设定大小划分三维体素栅格，每个体素栅格内的所有点都由该体素栅格内所有点的重心来显示；8)将三维点云转为八叉树地图；9)对新生成的八叉树地图按高度进行染色，同一高度对应同一颜色。
2.根据权利要求1所述的基于闭环轨迹的八叉树地图构建方法，其特征在于：所述步骤1)中提取图像中的orb特征点又包括步骤：i)提取出图像中的线段特征；ii)沿第i条线段的长度方向对第i条线段进行等距的预选点划分，确定划分距离的公式为：其中，d
i
为第i条线段对应的划分距离；n为要提取的特征点数；l
i
为第i条线段的长度，l
j
为第j条线段的长度；m为此幅图像中线段的总数；iii)对每个预选点进行orb特征提取的自适应阈值计算，阈值计算公式为：其中，λ为每个特征点对应的阈值，λ
max
为设置的阈值最大值，λ
min
为设置的阈值最小值，x为特征点距线段左端点的距离；iv)根据自适应阈值采用orbslam中的特征点提取算法对每个预选点进行orb特征提取，得到orb特征点。
技术总结本发明公开了一种基于闭环轨迹的八叉树地图构建方法，其包括1)根据相邻两帧图像进行位姿估计和优化，确定是否生成关键帧；2)根据新加入关键帧进行地图点的生成；3)进行闭环检测和闭环校正；4)保存图像文件和轨迹文件；5)读取轨迹文件获取关键帧对应的时间戳和位姿；6)读取图像文件生成三维点云；7)对三维点云进行滤波处理；8)将三维点云转为八叉树地图；9)对新生成的八叉树地图按高度进行染色。本发明通过闭环检测和闭环校正降低了位姿估计累计误差，使得所建立的地图与实际更一致，提高了建图准确度；并通过对三维点云进行直通滤波和体素滤波，减少了三维点云的数据量，缩短了八叉树地图的转化时间，保证了算法的实时性。保证了算法的实时性。保证了算法的实时性。
技术研发人员：宋永端 沈志熙 陈宇栋
受保护的技术使用者：重庆大学
技术研发日：2022.03.22
技术公布日：2022/7/5
