很多Jetson用戶在自動(dòng)化解決方案中選擇激光雷達(dá)進(jìn)行定位和感知。激光雷達(dá)使用3D點(diǎn)云描繪周圍的空間環(huán)境。點(diǎn)云可以高精度長(zhǎng)距離采樣物體表面信息以便于上層應(yīng)用的障礙感知、繪圖、定位和路徑規(guī)劃算法。

使用CUDA-PCL處理點(diǎn)云

CUDA PCL 1.0是基于CUDA開發(fā)的點(diǎn)云處理庫(kù)，在本文中，我們將介紹目前所有的三個(gè)庫(kù)：ICP，segmentation 和 filter。

CUDA-ICP

迭代最近點(diǎn)算法（Iterative Closest Point，ICP） 用于計(jì)算兩幀點(diǎn)云數(shù)據(jù)之間的坐標(biāo)變換矩陣，從而能夠使不同的坐標(biāo)下的點(diǎn)云數(shù)據(jù)合并到同一個(gè)坐標(biāo)系統(tǒng)中。ICP算法 通過計(jì)算兩幀點(diǎn)云的距離誤差從而修正變換矩陣（平移和旋轉(zhuǎn)）以便最小化距離誤差，通常兩幀點(diǎn)云之間的距離誤差是通過匹配點(diǎn)的距離計(jì)算得來。ICP算法應(yīng)用廣泛，能夠獲得很高的匹配結(jié)果且有很高的魯棒性，同時(shí)會(huì)耗費(fèi)大量的計(jì)算資源。為了改進(jìn)ICP算法在Jetson上的性能，我們推薦使用基于CUDA加速的CUDA-ICP。

使用CUDA-ICP

以下是CUDA ICP的使用實(shí)例

我們僅僅需要初始化相關(guān)的類對(duì)象，并調(diào)用接口函數(shù)即可。

    cudaICP icpTest(nPCountM, nQCountM, stream);
    icpTest.icp(cloud_source, nPCount,
            float *cloud_target, int nQCount,
            int Maxiterate, double threshold,
            Eigen::Matrix4f &transformation_matrix, stream);

CUDA-ICP 計(jì)算的輸出是 transformation_matrix，代表的含義如下：

源點(diǎn)云（P）* transformation = 目標(biāo)坐標(biāo)系的點(diǎn)云（Q）

因?yàn)榧す忸愋偷妮敵鳇c(diǎn)云的數(shù)量為固定值，所以CUDA-ICP在輸出化的時(shí)候，要求輸入兩幀點(diǎn)云的最大數(shù)量，從而分配計(jì)算資源。

class cudaICP
{
public:
    /*
       nPCountM and nQCountM are the maximum of count for input clouds
       They are used to pre-allocate memory.
    */
    cudaICP(int nPCountM, int nQCountM, cudaStream_t stream = 0);
    ~cudaICP(void);

    /*
    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
        Maxiterate: the threshold for iterations
        threshold: When the Epsilon of transformation_matrix is less than
            threshold, the function will return transformation_matrix.
    Output:
        transformation_matrix
    */

