Technical Walkthrough

基于 CUDA 加速的自主机器 SLAM 技术 – CUDA NDT

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

定位模块是自主机器最核心的模块之一,定位又包括全局定位和局部定位,对于自主机器,其精度需要达到厘米级别。本文我们将讨论全局定位,即确定自主机器在全局下的位置。传统的低速自主机器,类似于AMR等,其采用的定位方式通常以SLAM(simultaneous localization and mapping)的方法进行同时建图和定位,但是该方法实现代价高,难度大,并不适用于室外自主机器,类似于无人物流车,园区接驳车等的实时高精度定位需求。这些室外自主机器行驶速度快,距离远,环境复杂,使得SLAM的精度下降,同时远距离的行驶将导致实时构建的地图偏移过大。因此,如果在已有高精度的全局地图地图的情况下进行自主机器的定位,将极大的简化该问题。

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

CUDA-NDT

正态分布变换算法(Normal Distributions 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;
};

经过CUDA加速的NDT速度对比微加速版本提升了4倍左右,请参考以下Table2性能对比,经过NDT匹配的点云效果对比请参考Figure1和2。

Table 2. CUDA-NDT 与PCL-NDT的性能对比
Figure 2. 经过NDT匹配后的两帧点云。

开始使用 CUDA-NDT

我们希望通过本文介绍使用CUDA-NDT从而获得更好的点云注册性能。
因为NDT在Jetson上无法使用CUDA进行点云的加速处理,所以我们开发了基于CUDA的CUDA-NDT。
可以从这里获得相关的库和实例代码。