CN112837354B - 一种基于gpu的ndt点云配准算法、装置及电子设备 - Google Patents

一种基于gpu的ndt点云配准算法、装置及电子设备 Download PDF

Info

Publication number
CN112837354B
CN112837354B CN202110145479.8A CN202110145479A CN112837354B CN 112837354 B CN112837354 B CN 112837354B CN 202110145479 A CN202110145479 A CN 202110145479A CN 112837354 B CN112837354 B CN 112837354B
Authority
CN
China
Prior art keywords
point cloud
gpu
point
pose
points
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
CN202110145479.8A
Other languages
English (en)
Other versions
CN112837354A (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.)
Beijing Chaoxing Future Technology Co ltd
Original Assignee
Beijing Chaoxing Future 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 Beijing Chaoxing Future Technology Co ltd filed Critical Beijing Chaoxing Future Technology Co ltd
Priority to CN202110145479.8A priority Critical patent/CN112837354B/zh
Publication of CN112837354A publication Critical patent/CN112837354A/zh
Application granted granted Critical
Publication of CN112837354B publication Critical patent/CN112837354B/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/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T1/00—General purpose image data processing
    • G06T1/20—Processor architectures; Processor configuration, e.g. pipelining
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T1/00—General purpose image data processing
    • G06T1/60—Memory management
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/60—Rotation of whole images or parts thereof
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D10/00—Energy efficient computing, e.g. low power processors, power management or thermal management

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Operations Research (AREA)
  • Computing Systems (AREA)
  • Image Processing (AREA)

Abstract

本申请实施例中提供了一种基于GPU的NDT点云配准算法、装置及电子设备，属于点云配准计算机技术领域，算法包括：提取目标点云数据，提取源点云数据，计算高斯近似常量，给定初始猜测位姿，对源点云中的点进行变换，计算Jacobian矩阵和Hessian矩阵，判定经位姿变换后的源点云中的点是否为有效点，计算每个有效点的梯度向量
Figure DDA0002930041010000011
和Hessian矩阵H并分别加和，将加和后的
Figure DDA0002930041010000012
和H'利用Jacobi方法实现奇异值分解方程

Description