    void icp(float *cloud_source, int nPCount,

Figure 1. 執(zhí)行ICP之前的兩幀點(diǎn)云。

Figure 2. 經(jīng)過ICP匹配后的兩幀點(diǎn)云。

CUDA-Segmentation

點(diǎn)云地圖包含大量的地面點(diǎn)，不僅會(huì)使得地圖變的雜亂，也會(huì)干擾之后的點(diǎn)云的分類和識(shí)別。因此在實(shí)際處理中，我們會(huì)首先使用點(diǎn)云分割移除點(diǎn)云中的地面。CUDA-Segmentation 使用隨機(jī)樣本一致性算法（random sample consensus， Ransac）進(jìn)行點(diǎn)云的分割。

使用CUDA- Segmentation

以下代碼是CUDA Segmentation的使用實(shí)例。

我們直接初始化對(duì)象并調(diào)用相關(guān)的接口函數(shù)即可。

  //Now Just support: SAC_RANSAC + SACMODEL_PLANE
  std::vector indexV;
  cudaSegmentation cudaSeg(SACMODEL_PLANE, SAC_RANSAC, stream);
  segParam_t setP;
  setP.distanceThreshold = 0.01; 
  setP.maxIterations = 50;
  setP.probability = 0.99;
  setP.optimizeCoefficients = true;
  cudaSeg.set(setP);
  cudaSeg.segment(input, nCount, index, modelCoefficients);
  for(int i = 0; i < nCount; i++)
  {
    if(index[i] == 1) 
    indexV.push_back(i);
  }

CUDA Segmentation分割擁有nCount個(gè)的點(diǎn)云，并輸出索引表index用于指示輸入點(diǎn)云中的被分割掉的點(diǎn)和modelCoefficients用于指示尋找的平面公式。

typedef struct {
  double distanceThreshold; 
  int maxIterations;
  double probability;
  bool optimizeCoefficients;
} segParam_t;

class cudaSegmentation
{
public:
    //Now Just support: SAC_RANSAC + SACMODEL_PLANE
    cudaSegmentation(int ModelType, int MethodType, cudaStream_t stream = 0);

    ~cudaSegmentation(void);

    /*
    Input:
        cloud_in: data pointer for points cloud
        nCount: count of points in cloud_in
    Output:
        Index: data pointer which has the index of points in a plane from input
      modelCoefficients: data pointer which has the group of coefficients of the plane
    */
    int set(segParam_t param);
    void segment(float *cloud_in, int nCount,
            int *index, float *modelCoefficients);
private:
    void *m_handle = NULL;
}; 

讓我們查看下面的實(shí)例，第一張圖是原始的點(diǎn)云，第二張圖是經(jīng)過算法尋找到的平面。這是一個(gè)非常典型的平面移除的應(yīng)用。

Figure 3. cuda-segmentaion處理之前的點(diǎn)云。

Figure 4. cuda-segmentaion找到的點(diǎn)云平面。

CUDA-Filter

濾波器是在分割，檢測(cè)和識(shí)別之前，點(diǎn)云數(shù)據(jù)中最重要的處理。

帶通點(diǎn)云濾波是最簡(jiǎn)單的方法，僅僅是過濾X，Y和Z軸方向的點(diǎn)云。

目前CUDA-Filter僅支持帶通操作，更多的濾波器會(huì)在后續(xù)加入。

使用CUDA- Filter

這個(gè)是CUDA Filter的使用實(shí)例。

我們僅僅需要初始化對(duì)象并調(diào)用相關(guān)的接口函數(shù)即可。

我們僅僅需要初始化對(duì)象并調(diào)用相關(guān)的接口函數(shù)即可。

  cudaFilter filterTest(stream);
  FilterParam_t setP;
  FilterType_t type = PASSTHROUGH;
  setP.type = type;
  setP.dim = 2;
  setP.upFilterLimits = 1.0;
  setP.downFilterLimits = 0.0;
  setP.limitsNegative = false;
  filterTest.set(setP);
  filterTest.filter(output, &countLeft, input, nCount);

CUDA-Filter使用指定的參數(shù)過濾nCount有個(gè)點(diǎn)的點(diǎn)云數(shù)據(jù)，過濾后輸出的點(diǎn)云數(shù)量為countLeft。


typedef struct {
    FilterType_t type;
    //0=x,1=y,2=z
    int dim;
    float upFilterLimits;
    float downFilterLimits;
    bool limitsNegative;

} FilterParam_t;

class cudaFilter
{
public:
    cudaFilter(cudaStream_t stream = 0);
    ~cudaFilter(void);
    int set(FilterParam_t param);
    /*
    Input:
        source: data pointer for points cloud
        nCount: count of points in cloud_in
    Output:
        output: data pointer which has points filtered by CUDA
        countLeft: count of points in output
    */
    int filter(void *output, unsigned int *countLeft, void *source, unsigned int nCount);

    void *m_handle = NULL;
}; 

讓我們看下X軸方向的帶通濾波實(shí)例。

Figure 5. 原始點(diǎn)云。

Figure 6. X周過濾后的數(shù)據(jù)。

開始使用 CUDA-PCL

我們希望通過本文介紹使用CUDA-PCL從而獲得更好的點(diǎn)云處理性能。

因?yàn)镻CL在Jetson上無法使用CUDA進(jìn)行點(diǎn)云的加速處理，所以我們開發(fā)了基于CUDA的點(diǎn)云處理庫(kù)CUDA-PCL。

關(guān)于作者

范磊是英偉達(dá)高級(jí)CUDA軟件工程師，在TSE China 小組致力于CUDA軟件方案的開發(fā)和優(yōu)化。

李雨倩負(fù)責(zé)基于Jetson的自主機(jī)器解決方案和生態(tài)發(fā)展建設(shè)，讓開發(fā)者在Jetson上開發(fā)機(jī)器人應(yīng)用獲得更好更全面的體驗(yàn)和支持。

審核編輯：郭婷