给定一个二叉树和一个目标和python,python设计二叉树结构

　　本发明涉及机器人视觉和图像处理领域，具体涉及一种基于ORBSLAM2的八叉树构建方法。

　　背景：

　　SLAM技术的发展促进了定位、跟踪和路径规划技术的发展，进而影响到无人机、无人驾驶、机器人等热门研究领域。其中，ORB-SLAM是一种基于ORB特征的三维定位和地图构建算法(SLAM)。该算法由Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions on Robotics上，然后将其扩展到立体和rg B-d传感器，称为ORBSLAM2。ORB-SLAM增加了基于PTAM架构的地图初始化和闭环检测功能，优化了关键帧选择方法，提高了处理速度和附加

　　然而，ORB-SLAM2可以基于特征点获得稀疏点云来研究定位，但该框架没有提供构建地图部分的模块。在具体应用中，地图的目的不仅仅是辅助定位，还要满足app应用层的需求。比如机器人导航、避障、交互、局部3D重建等。机器人需要知道地图哪里不能通过，哪里可以通过，需要制作密集的地图。

　　技术要素：

　　本发明所要解决的技术问题是针对现有技术的不足，提供一种基于ORBSLAM2的八叉树构建方法.

　　为了解决本发明的技术问题，采用的技术方案是一种基于ORBSLAM2的八叉树构建方法，包括以下步骤。

　　1)数据收集

　　通过ORB-SLAM2的RGBD相机数据视觉测距仪，获取相机的姿态信息和关键帧图像信息；

　　2)构建点云地图。

　　2.1)将关键帧图像信息转换成点云信息；

　　2.2)根据摄像机姿态信息和点云信息，进行姿态变换，完成点云拼接，获得点云图；

　　2.3)对点云地图进行过滤，去除除Kinect传感器范围外的无效测量点和平均距离过大的离群点云点，然后使用下采样对点云地图进行淡化处理；

　　3)将点云地图转换成八叉树地图。

　　根据上述结构，在步骤1)中，基于ORB-SLAM2的RGBD摄像机数据的视距计算，获得摄像机的姿态信息和关键帧图像信息。具体如下。

　　启动机器人移动平台，启动控制PC，调整主机PC。此时可以在ROS上订阅Kinect的专题，实时观看摆放环境的图像信息。使用PC移动机器人，注意Kinect传感器的特性。在完成一个动作的过程中，旋转角度应控制在5以内，平移距离应控制在10cm以内；

　　然后，将时间和位置对应色深图像对逐一保存在ROS下，并按时间顺序命名；

　　根据ORB-SLAM2要求的数据格式，对上一步采集的图形进行相关处理，然后在主机PC上启动ORB-SLAM2，输入并运行采集的处理数据；

　　最后，从ORB-SLAM2的输出中获得摄像机姿态信息和关键帧图像信息，作为下一步处理的输入。

　　在上述方法中，步骤2.1)将关键帧图像信息转换为点云信息。具体如下。

　　将一幅空间点图像中的像素坐标设为[u，v，d]，点云信息中的每个点Xi有R，G，B，X，Y，Z六个分量，分别代表该点的颜色和空间位置；

　　图像中的云托管位置(x，y，z)和像素坐标通过以下公式进行转换：

　　z=d/s

　　x=(美国CX )z/fx

　　y=(v-cy )z/fy

　　这里，fx，fy是相机在X轴和Y轴上的焦距，Cx，Cy是相机坐标系中相机的光圈中心，S是深度图的缩放因子，(U，V，D)是该点在关键帧图像中的像素坐标，这里，D是深度。

　　将相机的姿态设置为[x，Y，Z，qx，qy，qz，qw]。这里，X，Y，Z，qx，qy，qz，qw分别代表摄像机的三维空间位置[x，Y，z]和摄像机的姿态四元数四个量；

　　然后，对点云信息进行姿态变换，得到最终的点云信息。

　　其中，

　　R3是33的旋转矩阵，t31是31的位移向量。

　　根据上述方法，过滤点云图像的步骤2.3)包括以下步骤：

　　2.3.1)在生成每一帧的点云时，

　　在上述方法中，为了在上述步骤3)中将点云地图转换为八叉树地图，采用了以下方法。

　　3.1)使用PCL的Octomap包，将上一步得到的点群转换为。bt型八叉树图。在这种情况下，地图没有颜色，所以需要下一步；

　　3.2)基于Octomap提供的ColorOcTree函数，对转换后的点云添加颜色信息；

　　3.3)使用Octomap库，将步骤3.2)中的点云地图转换为。ot八叉树图。

　　本发明的效果是在ORBSLAM2框架上提出了一种针对RGBD相机数据的八叉树地图构建方法，理论上是可靠的，实践上是可行的。

　　由于ORB-SLAM2得到的是稀疏的特征点地图，只能用于研究和定位，对周围环境描述不够。更重要的是，对移动机器人导航影响不大。基于此，这种方法构建的密集地图提供了地图本身的可视化需求，使我们能够大致了解环境。针对一般点云影像数据冗余的缺点，对地图进行过滤、优化和下采样，以三维八叉树的形式存储，可以大大降低地图的存储规模，使我们能够快速浏览场景的每个角落。再者，弥补了ORB-SLAM2在研究地图应用方面的不足，更能满足当今移动机器人行业快速发展对SLAM的整体要求。整体来看，这种地图构造方法对原有的ORBSLAM2进行了扩展，得到的八叉树地图比一般的稀疏地图具有更强的适用性和更低的存储成本。更重要的是，该方法对于后续应用层ORBSLAM2的研究具有重要的现实意义。

　　附图说明

　　将参考附图和实施例进一步解释本发明，其中：

　　图1是本发明实施例的方法流程图；

　　图2是本发明实施例的硬件和软件实验平台的示意图；

　　图3是本发明实施例中ORBSLAM2的算法框架图；

　　图4是本发明实施例中JS-R机器人平台和KinectV1模型的示意图；

　　图5是本发明实施例收集的数据帧和工作空间文件的示意图；

　　图6是根据本发明实施例的ORBSLAM2的操作示意图；

　　图7是根据本发明实施例的密集点云构建的示意图；

　　图8是根据本发明实施例的八叉树构造的示意图。

　　详细实施模式

　　为了使本发明的目的、技术方案和优点更加清晰，下面将结合实例对本发明进行进一步的详细说明。应当理解，这里描述的具体实施例仅用于解释本发明，而不是限制本发明。

　　如图1所示，本发明提供了一种基于ORBSLAM2的八叉树映射方法，包括以下步骤：

　　S1，基于ORB-SLAM2视觉里程表输入数据帧，运行获取相机姿态和关键帧姿态信息；

　　S11:数据采集及预处理：启动JS-R机器人上位机，通过ROS控制移动平台和移动机器人平台。这里，由于深度相机的初始化要求，本实施例中的平行移动距离控制在5-10cm，旋转角度尽量不要太大；通过ROS将Kinect传感器接收到的图像信息(颜色图和深度图)保存为一帧一帧的图片，并相应地匹配颜色图和深度图，如图2所示。为了方便和有序，颜色图和深度图被命名为“00001.png”、“00002.png”和.“00xxx.png”，分别放在“颜色”和“深度”两个文件夹中。需要注意的是，这个环节中数据帧在时间和空间上的距离不能太大，否则很容易在ORBSLAM2运行初始线程时丢帧，导致相机姿态计算失败。

　　S12:生成色彩深度图索引txt文件：在上一步的基础上，需要生成一个ORBSLAM2官方规定的txt文本，用来表示图像数据集的内容和关系。参考格式“0RGB/0.png0depth/0.png，1RGB/1.png1 depth/1.png，”如示例所示，编辑并保存它。

　　S13:制作相机参数文件：Kinect v1深度相机有一个RGB彩色摄像头，一个红外CMOS摄像头和一个红外发射器，如图4所示。摄像机的红外CMOS摄像头和红外发射器水平分布。ROS中颜色图和深度图的主题和数据格式有：(1)RGB图像：/camera/rgb/image_color，ROS数据格式：sensor_msgs/Image，OPENCV数据格式：mat，图像大小：640*480，像素数据类型：8UC3。(2)深度图像：/camera/depth/image，ROS数据格式：sensor_msgs/Image，OPENCV数据格式：mat，图像大小：640*480，像素数据类型：32FC。

　　在ROS平台上，camera_calibration包用于标定相机。该功能包基于冷月饼标定方法，通过角点检测进行标定，使实际物理坐标与图像坐标相匹配。yaml相机参数文件将由该功能包自动生成。根据ORBSLAM2例子中的参数文件(比如。/Examples/TUM1.yaml)。