一种基于GPU的NDT点云配准算法、装置及电子设备
技术领域
本申请涉及点云配准计算机技术领域，尤其涉及一种基于GPU的NDT点云配准算法、装置及电子设备。
背景技术
自动驾驶的定位任务常由GNSS(Global Navigation Satellite System，全球导航卫星系统)，IMU(Inertial Measurement Unit，惯性测量单元)和RTK(Real-TimeKinematic，载波相位差分技术)组合或独立完成，但这些定位方法在运行时常受一些现实条件的约束。例如，在峡谷和高楼中GNSS的定位精度可能由1-3米下降到10-50米；IMU在连续积分后因定位误差被累积导致定位精度变差；RTK在大部分地区并没有被使用等等。因此，在当今的自动驾驶定位任务中，经过组合导航定位被引入。这种组合导航将几种定位方法相融合，以得到更加精准且鲁棒的定位结果。
在基于激光雷达定位任务中，点云配准作为辅助配准的方法被引入，用以对上述定位方法进行校准。NDT(Normal Distributions Transform，正态分布变换)配准是一种点云配准算法。这种算法利用正态分布对三维目标点云中的物体进行描述，将源点云扫描出的物体与目标点云中以正态分布描述的物体进行匹配，并利用线搜索的迭代方法搜索最佳变换位姿。因为激光雷达扫描后得到的数据量庞大，且NDT算法引入了对物体的多维描述，所以NDT算法对计算单元的算力要求较高。
目前的NDT算法均由CPU进行计算，然而CPU在进行大规模计算任务时并不能保证算法的低延时，其原因是CPU上有限的计算单元；这种问题在车载计算平台上更加明显，因为CPU的算力进一步被CPU数量局限。而在自动驾驶的应用场景中需要采用高线束雷达，一般情况下需要保证在16线以上，这就意味着在算法中需要处理大量的点，因此大算力计算单元需要被应用。
发明内容
有鉴于此，本申请实施例提供一种基于GPU的NDT点云配准算法、装置及电子设备，利用GPU的并行计算能力和丰富的计算单元代替CPU完成计算任务，并针对GPU架构进行了算法部署，以充分发挥GPU的计算能力，至少部分解决现有技术中存在的问题。
第一方面，本申请实施例提供了一种基于GPU的NDT点云配准算法，包括以下步骤:
提取目标点云数据，包括：
将目标点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存中并存储；
将目标点云划分为若干个体素，每个体素对应有参数；及
分别为每个参数分配GPU显存空间，计算每个体素对应的参数，将计算结果存储于各参数对应的显存空间；
提取源点云数据，将源点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存；
计算高斯近似常量；
变换位姿求解过程，包括：
给定初始猜测位姿
Figure BDA0002930040000000032
，根据/>
Figure BDA0002930040000000033
计算变换方程，并将变换方程内的参数存入GPU常量内存；
利用变换方程内的参数对源点云中的点进行变换，得到旋转点云集；
利用所述变换方程内的参数计算Jacobian矩阵JE和Hessian矩阵HE，并将计算结果分别拷入GPU常量内存；
判定旋转点云集中的点是否为有效点；
根据各体素中的参数和高斯近似常量得到分数方程；
根据JE和HE，计算每个有效点关于分数方程的梯度向量
Figure BDA0002930040000000031
和二阶导数Hessian矩阵H，并分别加和所有有效点的/>
Figure BDA0002930040000000041
和H，得到加和后的梯度向量/>
Figure BDA0002930040000000042
和二阶导数Hessian矩阵H'；
将得到的
Figure BDA0002930040000000043
和H'的结果从GPU拷入CPU，利用Jacobi方法实现奇异值分解并解方程：/>
Figure BDA0002930040000000045
其中，/>
Figure BDA0002930040000000046
为位姿增量，求出/>
Figure BDA0002930040000000047
的范数并对/>
Figure BDA0002930040000000048
归一化；
将
Figure BDA0002930040000000044
的范数和归一化结果代入线搜索算法，更新/>
Figure BDA0002930040000000049
并将该位姿增量与本次迭代的初始位姿相加得到新的位姿/>
Figure BDA00029300400000000410
及
重复变换位姿求解过程，直到
Figure BDA00029300400000000411
的范数固定不变或变换位姿求解过程的迭代次数等于预设最大迭代次数后停止迭代，得出最优变换位姿/>
Figure BDA00029300400000000412
根据本申请实施例的一种具体实现方式，所述将目标点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存中并存储，其中，存储形式为所述XYZ轴坐标数据按数组形式分别储存，每一个所述数组的长度等于所述目标点云数据中点的数量，且所述XYZ轴坐标数据分别对应的数组空间均为显存中的连续地址空间。
根据本申请实施例的一种具体实现方式，所述将目标点云划分为若干个体素包括：
在GPU中利用线程束级操作完成快速归约算法，找到所述XYZ轴的最大值和最小值确定所述目标点云边界；
根据所述目标点云边界和预设体素分辨率计算得到XYZ三个维度上所述体素的个数，将三个维度上所述体素的个数相乘得到总的体素个数。
根据本申请实施例的一种具体实现方式，所述将目标点云划分为若干个体素，每个体素对应有参数，其中，所述参数包括协方差矩阵，重心向量和协方差矩阵的逆矩阵。
根据本申请实施例的一种具体实现方式，所述旋转点云集中的点是否为有效点的方法为对所述旋转点云集中的点进行半径搜索，或根据所述旋转点云集中点的坐标映射入所属体素后，判断该点是否存在候选体素，以及该点的候选体素中是否有一个体素包含5个以上的目标点云集的点。
根据本申请实施例的一种具体实现方式，所述半径搜索的步骤包括：
将以所述旋转点云集中的一点为圆心，R为半径的球体相切的所述体素作为该点的候选体素并记录，完成所述旋转点云集中所有点的所述候选体素记录，其中，R为所述体素的分辨率；
遍历并检查各点的所述候选体素，若点到与该点对应的所述候选体素的重心距离大于R，则将该体素从该点的所述候选体素中剔除；
遍历所述旋转点云集中的所有点，若某一点有所述候选体素且该候选体素中含有5个以上目标点云中的点则将此点标记为有效点，否则将此点标记为非法点。
根据本申请实施例的一种具体实现方式，所述线搜索算法为More-Thuente线搜索。
根据本申请实施例的一种具体实现方式，所述根据JE和HE，计算每个有效点关于分数方程的梯度向量
Figure BDA0002930040000000061
和二阶导数Hessian矩阵H，并分别加和所有有效点的/>
Figure BDA0002930040000000062
和H，其中，所述分别加和所有有效点的/>
Figure BDA0002930040000000063
和H时采用GPU线束级归约操作。
第二方面，本申请实施例还提供一种基于GPU的NDT点云配准算法的配准装置，所述配准装置包括：
目标点云数据提取模块,所述目标点云数据提取模块用于提取目标点云数据，包括将目标点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存中并存储；将目标点云划分为若干个体素，每个体素对应有参数；及分别为每个参数分配GPU显存空间，计算每个体素对应的参数，将计算结果存储于各参数对应的显存空间；
源点云数据提取模块，所述源点云数据提取模块用于将源点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存；
高斯近似常量计算模块，所述高斯近似常量计算模块用于计算所述高斯近似常量；及
变换位姿求解模块，所述变换位姿求解模块用于实现点云最优变换位姿搜索过程，包括：
给定初始猜测位姿
Figure BDA0002930040000000071
根据/>
Figure BDA0002930040000000073
计算变换方程，并将变换方程内的参数存入GPU常量内存；
利用变换方程内的参数对源点云中的点进行变换，得到旋转点云集；
利用所述变换方程内的参数计算Jacobian矩阵JE和Hessian矩阵HE，并将计算结果分别拷入GPU常量内存；
判定旋转点云集中的点是否为有效点；
根据各体素中的参数和高斯近似常量得到分数方程；
根据JE和HE，计算每个有效点关于分数方程的梯度向量
Figure BDA0002930040000000074
和二阶导数Hessian矩阵H，并分别加和所有有效点的/>
Figure BDA0002930040000000075
和H，得到加和后的梯度向量/>
Figure BDA0002930040000000076
和二阶导数Hessian矩阵H'；
将得到的
Figure BDA0002930040000000077
和H'的结果从GPU拷入CPU，利用Jacobi方法实现奇异值分解并解方程：/>
Figure BDA0002930040000000078
其中，/>
Figure BDA0002930040000000079
为位姿增量，求出/>
Figure BDA00029300400000000713
的范数并对/>
Figure BDA00029300400000000710
归一化；
将
Figure BDA00029300400000000711
的范数和归一化结果代入线搜索算法，更新/>
Figure BDA00029300400000000712
并将该位姿增量与本次迭代的初始位姿相加得到新的位姿/>
Figure BDA0002930040000000084
及
重复变换位姿求解过程，直到
Figure BDA0002930040000000082
的范数固定不变或变换位姿求解过程的迭代次数等于预设最大迭代次数后停止迭代，得出最优变换位姿/>
Figure BDA0002930040000000085
第三方面，本申请实施例还提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现前述第一方面中任一实施例所述的基于GPU的NDT点云配准算法。
第四方面，本申请实施列还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述第一方面中任一实施例所述的基于GPU的NDT点云配准算法。
有益效果
本申请提出的基于GPU的NDT点云配准算法，针对CPU在进行大规模计算任务时并不能保证算法的低延时的问题，提出了一整套可以在GPU上执行的NDT点云配准算法，该算法流程根据GPU结构进行了部署，解决算法的在CPU上执行时的高延时问题，提高了GPU的工作效率。
具体实施方式
下面对本申请实施例进行详细描述。
以下通过特定的具体实例说明本申请的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本申请的其他优点与功效。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。本申请还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本申请的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本申请，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。
还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本申请的基本构想，图式中仅显示与本申请中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践所述方面。
本申请实施例提供的基于GPU的NDT点云配准算法中利用线搜索方法进行最优位姿计算，所采用的线搜索方法可以不做限定，不局限于本申请所列出的线搜索方法。在以下的描述中，以基于More-Thuente线搜索的NDT点云配准方法为例进行描述。
第一方面，本申请实施例提供了一种基于GPU的NDT点云配准算法，下面将具体描述点云配准算法，包括：
S1：提取目标点云数据，所述提取目标点云数据包括：
S1.1：在GPU显存中开辟3个内存空间，所述内存空间大小为4bytes×NT，其中NT为目标点云数量；
S1.2：将目标点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存中开辟的内存空间，这三个轴的数据在GPU显存中按数组形式分别储存，每一个数组的长度等于目标点云中点的数量NT，且这三个轴对应的数组空间均为连续地址空间；
S1.3：将目标点云划分为若干个体素，每个体素对应有参数；
S1.4：分别为每个参数分配空间，计算每个体素对应的参数，将计算结果存储于各参数对应的空间。
在本实施例中，所述将目标点云划分为若干个体素包括：根据提取的所述目标点云数据，在GPU中利用线程束级操作(Warp-Level Operation)完成快速归约算法，找到所述XYZ轴的最大值和最小值确定目标点云边界；根据所述目标点云边界和预设体素分辨率Rx，Ry和Rz得到三维体素网格，并计算得到XYZ三个维度上所述体素的个数Nx，Ny和Nz，其中，Rx，Ry和Rz分别代表所述体素在X、Y和Z轴的分辨率，在本实施例中Rx＝Ry＝Rz＝R，将三个维度上所述体素的个数Nx，Ny和Nz相乘得到总的体素个数NV。
进一步的，利用线程束级快速归约算法(Warp-Level Reduction)，具体包括：首先在GPU的每个线程块内分配Pt个线程，Pt需为32的整数倍且32≤Pt≤1024；根据所述Pt确定线程块个数Pb，
Figure BDA0002930040000000111
在GPU显存中创建一个输出缓存器，其大小为4bytes×Pb；在每个线程中以bx×Pt+tx为起始索引，Pb×Pt为步长，NT为循环终止条件遍历目标点云中的点，以线程为单位找出最大值，其中bx为线程块索引，tx为线程索引；将所有线程找出的最大值用Warp Operation操作在线程块内进行比较，找出线程块内的最大值，并由该线程块中的零号线程将最大值写入所述输出缓存器，所述输出缓存器的写入位置由执行该操作的线程块索引bx决定；为线程块分配1024个线程，为GPU分配1个线程块,每个线程遍历输出缓存器中的一个元素，再次利用Warp Operation操作找出该线程块内的最大值，并由零号线程输出该最大值；重复所述线程束级快速归约算法三次，找出最大X，Y，Z的值并确定所述目标点云边界。
需要说明的是所述体素的参数包括协方差矩阵Σ，重心向量
Figure BDA0002930040000000121
和协方差矩阵的逆矩阵Σ-1，其中，所述协方差矩阵和重心向量来表述对应的体素中描绘点云分布的三维正态分布，所述协方差矩阵的逆矩阵用于后续点云配准过程中计算源点云中的点在该体素中的概率密度。
在为所述参数分配空间时，具体分配方法为：所述协方差矩阵和协方差矩阵的逆矩阵中分别有9个元素，因此在GPU显存中分别为两个矩阵开辟9个数组空间，每个数组空间大小等于体素个数，且这9个数组在GPU显存中为连续的地址空间，即GPU显存中存储这个矩阵的设备内存应为一个大小为9×4bytes×NV的连续地址空间，每个矩阵的9个数组分别依次存储矩阵的9个元素。
例如，在一个申请实施例中，为所述协方差矩阵分配空间，规定空间中X，Y，Z轴坐标值最小的体素为索引是0的体素，依据X，Y，Z轴的先后顺序，按坐标数值增大的方向从0到NT-1顺序为所有体素分配索引，第一个数组依次存储所有体素对应协方差矩阵中的第一个元素，第二个数组依次存储协方差矩阵的第二个元素，共完成所述协方差矩阵的9个元素的存储。
所述协方差矩阵的逆矩阵和重心向量与所述协方差矩阵的分配空间方法相同，在此不再赘述，可参照所述协方差矩阵分配空间的方法进行分配，其中，为重心向量在GPU显存中分配空间，这样做的目的是在读取体素中参数，即两个矩阵和一个向量时做到对GPU内存的合并访问，以增加访存效率。
在本申请实施例中，所述计算每个体素对应的参数具体包括：
为每一个目标点云中的点分配一个GPU线程，设线程块中线程的数量为Pt'，则线程块的数量
Figure BDA0002930040000000131
根据点的X，Y，Z坐标将点映射入所述三维体素网格；
按所属分配线程，遍历体素内所有点，将体素内所有点的XYZ轴坐标分别相加后除以体素内点的数量，得到重心向量，将每个体素的重心向量储存到对应的重心向量空间；
按所属分配线程，根据定义，协方差矩阵的第i,j项如下：
Σi,j＝cov(Xi,Xj)＝E[(Xi-ui)(Xj-uj)]，即
Figure BDA0002930040000000132
其中，
Figure BDA0002930040000000134
是由n个随机向量组成的列向量，ui是其中第i个元素的期望值，即ui＝E(Xi)，根据上式计算每个体素的协方差矩阵Σ，并将计算结果存入对应的内存空间。值得注意的是，后续的过程中需要计算协方差矩阵的逆矩阵，因此，为防止体素中的点完全共面或共线，即协方差矩阵是奇异的并且不能求逆，只有含有5个点以上的体素被计算协方差矩阵，否则该体素对应的协方差矩阵中的所有元素全部置零；更进一步的，为了防止在计算中出现数值问题，当协方差矩阵近乎奇异时，即当协方差矩阵中的最大特征值λ3比其他任意两个特征值大100倍时，那么比这个最大特征值小100倍以上的特征值λj会被置为
Figure BDA0002930040000000141
此时协方差矩阵变为Σ'＝VΛ'V，其中V包含Σ的特征向量，Λ'为
Figure BDA0002930040000000142
根据所述分配线程，根据定义Σ×Σ-1＝I，计算协方差矩阵的逆矩阵，其中I是单位矩阵，将得到的协方差矩阵的逆矩阵存入对应的内存空间。
S2、提取源点云数据，所述源点云数据的内存分配和提取方法可参照目标点云，将源点云χ中点的XYZ轴坐标数据从CPU内存中拷入GPU显存，记源点云χ中点的数量为Ns。
S3、计算高斯近似常量，所述高斯近似常量包括c1，c2，d1，d2和d3，具体计算过程为：
c1和c2为一个常数，令如下公式：
Figure BDA0002930040000000151
在体素跨越的空间中等于1可求出该常数，其中，p0是离群值(β)的比例，在本发明中β设为0.55，c1和c2和可被简算为：
c1＝10×(1-β)，
Figure BDA0002930040000000152
根据如下公式计算d1，d2和d3：
d3＝-log(c2)，
d1＝-log(c1+c2)-d3，
Figure BDA0002930040000000153
S4、给定初始猜测位姿
Figure BDA0002930040000000154
其中/>
Figure BDA0002930040000000155
根据/>
Figure BDA0002930040000000156
计算变换方程，并将变换方程内的参数存入GPU常量内存。
需要说明的是，
Figure BDA0002930040000000157
可由其他定位系统给出，在本申请中并不做任何限定，例如，可以由IMU，GNSS，RTK，里程计信息等等给出。
S5、利用变换方程内的参数对源点云中的点进行变换，得到旋转点云集。所述变换方程如下：
Figure BDA0002930040000000161
其中，ci＝cosφi，si＝sinφi；计算该变换方程，将方程内所有参数存入GPU常量内存；
所述变换过程为，为每一个源点云中的点分配一个GPU线程，设线程块中线程的数量为Pt”，则线程块的数量
Figure BDA0002930040000000162
读取常量内存中的参数对点进行变换，得到旋转后的旋转点云集χ'；读取常量内存时常量内存会被从全局内存缓存至GPU流处理器上的常量缓存，这个操作将对于全局内存的访问转换为片上缓存，从而加快访存速度。
S6、利用所述变换方程内的参数计算Jacobian矩阵JE和Hessian矩阵HE，并将计算结果分别拷入GPU常量内存。
所述Jacobian矩阵JE的计算公式为：
Figure BDA0002930040000000163
其中，
a＝x1(-sxsz+cxsycz)+x2(-sxcz-cxsysz)+x3(-cxcy)
b＝x1(cxsz+sxsycz)+x2(-sxsysz+cxcz)+x3(-sxcy)
c＝x1(-sycz)+x2(sysz)+x3(cy)
d＝x1(sxcycz)+x2(-sxcysz)+x3(sxsy)
e＝x1(-cxcycz)+x2(cxcysz)+x3(-cxsy)
f＝x1(-cysz)+x2(-cycz)
g＝x1(cxcz-sxsysz)+x2(-cxsz-sxsycz)
h＝x1(sxcz+cxsysz)+x2(cxsycz-sxsz)；
所述Hessian矩阵HE的计算公式为：
Figure BDA0002930040000000174
其中，
Figure BDA0002930040000000171
Figure BDA0002930040000000172
Figure BDA0002930040000000173
Figure BDA0002930040000000181
Figure BDA0002930040000000182
Figure BDA0002930040000000183
S7、判定χ'中的点是否为有效点。
根据本申请实施例的一种具体实现方式，所述判定χ'中的点是否为有效点的方法为对所述源点云中的点进行半径搜索或根据所述源点云中点的坐标映射入所属体素。优选的，采用半径搜索法判定有效点，这个操作是为了在单次的变换位姿求解的过程中让结果更加鲁棒，具体过程为：分配线程池，参照源点云旋转时的分配方法在每个线程中查找与以该点为圆心，R为半径的球体相切的体素，作为该点的候选体素并记录这些候选体素；分配线程池，在每个线程中遍历并检查步骤10.1中所描述的候选体素，如果点到该候选体素的重心的距离大于R，则将该体素从候选体素中剔除；分配线程池，遍历χ'中的所有点，若该点有候选体素则将此点标记为有效点，否则将此点标记为非法点，记有效点的数量为Nc。
S8、根据体素的参数和高斯近似常量计算每个有效点的分数，并将各有效点对应的分数加和得到分数方程，具体计算过程为：
为每一个有效点分配一个GPU线程，设线程块中线程的数量为Pt”'，则线程块的数量
Figure BDA0002930040000000191
在线程中遍历该有效点的所有的候选体素，根据如下分数计算式：
Figure BDA0002930040000000192
计算该点对当前位姿变换的影响，其中
Figure BDA0002930040000000194
和/>
Figure BDA0002930040000000195
代表该有效点所在的候选体素，K代表候选体素个数；至此，给出一个源点云集χ，一个变换位姿/>
Figure BDA00029300400000001915
一个变换方程/>
Figure BDA00029300400000001916
那么对于变换位姿/>
Figure BDA0002930040000000198
在NDT配准的分数方程的和/>
Figure BDA0002930040000000199
即分数方程可以表示为
Figure BDA0002930040000000193
S9、根据JE和HE，计算分数方程对变换位姿
Figure BDA00029300400000001910
的梯度向量/>
Figure BDA00029300400000001911
和二阶导数Hessian矩阵H，具体计算过程为：
根据JE和HE，计算每个有效点关于分数方程的梯度向量
Figure BDA00029300400000001912
和二阶导数Hessian矩阵H，并分别加和所有有效点的/>
Figure BDA00029300400000001913
和H，得到加和后的梯度向量/>
Figure BDA00029300400000001914
和二阶导数Hessian矩阵H'；
如步骤S8中描述分配线程池，遍历所有有效点，根据公式：
Figure BDA0002930040000000201
计算该点的梯度向量，其中gi表示表示梯度向量中的第i个元素，
Figure BDA0002930040000000202
由JE的第i列给出，/>
Figure BDA0002930040000000206
是/>
Figure BDA0002930040000000207
按照/>
Figure BDA0002930040000000208
旋转后得到的，由步骤S5计算得出；
如步骤S8中描述分配线程池，遍历所有有效点，根据公式
Figure BDA0002930040000000203
计算Hessian矩阵的i，j元素，其中
Figure BDA0002930040000000204
由HE给出。
本实施例中，分数方程的梯度
Figure BDA0002930040000000209
和二阶导数Hessian矩阵H'由各有效点的梯度/>
Figure BDA00029300400000002010
和二阶导数Hessian矩阵H分别加和得到，所述加和所有有效点的/>
Figure BDA00029300400000002011
和H是与线束级操作同理，是利用GPU Warp-Level Reduction方法。
S10、将得到的
Figure BDA00029300400000002012
和H'的结果从GPU拷入CPU，利用Jacobi方法实现奇异值分解并解方程：/>
Figure BDA0002930040000000205
其中，/>
Figure BDA00029300400000002013
为位姿增量，求出/>
Figure BDA00029300400000002014
的范数并对/>
Figure BDA00029300400000002015
归一化，/>
Figure BDA00029300400000002016
的范数即为所有向量元素的平方和的平方根。
S11、将
Figure BDA00029300400000002017
的范数和归一化结果代入More-Thuente线搜索算法，更新/>
Figure BDA00029300400000002018
将更新后的位姿增量/>
Figure BDA0002930040000000212
与本次迭代最开始的位姿相加得到新的位姿/>
Figure BDA0002930040000000215
S12、重复步骤S4-S11，直到
Figure BDA0002930040000000214
的范数固定不变或S4-S11的迭代次数等于最大迭代次数后停止迭代，得出最优变换位姿/>
Figure BDA0002930040000000211
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
第二方面，本申请实施例还提供一种基于GPU的NDT点云配准算法的配准装置，所述配准装置包括：
目标点云数据提取模块,所述目标点云数据提取模块用于提取目标点云数据，包括将目标点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存中并存储；将目标点云划分为若干个体素，每个体素对应有参数；及分别为每个参数分配GPU显存空间，计算每个体素对应的参数，将计算结果存储于各参数对应的显存空间；
源点云数据提取模块，所述源点云数据提取模块用于将源点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存；
高斯近似常量计算模块，所述高斯近似常量计算模块用于计算所述高斯近似常量；及
变换位姿求解模块，所述变换位姿求解模块用于实现点云最优变换位姿搜索过程，包括：
给定初始猜测位姿
Figure BDA00029300400000002214
根据/>
Figure BDA0002930040000000223
计算变换方程，并将变换方程内的参数存入GPU常量内存；
利用变换方程内的参数对源点云中的点进行变换，得到旋转点云集；
利用所述变换方程内的参数计算Jacobian矩阵JE和Hessian矩阵HE，并将计算结果分别拷入GPU常量内存；
判定旋转点云集中的点是否为有效点；
根据各体素中的参数和高斯近似常量得到分数方程；
根据JE和HE，计算每个有效点关于分数方程的梯度向量
Figure BDA0002930040000000224
和二阶导数Hessian矩阵H，并分别加和所有有效点的/>
Figure BDA0002930040000000225
和H，得到加和后的梯度向量/>
Figure BDA0002930040000000226
和二阶导数Hessian矩阵H'；
将得到的
Figure BDA0002930040000000227
和H'的结果从GPU拷入CPU，利用Jacobi方法实现奇异值分解并解方程：/>
Figure BDA0002930040000000221
其中，/>
Figure BDA0002930040000000228
为位姿增量，求出/>
Figure BDA0002930040000000229
的范数并对/>
Figure BDA00029300400000002210
归一化；
将
Figure BDA00029300400000002211
的范数和归一化结果代入线搜索算法，更新/>
Figure BDA00029300400000002212
并将该位姿增量与本次迭代的初始位姿相加得到新的位姿/>
Figure BDA00029300400000002215
及
重复变换位姿求解过程，直到
Figure BDA0002930040000000232
的范数固定不变或变换位姿求解过程的迭代次数等于预设最大迭代次数后停止迭代，得出最优变换位姿/>
Figure BDA0002930040000000231
第三方面，本申请实施例还提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现前述第一方面中任一实施例所述的基于GPU的NDT点云配准算法。
第四方面，本申请实施列还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述第一方面中任一实施例所述的基于GPU的NDT点云配准算法。
本申请提供的实施例针对CPU在进行大规模计算任务时并不能保证算法的低延时的问题，发明了一种基于GPU的NDT点云配准算法，提出了一整套可以在GPU上执行的NDT点云配准算法，该算法流程根据GPU结构进行了部署，降低算法在CPU端的高延时问题，提高了GPU工作效率。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。

