CN114757980B - 一种连续帧点云数据的离线配准方法 - Google Patents

一种连续帧点云数据的离线配准方法 Download PDF

Info

Publication number
CN114757980B
CN114757980B CN202210515961.0A CN202210515961A CN114757980B CN 114757980 B CN114757980 B CN 114757980B CN 202210515961 A CN202210515961 A CN 202210515961A CN 114757980 B CN114757980 B CN 114757980B
Authority
CN
China
Prior art keywords
pose
frame
point cloud
registration
cloud data
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
CN202210515961.0A
Other languages
English (en)
Other versions
CN114757980A (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.)
Suzhou Qingyu Technology Co Ltd
Original Assignee
Suzhou Qingyu Technology 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 Suzhou Qingyu Technology Co Ltd filed Critical Suzhou Qingyu Technology Co Ltd
Priority to CN202210515961.0A priority Critical patent/CN114757980B/zh
Publication of CN114757980A publication Critical patent/CN114757980A/zh
Application granted granted Critical
Publication of CN114757980B publication Critical patent/CN114757980B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/32—Determination of transform parameters for the alignment of images, i.e. image registration using correlation-based methods
    • 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/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • 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
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Image Analysis (AREA)

Abstract

本发明实施例涉及一种连续帧点云数据的离线配准方法，所述方法包括：获取第一帧点云数据序列；获取与各个第一帧点云数据对应的第一帧位姿数据；对第一帧点云数据序列进行配准帧数据对识别；根据各个第一配准对进行点云帧相对位姿变换估计处理得到第一位姿变换矩阵和第一协方差矩阵；并根据第一协方差矩阵进行置信度矩阵转换；并使用第一置信度矩阵对第一位姿变换矩阵进行加权；以各个第一帧位姿数据构建对应节点，并为相邻节点创建关联关系，并以节点间对应的第一加权位姿变换矩阵构建节点边约束关系；进行位姿图优化得到第一优化位姿变换矩阵；根据第一优化位姿变换矩阵对第一帧点云数据序列进行位姿配准。通过本发明可以提高点云数据配准效率。

Description

一种连续帧点云数据的离线配准方法
技术领域
本发明涉及数据处理技术领域，特别涉及一种连续帧点云数据的离线配准方法。
背景技术
自动驾驶系统定位模块使用人工智能模型对激光雷达的三维点云进行目标检测，为保证模型识别精度往往需要采集大量的连续帧点云数据进行模型训练。在使用采集数据对模型训练之前，为降低训练数据噪声需要对采集到的连续帧点云数据进行人工配准。而随着训练数据量的增大，受人力因素影响，连续帧点云数据的配准效率会变得非常低。
发明内容
本发明的目的，就是针对现有技术的缺陷，提供一种连续帧点云数据的离线配准方法、电子设备及计算机可读存储介质，在获得连续帧点云数据和对应的位姿数据之后对各帧点云数据进行配准对筛选，并基于迭代最近点(Iterative Closest Point，ICP)算法对各个配准对里的两帧点云数据进行相对位姿估计得到对应的相对位姿矩阵和协方差矩阵；再基于各帧点云数据对应的位姿数据构建位姿图，并按最小二乘算法进行位姿图优化得到最终的优化位姿矩阵；最后使用优化位姿矩阵对连续帧点云数据的各帧点云数据进行配准。通过本发明，可以不受人力因素影响对海量的连续帧点云数据进行快速配准，达到提高点云数据配准效率的目的。
为实现上述目的，本发明实施例第一方面提供了一种连续帧点云数据的离线配准方法，所述方法包括：
获取多帧点云数据构成第一帧点云数据序列；所述第一帧点云数据序列包括多个第一帧点云数据；相邻两个所述第一帧点云数据的前一帧采集时间早于后一帧采集时间；
获取与各个所述第一帧点云数据对应的位姿数据作为对应的第一帧位姿数据；
对所述第一帧点云数据序列进行配准帧数据对识别处理生成多组第一配准对；所述第一配准对的类型包括里程计配准对类型和回环配准对类型；
根据各个所述第一配准对对应的两个所述第一帧位姿数据进行点云帧相对位姿变换估计处理得到对应的第一位姿变换矩阵和第一协方差矩阵；并根据所述第一协方差矩阵进行置信度矩阵转换处理得到对应的第一置信度矩阵；并使用所述第一置信度矩阵对所述第一位姿变换矩阵进行加权处理得到对应的第一加权位姿变换矩阵；
构建位姿图，以各个所述第一帧位姿数据构建所述位姿图中的对应节点，并为相邻节点创建关联关系，并以节点间对应的所述第一加权位姿变换矩阵构建节点边约束关系；
对所述位姿图进行位姿图优化处理得到对应的第一优化位姿变换矩阵；
根据所述第一优化位姿变换矩阵对所述第一帧点云数据序列进行位姿配准。
优选的，所述对所述第一帧点云数据序列进行配准帧数据对识别处理生成多组第一配准对，具体包括：
对所述第一帧点云数据序列中任意相邻的两帧所述第一帧点云数据作为一组所述第一配准对，并设置对应类型为里程计配准对类型；
对所述第一帧点云数据序列中采集时间间隔超过设定间隔阈值的任意两帧所述第一帧点云数据，进行重叠度计算生成对应的第一重叠度数据；并在所述第一重叠度数据超过预设的重叠度阈值时，将对应的两帧所述第一帧点云数据作为一组所述第一配准对，并设置对应类型为回环配准对类型；所述设定间隔阈值大于相邻两帧所述第一帧点云数据的采集时间间隔。
优选的，所述根据各个所述第一配准对对应的两个所述第一帧位姿数据进行点云帧相对位姿变换估计处理得到对应的第一位姿变换矩阵和第一协方差矩阵，具体包括：
对于类型为里程计配准对类型的所述第一配准对，以所述第一配准对中采集时间靠前的所述第一帧位姿数据为第一源帧位姿、采集时间靠后的所述第一帧位姿数据为第一目标帧位姿；
对于类型为回环配准对类型的所述第一配准对，以所述第一配准对中采集时间靠后的所述第一帧位姿数据为第一源帧位姿、采集时间靠前的所述第一帧位姿数据为第一目标帧位姿；
基于迭代最近点ICP算法对所述第一源帧位姿到所述第一目标帧位姿的位姿变换矩阵进行估计从而得到对应的所述第一位姿变换矩阵和所述第一协方差矩阵。
优选的，所述根据所述第一协方差矩阵进行置信度矩阵转换处理得到对应的第一置信度矩阵，具体包括：
将所述第一协方差矩阵的逆矩阵作为对应的所述第一置信度矩阵。
优选的，所述构建位姿图，以各个所述第一帧位姿数据构建所述位姿图中的对应节点，并为相邻节点创建关联关系，并以节点间对应的所述第一加权位姿变换矩阵构建节点边约束关系，具体包括：
初始化所述位姿图的节点数量为所述第一帧位姿数据的数量，并使每个节点对应一个所述第一帧位姿数据；
按所述第一帧位姿数据的时间先后顺序对各个对应节点进行顺次连接；并为本次连接的各个相邻节点创建里程计关联关系；并将本次连接中相邻节点间的节点边记为第一类节点边，并以相邻节点对应的所述第一配准对的所述第一加权位姿变换矩阵作为所述第一类节点边的第一节点边约束关系；
对类型为回环配准对类型的所述第一配准对进行遍历，并将当前遍历的所述第一配准对作为当前回环配准对；并将所述当前回环配准对中采集时间靠后的所述第一帧位姿数据作为第二源帧位姿、采集时间靠前的所述第一帧位姿数据作为第二目标帧位姿；并将所述位姿图中与所述第二源帧位姿对应的节点记为当前源节点，与所述第二目标帧位姿对应的节点记为当前目标节点；并在所述当前源节点与所述当前目标节点间创建回环关联关系；并从所述当前源节点向所述当前目标节点建立对应的第二类节点边；并以所述当前回环配准对的所述第一加权位姿变换矩阵作为所述第二类节点边的第二节点边约束关系。
优选的，所述对所述位姿图进行位姿图优化处理得到对应的第一优化位姿变换矩阵，具体包括：
根据所述位姿图中关联关系为里程计关联关系的各连接节点间的第一节点边约束关系，按最小二乘法构建对应的非线性最小二乘问题；并通过求解所述非线性最小二乘问题得到对应的所述第一优化位姿变换矩阵。
本发明实施例第二方面提供了一种电子设备，包括：存储器、处理器和收发器；
所述处理器用于与所述存储器耦合，读取并执行所述存储器中的指令，以实现上述第一方面所述的方法步骤；
所述收发器与所述处理器耦合，由所述处理器控制所述收发器进行消息收发。
本发明实施例第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，当所述计算机指令被计算机执行时，使得所述计算机执行上述第一方面所述的方法的指令。
本发明实施例提供了一种连续帧点云数据的离线配准方法、电子设备及计算机可读存储介质，在获得连续帧点云数据和对应的位姿数据之后对各帧点云数据进行配准对筛选，并基于ICP算法对各个配准对里的两帧点云数据进行相对位姿估计得到对应的相对位姿矩阵和协方差矩阵；再基于各帧点云数据对应的位姿数据构建位姿图，并按最小二乘算法进行位姿图优化得到最终的优化位姿矩阵；最后使用优化位姿矩阵对连续帧点云数据的各帧点云数据进行配准。通过本发明，可以在不受人力因素的影响下对海量的连续帧点云数据进行快速配准，节约了人力成本，提高了点云数据配准效率。
附图说明
图1为本发明实施例一提供的一种连续帧点云数据的离线配准方法示意图；
图2为本发明实施例一提供的位姿图的示意图；
图3为本发明实施例二提供的一种电子设备的结构示意图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
本发明实施例一提供一种连续帧点云数据的离线配准方法，如图1为本发明实施例一提供的一种连续帧点云数据的离线配准方法示意图所示，本方法主要包括如下步骤：
步骤1，获取多帧点云数据构成第一帧点云数据序列；
其中，第一帧点云数据序列包括多个第一帧点云数据；相邻两个第一帧点云数据的前一帧采集时间早于后一帧采集时间。
这里，多帧点云数据的采集端为车辆自动驾驶系统感知模块的雷达；多帧点云数据为持续的多帧点云数据，相邻帧也就是相邻两个第一帧点云数据之间的采集时间间隔具体由采集端对雷达单帧间隔要求决定，默认为0.1秒，也可为其他时间间隔设定值。
步骤2，获取与各个第一帧点云数据对应的位姿数据作为对应的第一帧位姿数据。
这里，位姿数据的采集端为车辆自动驾驶系统惯性测量单元(InertialMeasurement Unit，IMU)，也可为自动驾驶系统定位模块的其他位姿采集单元；位姿数据中带有旋转分量和平移分量；本发明实施例默认每个第一帧点云数据对应一个第一帧位姿数据。
需要说明的是，在具体实施过程中可能会出现其中一个或多个第一帧点云数据对应的位姿数据丢失或漏采的情况，也就是一个或多个第一帧点云数据对应的第一帧位姿数据为空的情况，为保证每个第一帧点云数据对应的第一帧位姿数据为有效位姿，本发明实施例还给出一种位姿数据的补齐处理流程，具体为：按时间先后顺序对所有第一帧位姿数据进行排序得到对应的第一帧位姿数据序列；对第一帧位姿数据序列中各个第一帧位姿数据进行遍历，若当前遍历的第一帧位姿数据为空则将其视为无效位姿数据；并将当前无效位姿数据之前与之后最近的不为空的第一帧位姿数据视为对应的第一前有效位姿数据和第一后有效位姿数据；并根据第一前、后有效位姿数据的位姿信息，以及第一前、后有效位姿数据与当前无效位姿数据的相对间隔，对当前无效位姿数据进行插值补充。
步骤3，对第一帧点云数据序列进行配准帧数据对识别处理生成多组第一配准对；
其中，第一配准对的类型包括里程计配准对类型和回环配准对类型；
具体包括：步骤31，对第一帧点云数据序列中任意相邻的两帧第一帧点云数据作为一组第一配准对，并设置对应类型为里程计配准对类型；
这里，当前步骤实际是按里程计方式也就是按行程时间先后顺序将相邻的两帧第一帧点云数据作为一组第一配准对；
例如，车辆行程时间从时刻1到时刻4，得到4个连续帧点云数据构成第一帧点云数据序列(第一帧点云数据1，第一帧点云数据2，第一帧点云数据3，第一帧点云数据4)；那么就能得到3组类型为里程计配准对类型第一配准对，分别为：第一配准对1(第一帧点云数据1，第一帧点云数据2)，第一配准对2(第一帧点云数据2，第一帧点云数据3)，第一配准对3(第一帧点云数据3，第一帧点云数据4)；
步骤32，对第一帧点云数据序列中采集时间间隔超过设定间隔阈值的任意两帧第一帧点云数据，进行重叠度计算生成对应的第一重叠度数据；并在第一重叠度数据超过预设的重叠度阈值时，将对应的两帧第一帧点云数据作为一组第一配准对，并设置对应类型为回环配准对类型；
其中，设定间隔阈值的设定值应大于相邻两帧第一帧点云数据的采集时间间隔；设定间隔阈值默认设为1秒，也可为其他大于采集时间间隔(默认为0.1秒)的设定值；重叠度阈值默认为0.5，也可以基于具体实现设为其他阈值。
这里，在里程计也就是在车辆行程过程中可能会重复经过一些位置，这些重复途经的位置就被称为回环位置；因为在回环位置采集的点云帧肯定会出现较大重合，所以当前步骤通过计算任一两帧第一帧点云数据的重叠度(Intersection over Union，IoU)来辨识第一帧点云数据序列中同一回环位置上不同行程时间点的两帧点云数据，并将二者纳入一个类型为回环配准对类型的第一配准对。
步骤4，根据各个第一配准对对应的两个第一帧位姿数据进行点云帧相对位姿变换估计处理得到对应的第一位姿变换矩阵和第一协方差矩阵；并根据第一协方差矩阵进行置信度矩阵转换处理得到对应的第一置信度矩阵；并使用第一置信度矩阵对第一位姿变换矩阵进行加权处理得到对应的第一加权位姿变换矩阵；
这里，当前步骤实际是以配准对为单位，对两个相关(里程计相关或回环相关)的第一帧点云数据的第一帧位姿数据的加权相对位姿变化进行估算，估算时先基于ICP算法进行相对位姿变换估计得到相对位姿变换矩阵和协方差矩阵也就是第一位姿变换矩阵和第一协方差矩阵，再基于信息矩阵也就是置信度矩阵与协方差矩阵互逆的关系得到第一置信度矩阵，再基于第一置信度矩阵对第一位姿变换矩阵进行加权修正得到第一加权位姿变换矩阵；
具体包括：步骤41，根据第一配准对对应的两个第一帧位姿数据进行点云帧相对位姿变换估计处理得到对应的第一位姿变换矩阵和第一协方差矩阵；
具体为：对于类型为里程计配准对类型的第一配准对，以第一配准对中采集时间靠前的第一帧位姿数据为第一源帧位姿、采集时间靠后的第一帧位姿数据为第一目标帧位姿；对于类型为回环配准对类型的第一配准对，以第一配准对中采集时间靠后的第一帧位姿数据为第一源帧位姿、采集时间靠前的第一帧位姿数据为第一目标帧位姿；并基于迭代最近点ICP算法对第一源帧位姿到第一目标帧位姿的位姿变换矩阵进行估计从而得到对应的第一位姿变换矩阵和第一协方差矩阵；
这里，在确定源帧位姿和目标帧位姿时，基于第一配准对的类型进行确定：类型为里程计配准对类型则以采集时间靠前的为源帧位姿、以采集时间靠后的为目标帧位姿；类型为回环配准对类型则相反，以采集时间靠后的为源帧位姿、以采集时间靠前的为目标帧位姿；这是为了对应后续位姿图的节点关联关系；
另外，本发明实施例基于ICP算法对第一源帧位姿到第一目标帧位姿的位姿变换矩阵进行估计时有多种实现方式；
其中一种实现方式就是基于点到点ICP算法的实现方式，具体为：
步骤A1，在第一源帧位姿对应的第一帧点云数据中确定指定数量n的点云作为第一参考点云ai，1≤i≤n；并在第一目标帧位姿对应的第一帧点云数据中基于最小距离原则确定各个第一参考点云匹配的第一匹配点云bi；
步骤A2，基于欧式变换构建对应的目标函数f1(R,T)：
R为旋转矩阵，T为位移矩阵；
步骤A3，对使得目标函数f1(R,T)达到最小值的旋转矩阵R和位移矩阵T进行求解，并由求解结果构成对应的第一位姿变换矩阵P[R,T]；并基于目标函数f1(R,T)计算对应的协方差得到第一协方差矩阵；
其中另一种实现方式就是基于点到面ICP算法的实现方式，具体为：
步骤B1，在第一源帧位姿对应的第一帧点云数据中确定指定数量n的点云作为第一参考点云ai，1≤i≤n；并在第一目标帧位姿对应的第一帧点云数据中预设的参考面上确定各个第一参考点云匹配的第一匹配点云bi；
步骤B2，基于欧式变换构建对应的目标函数f2(R,T)：
R为旋转矩阵，T为位移矩阵，ci为第一匹配点云bi在参考面上的对应法向量；
步骤B3，对使得目标函数f2(R,T)达到最小值的旋转矩阵R和位移矩阵T进行求解，并由求解结果构成对应的第一位姿变换矩阵P[R,T]；并基于目标函数f2(R,T)计算对应的协方差得到第一协方差矩阵；
其中另一种实现方式就是基于点到线ICP算法的实现方式，具体为：
步骤C1，在第一源帧位姿对应的第一帧点云数据中确定指定数量n的点云作为第一参考点云ai，1≤i≤n；并在第一目标帧位姿对应的第一帧点云数据中预设的参考线上确定各个第一参考点云匹配的第一匹配点云bi；
步骤C2，基于欧式变换构建对应的目标函数f3(R,T)：
R为旋转矩阵，T为位移矩阵，di为第一匹配点云bi所在参考线的方向向量；
步骤C3，对使得目标函数f3(R,T)达到最小值的旋转矩阵R和位移矩阵T进行求解，并由求解结果构成对应的第一位姿变换矩阵P[R,T]；并基于目标函数f3(R,T)计算对应的协方差得到第一协方差矩阵；
上述三种点到点、点到面和点到线ICP算法的具体算法推算步骤，诸如如何选择参考点与匹配点，如何根据目标函数具体计算得到第一位姿变换矩阵P[R,T]和第一协方差矩阵体计算等，都可参考相关技术实现，在此不做一一赘述；
步骤42，根据第一协方差矩阵进行置信度矩阵转换处理得到对应的第一置信度矩阵；
具体为：将第一协方差矩阵的逆矩阵作为对应的第一置信度矩阵；
这里，置信度矩阵也叫信息矩阵，实际就是第一协方差矩阵的逆；
例如，第一协方差矩阵为Ω，第一置信度矩阵为W，则W＝Ω-1；
需要说明的是，也可以定义第一置信度矩阵W与第一协方差矩阵Ω的逆相关，诸如W＝Ω-1μ，其中μ为修正量；
步骤43，使用第一置信度矩阵对第一位姿变换矩阵进行加权处理得到对应的第一加权位姿变换矩阵。
例如，第一置信度矩阵为W，第一位姿变换矩阵为P，则第一加权位姿变换矩阵P*＝W·P。
步骤5，构建位姿图，以各个第一帧位姿数据构建位姿图中的对应节点，并为相邻节点创建关联关系，并以节点间对应的第一加权位姿变换矩阵构建节点边约束关系；
具体包括：步骤51，初始化位姿图的节点数量为第一帧位姿数据的数量，并使每个节点对应一个第一帧位姿数据；
例如，第一帧点云数据序列包括4个第一帧点云数据，对应4个第一帧位姿数据1、2、3、4，那么如图2为本发明实施例一提供的位姿图的示意图所示，位姿图的节点数量为4，位姿图有4个节点分别为节点N1、N2、N3、N4分别对应第一帧位姿数据1、2、3、4；
步骤52，按第一帧位姿数据的时间先后顺序对各个对应节点进行顺次连接；并为本次连接的各个相邻节点创建里程计关联关系；并将本次连接中相邻节点间的节点边记为第一类节点边，并以相邻节点对应的第一配准对的第一加权位姿变换矩阵作为第一类节点边的第一节点边约束关系；
例如，已知第一帧位姿数据1、2、3、4是按时间先后顺序排列的，那么如图2所示，当前步骤会完成从节点N1到N2、节点N2到N3以及节点N3到N4的三组连接，对应生成三条第一类节点边1、2、3；第一类节点边1的第一节点边约束关系为由第一帧位姿数据1、2对应的第一配准对的第一加权位姿变换矩阵第一类节点边2的第一节点边约束关系为由第一帧位姿数据2、3对应的第一配准对的第一加权位姿变换矩阵/>第一类节点边3的第一节点边约束关系为由第一帧位姿数据3、4对应的第一配准对的第一加权位姿变换矩阵/>图2中，节点N1、N2、N3、N4中各个相邻节点的关联关系为里程计关联关系；
步骤53，对类型为回环配准对类型的第一配准对进行遍历，并将当前遍历的第一配准对作为当前回环配准对；并将当前回环配准对中采集时间靠后的第一帧位姿数据作为第二源帧位姿、采集时间靠前的第一帧位姿数据作为第二目标帧位姿；并将位姿图中与第二源帧位姿对应的节点记为当前源节点，与第二目标帧位姿对应的节点记为当前目标节点；并在当前源节点与当前目标节点间创建回环关联关系；并从当前源节点向当前目标节点建立对应的第二类节点边；并以当前回环配准对的第一加权位姿变换矩阵作为第二类节点边的第二节点边约束关系。
例如，已知第一帧点云数据序列中第一帧点云数据2、4构成一组类型为回环配准对类型的第一配准对，那么第一配准对中采集时间靠后的帧点云数据为第一帧点云数据4则当前源节点为节点N4，第一配准对中采集时间靠前的帧点云数据为第一帧点云数据2则当前目标节点为节点N2；那么如图2所示，当前步骤会完成从节点N4到N2的一次连接，对应生成一条第二类节点边1；第二类节点边1的第一节点边约束关系为由第一帧位姿数据4、2对应的第一配准对的第一加权位姿变换矩阵
步骤6，对位姿图进行位姿图优化处理得到对应的第一优化位姿变换矩阵；
具体包括：根据位姿图中关联关系为里程计关联关系的各连接节点间的第一节点边约束关系，按最小二乘法构建对应的非线性最小二乘问题；并通过求解非线性最小二乘问题得到对应的第一优化位姿变换矩阵。
这里，根据位姿图中关联关系为里程计关联关系的各连接节点间的第一节点边约束关系，按最小二乘法构建对应的非线性最小二乘问题，具体为：统计关联关系为里程计关联关系的连接节点总数记为m；并将位姿图中关联关系为里程计关联关系的第1个节点对应的第一帧位姿数据作为初始状态量A1；并根据各连接节点间的第一节点边约束关系也就是第一加权位姿变换矩阵构建里程计运动模型，从而根据初始状态量A1和里程计运动模型预测出各个连接节点的位姿预测状态量Aj，1≤j≤m；再以各连接节点对应的第一帧位姿数据为位姿观测量Bj，并给出位姿观测量Bj与位姿预测状态量Aj间的位姿变换关系：Bj＝xj·Aj,其中，xj为位姿观测量Bj与位姿预测状态量Aj间的位姿变换量；再按最小二乘法基于各个节点对应的观测误差errj＝(位姿预测状态量Aj-位姿观测量Bj)或者观测误差平方构建当前位姿图对应的非线性最小二乘问题也就是非线性最小二乘目标函数F；
通过求解非线性最小二乘问题得到对应的第一优化位姿变换矩阵，具体为：求使得非线性最小二乘目标函数F达到最小值的位姿变换矩阵X[x1,x2…xj…xm]，并将最终得到的求解结果作为对应的第一优化位姿变换矩阵X[x1,x2…xj…xm]。
步骤7，根据第一优化位姿变换矩阵对第一帧点云数据序列进行位姿配准；
具体为：以第一优化位姿变换矩阵为点云坐标修正矩阵，对第一帧点云数据序列中各个第一帧点云数据的点云坐标进行修正。
这里，实际就是使用第一优化位姿变换矩阵X[x1,x2…xj…xm]中的各个xj对与之对应的第一帧点云数据的点云坐标进行位姿修正。
需要说明的是，在通过上述步骤1-7完成对连续帧点云数据也就是第一帧点云数据序列的离线配准之后，即可对第一帧点云数据序列中各个第一帧点云数据进行顺次拼接从而得到一张点云错位误差最小的完整点云数据。持续循环执行上述步骤1-7以及完成后的帧点云数据拼接，就可以最终产生一个完整的场景点云集合。
需要说明的是，在上述对第一帧点云数据序列中各个第一帧点云数据进行顺次拼接时，可能会在回环位置出现点云重影，例如图2中互为回环关联关系的节点2和4对应的第一帧点云数据的部分或全部点云出现点云错位。为解决这个问题，本发明实施例还在对第一帧点云数据序列中各个第一帧点云数据进行顺次拼接之后，再对互为回环关联关系的两个节点对应的第一帧点云数据进行回环重叠位置点云配准，具体为：将步骤7中基于第一优化位姿变换矩阵完成位姿配准的第一帧点云数据序列记为第一配准序列；并基于位姿图，将第一配准序列中互为回环关联关系的两个节点对应的第一帧点云数据按时间先后记为对应的第一、第二回环帧；再对第一、第二回环帧进行IoU区域识别记为对应的第一、第二重叠区域；再从第一、第二重叠区域中确定出配对的点、面或线作为对应的第一、第二参考点或第一、第二参考面或第一、第二参考线；再对从第一参考点到第二参考点、或从第一参考面到第二参考面、或从第一参考线到第二参考线的位姿变化进行计算得到对应的第一回环位姿变换矩阵；再基于第一回环位姿变换矩阵对第一、第二回环帧中时间靠后的第二回环帧进行反向点云坐标校准。这样一来，就可以消除回环位置的第一、第二回环帧中重叠区域的点云错位，从而解决回环位置的点云重影问题。
图3为本发明实施例二提供的一种电子设备的结构示意图。该电子设备可以为前述的终端设备或者服务器，也可以为与前述终端设备或者服务器连接的实现本发明实施例方法的终端设备或服务器。如图3所示，该电子设备可以包括：处理器301(例如CPU)、存储器302、收发器303；收发器303耦合至处理器301，处理器301控制收发器303的收发动作。存储器302中可以存储各种指令，以用于完成各种处理功能以及实现前述方法实施例描述的处理步骤。优选的，本发明实施例涉及的电子设备还包括：电源304、系统总线305以及通信端口306。系统总线305用于实现元件之间的通信连接。上述通信端口306用于电子设备与其他外设之间进行连接通信。
在图3中提到的系统总线305可以是外设部件互连标准(Peripheral ComponentInterconnect，PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，EISA)总线等。该系统总线可以分为地址总线、数据总线、控制总线等。为便于表示，图中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。通信接口用于实现数据库访问装置与其他设备(例如客户端、读写库和只读库)之间的通信。存储器可能包含随机存取存储器(Random Access Memory，RAM)，也可能还包括非易失性存储器(Non-Volatile Memory)，例如至少一个磁盘存储器。
上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)、图形处理器(Graphics Processing Unit，GPU)等；还可以是数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(FieldProgrammable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
需要说明的是，本发明实施例还提供一种计算机可读存储介质，该计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述实施例中提供的方法和处理过程。
本发明实施例还提供一种运行指令的芯片，该芯片用于执行前述方法实施例描述的处理步骤。
本发明实施例提供了一种连续帧点云数据的离线配准方法、电子设备及计算机可读存储介质，在获得连续帧点云数据和对应的位姿数据之后对各帧点云数据进行配准对筛选，并基于ICP算法对各个配准对里的两帧点云数据进行相对位姿估计得到对应的相对位姿矩阵和协方差矩阵；再基于各帧点云数据对应的位姿数据构建位姿图，并按最小二乘算法进行位姿图优化得到最终的优化位姿矩阵；最后使用优化位姿矩阵对连续帧点云数据的各帧点云数据进行配准。通过本发明，可以在不受人力因素的影响下对海量的连续帧点云数据进行快速配准，节约了人力成本，提高了点云数据配准效率。
专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种连续帧点云数据的离线配准方法，其特征在于，所述方法包括：
获取多帧点云数据构成第一帧点云数据序列；所述第一帧点云数据序列包括多个第一帧点云数据；相邻两个所述第一帧点云数据的前一帧采集时间早于后一帧采集时间；
获取与各个所述第一帧点云数据对应的位姿数据作为对应的第一帧位姿数据；
对所述第一帧点云数据序列进行配准帧数据对识别处理生成多组第一配准对；所述第一配准对的类型包括里程计配准对类型和回环配准对类型；
根据各个所述第一配准对对应的两个所述第一帧位姿数据进行点云帧相对位姿变换估计处理得到对应的第一位姿变换矩阵和第一协方差矩阵；并根据所述第一协方差矩阵进行置信度矩阵转换处理得到对应的第一置信度矩阵；并使用所述第一置信度矩阵对所述第一位姿变换矩阵进行加权处理得到对应的第一加权位姿变换矩阵；
构建位姿图，以各个所述第一帧位姿数据构建所述位姿图中的对应节点，并为相邻节点创建关联关系，并以节点间对应的所述第一加权位姿变换矩阵构建节点边约束关系；
对所述位姿图进行位姿图优化处理得到对应的第一优化位姿变换矩阵；
根据所述第一优化位姿变换矩阵对所述第一帧点云数据序列进行位姿配准。
2.根据权利要求1所述的连续帧点云数据的离线配准方法，其特征在于，所述对所述第一帧点云数据序列进行配准帧数据对识别处理生成多组第一配准对，具体包括：
对所述第一帧点云数据序列中任意相邻的两帧所述第一帧点云数据作为一组所述第一配准对，并设置对应类型为里程计配准对类型；
对所述第一帧点云数据序列中采集时间间隔超过设定间隔阈值的任意两帧所述第一帧点云数据，进行重叠度计算生成对应的第一重叠度数据；并在所述第一重叠度数据超过预设的重叠度阈值时，将对应的两帧所述第一帧点云数据作为一组所述第一配准对，并设置对应类型为回环配准对类型；所述设定间隔阈值大于相邻两帧所述第一帧点云数据的采集时间间隔。
3.根据权利要求1所述的连续帧点云数据的离线配准方法，其特征在于，所述根据各个所述第一配准对对应的两个所述第一帧位姿数据进行点云帧相对位姿变换估计处理得到对应的第一位姿变换矩阵和第一协方差矩阵，具体包括：
对于类型为里程计配准对类型的所述第一配准对，以所述第一配准对中采集时间靠前的所述第一帧位姿数据为第一源帧位姿、采集时间靠后的所述第一帧位姿数据为第一目标帧位姿；
对于类型为回环配准对类型的所述第一配准对，以所述第一配准对中采集时间靠后的所述第一帧位姿数据为第一源帧位姿、采集时间靠前的所述第一帧位姿数据为第一目标帧位姿；
基于迭代最近点ICP算法对所述第一源帧位姿到所述第一目标帧位姿的位姿变换矩阵进行估计从而得到对应的所述第一位姿变换矩阵和所述第一协方差矩阵。
4.根据权利要求1所述的连续帧点云数据的离线配准方法，其特征在于，所述根据所述第一协方差矩阵进行置信度矩阵转换处理得到对应的第一置信度矩阵，具体包括：
将所述第一协方差矩阵的逆矩阵作为对应的所述第一置信度矩阵。
5.根据权利要求1所述的连续帧点云数据的离线配准方法，其特征在于，所述构建位姿图，以各个所述第一帧位姿数据构建所述位姿图中的对应节点，并为相邻节点创建关联关系，并以节点间对应的所述第一加权位姿变换矩阵构建节点边约束关系，具体包括：
初始化所述位姿图的节点数量为所述第一帧位姿数据的数量，并使每个节点对应一个所述第一帧位姿数据；
按所述第一帧位姿数据的时间先后顺序对各个对应节点进行顺次连接；并为本次连接的各个相邻节点创建里程计关联关系；并将本次连接中相邻节点间的节点边记为第一类节点边，并以相邻节点对应的所述第一配准对的所述第一加权位姿变换矩阵作为所述第一类节点边的第一节点边约束关系；
对类型为回环配准对类型的所述第一配准对进行遍历，并将当前遍历的所述第一配准对作为当前回环配准对；并将所述当前回环配准对中采集时间靠后的所述第一帧位姿数据作为第二源帧位姿、采集时间靠前的所述第一帧位姿数据作为第二目标帧位姿；并将所述位姿图中与所述第二源帧位姿对应的节点记为当前源节点，与所述第二目标帧位姿对应的节点记为当前目标节点；并在所述当前源节点与所述当前目标节点间创建回环关联关系；并从所述当前源节点向所述当前目标节点建立对应的第二类节点边；并以所述当前回环配准对的所述第一加权位姿变换矩阵作为所述第二类节点边的第二节点边约束关系。
6.根据权利要求1所述的连续帧点云数据的离线配准方法，其特征在于，所述对所述位姿图进行位姿图优化处理得到对应的第一优化位姿变换矩阵，具体包括：
根据所述位姿图中关联关系为里程计关联关系的各连接节点间的第一节点边约束关系，按最小二乘法构建对应的非线性最小二乘问题；并通过求解所述非线性最小二乘问题得到对应的所述第一优化位姿变换矩阵。
7.一种电子设备，其特征在于，包括：存储器、处理器和收发器；
所述处理器用于与所述存储器耦合，读取并执行所述存储器中的指令，以实现权利要求1-6任一项所述的方法步骤；
所述收发器与所述处理器耦合，由所述处理器控制所述收发器进行消息收发。
8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，当所述计算机指令被计算机执行时，使得所述计算机执行权利要求1-6任一项所述的方法的指令。
CN202210515961.0A 2022-05-12 2022-05-12 一种连续帧点云数据的离线配准方法 Active CN114757980B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210515961.0A CN114757980B (zh) 2022-05-12 2022-05-12 一种连续帧点云数据的离线配准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210515961.0A CN114757980B (zh) 2022-05-12 2022-05-12 一种连续帧点云数据的离线配准方法

Publications (2)

Publication Number Publication Date
CN114757980A CN114757980A (zh) 2022-07-15
CN114757980B true CN114757980B (zh) 2024-03-26

Family

ID=82334926

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210515961.0A Active CN114757980B (zh) 2022-05-12 2022-05-12 一种连续帧点云数据的离线配准方法

Country Status (1)

Country Link
CN (1) CN114757980B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109974712A (zh) * 2019-04-22 2019-07-05 广东亿嘉和科技有限公司 一种基于图优化的变电站巡检机器人建图方法
CN111080682A (zh) * 2019-12-05 2020-04-28 北京京东乾石科技有限公司 点云数据的配准方法及装置
CN112767456A (zh) * 2021-01-18 2021-05-07 南京理工大学 一种三维激光点云快速重定位方法
CN113989451A (zh) * 2021-10-28 2022-01-28 北京百度网讯科技有限公司 高精地图构建方法、装置及电子设备
CN114119898A (zh) * 2020-08-28 2022-03-01 广东博智林机器人有限公司 一种三维重建方法、装置、设备及存储介质

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3612854B1 (en) * 2018-07-02 2023-03-15 Beijing Didi Infinity Technology and Development Co., Ltd. Vehicle navigation system using pose estimation based on point cloud

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109974712A (zh) * 2019-04-22 2019-07-05 广东亿嘉和科技有限公司 一种基于图优化的变电站巡检机器人建图方法
CN111080682A (zh) * 2019-12-05 2020-04-28 北京京东乾石科技有限公司 点云数据的配准方法及装置
CN114119898A (zh) * 2020-08-28 2022-03-01 广东博智林机器人有限公司 一种三维重建方法、装置、设备及存储介质
CN112767456A (zh) * 2021-01-18 2021-05-07 南京理工大学 一种三维激光点云快速重定位方法
CN113989451A (zh) * 2021-10-28 2022-01-28 北京百度网讯科技有限公司 高精地图构建方法、装置及电子设备

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于RGB-D相机数据的SLAM算法;洪亮;冯常;;电子设计工程;20180505(第09期);全文 *
基于空间位置不确定性约束的改进闭环检测算法;李维鹏;张国良;姚二亮;徐君;;机器人;20160515(第03期);全文 *

Also Published As

Publication number Publication date
CN114757980A (zh) 2022-07-15

Similar Documents

Publication Publication Date Title
CN107481292B (zh) 车载摄像头的姿态误差估计方法和装置
CN106981074B (zh) 用于估计相机相对于表面的取向的方法和装置
CN109446886B (zh) 基于无人车的障碍物检测方法、装置、设备以及存储介质
EP3537382A1 (en) Device and method for calculating a vehicle trailer pose using a camera
CN109782269B (zh) 一种分布式多平台协同有源目标跟踪方法
CN114926726B (zh) 基于多任务网络的无人船感知方法及相关设备
CN113610141A (zh) 自动驾驶多传感器融合感知模型的鲁棒性测试方法及系统
CN115393815A (zh) 道路信息生成方法、装置、电子设备和计算机可读介质
CN111739071A (zh) 基于初始值的快速迭代配准方法、介质、终端和装置
CN109190757B (zh) 任务处理方法、装置、设备及计算机可读存储介质
CN114757980B (zh) 一种连续帧点云数据的离线配准方法
CN114387353A (zh) 一种相机标定方法、标定装置及计算机可读存储介质
CN113720349A (zh) 一种基于卡尔曼滤波的里程计信息平滑方法
CN111765883B (zh) 机器人蒙特卡罗定位方法、设备及存储介质
CN116430398A (zh) 基于tof相机与双目视觉数据融合的测距方法及设备
CN116385369A (zh) 深度图像质量评价方法、装置、电子设备及存储介质
CN114663478A (zh) 一种根据多参考点预测信息估计锚点位置的方法
CN112288817B (zh) 基于图像的三维重建处理方法及装置
CN111833395B (zh) 一种基于神经网络模型的测向体制单目标定位方法和装置
CN114549429A (zh) 基于超图结构的深度数据质量评价方法及装置
CN113052886A (zh) 一种采用双目原理的双tof相机深度信息获取方法
CN117493775B (zh) 数据链的相对导航方法、装置、电子设备及存储介质
CN115131426B (zh) 一种对车辆后轴中心点进行估计的处理方法
CN115375695B (zh) 一种动态星敏感器暗弱恒星光斑的检测方法及装置、介质
CN114820564A (zh) 一种观测噪声的处理方法和装置

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