　　S14、基于ORB-SLAM2的RGBD摄像机数据输入，如图3所示，按照ORBSLAM2官方规定的数据输入格式，输入上一步得到的数据帧和摄像机参数文件。图5是本发明实施例收集的数据帧和工作空间文件的示意图。然后启动ORBSLAM2，开始进入三个线程：跟踪线程，从图像中提取ORB特征，根据前一帧估计姿态，通过全局重定位初始化姿态，跟踪重建的局部地图，优化姿态，根据预设的条件和规则确定新的关键帧。局部地图线程，这部分包括插入关键帧，对最近生成的地图点进行验证和过滤，然后生成新的地图点，使用局部光束法平差(Local BA)，最后对插入的关键帧进行过滤，去除冗余的关键帧。循环检测线程，这部分主要分为两个过程，即闭环检测和闭环修正。闭环检测先用DBoW检测，再用Sim3算法计算相似度变换。闭环校正，主要是闭环融合和本质图的图优化，最终得到摄像机轨迹，CameraTrajectory.txt；图6是本实施例的ORBSLAM2的运行界面。

　　S2，构建点云地图，优化点云过滤：

　　S21:图像在计算机中的表示：

　　在灰度图像中，每个像素位置(x，y)对应一个灰度值I，所以宽度为W、高度为H的图像的数学形式可以记录为矩阵，如公式(7)所示：

　　I(x，y)Rwh (7)

　　为了让计算机表示整个实数空间，需要对一定范围内的图像进行量化。我们使用0到255之间的整数(即无符号字符，一个字节)来表示图像的灰度。于是，分辨率为640宽度和480高度的灰度可以表示如下：

　　无符号字符图像[480][640] (8)

　　在公式(8)中，第一个下标是索引组的行，第二个下标是列。

　　在传统的像素坐标系定义中，像素坐标系的原点位于图像的左上角，X轴在右侧，Y轴在底部(即前述坐标(U，V，D)中分别对应U和V的坐标轴)。如果它有第三个轴，根据右手定则，Z轴向前。这个定义和相机坐标系是一致的。我们通常说图像的宽度和列数对应X轴，而图像的行数或高度对应其Y轴。

　　一个灰色像素可以用一个8位整数记录，即0到255之间的值。当我们有更多的信息要记录时，一个字节是不够的。在RGB-D相机的深度图中，记录了每个像素到相机的距离。这个距离通常以毫米为单位，RGB-D相机的范围通常在十米左右，超过了255的最大范围。此时需要16位整数(C中的无符号short)来记录一个深度图的信息，即0到65536之间的值。如果换算成毫米，最大可达65米，对于一台RGB-D相机来说已经足够了。

　　S22:点云拼接：

　　首先，程序提供10幅RGB-D图像，并且知道每幅图像的内部和外部参数。根据RGB-D图像和摄像机内部参考，我们可以计算出任意像素在摄像机坐标系中的位置。同时，根据相机姿态，计算出这些像素在世界坐标系中的位置。找出所有像素的空间坐标，然后把上面准备的10对图像(一一对应的彩色图像和深度图像)放到slam/densemap/corner1201文件中。在color/下有从9.png10到9.png10的10个彩色图像，在depth/下有10个对应的深度图像。同时，CameraTrajectory.txt文件给出了10幅图像的相机姿态(Twc格式)。比如第一对图的外部参数是：【0:228993；0:00645704;0:0287837;0:0004327;0:113131;0:0326832;0:993042];

　　完成以下两件事：(1)根据内参计算一对RGB-D图像对应的点云；(2)根据每张图片的相机位姿(即外部参考)，将点云相加形成地图，即完成点云的拼接。

　　本实施例的点云库使用PCL(点云库)。安装后，PCL的头文件将安装在/usr/include/pcl-1.7中，库文件位于/usr/lib/中。编译点云拼接程序。同样，确保Eigen库和Opencv库在编译前安装好。最后运行可执行程序，生成的点云保存为shiyanshi1201.pcd文件。

　　S23、点云过滤优化：

　　在上面的步骤中，我们初步获得了密集的点云图像，在实际的地图构建中，我们会对点云进行一些滤波处理，以获得更好的视觉效果。在实践中，主要使用两种滤波器：外点去除滤波器和下采样滤波器。主要变化如下：

　　(1)生成每帧的点云时，去除深度值d过大或无效的点。这主要是考虑到Kinect的有效范围，超出范围后的深度值会有较大误差。本发明经测试，d值在5000-8000之间效果较好。

　　(2)利用统计滤波方法去除孤立点。过滤器计算每个点与其最近的N个点之间的距离值的分布，并移除平均距离过大的点。这样，我们保留了那些“粘在一起”的点，去掉了孤立的噪声点。本发明经测试，当n为40-80时效果更好。

　　(3)最后，使用体素滤波器进行下采样。由于多个视角的重叠，重叠区域会出现大量位置非常接近的点。这将无用地占用大量内存空间。体素过滤保证了一定大小的立方体(或体素)中只有一个点，相当于对三维空间进行了降采样，节省了大量的存储空间。本发明经测试，s为0.001-0.1时效果较好。

　　运行可执行程序，将生成的点云保存为shiyanshi120101.pcd文件。图7展示了实验室一角的点云贴图效果。

　　S3，面向应用的八叉树图转换。

　　S31:安装Octo-map:

　　具体安装编译请参考官方在线教程。如果编译没有给出任何警告，那么编译是成功的。转到下一步。

　　步骤32:将pcd转换为Octomap:

　　编译后会产生一个名为pcd2octomap的可执行文件，放在代码根目录的后向期望/文件夹中。在代码根目录中这样调用它：

　　向后期望/PC D2 octo mapdata/XXX . PC ddata/XXX . Bt

　　它的源代码是src/pcd2octomap.cpp这段代码以命令行参数1为输入文件，参数2为输出文件，将输入的pcd格式点云转换成octomap格式点云。

　　步骤33:添加颜色信息：

　　在上一步中，我们将pcd点云转换为Octo-map。但是pcd点云是有颜色信息的，Octomap提供了ColorOcTree类，可以帮助你存储颜色信息。大部分代码和上一步一样，除了把OcTree改成ColorOcTree，调用integrateNodeColor混合颜色。修改完代码后，会编译程序pcd2colorOctomap来完成颜色转换。同时，后缀改为。ot文件。呼叫格式：

　　向后期望/PC D2 colortocmapdata/XXX . PC ddata/XXX . ot

　　这一部分针对上一步得到的点云地图，功能比较初级，使用PCL包中的Octo-map库将其转换为适用性更强、存储成本更低的八叉树地图(本实施例中的点云地图大小为8.9MB，而八叉树地图约为105Kb，是近90倍的压缩率)。在实验室的角落里建立一个八叉树图的效果如图8所示。

　　本发明公开了一种基于ORB-SLAM2的八叉树映射方法，属于机器人视觉和图像处理领域。该方法主要包括三个部分：基于ORB-SLAM2的RGBD相机数据视觉里程计、点云地图构建及点云滤波优化、面向应用的八叉树地图转换。第一部分包括三个运行线程：数据采集和预处理，启动ORB-SLAM2，从而获得基于ORB-SLAM2的优化相机姿态。第二部分，针对上一步得到的相机姿态信息、深度相机模型和数学表达式，构建点云。生成每一帧点云时，去除深度值过大或无效的点，然后通过统计滤波去除孤立点。最后，使用体素滤波器进行下采样，以节省存储空间。第三部分，针对上一步得到的比较初等的点云地图，利用PCL包中的Octo-map库，将其转换成一个可以较低成本应用和存储的八叉树地图。本发明提出的基于ORB-SLAM2的八叉树映射方法是在原有ORBSLAM2框架上自主研发的RGBD相机数据的密集映射方法，不涉及三维视觉对计算机GPU或硬件平台的硬性要求。该方法理论充分，实践可行。与普通稀疏图相比，构建的八叉树图具有更强的适用性和更低的存储成本。更重要的是，这种方法对于ORBSLAM2甚至是视觉SLAM2更有效。

　　应当理解，本领域技术人员可以根据以上描述进行改进或改变，并且所有这些改进和改变应当落入所附权利要求的保护范围内。

郑重声明：本文由网友发布，不代表盛行IT的观点，版权归原作者所有，仅为传播更多信息之目的，如有侵权请联系，我们将第一时间修改或删除，多谢。