Claims (11)

1.一种基于GPU的NDT点云配准算法，其特征在于，包括以下步骤:
提取目标点云数据，包括：
将目标点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存中并存储；
将目标点云划分为若干个体素，每个体素对应有参数；及
分别为每个参数分配GPU显存空间，计算每个体素对应的参数，将计算结果存储于各参数对应的显存空间；
提取源点云数据，将源点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存；
计算高斯近似常量；
变换位姿求解过程，包括：
给定初始猜测位姿
Figure FDA0002930039990000011
根据/>
Figure FDA0002930039990000012
计算变换方程，并将变换方程内的参数存入GPU常量内存；
利用变换方程内的参数对源点云中的点进行变换，得到旋转点云集；
利用所述变换方程内的参数计算Jacobian矩阵JE和Hessian矩阵HE，并将计算结果分别拷入GPU常量内存；
判定旋转点云集中的点是否为有效点；
根据各体素中的参数和高斯近似常量得到分数方程；
根据JE和HE，计算每个有效点关于分数方程的梯度向量
Figure FDA0002930039990000021
和二阶导数Hessian矩阵H，并分别加和所有有效点的/>
Figure FDA0002930039990000022
和H，得到加和后的梯度向量/>
Figure FDA0002930039990000023
和二阶导数Hessian矩阵H'；
将得到的
Figure FDA0002930039990000024
和H'的结果从GPU拷入CPU，利用Jacobi方法实现奇异值分解并解方程：
Figure FDA0002930039990000025
其中，/>
Figure FDA0002930039990000026
为位姿增量，求出/>
Figure FDA0002930039990000027
的范数并对/>
Figure FDA0002930039990000028
归一化；
将
Figure FDA0002930039990000029
的范数和归一化结果代入线搜索算法，更新/>
Figure FDA00029300399900000210
并将该位姿增量与本次迭代的初始位姿相加得到新的位姿/>
Figure FDA00029300399900000211
及
重复变换位姿求解过程，直到
Figure FDA00029300399900000212
的范数固定不变或变换位姿求解过程的迭代次数等于预设最大迭代次数后停止迭代，得出最优变换位姿/>
Figure FDA00029300399900000213
2.根据权利要求1所述的基于GPU的NDT点云配准算法，其特征在于，所述将目标点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存中并存储，其中，存储形式为所述XYZ轴坐标数据按数组形式分别储存，每一个所述数组的长度等于所述目标点云数据中点的数量，且所述XYZ轴坐标数据分别对应的数组空间均为显存中的连续地址空间。
3.根据权利要求1所述的基于GPU的NDT点云配准算法，其特征在于，所述将目标点云划分为若干个体素包括：
在GPU中利用线程束级操作完成快速归约算法，找到所述XYZ轴的最大值和最小值确定所述目标点云边界；
根据所述目标点云边界和预设体素分辨率计算得到XYZ三个维度上所述体素的个数，将三个维度上所述体素的个数相乘得到总的体素个数。
4.根据权利要求1所述的基于GPU的NDT点云配准算法，其特征在于，所述将目标点云划分为若干个体素，每个体素对应有参数，其中，所述参数包括协方差矩阵，重心向量和协方差矩阵的逆矩阵。
5.根据权利要求1所述的基于GPU的NDT点云配准算法，其特征在于，所述旋转点云集中的点是否为有效点的方法为对所述旋转点云集中的点进行半径搜索，或根据所述旋转点云集中点的坐标映射入所属体素后，判断该点是否存在候选体素，以及该点的候选体素中是否有一个体素包含5个以上的目标点云集的点。
6.根据权利要求5所述的基于GPU的NDT点云配准算法，其特征在于，所述半径搜索的步骤包括：
将以所述旋转点云集中的一点为圆心，R为半径的球体相切的所述体素作为该点的候选体素并记录，完成所述旋转点云集中所有点的所述候选体素记录，其中，R为所述体素的分辨率；
遍历并检查各点的所述候选体素，若点到与该点对应的所述候选体素的重心距离大于R，则将该体素从该点的所述候选体素中剔除；
遍历所述旋转点云集中的所有点，若某一点有所述候选体素且该候选体素中含有5个以上目标点云中的点则将此点标记为有效点，否则将此点标记为非法点。
7.根据权利要求1所述的基于GPU的NDT点云配准算法，其特征在于，所述线搜索算法为More-Thuente线搜索。
8.根据权利要求1所述的基于GPU的NDT点云配准算法，其特征在于，所述根据JE和HE，计算每个有效点关于分数方程的梯度向量
Figure FDA0002930039990000041
和二阶导数Hessian矩阵H，并分别加和所有有效点的/>
Figure FDA0002930039990000042
和H，其中，所述分别加和所有有效点的/>
Figure FDA0002930039990000043
和H时采用GPU线束级归约操作。
9.一种基于GPU的NDT点云配准算法的配准装置，其特征在于，所述配准装置包括：
目标点云数据提取模块,所述目标点云数据提取模块用于提取目标点云数据，包括将目标点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存中并存储；将目标点云划分为若干个体素，每个体素对应有参数；及分别为每个参数分配GPU显存空间，计算每个体素对应的参数，将计算结果存储于各参数对应的显存空间；
源点云数据提取模块，所述源点云数据提取模块用于将源点云中点的XYZ轴坐标数据从CPU内存中拷入GPU显存；
高斯近似常量计算模块，所述高斯近似常量计算模块用于计算所述高斯近似常量；及
变换位姿求解模块，所述变换位姿求解模块用于实现点云最优变换位姿搜索过程，包括：
给定初始猜测位姿
Figure FDA0002930039990000051
根据/>
Figure FDA0002930039990000052
计算变换方程，并将变换方程内的参数存入GPU常量内存；
利用变换方程内的参数对源点云中的点进行变换，得到旋转点云集；
利用所述变换方程内的参数计算Jacobian矩阵JE和Hessian矩阵HE，并将计算结果分别拷入GPU常量内存；
判定旋转点云集中的点是否为有效点；
根据各体素中的参数和高斯近似常量得到分数方程；
根据JE和HE，计算每个有效点关于分数方程的梯度向量
Figure FDA0002930039990000053
和二阶导数Hessian矩阵H，并分别加和所有有效点的/>
Figure FDA0002930039990000061
和H，得到加和后的梯度向量/>
Figure FDA0002930039990000062
和二阶导数Hessian矩阵H'；
将得到的
Figure FDA0002930039990000063
和H'的结果从GPU拷入CPU，利用Jacobi方法实现奇异值分解并解方程：
Figure FDA0002930039990000064
其中，/>
Figure FDA0002930039990000065
为位姿增量，求出/>
Figure FDA0002930039990000066
的范数并对/>
Figure FDA0002930039990000067
归一化；
将
Figure FDA0002930039990000068
的范数和归一化结果代入线搜索算法，更新/>
Figure FDA0002930039990000069
并将该位姿增量与本次迭代的初始位姿相加得到新的位姿/>
Figure FDA00029300399900000610
及
重复变换位姿求解过程，直到
Figure FDA00029300399900000611
的范数固定不变或变换位姿求解过程的迭代次数等于预设最大迭代次数后停止迭代，得出最优变换位姿/>
Figure FDA00029300399900000612
10.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至8中任一项所述的基于GPU的NDT点云配准算法。
11.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8任一项所述的基于GPU的NDT点云配准算法。
CN202110145479.8A 2021-02-02 2021-02-02 一种基于gpu的ndt点云配准算法、装置及电子设备 Active CN112837354B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110145479.8A CN112837354B (zh) 2021-02-02 2021-02-02 一种基于gpu的ndt点云配准算法、装置及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110145479.8A CN112837354B (zh) 2021-02-02 2021-02-02 一种基于gpu的ndt点云配准算法、装置及电子设备

