CUDA-NDT的定义与使用实例-电子发烧友网

随着自主机器的发展，我们可以在生活中经常看到自主机器的应用。有传统应用的仓库工厂 AMR、机械臂、银行酒店里面的服务机器人、家庭机器人、无人物流车、自主矿卡等等。不同的自主机器，软件架构的方案也不一样，但核心的模块定位、导航、感知、控制等都是相通的。

定位模块是自主机器最核心的模块之一，定位又包括全局定位和局部定位，对于自主机器，其精度需要达到厘米级别。

本文我们将讨论全局定位，即确定自主机器在全局下的位置。传统的低速自主机器，类似于 AMR 等，其采用的定位方式通常以 SLAM（Simultaneous Localization and Mapping）的方法进行同时建图和定位，但是该方法实现代价高、难度大，并不适用于室外自主机器——类似于无人物流车、园区接驳车等的实时高精度定位需求。这些室外自主机器行驶速度快、距离远、环境复杂，使得 SLAM 的精度下降，同时远距离的行驶将导致实时构建的地图偏移过大。因此，如果在已有高精度的全局地图的情况下进行自主机器的定位，将极大的简化该问题。

因此，将问题分为独立的两部分：建图 Mapping 和定位 Matching。NDT 是一种点云配准算法，可同时用于点云的建图和定位。

CUDA-NDT

正态分布变换算法（NormalDistributions Transform, NDT）同 ICP 算法的功能一致，即，用于计算两帧点云数据之间的坐标变换矩阵，从而能够使不同的坐标下的点云数据合并到同一个坐标系统中。不同的是 NDT 算法对初值不敏感，且不需要进行对应点的特征计算，所以速度较快。NDT 算法使用应用于 3D 点统计模型的标准优化技术来确定两个点云之间最可能的配准。

NDT 算法和 ICP 算法可以结合使用，以提高配准精度和速度。首先，NDT 算法可用于粗配准，得到转换参数；然后使用 ICP 算法结合参数进行精细配准。为了改进 NDT 算法在 NVIDIA Jetson 上的性能，我们推荐使用基于 CUDA 加速的 CUDA-NDT。

使用CUDA-NDT

以下是 CUDA-NDT 的使用实例。我们需要初始化相关的类对象，设置相关的参数，并调用接口函数。

cudaNDT ndtTest(nPCountM, nQCountM, stream);ndtTest.setInputSource(source);ndtTest.setInputTarget(target);ndtTest.setResolution(resolution);ndtTest.setMaximumIterations(nr_iterations);ndtTest.setTransformationEpsilon(epsilon);ndtTest.setStepSize(step_size);    ndtTest.ndt(cloud_source, nPCount,               cloud_target, nQCount, guess,transformation_matrix,stream);

CUDA-NDT 计算的输出是 transformation_matrix，代表的含义如下：

源点云 (P)* transformation_matrix = 目标坐标系的点云 (Q)

因为激光类型的输出点云的数量为固定值，所以 CUDA-NDT 在输出化的时候，要求输入两帧点云的最大数量，从而分配计算资源。

class cudaNDT{public:    /*       nPCountM and nQCountM are the maximum of count for input clouds       They are used to pre-allocate memory.    */    cudaNDT(int nPCountM, int nQCountM, cudaStream_t stream = 0);    ~cudaNDT(void);void setInputSource (void *source);void setInpuTarget (void *target);void setResolution (float resolution);void setMaximumIterations (int nr_iterations);void setTransformationEpsilon (double epsilon);void setStepSize (double step_size);    /*    cloud_target = transformation_matrix * cloud_source    When the Epsilon of transformation_matrix is less than threshold,    the function will return transformation_matrix.    Input:        cloud_source, cloud_target: data pointer for points cloud        nPCount: the points number of cloud_source        nQCount: the points number of cloud_target        guess: initial guess of transformation_matrix        stream: CUDA stream    Output:        transformation_matrix: rigid transformation matrix    */
    void ndt(float *cloud_source, int nPCount,            float *cloud_target, int nQCount,            float *guess, void *transformation_matrix,            cudaStream_t stream = 0);    void *m_handle = NULL;};