CN117906595B - 基于特征点法视觉slam的场景理解导航方法及*** - Google Patents
基于特征点法视觉slam的场景理解导航方法及*** Download PDFInfo
- Publication number
- CN117906595B CN117906595B CN202410317058.2A CN202410317058A CN117906595B CN 117906595 B CN117906595 B CN 117906595B CN 202410317058 A CN202410317058 A CN 202410317058A CN 117906595 B CN117906595 B CN 117906595B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- map
- ros
- point
- camera
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 75
- 230000011218 segmentation Effects 0.000 claims abstract description 21
- 238000004891 communication Methods 0.000 claims abstract description 11
- 230000005540 biological transmission Effects 0.000 claims abstract description 10
- 230000002194 synthesizing effect Effects 0.000 claims abstract description 7
- 230000009466 transformation Effects 0.000 claims description 23
- 239000011159 matrix material Substances 0.000 claims description 19
- 230000000007 visual effect Effects 0.000 claims description 18
- 238000013519 translation Methods 0.000 claims description 12
- 238000001514 detection method Methods 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 5
- 238000013507 mapping Methods 0.000 claims description 5
- 238000005192 partition Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 230000015572 biosynthetic process Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 238000003786 synthesis reaction Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 11
- 238000013135 deep learning Methods 0.000 description 5
- 241000282414 Homo sapiens Species 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000000605 extraction Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 230000003190 augmentative effect Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000033001 locomotion Effects 0.000 description 2
- 239000003086 colorant Substances 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000002372 labelling Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000015654 memory Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
- 238000012800 visualization Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/005—Tree description, e.g. octree, quadtree
-
- 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
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3863—Structures of map data
- G01C21/387—Organisation of map data, e.g. version management or database structures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T11/00—2D [Two Dimensional] image generation
- G06T11/20—Drawing from basic elements, e.g. lines or circles
- G06T11/206—Drawing of charts or graphs
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/55—Depth or shape recovery from multiple images
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/70—Labelling scene content, e.g. deriving syntactic or semantic representations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Geometry (AREA)
- Software Systems (AREA)
- Computer Graphics (AREA)
- Databases & Information Systems (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Computational Linguistics (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于特征点法视觉SLAM的场景理解导航方法及***,包括:获取相机的实时位姿信息并通过ROS发布;将获取的RGB图和深度图合成3D点云;根据语义和深度信息,获取机器人与物体的相对位置关系;根据位姿信息将3D点云中每一帧点云拼接成全局点云地图,并发布到ROS中;获取地图点信息,并转换为点云,得到点云地图,将点云地图数据发布到ROS中,将点云地图转换为三维八叉树地图和二维栅格地图;将ORB‑SLAM3模块和实例分割网络的结果通过ROS***以话题的形式发布订阅进行通讯传输,使用ROS导航包实施导航操作。将ORB‑SLAM3与实时实例分割方法结合建立了稠密的语义地图,能够进行场景理解。
Description
技术领域
本发明属于机器视觉与机器人技术领域,本发明涉及一种基于特征点法视觉SLAM的场景理解导航方法及***。
背景技术
同时定位与地图构建技术(Simultaneous Localization And Mapping,SLAM)是机器人领域的热门问题,也是实现机器人在复杂场景自主运动的关键技术,在移动机器人、自动驾驶、无人机和增强现实(Augmented reality,AR)与虚拟现实(Virtual reality,VR)等领域有广泛的应用。
然而,传统的基于特征点法的视觉SLAM(如ORB-SLAM3)只能建立稀疏的点云地图用来定位,无法像激光SLAM那样建立二维占据栅格地图和三维八叉树地图,因此无法利用建立好的地图进行导航和避障等操作。另一方面,在传统视觉SLAM中,视觉SLAM所需要的视觉传感器可以获取大量的环境信息,如颜色、纹理和形状等,一些特殊的视觉传感器(例如深度相机)还可以获取相机与环境中物体的相对距离等,而稀疏点云地图浪费了这些丰富且有用的信息。
近年来深度学习技术取得了巨大进展,在计算机视觉领域取得了很多突破,可以结合深度学习方法来获取包括物体等场景元素的识别和标注的语义信息,并将其融合到SLAM***中,使得机器人能够以人类的视角更好地理解环境结构、语义含义以及机器人与周围物体的度量信息。另外,机器人操作***(Robot Operating System, ROS)提供了丰富的功能和接口。目前虽然有将深度学习方法融合到SLAM***中,但是两者之间的通讯并不高效,实时性较差。
如公告号为CN115393538 A的发明公开了一种基于深度学习的室内动态场景的视觉SLAM方法及***,在ORB-SLAM2框架的基础上,利用GCNv2特征提取算法代替ORB-SLAM2的相应模块,添加动态点去除模块和点云建图线程,有效减少动态特征点对视觉里程计的干扰。该GCNv2特征提取算法对图像信息进行特征提取,无法建立语义地图,不能够帮助机器人以人类的角度去理解所处的环境。
发明内容
本发明的目的在于提供一种基于特征点法视觉SLAM的场景理解导航方法及***,将ORB-SLAM3建立的稀疏点云拓展为可进行导航的地图和可进行场景理解的语义度量地图,模块之间的结果通过ROS***以话题的形式发布订阅进行通讯传输,这种架构使得***运行更加高效、实时,并且方便扩展其他功能模块。
实现本发明目的的技术解决方案为:
一种基于特征点法视觉SLAM的场景理解导航方法,包括以下步骤:
S01:从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布;
S02:将深度相机获取的RGB图和深度图合成3D点云;
S03:通过实例分割网络获取的语义和深度信息,获取机器人与物体的相对位置关系;
S04:根据实时位姿信息将3D点云中每一帧点云拼接成全局点云地图,将全局点云地图发布到ROS中;
S05:获取ORB-SLAM3的地图点信息,并转换为点云,得到点云地图,将点云地图数据发布到ROS中,将点云地图转换为三维八叉树地图和二维栅格地图;
S06:将ORB-SLAM3模块和实例分割网络的结果通过ROS***以话题的形式发布订阅进行通讯传输,使用ROS导航包实施导航操作。
优选的技术方案中,所述步骤S01中从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布的方法包括:
S11:从ORB-SLAM3的TrackRGBD接口获取Sophus::SE3f格式的相机位姿;
S12:将Sophus::SE3f格式的位姿转换为cv::Mat格式Tcw,,/>为旋转矩阵,/>,t为平移向量,/>;为相机位姿的旋转矩阵函数,/>为相机位姿的平移矩阵函数;
S13:将相机逆位姿Tcw转换为正位姿Twc,并从中提取旋转矩阵RWC和平移向量tWC;
S14:将旋转矩阵RWC构建为一个四元数q;
S15:将相机的平移向量tWC和旋转四元数q分别复制给一个ROS消息类型的变量,用于发布相机位姿信息。
优选的技术方案中,所述步骤S02中合成3D点云的方法包括:
S21:读取深度图像的像素点,获取深度值d;
S22:根据相机内参和深度值d,计算当前像素点的三维坐标;
计算方法为:
其中:为像素坐标系下的像素点坐标,/>为像素点p的三维坐标值,,/>,/>,/>为相机内参,/>为缩放因子;
S23:获取彩色图像对应像素点的RGB颜色值,将RGB颜色值存储在点p的b、g、r成员变量中;
S24:对每一帧图像进行上述处理生成3D点云,对生成的点云进行旋转和缝合操作,将其转换到指定的坐标系中;
S25:对点云进行体素滤波。
优选的技术方案中,所述步骤S03中获取机器人与物体的相对位置关系的方法包括:
S31:将YOLOv8-seg实例分割网络使用C++版本OpenCV-DNN模块进行部署;
S32:将部署后的YOLOv8-seg写成ROS节点,并发布检测和分割结果;
S33:将RGB图送入到YOLOv8-seg实例分割网络获取每个类别的目标检测结果;
S34:从结果中读取每个类别的矩形框在像素坐标系下的坐标,并求出矩形框中心点坐标;
S35:根据像素坐标系下的每个类别的中心点坐标对点云坐标系进行索引,获取图像像素坐标系下的中心点坐标对应的点云坐标/>;
S36:通过ROS下tf坐标变换计算相机位姿与点云中每一类物体中心点位置的坐标变换关系;
S37:将坐标变换关系通过ROS发布,然后在rviz中订阅。
优选的技术方案中,所述步骤S04中拼接成全局点云地图的方法包括:
S41:将获取的相机的实时位姿转换为相机到世界坐标系的变换矩阵;
S42:根据变换矩阵计算相邻两帧点云之间的相对变换;
S43:将当前帧中的点云与相应的位姿进行配准,并将配准后的点云添加到全局点云地图中。
优选的技术方案中,所述步骤S05中得到点云地图的方法包括:
S51:读取ORB-SLAM3中的地图点信息;
S52:创建sensor_msgs::PointCloud2类型的点云数据,并对其进行初始化;
S53:遍历地图点,将其转换为Eigen::Vector3d类型,并将转换后的坐标储存在数组中,使用memcpy函数将数组中的数据复制到sensor_msgs::PointCloud2类型的点云数据的具***置,将点云数据发布到ROS。
优选的技术方案中,所述步骤S05中将点云地图转换为三维八叉树地图和二维栅格地图的方法包括:
S61:根据点云的范围,分辨率和栅格地图的大小,创建并初始化一个空的OccupancyGrid数据结构;
S62:对于每个点云数据,将其坐标转换为OccupancyGrid上的栅格单元,并将对应的栅格单元标记为相应的状态,所述状态包括空闲、自由或未知;
S63:将生成的OccupancyGrid数据发布到相应的话题;
S64:创建并初始化一个OccupancyMap数据结构,并设置其基本参数;
S65:遍历OccupancyGrid中的每个单元格,将单元格信息映射到OccupancyMap中;
S66:将生成的OccupancyMap数据发布到相应的话题。
优选的技术方案中,所述步骤S06中使用ROS导航包实施导航操作的方法包括:
S71:移动机器人或者相机,增量式建立所需要环境的栅格地图;
S72:建立完成后,保存栅格地图;
S73:使用ROS的move_base节点,在rviz中进行导航操作。
本发明又公开了一种基于特征点法视觉SLAM的场景理解导航***,包括:
相机实时位姿获取发布模块,从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布;
3D点云合成模块,将深度相机获取的RGB图和深度图合成3D点云;
场景理解模块,通过实例分割网络获取的语义和深度信息,获取机器人与物体的相对位置关系;
全局点云地图生成发布模块,根据实时位姿信息将3D点云中每一帧点云拼接成全局点云地图,将全局点云地图发布到ROS中;
ORB-SLAM3视觉导航地图模块,获取ORB-SLAM3的地图点信息,并转换为点云,得到点云地图,将点云地图数据发布到ROS中,将点云地图转换为三维八叉树地图和二维栅格地图;
导航模块,将ORB-SLAM3模块和实例分割网络的结果通过ROS***以话题的形式发布订阅进行通讯传输,使用ROS导航包实施导航操作。
本发明又公开了一种计算机存储介质,其上存储有计算机程序,所述计算机程序被执行时实现上述的基于特征点法视觉SLAM的场景理解导航方法。
本发明与现有技术相比,其显著优点为:
1、发明改进了ORB-SLAM3只能建立稀疏地图的弊端,结合了当下速度最快,准确度最高的YOLOv8-seg实时实例分割方法建立了稠密的语义地图,能够帮助机器人以人类的角度去理解所处的环境;另外,实时发布机器人与环境中先验物体的相对位置和距离关系,方便机器人实现抓取等动作。
2、本发明将稀疏地图转化为了类似激光雷达建立的占据栅格地图。通过对栅格地图进行分析和处理,可以实现路径规划、避障、导航等功能,大大拓展了视觉SLAM的工作范围,使得视觉SLAM***更加全面和灵活。
3、本发明各模块均在ROS框架下运行,将ORB-SLAM3模块和YOLOv8-seg实例分割模块均改写成了ROS节点,模块之间的结果通过ROS***以话题的形式发布订阅进行通讯传输。这种架构使得***运行更加高效、实时,并且方便扩展其他功能模块。同时,由于ROS框架广泛应用于机器人领域,本发明也使得整个***更易于与其他ROS节点进行集成和协同工作,进一步拓展了视觉SLAM在机器人导航、环境感知等方面的应用潜力。
附图说明
图1为本实施例基于特征点法视觉SLAM的场景理解导航方法的流程图;
图2为本实施例基于特征点法视觉SLAM的场景理解导航***的工作流程图;
图3为纯视觉建立的三维八叉树地图;
图4为纯视觉建立的二维占据栅格地图;
图5为场景理解示意图。
具体实施方式
本发明的原理是:将ORB-SLAM3建立的稀疏点云拓展为可进行导航的地图和可进行场景理解的语义度量地图,模块之间的结果通过ROS***以话题的形式发布订阅进行通讯传输,这种架构使得***运行更加高效、实时,并且方便扩展其他功能模块。
实施例1:
如图1所示,一种基于特征点法视觉SLAM的场景理解导航方法,包括以下步骤:
S01:从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布;
S02:将深度相机获取的RGB图和深度图合成3D点云;
S03:通过实例分割网络获取的语义和深度信息,获取机器人与物体的相对位置关系;
S04:根据实时位姿信息将3D点云中每一帧点云拼接成全局点云地图,将全局点云地图发布到ROS中;
S05:获取ORB-SLAM3的地图点信息,并转换为点云,得到点云地图,将点云地图数据发布到ROS中,将点云地图转换为三维八叉树地图和二维栅格地图;
S06:将ORB-SLAM3模块和实例分割网络的结果通过ROS***以话题的形式发布订阅进行通讯传输,使用ROS导航包实施导航操作。
这里每一步骤的具体实现参见下面基于特征点法视觉SLAM的场景理解导航***的工作流程,这里不再赘述。
另一实施例中,一种计算机存储介质,其上存储有计算机程序,所述计算机程序被执行时实现上述的基于特征点法视觉SLAM的场景理解导航方法。
该基于特征点法视觉SLAM的场景理解导航方法可以采用上述的任一种的于特征点法的视觉SLAM的导航方法,具体实现这里不再赘述。
又一实施例中,一种基于特征点法视觉SLAM的场景理解导航***,包括:
相机实时位姿获取发布模块,从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布;
3D点云合成模块,将深度相机获取的RGB图和深度图合成3D点云;
场景理解模块,通过实例分割网络获取的语义和深度信息,获取机器人与物体的相对位置关系;
全局点云地图生成发布模块,根据实时位姿信息将3D点云中每一帧点云拼接成全局点云地图,将全局点云地图发布到ROS中;
ORB-SLAM3视觉导航地图模块,获取ORB-SLAM3的地图点信息,并转换为点云,得到点云地图,将点云地图数据发布到ROS中,将点云地图转换为三维八叉树地图和二维栅格地图;
导航模块,将ORB-SLAM3模块和实例分割网络的结果通过ROS***以话题的形式发布订阅进行通讯传输,使用ROS导航包实施导航操作。
具体的,下面以一较佳的实施例为例对基于特征点法视觉SLAM的场景理解导航***的工作流程说明如图2所示:
S1:从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布。
具体实现包括以下步骤:
S11:安装,运行ORB-SLAM3;
S12:从ORB-SLAM3的TrackRGBD接口获取Sophus::SE3f格式的相机位姿;
S13:将Sophus::SE3f格式的位姿转换为cv::Mat格式Tcw;
具体的,TrackRGBD是ORB-SLAM3中用来处理RGB-D相机数据的接口也,返回的是Sophus::SE3f格式的相机位姿;Sophus::SE3f是Sophus C++库中的一种数据类型,用于表示三维空间中的刚体变换,包含了平移和旋转,函数返回的是旋转部分,/>函数返回的是平移部分。
旋转矩阵;平移向量,则/>。
S14:将相机逆位姿Tcw转换为正位姿Twc,并从中提取旋转矩阵RWC和平移向量tWC;
S15:将旋转矩阵RWC构建为一个四元数q;
S16:将相机的平移向量tWC和旋转四元数q分别复制给一个ROS消息类型的变量,用于发布相机位姿信息。
S2:通过深度相机获取的RGB图和深度图合成3D点云。
具体实现包括以下步骤:
S21:读取深度图像的像素点,获取深度值d;
S22:根据相机内参和深度值d,计算当前像素点的三维坐标;
具体的,计算方法如下:
其中:为像素坐标系下的像素点坐标,/>为三维坐标p的值,/>,/>,,/>为相机内参,/>为缩放因子。
S23:获取彩色图像对应像素点的RGB颜色值,将RGB颜色值存储在点p的b、g、r成员变量中;
S24:重复上面步骤,对每一帧图像生成3D点云;
S25:对生成的点云进行旋转和缝合操作,将其转换到指定的坐标系中;
S26:对点云进行体素滤波,以减少点云数据的数量和噪声。
S3:结合语义和深度信息,获取机器人与物体的相对位置关系。如图5所示。环境中的先验物体已经被标记成不同的颜色,同时通过RGB-D相机实时指出机器人与先验物体的相对位置和距离关系。
具体实现包括以下步骤:
S31:将YOLOv8-seg实例分割网络使用C++版本OpenCV-DNN模块进行部署;
S32:将部署后的YOLOv8-seg写成ROS节点,并发布检测和分割结果;
S33:将RGB图送入到YOLOv8-seg实例分割网络获取每个类别的目标检测结果;
S34:从结果中读取每个类别的矩形框在像素坐标系下的坐标,并求出矩形框中心点坐标;
S35:根据像素坐标系下的每个类别的中心点坐标对点云坐标系进行索引,获取图片像素坐标系下的中心点坐标对应的点云坐标/>;
具体的:
其中:为索引值,/>为图片或者相机的横向分辨率。将索引值输入到生成好的单帧点云中就可以获得点云的坐标/>,其中/>为机器人与物体的距离。
S36:通过ROS下tf坐标变换计算相机位姿与点云中每一类物体中心点位置的坐标变换关系,坐标变换关系即为机器人与物体的相对位置关系;计算量小。
S37:将坐标变换关系通过ROS发布,然后在rviz中订阅。
S4:结合S1获取的位姿信息,将S2中每一帧点云拼接成全局一致的3D点云。
RViz(Robot Visualization)是一个用于可视化机器人***的开源工具,用于显示和调试机器人的传感器数据、状态信息和运动规划等。
具体实现包括以下步骤:
S41:将S1中的位姿转换为相机到世界坐标系的变换矩阵;
S42:根据变换矩阵计算相邻两帧点云之间的相对变换;
S43:将当前帧中的点云与相应的位姿进行配准,并将配准后的点云添加到全局点云地图中;
S44:将全局点云地图发布到ROS中。
S5:获取ORB-SLAM3的地图点信息,并转换为点云。
具体实现包括以下步骤:
S51:读取ORB-SLAM3中的地图点信息;
S52:创建sensor_msgs::PointCloud2类型的变量,并对其进行初始化;
S53:遍历地图点,将其转换为Eigen::Vector3d类型,并将转换后的坐标储存在数组中,使用memcpy函数将数组中的数据复制到点云数据的具***置;
S54:将点云数据发布到ROS,模拟多线激光雷达生成的点云。
S6:将点云地图转换为三维八叉树地图和二维栅格地图,如图3、图4所示。图3中方格则代表环境中存在的障碍物。图4中其他区域代表机器人可以通行的部分,黑色区域则代表该位置存在障碍物,在进行路径规划和导航时需要机器人避开此区域。
memcpy函数是C/C++语言中的一个标准库函数,用于在内存之间进行数据拷贝。
具体实现包括以下步骤:
S61:根据点云的范围,分辨率和栅格地图的大小,创建并初始化一个空的OccupancyGrid数据结构;OccupancyGrid是ROS(机器人操作***)中定义的一种消息类型,用于表示三维概率地图,也就是octomap地图。
S62:对于每个点云数据,将其坐标转换为OccupancyGrid上的栅格单元,并将对应的栅格单元标记为相应的状态,如空闲、自由或未知;
S63:将生成的OccupancyGrid数据发布到相应的话题;
S64:创建并初始化一个OccupancyMap数据结构,并设置其基本参数;OccupancyMap主要是用来将三维的OccupancyGrid概率地图转换为二维的占据栅格地图。
S65:遍历OccupancyGrid中的每个单元格,将单元格信息映射到OccupancyMap中;
S66:将生成的OccupancyMap数据发布到相应的话题。
S7:保存建立好的栅格地图,使用ROS导航包实施导航操作。
具体实现包括以下步骤:
S71:缓慢移动机器人或者相机,增量式建立所需要环境的栅格地图;
S72:建立完成后,保存栅格地图;
S73:使用ROS的move_base节点,在rviz中进行导航操作。
在ROS中,move_base是一个用于实现机器人导航功能的重要软件包。move_base包含了一系列节点和算法,用于实现机器人在未知环境中的路径规划、避障和导航功能。
上述实施例为本发明优选地实施方式,但本发明的实施方式并不受上述实施例的限制,其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化,均应为等效的置换方式,都包含在本发明的保护范围之内。
Claims (9)
1.一种基于特征点法视觉SLAM的场景理解导航方法,其特征在于,包括以下步骤:
S01:从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布;
S02:将深度相机获取的RGB图和深度图合成3D点云;
S03:通过实例分割网络获取的语义和深度信息,获取机器人与物体的相对位置关系;所述获取机器人与物体的相对位置关系的方法包括:
S31:将YOLOv8-seg实例分割网络使用C++版本OpenCV-DNN模块进行部署;
S32:将部署后的YOLOv8-seg写成ROS节点,并发布检测和分割结果;
S33:将RGB图送入到YOLOv8-seg实例分割网络获取每个类别的目标检测结果;
S34:从结果中读取每个类别的矩形框在像素坐标系下的坐标,并求出矩形框中心点坐标;
S35:根据像素坐标系下的每个类别的中心点坐标对点云坐标系进行索引,获取图像像素坐标系下的中心点坐标对应的点云坐标/>;
S36:通过ROS下tf坐标变换计算相机位姿与点云中每一类物体中心点位置的坐标变换关系;
S37:将坐标变换关系通过ROS发布,然后在rviz中订阅;
S04:根据实时位姿信息将3D点云中每一帧点云拼接成全局点云地图,将全局点云地图发布到ROS中;
S05:获取ORB-SLAM3的地图点信息,并转换为点云,得到点云地图,将点云地图数据发布到ROS中,将点云地图转换为三维八叉树地图和二维栅格地图;
S06:将ORB-SLAM3模块和实例分割网络的结果通过ROS***以话题的形式发布订阅进行通讯传输,使用ROS导航包实施导航操作。
2.根据权利要求1所述的基于特征点法视觉SLAM的场景理解导航方法,其特征在于,所述步骤S01中从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布的方法包括:
S11:从ORB-SLAM3的TrackRGBD接口获取Sophus::SE3f格式的相机位姿;
S12:将Sophus::SE3f格式的位姿转换为cv::Mat格式Tcw,,/>为旋转矩阵,/>,t为平移向量,/>;为相机位姿的旋转矩阵函数,/>为相机位姿的平移矩阵函数;
S13:将相机逆位姿Tcw转换为正位姿Twc,并从中提取旋转矩阵RWC和平移向量tWC;
S14:将旋转矩阵RWC构建为一个四元数q;
S15:将相机的平移向量tWC和旋转四元数q分别复制给一个ROS消息类型的变量,用于发布相机位姿信息。
3.根据权利要求1所述的基于特征点法视觉SLAM的场景理解导航方法,其特征在于,所述步骤S02中合成3D点云的方法包括:
S21:读取深度图像的像素点,获取深度值d;
S22:根据相机内参和深度值d,计算当前像素点的三维坐标;
计算方法为:
,
其中:为像素坐标系下的像素点坐标,/>为像素点p的三维坐标值,/>,/>,,/>为相机内参,/>为缩放因子;
S23:获取彩色图像对应像素点的RGB颜色值,将RGB颜色值存储在点p的b、g、r成员变量中;
S24:对每一帧图像进行上述处理生成3D点云,对生成的点云进行旋转和缝合操作,将其转换到指定的坐标系中;
S25:对点云进行体素滤波。
4.根据权利要求1所述的基于特征点法视觉SLAM的场景理解导航方法,其特征在于,所述步骤S04中拼接成全局点云地图的方法包括:
S41:将获取的相机的实时位姿转换为相机到世界坐标系的变换矩阵;
S42:根据变换矩阵计算相邻两帧点云之间的相对变换;
S43:将当前帧中的点云与相应的位姿进行配准,并将配准后的点云添加到全局点云地图中。
5.根据权利要求1所述的基于特征点法视觉SLAM的场景理解导航方法,其特征在于,所述步骤S05中得到点云地图的方法包括:
S51:读取ORB-SLAM3中的地图点信息;
S52:创建sensor_msgs::PointCloud2类型的点云数据,并对其进行初始化;
S53:遍历地图点,将其转换为Eigen::Vector3d类型,并将转换后的坐标储存在数组中,使用memcpy函数将数组中的数据复制到sensor_msgs::PointCloud2类型的点云数据的具***置,将点云数据发布到ROS。
6.根据权利要求1所述的基于特征点法视觉SLAM的场景理解导航方法,其特征在于,所述步骤S05中将点云地图转换为三维八叉树地图和二维栅格地图的方法包括:
S61:根据点云的范围,分辨率和栅格地图的大小,创建并初始化一个空的OccupancyGrid数据结构;
S62:对于每个点云数据,将其坐标转换为OccupancyGrid上的栅格单元,并将对应的栅格单元标记为相应的状态,所述状态包括空闲、自由或未知;
S63:将生成的OccupancyGrid数据发布到相应的话题;
S64:创建并初始化一个OccupancyMap数据结构,并设置其基本参数;
S65:遍历OccupancyGrid中的每个单元格,将单元格信息映射到OccupancyMap中;
S66:将生成的OccupancyMap数据发布到相应的话题。
7.根据权利要求1所述的基于特征点法视觉SLAM的场景理解导航方法,其特征在于,所述步骤S06中使用ROS导航包实施导航操作的方法包括:
S71:移动机器人或者相机,增量式建立所需要环境的栅格地图;
S72:建立完成后,保存栅格地图;
S73:使用ROS的move_base节点,在rviz中进行导航操作。
8.一种基于特征点法视觉SLAM的场景理解导航***,其特征在于,包括:
相机实时位姿获取发布模块,从ORB-SLAM3获取相机的实时位姿信息并通过ROS发布;
3D点云合成模块,将深度相机获取的RGB图和深度图合成3D点云;
场景理解模块,通过实例分割网络获取的语义和深度信息,获取机器人与物体的相对位置关系;所述获取机器人与物体的相对位置关系的方法包括:
S31:将YOLOv8-seg实例分割网络使用C++版本OpenCV-DNN模块进行部署;
S32:将部署后的YOLOv8-seg写成ROS节点,并发布检测和分割结果;
S33:将RGB图送入到YOLOv8-seg实例分割网络获取每个类别的目标检测结果;
S34:从结果中读取每个类别的矩形框在像素坐标系下的坐标,并求出矩形框中心点坐标;
S35:根据像素坐标系下的每个类别的中心点坐标对点云坐标系进行索引,获取图像像素坐标系下的中心点坐标对应的点云坐标/>;
S36:通过ROS下tf坐标变换计算相机位姿与点云中每一类物体中心点位置的坐标变换关系;
S37:将坐标变换关系通过ROS发布,然后在rviz中订阅;
全局点云地图生成发布模块,根据实时位姿信息将3D点云中每一帧点云拼接成全局点云地图,将全局点云地图发布到ROS中;
ORB-SLAM3视觉导航地图模块,获取ORB-SLAM3的地图点信息,并转换为点云,得到点云地图,将点云地图数据发布到ROS中,将点云地图转换为三维八叉树地图和二维栅格地图;
导航模块,将ORB-SLAM3模块和实例分割网络的结果通过ROS***以话题的形式发布订阅进行通讯传输,使用ROS导航包实施导航操作。
9.一种计算机存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被执行时实现权利要求1-7任一项所述的基于特征点法视觉SLAM的场景理解导航方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410317058.2A CN117906595B (zh) | 2024-03-20 | 2024-03-20 | 基于特征点法视觉slam的场景理解导航方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410317058.2A CN117906595B (zh) | 2024-03-20 | 2024-03-20 | 基于特征点法视觉slam的场景理解导航方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117906595A CN117906595A (zh) | 2024-04-19 |
CN117906595B true CN117906595B (zh) | 2024-06-21 |
Family
ID=90686301
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202410317058.2A Active CN117906595B (zh) | 2024-03-20 | 2024-03-20 | 基于特征点法视觉slam的场景理解导航方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117906595B (zh) |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110264563A (zh) * | 2019-05-23 | 2019-09-20 | 武汉科技大学 | 一种基于orbslam2的八叉树建图方法 |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111814683B (zh) * | 2020-07-09 | 2022-12-09 | 北京航空航天大学 | 一种基于语义先验和深度学习特征的鲁棒视觉slam方法 |
CN113379910B (zh) * | 2021-06-09 | 2023-06-02 | 山东大学 | 基于slam的移动机器人矿山场景重建方法及*** |
CN114332360A (zh) * | 2021-12-10 | 2022-04-12 | 深圳先进技术研究院 | 一种协同三维建图方法及*** |
CN115035260A (zh) * | 2022-05-27 | 2022-09-09 | 哈尔滨工程大学 | 一种室内移动机器人三维语义地图构建方法 |
CN114898062A (zh) * | 2022-05-28 | 2022-08-12 | 广东工业大学 | 一种基于动态场景下slam的地图构建方法及装置 |
CN115393538A (zh) * | 2022-08-22 | 2022-11-25 | 山东大学 | 基于深度学习的室内动态场景的视觉slam方法及*** |
CN116030130A (zh) * | 2022-12-29 | 2023-04-28 | 西北工业大学 | 一种动态环境下的混合语义slam方法 |
CN116295412A (zh) * | 2023-03-01 | 2023-06-23 | 南京航空航天大学 | 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法 |
-
2024
- 2024-03-20 CN CN202410317058.2A patent/CN117906595B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110264563A (zh) * | 2019-05-23 | 2019-09-20 | 武汉科技大学 | 一种基于orbslam2的八叉树建图方法 |
Non-Patent Citations (1)
Title |
---|
基于语义ORB-SLAM2算法的移动机器人自主导航方法研究;陈国军;陈巍;郁汉琪;王涵立;;机床与液压;20200515(第09期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN117906595A (zh) | 2024-04-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108401461A (zh) | 三维建图方法、装置、***、云端平台、电子设备和计算机程序产品 | |
Schmid et al. | View planning for multi-view stereo 3D reconstruction using an autonomous multicopter | |
CN108876814B (zh) | 一种生成姿态流图像的方法 | |
Chen et al. | 3d point cloud processing and learning for autonomous driving | |
JP2022077976A (ja) | 画像ベースの測位方法及びシステム | |
CN111275015A (zh) | 一种基于无人机的电力巡线电塔检测识别方法及*** | |
CN115641401A (zh) | 一种三维实景模型的构建方法及相关装置 | |
CN112991534B (zh) | 一种基于多粒度物体模型的室内语义地图构建方法及*** | |
CN111696199A (zh) | 同步定位建图的地空融合精密三维建模方法 | |
CN113192200B (zh) | 一种基于空三并行计算算法的城市实景三维模型的构建方法 | |
WO2024087962A1 (zh) | 车厢姿态识别***、方法、电子设备及存储介质 | |
CN117906595B (zh) | 基于特征点法视觉slam的场景理解导航方法及*** | |
CN117372613A (zh) | 一种基于图像的多目标三维建筑模型快速构建方法 | |
Patel et al. | Collaborative mapping of archaeological sites using multiple uavs | |
CN111354044B (zh) | 一种基于正弦曲线拟合的全景视觉罗盘估计方法及其应用 | |
JP3512919B2 (ja) | 物体形状・カメラ視点移動の復元装置及び復元方法 | |
Leonardis et al. | Confluence of computer vision and computer graphics | |
Kawasaki et al. | Light field rendering for large-scale scenes | |
Fu et al. | Costmap construction and pseudo-lidar conversion method of mobile robot based on monocular camera | |
Zhang et al. | Colorful Reconstruction from Solid-State-LiDAR and Monocular Version | |
CN118379330B (zh) | 基于四摄像头的目标跟踪方法、集群机器人和存储介质 | |
CN118071955B (zh) | 一种基于Three.JS实现三维地图埋点方法 | |
Chen et al. | 3DVPS: A 3D Point Cloud-Based Visual Positioning System | |
Joska | WildPose: Long-Range 3D Motion Tracking of Cheetahs in the Wild Using Multi-Sensor Fusion | |
Gallego et al. | 3D reconstruction and mapping from stereo pairs with geometrical rectification |
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 |