Publications (2)

Publication Number Publication Date
CN112837354A CN112837354A (zh) 2021-05-25
CN112837354B true CN112837354B (zh) 2023-06-16

Family

ID=75931763

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110145479.8A Active CN112837354B (zh) 2021-02-02 2021-02-02 一种基于gpu的ndt点云配准算法、装置及电子设备

Country Status (1)

Country Link
CN (1) CN112837354B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113536232B (zh) * 2021-06-28 2023-03-21 上海科技大学 用于无人驾驶中激光点云定位的正态分布变换方法
CN113689476A (zh) * 2021-08-26 2021-11-23 中国科学院植物研究所 一种点云配准方法、装置、存储介质和计算机设备
CN114549605B (zh) * 2021-12-31 2023-08-04 广州景骐科技有限公司 基于点云匹配的图像优化方法、装置、设备及存储介质
CN117689698B (zh) * 2024-02-04 2024-04-19 安徽蔚来智驾科技有限公司 点云配准方法、智能设备及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107204009A (zh) * 2017-05-23 2017-09-26 哈尔滨工业大学 基于仿射变换模型cpd算法的三维点云配准方法
CN108564605A (zh) * 2018-04-09 2018-09-21 大连理工大学 一种三维测量点云优化配准方法
CN110097582A (zh) * 2019-05-16 2019-08-06 广西师范大学 一种点云优化配准与实时显示系统及工作方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8401242B2 (en) * 2011-01-31 2013-03-19 Microsoft Corporation Real-time camera tracking using depth maps

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107204009A (zh) * 2017-05-23 2017-09-26 哈尔滨工业大学 基于仿射变换模型cpd算法的三维点云配准方法
CN108564605A (zh) * 2018-04-09 2018-09-21 大连理工大学 一种三维测量点云优化配准方法
CN110097582A (zh) * 2019-05-16 2019-08-06 广西师范大学 一种点云优化配准与实时显示系统及工作方法

