CN115655264B - 位姿估计方法及设备 - Google Patents
位姿估计方法及设备 Download PDFInfo
- Publication number
- CN115655264B CN115655264B CN202211161417.7A CN202211161417A CN115655264B CN 115655264 B CN115655264 B CN 115655264B CN 202211161417 A CN202211161417 A CN 202211161417A CN 115655264 B CN115655264 B CN 115655264B
- Authority
- CN
- China
- Prior art keywords
- frame
- camera data
- image feature
- feature points
- information
- 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 67
- 230000015654 memory Effects 0.000 claims description 32
- 230000011218 segmentation Effects 0.000 claims description 25
- 230000004927 fusion Effects 0.000 claims description 23
- 238000007781 pre-processing Methods 0.000 claims description 18
- 238000004590 computer program Methods 0.000 claims description 15
- 230000010354 integration Effects 0.000 claims description 13
- 238000005457 optimization Methods 0.000 claims description 11
- 239000000284 extract Substances 0.000 claims description 9
- 230000001629 suppression Effects 0.000 claims description 4
- 230000010365 information processing Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 abstract description 10
- 230000008569 process Effects 0.000 abstract description 8
- 230000007613 environmental effect Effects 0.000 abstract description 3
- 230000000875 corresponding effect Effects 0.000 description 8
- 230000005291 magnetic effect Effects 0.000 description 6
- 230000006870 function Effects 0.000 description 4
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 210000003141 lower extremity Anatomy 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000003936 working memory Effects 0.000 description 1
Abstract
本发明提供一种位姿估计方法及设备，本发明旨在充分利用不同传感器的优势，激光雷达可以直接测量获得空间点云的三维信息，并且测量结果误差范围稳定，不会在长时间的工作之后发生尺度漂移，这在本发明的位姿估计过程中是一项重要的鲁棒性来源；同时利用相机图像可以方便的提取环境特征和语义信息，结合IMU信息，构建了一种融合激光雷达测量、视觉测量和IMU的位姿估计系统，可以以较高的频率稳定的输出准确的位姿信息，供下游模块使用。
Description
技术领域
本发明涉及一种位姿估计方法及设备。
背景技术
位姿估计在机器人和无人驾驶等领域有十分重要的作用，机器人需要稳定并迅速的获知自身在环境当中的位置和姿态，并采取相应的行动。
激光雷达和相机都可以单独的进行位姿估计，激光雷达可以直接测量出深度信息，但是提取特征等抽象信息较为困难，且其深度信息的测量精确度易受雨雪等天气的影响。
借由相机拍摄的图像信息可以提取丰富的特征信息或语义信息。单目相机由于相机投影过程的原理，测量过程会丢失图像像素点的深度信息，基于单目相机的视觉里程计在非结构化环境和室外大规模场景下难以获得全局一致的轨迹，且随着时间的推移会产生较大的轨迹漂移，并且纯单目相机视觉里程计所产生的轨迹也是没有绝对尺度的，因此单目相机视觉里程计所产生的定位结果无法用于机器人在真实环境中的导航。
发明内容
针对现有技术存在的问题，本发明提供一种位姿估计方法及设备。
本发明提供一种位姿估计方法，包括：
数据预处理模块获取装置的各帧相机数据，从各帧相机数据的前帧相机数据和后帧相机数据中提取图像特征点，并基于提取到的图像特征点，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息；
数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点；
帧间位姿估计模块获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的位姿信息。
进一步的，上述位姿估计方法中，数据预处理模块获取装置的各帧相机数据，从各帧相机数据的前帧相机数据和后帧相机数据中提取图像特征点，并基于提取到的图像特征点，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息，包括：
语义分割模块获取装置的各帧相机数据，并输出各帧相机数据中的每个像素语义分割信息，并为每个像素打上分类语义标签，以得到语义分割图像；
数据预处理模块分别从前帧相机数据和后帧相机数据中提取图像特征点，并基于所述语义分割图像，对提取到的前帧相机数据和后帧相机数据的图像特征点中的动态图像特征点进行剔除，对未剔除的剩余的图像特征点进行匹配，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息。
进一步的，上述位姿估计方法中，对未剔除的剩余的图像特征点进行匹配，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息之后，还包括：
通过非极大抑制方法，从前帧相机数据和后帧相机数据的图像特征点之间的匹配信息中，剔除错误的匹配信息，以得到前帧相机数据和后帧相机数据的图像特征点之间的正确匹配信息；
数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点，包括：
数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的正确图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点。
进一步的，上述位姿估计方法中，数据融合模块将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点，包括：
数据融合模块获取数据预处理模块所提取到的前帧相机数据和后帧相机数据中图像特征点，在各帧相机数据提取到的图像特征点周围提取一圈不共线的激光反投影点集F；
数据融合模块结合所述语义分割图像，从激光反投影点集F中筛选出与各图像特征点的语义标签相同的激光反投影点，获取筛选出的激光反投影点对应的激光点，形成点集Fseg；
对点集Fseg激光点进行平面特征拟合，以得到平面特征拟合结果以及平面特征拟合的深度信息；
将各个平面特征拟合结果以及平面特征拟合的深度信息，作为对应图像特征点的深度信息值。
进一步的，上述位姿估计方法中，将各个平面特征拟合结果以及平面特征拟合的深度信息，作为对应图像特征点的深度信息值之后，还包括：
对于深度信息值超过阈值的图像特征点，去除其深度信息值。
进一步的，上述位姿估计方法中，帧间位姿估计模块获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的位姿信息，还包括：
帧间位姿估计模块获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的初步位姿信息；
帧间位姿估计模块获取IMU预积分，基于IMU预积分和装置的初步位姿信息进行跟踪，以得到各帧相机数据之间的跟踪结果；
帧间位姿估计模块根据跟踪结果，判断是否创建滑动窗口关键帧地图或是否在滑动窗口关键帧地图中插入新的关键帧。
进一步的，上述位姿估计方法中，根据跟踪结果判断是否创建滑动窗口关键帧地图或是否在滑动窗口关键帧地图中插入新的关键帧之后，还包括：
若帧间位姿估计模块判断为创建滑动窗口关键帧地图或在滑动窗口关键帧地图中插入新的关键帧，则后端优化模块创建滑动窗口关键帧地图或在滑动窗口关键帧地图中插入新的关键帧，以得到更新后的滑动窗口关键帧地图；
后端优化模块对滑动窗口关键帧地图中的装置的位姿信息进行优化，以得到优化后的装置的位姿信息。
进一步的，上述位姿估计方法中，帧间位姿估计模块获取IMU预积分之前，还包括：
后端优化模块进行IMU初始化，基于IMU初始化的结果和惯性传感器（IMU）信息得到IMU预积分。
根据本发明的另一方面，还提供一种位姿估计设备，包括：
数据预处理模块，用于获取装置的各帧相机数据，从各帧相机数据的前帧相机数据和后帧相机数据中提取图像特征点，并基于提取到的图像特征点，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息；
数据融合模块，用于获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点；
帧间位姿估计模块，用于获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的位姿信息。
根据本发明的另一方面，还提供一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行以实现上述任一项所述的方法。
根据本发明的另一方面，还提供一种用于在网络设备端信息处理的设备，该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该设备执行上述任一项所述的方法。
本发明旨在充分利用不同传感器的优势，激光雷达可以直接测量获得空间点云的三维信息，并且测量结果误差范围稳定，不会在长时间的工作之后发生尺度漂移，这在本发明的位姿估计过程中是一项重要的鲁棒性来源；同时利用相机图像可以方便的提取环境特征和语义信息，结合IMU信息，构建了一种融合激光雷达测量、视觉测量和IMU的位姿估计系统，可以以较高的频率稳定的输出准确的位姿信息，供下游模块使用。
附图说明
图1是本发明一实施例的位姿估计方法及设备的原理图。
具体实施方式
下面结合附图对本发明作进一步详细描述。
在本申请一个典型的配置中，终端、服务网络的设备和可信方均包括一个或多个处理器 (CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器 (RAM) 和/或非易失性内存等形式，如只读存储器 (ROM) 或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存 (PRAM)、静态随机存取存储器 (SRAM)、动态随机存取存储器 (DRAM)、其他类型的随机存取存储器 (RAM)、只读存储器 (ROM)、电可擦除可编程只读存储器 (EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘 (DVD) 或其他光学存储、 磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括非暂存电脑可读媒体 (transitory media)，如调制的数据信号和载波。
如图1所示，本发明提供一种位姿估计方法，所述方法包括：
步骤S2，数据预处理模块获取装置的各帧相机数据，从各帧相机数据的前帧相机数据和后帧相机数据中提取图像特征点，并基于提取到的图像特征点，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息；
在此，所述装置可以是需要定位的机器人或车辆；
各帧相机数据中，时间上每相邻的两帧相机数据，分别为前帧相机数据和后帧相机数据；
步骤S3，数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点；
步骤S4，帧间位姿估计模块获取装置的惯性传感器（IMU）信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器（IMU）信息，得到装置的位姿信息。
在此，所谓装置的位姿信息，即装置在世界坐标系下(x，y，z)的三维坐标，以及（yaw，pitch，roll）的姿态信息，其中三维坐标的单位为米，姿态信息的单位为°。
如图1所示，本发明位姿估计有三种传感器数据来源，分别为激光雷达、相机、IMU。
本发明可以包含语义分割、数据预处理、数据融合、帧间位姿估计、后端优化等模块。
本发明旨在充分利用不同传感器的优势，激光雷达可以直接测量获得空间点云的三维信息，并且测量结果误差范围稳定，不会在长时间的工作之后发生尺度漂移，这在本发明的位姿估计过程中是一项重要的鲁棒性来源；同时利用相机图像可以方便的提取环境特征和语义信息，结合IMU信息，构建了一种融合激光雷达测量、视觉测量和IMU的位姿估计系统，可以以较高的频率稳定的输出准确的位姿信息，供下游模块使用。
本发明通过将激光雷达数据和各帧相机数据的图像特征提取信息相融合，从而对相机位姿实现稳定的位姿估计。采用本方法，平均平移误差小于0.71%，平均旋转误差小于0.0028 deg/m。基于本方法可以方便的进行扩展，并应用于增强现实、虚拟现实、三维重建、SLAM等多个应用场景下，满足不同应用场景的开发需求。
本发明的位姿估计方法一实施例中，步骤S2，数据预处理模块获取装置的各帧相机数据，从各帧相机数据的前帧相机数据和后帧相机数据中提取图像特征点，并基于提取到的图像特征点，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息，包括：
步骤S21，语义分割模块获取装置的各帧相机数据，并输出各帧相机数据中的每个像素语义分割信息，并为每个像素打上分类语义标签，以得到语义分割图像；
在此，语义分割也可以使用语义检测代替，其主要目的在于为目标检测对象打上语义标签；
步骤S22，数据预处理模块分别从前帧相机数据和后帧相机数据中提取图像特征点，并基于所述语义分割图像，对提取到的前帧相机数据和后帧相机数据的图像特征点中的动态图像特征点进行剔除，对未剔除的剩余的图像特征点进行匹配，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息。
在此，本实施例通过语义分割，得到语义分割图像，并对提取到的前帧相机数据和后帧相机数据的图像特征点中的动态图像特征点进行剔除，可以准确、可靠的得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息。
本发明的位姿估计方法一实施例中，步骤S22，对未剔除的剩余的图像特征点进行匹配，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息之后，还包括：
步骤S23，通过非极大抑制方法，从前帧相机数据和后帧相机数据的图像特征点之间的匹配信息中，剔除错误的匹配信息，以得到前帧相机数据和后帧相机数据的图像特征点之间的正确匹配信息；
步骤S3，数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点，包括：
步骤S311，数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的正确图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点。
在此，本实施例可以在上一实施例的对提取到的前帧相机数据和后帧相机数据的图像特征点中的动态图像特征点进行剔除的基础上，通过非极大抑制方法，从前帧相机数据和后帧相机数据的图像特征点之间的匹配信息中，剔除错误的匹配信息，可以得到更可靠的前帧相机数据和后帧相机数据的图像特征点之间的正确匹配信息。
本发明的位姿估计方法一实施例中，步骤S4，帧间位姿估计模块获取装置的惯性传感器（IMU）信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器（IMU）信息，得到装置的位姿信息，还包括：
步骤S41，帧间位姿估计模块获取装置的惯性传感器（IMU）信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器（IMU）信息，得到装置的初步位姿信息；
步骤S42，帧间位姿估计模块获取IMU预积分，基于IMU预积分和装置的初步位姿信息进行跟踪，以得到各帧相机数据之间的跟踪结果；
步骤S43，帧间位姿估计模块根据跟踪结果，判断是否创建滑动窗口关键帧地图或是否在滑动窗口关键帧地图中插入新的关键帧。
在此，若没有滑动窗口关键帧地图，需要创建滑动窗口关键帧地图；若已经创建好滑动窗口关键帧地图，则后续可以根据需要，在滑动窗口关键帧地图中插入新的关键帧。
本实施例通过基于IMU预积分和装置的初步位姿信息进行跟踪，以得到各帧相机数据之间的跟踪结果，根据跟踪结果，判断是否创建滑动窗口关键帧地图或是否在滑动窗口关键帧地图中插入新的关键帧，便于后续得到更准确的装置的位姿信息。
本发明的位姿估计方法一实施例中，步骤S43，根据跟踪结果判断是否创建滑动窗口关键帧地图或是否在滑动窗口关键帧地图中插入新的关键帧之后，还包括：
步骤S44，若帧间位姿估计模块判断为创建滑动窗口关键帧地图或在滑动窗口关键帧地图中插入新的关键帧，则后端优化模块创建滑动窗口关键帧地图或在滑动窗口关键帧地图中插入新的关键帧，以得到更新后的滑动窗口关键帧地图；
步骤S45，后端优化模块对滑动窗口关键帧地图中的装置的位姿信息进行优化，以得到优化后的装置的位姿信息。
在此，如图1所示，得到优化后的装置的位置信息，例如，可以是相机相对于初始坐标系的R矩阵和T矩阵。
本实施例基于是否创建滑动窗口关键帧地图或是否在滑动窗口关键帧地图中插入新的关键帧的判断结果，创建滑动窗口关键帧地图或在滑动窗口关键帧地图中插入新的关键帧，可以对滑动窗口关键帧地图中的装置的位姿信息进行优化，以得到更准确的优化后的装置的位姿信息。
本发明的位姿估计方法一实施例中，步骤S42，帧间位姿估计模块获取IMU预积分之前，还包括：
后端优化模块进行IMU初始化，基于IMU初始化的结果和惯性传感器（IMU）信息得到IMU预积分。
在此，本实施例通过后端优化模块进行IMU初始化，可以得到准确的IMU预积分，以供帧间位姿估计模块获取。
本发明的位姿估计方法一实施例中，步骤S3，数据融合模块将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点，包括：
步骤S321，数据融合模块获取数据预处理模块所提取到的前帧相机数据和后帧相机数据中图像特征点，在各帧相机数据提取到的图像特征点周围提取一圈不共线的激光反投影点集F；
步骤S322，数据融合模块结合所述语义分割图像，从激光反投影点集F中筛选出与各图像特征点的语义标签相同的激光反投影点，获取筛选出的激光反投影点对应的激光点，形成点集Fseg；
步骤S323，对点集Fseg激光点进行平面特征拟合，以得到平面特征拟合结果以及平面特征拟合的深度信息；
步骤S324，将各个平面特征拟合结果以及平面特征拟合的深度信息，作为对应图像特征点的深度信息值。
在此，本发明的位姿估计方法一实施例中，步骤S324，将各个平面特征拟合结果以及平面特征拟合的深度信息，作为对应图像特征点的深度信息值之后，还包括：
步骤S325，对于深度信息值超过阈值的图像特征点，去除其深度信息值。
在此，对于深度信息值超过阈值的图像特征点，不赋予深度信息值，如设置为-1，因为太远的图像特征点，其深度信息值不被信任，容易带入较大误差，所以可以把这些图像特征点的深度信息值去除。
根据本发明的另一方面，还提供一种位姿估计设备，包括：
数据预处理模块，用于获取装置的各帧相机数据，从各帧相机数据的前帧相机数据和后帧相机数据中提取图像特征点，并基于提取到的图像特征点，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息；
数据融合模块，用于获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点；
帧间位姿估计模块，用于获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的位姿信息。
根据本发明的另一方面，还提供一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行以实现上述任一项所述的方法。
根据本发明的另一方面，还提供一种用于在网络设备端信息处理的设备，该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该设备执行上述任一项所述的方法。
本发明各设备实施例的详细内容具体可参见各方法实施例的对应部分，在此，不再赘述。
本发明方法所执行的步骤已经在上述自适应下肢约束系统中做出说明，这里不再赘述。然而，本发明的方法可以集成在特定的控制装置中。
本发明的该控制装置可以是一种计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行如以上各方法实施例中的方法的部分或全部步骤。
本发明的该控制装置可以是一种应用发布平台，其中，应用发布平台用于发布计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行如以上各方法实施例中的方法的部分或全部步骤。
在本发明的各种实施例中，应理解，上述各过程的序号的大小并不意味着执行顺序的必然先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
本发明的各个系统、单元若以软件功能单元的形式实现时，可以存储在一个计算机可获取的存储器中。基于这样的理解，本发明的技术方案的部分或者全部，可以通过软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干请求用以使得一台或多台计算机设备(例如个人计算机、服务器或者网络设备等，具体可以是计算机设备中的处理器)执行本发明的各个实施例上述方法的部分或全部步骤。
本领域的技术人员可以理解，本发明所列举的各个实施例的全部或部分步骤可以通过计算机程序来指令相关的硬件来完成，这些计算机程序可以集中或分布式地存储于一个或多个计算机装置中，例如存储于可读存储介质中。上述计算机装置包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(ErasableProgrammable Read Only Memory，EPROM)、一次可编程只读存储器(One-timeProgrammable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。
本技术领域中的普通技术人员应当认识到，以上的实施例仅是用来说明本发明，而并非用作为对本发明的限定，只要在本发明的实质精神范围内，对以上所述实施例的变化、变型都将落在本发明的权利要求书范围内。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。
需要注意的是，本发明可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路（ASIC）、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中，本发明的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地，本发明的软件程序（包括相关的数据结构）可以被存储到计算机可读记录介质中，例如，RAM存储器，磁或光驱动器或软磁盘及类似设备。另外，本发明的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。
另外，本发明的一部分可被应用为计算机程序产品，例如计算机程序指令，当其被计算机执行时，通过该计算机的操作，可以调用或提供根据本发明的方法和/或技术方案。而调用本发明的方法的程序指令，可能被存储在固定的或可移动的记录介质中，和/或通过广播或其他信号承载媒体中的数据流而被传输，和/或被存储在根据所述程序指令运行的计算机设备的工作存储器中。在此，根据本发明的一个实施例包括一个装置，该装置包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该装置运行基于前述根据本发明的多个实施例的方法和/或技术方案。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。
Claims (9)
1.一种位姿估计方法，其特征在于，包括：
数据预处理模块获取装置的各帧相机数据，从各帧相机数据的前帧相机数据和后帧相机数据中提取图像特征点，并基于提取到的图像特征点，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息，包括：
语义分割模块获取装置的各帧相机数据，并输出各帧相机数据中的每个像素语义分割信息，并为每个像素打上分类语义标签，以得到语义分割图像；
数据预处理模块分别从前帧相机数据和后帧相机数据中提取图像特征点，并基于所述语义分割图像，对提取到的前帧相机数据和后帧相机数据的图像特征点中的动态图像特征点进行剔除，对未剔除的剩余的图像特征点进行匹配，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息；
数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点，包括：
数据融合模块获取数据预处理模块所提取到的前帧相机数据和后帧相机数据中图像特征点，在各帧相机数据提取到的图像特征点周围提取一圈不共线的激光反投影点集F；
数据融合模块结合所述语义分割图像，从激光反投影点集F中筛选出与各图像特征点的语义标签相同的激光反投影点，获取筛选出的激光反投影点对应的激光点，形成点集Fseg；
对点集Fseg激光点进行平面特征拟合，以得到平面特征拟合结果以及平面特征拟合的深度信息；
将各个平面特征拟合结果以及平面特征拟合的深度信息，作为对应图像特征点的深度信息值；
帧间位姿估计模块获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的位姿信息。
2.如权利要求1所述的位姿估计方法，其特征在于，对未剔除的剩余的图像特征点进行匹配，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息之后，还包括：
通过非极大抑制方法，从前帧相机数据和后帧相机数据的图像特征点之间的匹配信息中，剔除错误的匹配信息，以得到前帧相机数据和后帧相机数据的图像特征点之间的正确匹配信息；
数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点，包括：
数据融合模块获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的正确图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点。
3.如权利要求1所述的位姿估计方法，其特征在于，将各个平面特征拟合结果以及平面特征拟合的深度信息，作为对应图像特征点的深度信息值之后，还包括：
对于深度信息值超过阈值的图像特征点，去除其深度信息值。
4.如权利要求1所述的位姿估计方法，其特征在于，帧间位姿估计模块获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的位姿信息，还包括：
帧间位姿估计模块获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的初步位姿信息；
帧间位姿估计模块获取IMU预积分，基于IMU预积分和装置的初步位姿信息进行跟踪，以得到各帧相机数据之间的跟踪结果；
帧间位姿估计模块根据跟踪结果，判断是否创建滑动窗口关键帧地图或是否在滑动窗口关键帧地图中插入新的关键帧。
5.如权利要求4所述的位姿估计方法，其特征在于，根据跟踪结果判断是否创建滑动窗口关键帧地图或是否在滑动窗口关键帧地图中插入新的关键帧之后，还包括：
若帧间位姿估计模块判断为创建滑动窗口关键帧地图或在滑动窗口关键帧地图中插入新的关键帧，则后端优化模块创建滑动窗口关键帧地图或在滑动窗口关键帧地图中插入新的关键帧，以得到更新后的滑动窗口关键帧地图；
后端优化模块对滑动窗口关键帧地图中的装置的位姿信息进行优化，以得到优化后的装置的位姿信息。
6.如权利要求4所述的位姿估计方法，其特征在于，帧间位姿估计模块获取IMU预积分之前，还包括：
后端优化模块进行IMU初始化，基于IMU初始化的结果和惯性传感器（IMU）信息得到IMU预积分。
7.一种位姿估计设备，其特征在于，包括：
数据预处理模块，用于获取装置的各帧相机数据，从各帧相机数据的前帧相机数据和后帧相机数据中提取图像特征点，并基于提取到的图像特征点，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息，包括：
语义分割模块获取装置的各帧相机数据，并输出各帧相机数据中的每个像素语义分割信息，并为每个像素打上分类语义标签，以得到语义分割图像；
数据预处理模块分别从前帧相机数据和后帧相机数据中提取图像特征点，并基于所述语义分割图像，对提取到的前帧相机数据和后帧相机数据的图像特征点中的动态图像特征点进行剔除，对未剔除的剩余的图像特征点进行匹配，得到前帧相机数据和后帧相机数据的图像特征点之间的匹配信息；
数据融合模块，用于获取装置的激光雷达数据，并将前帧相机数据和后帧相机数据中提取到的图像特征点及其匹配信息和激光雷达数据相关联，得到各帧相机数据的带有深度信息的图像特征点，包括：
数据融合模块获取数据预处理模块所提取到的前帧相机数据和后帧相机数据中图像特征点，在各帧相机数据提取到的图像特征点周围提取一圈不共线的激光反投影点集F；
数据融合模块结合所述语义分割图像，从激光反投影点集F中筛选出与各图像特征点的语义标签相同的激光反投影点，获取筛选出的激光反投影点对应的激光点，形成点集Fseg；
对点集Fseg激光点进行平面特征拟合，以得到平面特征拟合结果以及平面特征拟合的深度信息；
将各个平面特征拟合结果以及平面特征拟合的深度信息，作为对应图像特征点的深度信息值；
帧间位姿估计模块，用于获取装置的惯性传感器信息，并基于各帧相机数据的带有深度信息的图像特征点及其匹配信息和惯性传感器信息，得到装置的位姿信息。
8.一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行以实现权利要求1至6中任一项所述的方法。
9.一种用于在网络设备端信息处理的设备，该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该设备执行权利要求1至6中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211161417.7A CN115655264B (zh) | 2022-09-23 | 位姿估计方法及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211161417.7A CN115655264B (zh) | 2022-09-23 | 位姿估计方法及设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115655264A CN115655264A (zh) | 2023-01-31 |
| CN115655264B true CN115655264B (zh) | 2024-07-05 |
Family
ID=
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110009739A (zh) * | 2019-01-29 | 2019-07-12 | 浙江省北大信息技术高等研究院 | 移动摄像机的数字视网膜的运动特征的提取与编码方法 |
| CN111968129A (zh) * | 2020-07-15 | 2020-11-20 | 上海交通大学 | 具有语义感知的即时定位与地图构建系统及方法 |
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110009739A (zh) * | 2019-01-29 | 2019-07-12 | 浙江省北大信息技术高等研究院 | 移动摄像机的数字视网膜的运动特征的提取与编码方法 |
| CN111968129A (zh) * | 2020-07-15 | 2020-11-20 | 上海交通大学 | 具有语义感知的即时定位与地图构建系统及方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110322500B (zh) | 即时定位与地图构建的优化方法及装置、介质和电子设备 | |
| US11994392B2 (en) | Square-root multi-state constraint Kalman filter for vision-aided inertial navigation system | |
| CN107888828B (zh) | 空间定位方法及装置、电子设备、以及存储介质 | |
| CN111830953B (zh) | 车辆自定位方法、装置及系统 | |
| CN110095752B (zh) | 定位方法、装置、设备和介质 | |
| CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法 | |
| CN109461208B (zh) | 三维地图处理方法、装置、介质和计算设备 | |
| CN110349212B (zh) | 即时定位与地图构建的优化方法及装置、介质和电子设备 | |
| CN111829532B (zh) | 一种飞行器重定位系统和重定位方法 | |
| CN111986261B (zh) | 一种车辆定位方法、装置、电子设备及存储介质 | |
| CN113012197A (zh) | 一种适用于动态交通场景的双目视觉里程计定位方法 | |
| EP3126864B1 (fr) | Procédé de géo-localisation de l'environnement d'un porteur | |
| US11069071B1 (en) | System and method for egomotion estimation | |
| US20220398825A1 (en) | Method for generating 3d reference points in a map of a scene | |
| CN115900712B (zh) | 一种信源可信度评价组合定位方法 | |
| Yang et al. | Robust and real-time pose tracking for augmented reality on mobile devices | |
| CN113763466B (zh) | 一种回环检测方法、装置、电子设备和存储介质 | |
| CN112200917A (zh) | 一种高精度增强现实方法及系统 | |
| CN115655264B (zh) | 位姿估计方法及设备 | |
| CN114111817B (zh) | 基于slam地图与高精度地图匹配的车辆定位方法及系统 | |
| WO2023283929A1 (zh) | 双目相机外参标定的方法及装置 | |
| CN116642511A (zh) | Ar导航图像渲染方法、装置、电子设备及存储介质 | |
| CN115655264A (zh) | 位姿估计方法及设备 | |
| CN112037261A (zh) | 一种图像动态特征去除方法及装置 | |
| Sun et al. | Accurate deep direct geo-localization from ground imagery and phone-grade gps |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |