一种ICP并行加速计算方法、系统、终端及介质技术方案

本申请涉及一种ICP并行加速计算方法、系统、终端及介质，其方法包括获取点云数据；依据点云数据及所需执行的计算操作确定所需线程数，所需执行的计算操作包括点云质心计算、点云去质心化、构建矩阵、SVD分解及旋转位移计算中的至少一种；依据所需线程数向GPU申请线程；依据所需执行的计算操作分配每个线程持有的点云数据；依据所需执行的计算操作确定每个线程对所持有的点云数据的计算步骤；获取最终的计算结果，并将计算结果反馈至CPU。本申请具有降低ICP计算时间成本，适配嵌入式设备，提升计算结果的实时性的效果。算结果的实时性的效果。算结果的实时性的效果。

【技术实现步骤摘要】
一种ICP并行加速计算方法、系统、终端及介质


[0001]本申请涉及ICP加速算法的领域，尤其是涉及一种ICP并行加速计算方法、系统、终端及介质。

技术介绍

[0002]目前，ICP(Iterative Closest Point迭代最近点)算法广泛应用于视觉SLAM(Simultaneous Localization and Mapping即时定位与地图构建)的RGBD方案和激光SLAM中，传感器采集3D点云数据，通过计算3D
‑
3D点的相对位姿，为进一步进行滤波或者非线性优化处理提供初值。RGBD方案中，点云可以通过光流或者描述子得到一个精确的点对匹配，激光方案中，通过多次迭代，逐渐逼近真实的3D点对，从而计算出相对位姿。
[0003]ICP算法主要涉及到点云质心计算、点云去质心化、构建矩阵、SVD分解及旋转位移计算等步骤，每个步骤均需要涉及到计算操作。
[0004]针对上述中的相关技术，专利技术人认为采集3D点云的传感器包括RGBD相机和多线激光雷达，分辨率都很高，另外如果计算结果有高精度的要求也必须要求使用高分辨率的传感器，导致所获取的点云数量非常大，随着点云数量的增多，ICP计算的时间开销也会剧增，且很多的SLAM方案需要部署在算力并不高的嵌入式设备中，导致计算结果的实时性无法得到保证，设备无法正常运行。

技术实现思路

[0005]第一方面，为了降低ICP计算时间成本，适配嵌入式设备，提升计算结果的实时性，本申请提供一种ICP并行加速计算方法。
[0006]本申请提供的一种ICP并行加速计算方法，采用如下的技术方案：
[0007]一种ICP并行加速计算方法，包括：
[0008]获取点云数据；
[0009]依据点云数据及所需执行的计算操作确定所需线程数，所需执行的计算操作包括点云质心计算、点云去质心化、构建矩阵、SVD分解及旋转位移计算中的至少一种；
[0010]依据所需线程数向GPU申请线程；
[0011]依据所需执行的计算操作分配每个线程持有的点云数据；
[0012]依据所需执行的计算操作确定每个线程对所持有的点云数据的计算步骤；
[0013]获取最终的计算结果，并将计算结果反馈至CPU。
[0014]通过采用上述技术方案，ICP算法中，包括点云质心计算、点云去质心化、构建矩阵、SVD分解及旋转位移计算的过程中都有大量的点云加法、向量乘法的操作，单纯采用CPU计算会导致算力紧张，速度减慢；而GPU相比CPU每个线程的计算能力虽然稍弱，但确可以提供的大量线程，ICP的计算可以利用此特性，将ICP的计算过程分解为众多的对单个线程计算能力要求不高的加法、乘法操作，对此类操作可以使用GPU的线程同时并行计算，从而达到ICP计算加速的目的，降低ICP计算时间成本，适配嵌入式设备，提升计算结果的实时性。
[0015]优选的，若所需执行的计算操作为点云质心计算，则在所述获取点云数据后，还包括：
[0016]向GPU申请与点云的点数量一致的线程个数；
[0017]对每个线程分配对应其线程号及下一线程号的点并执行相加操作；
[0018]执行累加步骤：对相邻两个线程的和值进行相加操作，得到新的和值；
[0019]重复执行累加步骤，直至只有一个线程持有和值，对其数据计算均值，得到点云的质心数据。
[0020]通过采用上述技术方案，每个线程执行加法操作，不断累加，所需线程数较多，但算法简单，分担了CPU的计算工作量，同时对于GPU的算力负荷较小。
[0021]优选的，若所需执行的计算操作为点云去质心化，则在所述得到点云的质心数据后，还包括：
[0022]向GPU申请与点云的点数量一致的线程个数；
[0023]对每个线程分配点云数据与对应的质心数据；
[0024]每个线程执行去质心的相减操作；
[0025]生成去质心后的点云数据，并将每个线程的所得差值反馈至CPU。
[0026]通过采用上述技术方案，通过申请GPU的多线程参与计算点云去质心时的相减操作，从而分担了CPU的计算工作量，降低了ICP计算时间成本，提升了计算结果的实时性。
[0027]优选的，若所需执行的计算操作为构建矩阵，则在所述生成去质心后的点云的步骤后，还包括：
[0028]获取去质心后的点云数据；
[0029]向GPU申请与两组点云数据的点对数量一致的线程数；
[0030]对每个线程分配两组去质心后的点云数据的各一个点；
[0031]每个线程执行两个点的相乘操作；
[0032]执行和值运算步骤：对相邻两个线程的乘积进行相加操作，得到和值；
[0033]重复执行和值运算步骤，直至只有一个线程持有和值，得到矩阵H，并将矩阵H反馈至CPU。
[0034]通过采用上述技术方案，通过申请GPU的多线程的方式参与点云数据的相乘与相加操作，获得矩阵H，方便计算位移t与旋转R，分担了CPU的计算工作量，降低了ICP计算时间成本，提升了计算结果的实时性。
[0035]优选的，若所需执行的计算操作为SVD分解，则在所述得到矩阵H的步骤后，还包括：
[0036]获取矩阵H；
[0037]对矩阵H进行SVD分解：
[0038]向GPU申请n3个线程，其中n为矩阵H的矩阵行数、列数；
[0039]给线程分配矩阵H的数与其转置矩阵的数，线程号分别对应H矩阵的行、列编号；
[0040]每个线程执行矩阵相乘中两个数的相乘操作，再执行和值运算步骤，最终得到矩阵U、V。
[0041]通过采用上述技术方案，SVD分解的目的是将H矩阵分解为H＝U∑V
T
的形式，设H为一个m
×
n大小的矩阵，则U为m
×
m正交矩阵，∑为m
×
n对角矩阵，V为n
×
n正交矩阵，其中V＝
H
T
H，U＝HH
T
，而当H满秩时，旋转R＝VU
T
，而根据t＝μ
q
‑
Rμ
p
即可计算出位移t，其中矩阵相乘中的乘法和加法计算可由GPU的多线程并行完成，提升ICP算法的计算效率。
[0042]优选的，若所需执行的计算操作为旋转位移计算，则在得到矩阵U、V后，还包括：
[0043]获取矩阵U、V；
[0044]代入计算公式R＝VU
T
；
[0045]向GPU申请线程，执行矩阵相乘操作；
[0046]计算旋转R及位移t。
[0047]通过采用上述技术方案，旋转R＝VU
T
，而根据t＝μ
q
‑
Rμ
p
，μ
p
、μ
q
分别为两组点云数据的质心数据，即可计算出位移t，其中矩阵相乘中的乘法和本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种ICP并行加速计算方法，其特征在于，包括：获取点云数据；依据点云数据及所需执行的计算操作确定所需线程数，所需执行的计算操作包括点云质心计算、点云去质心化、构建矩阵、SVD分解及旋转位移计算中的至少一种；依据所需线程数向GPU申请线程；依据所需执行的计算操作分配每个线程持有的点云数据；依据所需执行的计算操作确定每个线程对所持有的点云数据的计算步骤；获取最终的计算结果，并将计算结果反馈至CPU。2.根据权利要求1所述的ICP并行加速计算方法，其特征在于，若所需执行的计算操作为点云质心计算，则在所述获取点云数据后，还包括：向GPU申请与点云的点数量一致的线程个数；对每个线程分配对应其线程号及下一线程号的点并执行相加操作；执行累加步骤：对相邻两个线程的和值进行相加操作，得到新的和值；重复执行累加步骤，直至只有一个线程持有和值，对其数据计算均值，得到点云的质心数据。3.根据权利要求2所述的ICP并行加速计算方法，其特征在于，若所需执行的计算操作为点云去质心化，则在所述得到点云的质心数据后，还包括：向GPU申请与点云的点数量一致的线程个数；对每个线程分配点云数据与对应的质心数据；每个线程执行去质心的相减操作；生成去质心后的点云数据，并将每个线程的所得差值反馈至CPU。4.根据权利要求3所述的ICP并行加速计算方法，其特征在于，若所需执行的计算操作为构建矩阵，则在所述生成去质心后的点云的步骤后，还包括：获取去质心后的点云数据；向GPU申请与两组点云数据的点对数量一致的线程数；对每个线程分配两组去质心后的点云数据的各一个点；每个线程执行两个点的相乘操作；执行和值运算步骤：对相邻两个线程的乘积进行相加操作，得到和值；重复执行和值运算步骤，直至只有一个线程持有和值，得到矩阵H，并将矩阵H反馈至CPU。5.根据权利要求4所述的ICP并行加速计算方法，其特征在于，若所需执行的计算操作为SVD分解，则在所述得到矩阵H的步骤后，还包括：获取矩阵H；对矩阵H进行SVD分解：向GPU申请...

【专利技术属性】
技术研发人员：焦江磊，何贝，刘鹤云，张岩，
申请(专利权)人：北京斯年智驾科技有限公司，
类型：发明
国别省市：