Also Published As

Publication number Publication date
CN112837354A (zh) 2021-05-25

Similar Documents

Publication Publication Date Title
CN112837354B (zh) 一种基于gpu的ndt点云配准算法、装置及电子设备
US10269147B2 (en) Real-time camera position estimation with drift mitigation in incremental structure from motion
US10025879B2 (en) Tree data structures based on a plurality of local coordinate systems
US10269148B2 (en) Real-time image undistortion for incremental 3D reconstruction
US10826786B2 (en) Fast multi-scale point cloud registration with a hierarchical gaussian mixture
KR102068419B1 (ko) 포인트 클라우드 데이터 수집 궤적을 조정하기 위한 방법, 장치 및 컴퓨터 판독 가능한 매체
CN112066879A (zh) 基于计算机视觉的气浮运动模拟器位姿测量装置及方法
CN114936971A (zh) 一种面向水域的无人机遥感多光谱图像拼接方法及系统
Tran et al. On-device scalable image-based localization via prioritized cascade search and fast one-many ransac
Liu et al. Parallel processing of massive remote sensing images in a GPU architecture
CN111325663B (zh) 基于并行架构的三维点云匹配方法、装置和计算机设备
CN112508767B (zh) 一种基于gpu的gmm点云配准方法
CN113177974A (zh) 一种点云配准方法、装置、电子设备及存储介质
CN114187589A (zh) 一种目标检测方法、装置、设备和存储介质
CN116067367A (zh) 加速同时定位与地图构建的装置和包括该装置的电子设备
CN111598941A (zh) 一种杆塔倾斜度测量方法、装置、设备及存储介质
CN117372483A (zh) 一种点云数据处理方法、装置、设备及介质
Fickenscher et al. Cell-based update algorithm for occupancy grid maps and hybrid map for ADAS on embedded GPUs
JP6761388B2 (ja) 推定装置及びプログラム
Li-Juan et al. A new method for pose estimation from line correspondences
Bußler et al. Interactive Particle Tracing in Time-Varying Tetrahedral Grids.
CN115239899B (zh) 位姿图生成方法、高精地图生成方法和装置
Carabaño et al. Efficient implementation of a fast viewshed algorithm on SIMD architectures
JP5984142B2 (ja) 解析装置、解析方法、及び、プログラム
CN114943766A (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