Lidar obstacle detection and tracking: CUDA European clustering

Look directly at the code , The notes are very detailed

gpu_euclidean_cluster.cu

#include "gpu_euclidean_cluster.h"

GpuEuclideanCluster::GpuEuclideanCluster()
{
    
  x_ = NULL;
  y_ = NULL;
  z_ = NULL;

  size_ = 0;
  threshold_ = 0;
  cluster_indices_ = NULL;
  cluster_indices_host_ = NULL;
  min_cluster_pts_ = 0;
  max_cluster_pts_ = 1000000000;
  cluster_num_ = 0;
}

GpuEuclideanCluster::~GpuEuclideanCluster()
{
    
  CHECK(cudaFree(x_));
  CHECK(cudaFree(y_));
  CHECK(cudaFree(z_));
  CHECK(cudaFree(cluster_indices_));
  free(cluster_indices_host_);
}

void GpuEuclideanCluster::setInputPoints(float *x, float *y, float *z, int size)
{
    
  size_ = size;
  CHECK(cudaMalloc(&x_, size_ * sizeof(float)));
  CHECK(cudaMalloc(&y_, size_ * sizeof(float)));
  CHECK(cudaMalloc(&z_, size_ * sizeof(float)));

  CHECK(cudaMemcpy(x_, x, size_ * sizeof(float), cudaMemcpyHostToDevice));
  CHECK(cudaMemcpy(y_, y, size_ * sizeof(float), cudaMemcpyHostToDevice));
  CHECK(cudaMemcpy(z_, z, size_ * sizeof(float), cudaMemcpyHostToDevice));

  CHECK(cudaMalloc(&cluster_indices_, size_ * sizeof(int)));
  cluster_indices_host_ = (int *)malloc(size_ * sizeof(int));
}

void GpuEuclideanCluster::setThreshold(double threshold)
{
    
  threshold_ = threshold;
}

void GpuEuclideanCluster::setMinClusterPts(int min_cluster_pts)
{
    
  min_cluster_pts_ = min_cluster_pts;
}

void GpuEuclideanCluster::setMaxClusterPts(int max_cluster_pts)
{
    
  max_cluster_pts_ = max_cluster_pts;
}

__global__ void pclEuclideanInitialize(int *cluster_indices, int size)
{
    
  for (int index = threadIdx.x + blockIdx.x * blockDim.x; index < size; index += blockDim.x * gridDim.x)
  {
    
    cluster_indices[index] = index;
  }
}

//  Process by thread block , A point corresponds to a thread index , spot A Index of is greater than B The index of , If you order A With the point B The distance between is less than the threshold , The point of A Belong to B cluster , Record the target point cloud cluster to which each point belongs ( Thread index )
__global__ void blockLabelling(float *x, float *y, float *z, int *cluster_indices, int size, float threshold)
{
    
  int block_start = blockIdx.x * blockDim.x;
  int block_end = (block_start + blockDim.x <= size) ? (block_start + blockDim.x) : size;
  //  Thread index 
  int row = threadIdx.x + block_start;
  __shared__ int local_offset[BLOCK_SIZE_X];
  __shared__ float local_x[BLOCK_SIZE_X];
  __shared__ float local_y[BLOCK_SIZE_X];
  __shared__ float local_z[BLOCK_SIZE_X];
  __shared__ int local_cluster_changed[BLOCK_SIZE_X];

  if (row < block_end)
  {
    
    local_offset[threadIdx.x] = threadIdx.x;
    //  Set the point cloud coordinates xyz Stored in static shared memory 
    local_x[threadIdx.x] = x[row];
    local_y[threadIdx.y] = y[row];
    local_z[threadIdx.z] = z[row];
    //  Wait for all threads of the same thread block to complete the above steps , The coordinate information of all points is stored in shared memory 
    __syncthreads();
    //  Line segment processing 
    for (int column = block_start; column < block_end; column++)
    {
    
      //  The index for threadIdx.x The point cloud coordinates of are the same as those of other threads in the thread block x,y,z distance 
      float tmp_x = local_x[threadIdx.x] - local_x[column - block_start];
      float tmp_y = local_x[threadIdx.y] - local_y[column - block_start];
      float tmp_z = local_x[threadIdx.z] - local_z[column - block_start];
      // column_offset Similarities B
      int column_offset = local_offset[column - block_start];
      // row_offset Similarities A
      int row_offset = local_offset[threadIdx.x];
      //  Mark 
      local_cluster_changed[threadIdx.x] = 0;
      //  If A and B Distance between ( square root ) Less than threshold ,local_cluster_changed Marked as 1
      // A Corresponding thread index id  Greater than  B The corresponding thread index id
      if (row > column && column_offset != row_offset && norm3df(tmp_x, tmp_y, tmp_z) < threshold)
      {
    
        //  The test did not reach , Maybe the point cloud in a block does not belong to a point cloud cluster 
        local_cluster_changed[row_offset] = 1;
      }
      __syncthreads();
      //  If a thread's local_cluster_changed by 1, Then the cluster it is dealing with becomes B Cluster of , otherwise A The original cluster of remains unchanged 
      local_offset[threadIdx.x] = (local_cluster_changed[row_offset] == 1) ? column_offset : row_offset;
      __syncthreads();
    }
    __syncthreads();
    int new_cluster = cluster_indices[block_start + local_offset[threadIdx.x]];

    __syncthreads();
    // row Index threads , Represents a point cloud , Value is another thread index , Point to the point cloud cluster to which it belongs 
    cluster_indices[row] = new_cluster;
  }
}


//  scanning cluster_indices Array , Set the current point cluster Corresponding cluster_mark Element is marked as 1, If it doesn't exist cluster, So it's cluster_mark by 0
__global__ void clusterMark(int *cluster_list, int *cluster_mark, int size)
{
    
  for (int i = threadIdx.x + blockIdx.x * blockDim.x; i < size; i += blockDim.x * gridDim.x)
  {
    
    cluster_mark[cluster_list[i]] = 1;
  }
}

void GpuEuclideanCluster::exclusiveScan(int *input, int ele_num, int *sum)
{
    
  thrust::device_ptr<int> dev_ptr(input);
  // exclusive Does not include the currently entered value 
  thrust::exclusive_scan(dev_ptr, dev_ptr + ele_num, dev_ptr);
  CHECK(cudaDeviceSynchronize());

  *sum = *(dev_ptr + ele_num - 1);
}

__global__ void clusterCollector(int *old_cluster_list, int *new_cluster_list, int *cluster_location, int size)
/* old_cluster_list ：  The index is the thread index id, The value is another thread index in the thread block id, Indication point A Belongs to point cloud cluster B cluster_location ：  The index is the thread index corresponding to the point cloud cluster id, Value indicates the number of point cloud clusters  size ：  The number of point clouds  new_cluster_list :  Index is the subscript of point cloud cluster , The value is the point cloud cluster to which it belongs （ Thread index id) */
{
    
  for (int i = threadIdx.x + blockIdx.x * blockDim.x; i < size; i += blockDim.x * gridDim.x)
    // old_cluster_list[i]  Index the thread to which it belongs 
    // cluster_location[old_cluster_list[i]]  It means that old_cluster_list[i] Number of previous point cloud clusters , Each point cloud cluster is assigned a subscript , Which is equivalent to old_cluster_list[i] Subscript index of the point cloud cluster to which it belongs 
    // new_cluster_list： Index is the subscript of point cloud cluster , The value is the thread index i The point cloud cluster to which the corresponding point belongs old_cluster_list[i]
    new_cluster_list[cluster_location[old_cluster_list[i]]] = old_cluster_list[i];
}

/*  The clustering matrix records the relationship between each pair of clusters , Use only the lower half of the matrix , If a pair of clusters x and y Connect , Then the matrix element [x][y] by 1  To build this matrix , Every GPU The thread processes a point , Go through all the points , And care about it A and B Distance between . hypothesis A Belong to cluster A,B Belong to cluster B, If A and B The distance is less than the threshold , Then the matrix elements [x][y] by 1 */
__global__ void buildClusterMatrix(float *x, float *y, float *z, int *cluster_indices, int *cluster_matrix, int *cluster_offset,
                                   int size, int cluster_num, float threshold)
/* cluster_indices ：  The index is the thread index id, The value is another thread index in the thread block id, Indication point A Belongs to point cloud cluster B cluster_offset ：  The index is the thread index corresponding to the point cloud cluster id, Value indicates the number of point cloud clusters  size ：  The number of point clouds  cluster_num ：  Number of point cloud clusters  threshold ：  Clustering radius threshold  cluster_matrix ：  Required clustering matrix , Use only the lower half of the matrix , If a pair of clusters x and y Connect , Then the matrix element [x][y] by 1 */
{
    
  //  Thread index 
  int index = threadIdx.x + blockIdx.x * blockDim.x;
  int stride = blockDim.x * gridDim.x;
  __shared__ float local_x[BLOCK_SIZE_X];
  __shared__ float local_y[BLOCK_SIZE_X];
  __shared__ float local_z[BLOCK_SIZE_X];

  if (index > size)
    return;

  for (int column = index; column < size; column += stride)
  {
    
    //  Shared memory stores point cloud coordinates xyz
    local_x[threadIdx.x] = x[column];
    local_y[threadIdx.x] = y[column];
    local_z[threadIdx.x] = z[column];
    //  amount to y
    int column_cluster = cluster_indices[column]; //  The point cloud cluster to which it belongs , Index threads 
    int cc_offset = cluster_offset[column_cluster];

    __syncthreads();
    //  The second part of 
    for (int row = 0; row < column; row++)
    {
    
      float tmp_x = x[row] - local_x[threadIdx.x];
      float tmp_y = y[row] - local_y[threadIdx.x];
      float tmp_z = z[row] - local_z[threadIdx.x];
      //  amount to x
      int row_cluster = cluster_indices[row]; //  The point cloud cluster to which it belongs , Index threads 
      int rc_offset = cluster_offset[row_cluster];

      if (row_cluster != column_cluster && norm3df(tmp_x, tmp_y, tmp_z) < threshold)
        cluster_matrix[rc_offset * cluster_num + cc_offset] = 1;
    }
    __syncthreads();
  }
}

/*  The merging process is done in blocks , The input list of clusters is divided into smaller blocks ,, Each thread in the block processes one row of the matrix and traverses all columns of the matrix .  Each iteration requires synchronization , To ensure that all threads complete merging the clusters in the current column before moving to the next column .  In each iteration , Each thread checks whether the cluster corresponding to the current row is connected to the cluster corresponding to the current column .  If so , Then change the cluster of rows （ Merge ） Is a cluster of columns   Divide thread blocks according to the number of point cloud clusters , Merge point clouds by thread block , to update  */
__global__ void mergeClusters(int *cluster_matrix, int *cluster_list, int cluster_num)
/* cluster_matrix ：  Clustering matrix , Use only the lower half of the matrix , If a pair of clusters x and y Connect , Then the matrix element [x][y]  by 1 cluster_list ：  Index is the subscript of point cloud cluster ( Several point cloud clusters ), The value is the point cloud cluster to which it belongs （ Thread index id) cluster_num ：  Number of point cloud clusters  */
{
    
  int row_start = blockIdx.x * blockDim.x;
  int row_end = (row_start + blockDim.x <= cluster_num) ? row_start + blockDim.x : cluster_num;
  //  Thread index 
  int col = row_start + threadIdx.x;
  __shared__ int local_changed[BLOCK_SIZE_X];
  __shared__ int local_offset[BLOCK_SIZE_X];

  if (col < row_end)
  {
    
    local_offset[threadIdx.x] = threadIdx.x;
    __syncthreads();

    //  Process by thread block 
    for (int row = row_start; row < row_end; row++)
    {
    
      int col_offset = local_offset[threadIdx.x];
      int row_offset = local_offset[row - row_start];
      //  Mark 
      local_changed[threadIdx.x] = 0;
      __syncthreads();

      // row < col  Take the value of the lower half of the matrix  cluster_matrix  by 1 Indicates that two-point cloud clusters can be merged , remember local_changed The value is 1
      if (row < col && row_offset != col_offset && (cluster_matrix[row * cluster_num + col] == 1))
        local_changed[col_offset] = 1;
      __syncthreads();

      local_offset[threadIdx.x] = (local_changed[col_offset] == 1) ? row_offset : col_offset;
      __syncthreads();
    }

    __syncthreads();
    int new_cluster = cluster_list[row_start + local_offset[threadIdx.x]];

    __syncthreads();
    //  to update cluster_list, Merge point cloud clusters in a thread block 
    cluster_list[col] = new_cluster;
  }
}

__global__ void rebuildClusterMatrix(int *old_cluster_matrix, int *new_clusters, int *new_cluster_matrix, int *new_cluster_offset, int old_size, int new_size)
/* old_cluster_matrix ：  Clustering matrix of point cloud clusters before merging  new_clusters ：  Merge point cloud clusters by block , After the update cluster_list( Index is the subscript of point cloud cluster ( Several point cloud clusters ), The value is the target point cloud cluster to which it belongs （ Thread index of point id)), That is, the target point cloud cluster （ Thread index of point id) Changed  new_cluster_matrix ：  New clustering matrix required  new_cluster_offset ：  After the merger , The index is the target cluster that exists ( Thread index of point id), Value indicates the number of point cloud clusters  old_size ：  Number of point cloud clusters before merging  new_size ：  Number of merged point cloud clusters  */
{
    
  //  The number of blocks is still divided by the point cloud clusters before merging 
  for (int column = threadIdx.x + blockIdx.x * blockDim.x; column < old_size; column += blockDim.x * gridDim.x)
  {
    
    for (int row = 0; row < column; row++)
    {
     //  New point cloud cluster row index 
      int new_row = new_cluster_offset[new_clusters[row]];
      //  New point cloud cluster column index 
      int new_column = new_cluster_offset[new_clusters[column]];
      if (old_cluster_matrix[row * old_size + column] == 1)
        new_cluster_matrix[new_row * new_size + new_column] = 1;
    }
  }
}

//  After the merger , The thread index corresponding to each point cloud corresponds to the target cluster to which the point cloud belongs ( Thread index id), Thread index id It's big , to update cluster_indices_ Value , Index the point cloud cluster in the point cloud from  0  Reset 
__global__ void resetClusterIndexes(int *cluster_indices, int *cluster_offset, int size)
/* cluster_indices_ :  Indicates after consolidation , The thread index corresponding to each point cloud corresponds to the target cluster to which the point cloud belongs ( Thread index id) cluster_offset :  Indicates after consolidation , The index is the target cluster that exists ( Thread index of point id),  Value indicates the number of point cloud clusters  size :  The number of point clouds  */
{
    
  for (int i = threadIdx.x + blockIdx.x * blockDim.x; i < size; i += blockDim.x * gridDim.x)
  {
    
    int old_cluster = cluster_indices[i];

    cluster_indices[i] = cluster_offset[old_cluster];
  }
}

//  Reflect the changes in the clustering merging step to the clustering index of all input points , The cluster of the input point becomes the target cluster corresponding to its source cluster 
__global__ void reflexClusterChanges(int *cluster_indices, int *cluster_offset, int *cluster_list, int size)
/* cluster_indices :  The index is the thread index id, The value is another thread index in the thread block id, Indication point A Belongs to point cloud cluster B cluster_offset ：  The index is the thread index corresponding to the point cloud cluster id, Value indicates the number of point cloud clusters  cluster_list :  Merge point cloud clusters by block , After the update cluster_list, Index is the subscript of point cloud cluster ( Several point cloud clusters ), The value is the point cloud cluster to which it belongs （ Thread index of point id) size :  Number of point clouds  */
{
    
  for (int i = threadIdx.x + blockIdx.x * blockDim.x; i < size; i += blockDim.x * gridDim.x)
    //  Update the target cluster of the corresponding point cloud of each thread index ( Thread index id)
    cluster_indices[i] = cluster_list[cluster_offset[cluster_indices[i]]];
}

void GpuEuclideanCluster::extractClusters()
{
    
  int block_x, grid_x;
  block_x = (size_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : size_;
  grid_x = (size_ - 1) / block_x + 1;

  int *cluster_mark;
  int cluster_num, old_cluster_num;
  pclEuclideanInitialize<<<grid_x, block_x>>>(cluster_indices_, size_);
  //  You can block a host thread until the device finishes all previous tasks 
  CHECK(cudaDeviceSynchronize());

  old_cluster_num = cluster_num = size_;

  CHECK(cudaMalloc(&cluster_mark, (size_ + 1) * sizeof(int)));
  CHECK(cudaMemset(cluster_mark, 0, (size_ + 1) * sizeof(int)));
  //  branch grid_ Thread block processing , For each thread block's point cloud , If the point cloud of each thread block A And point cloud B The distance is less than the threshold , Then point cloud A It belongs to point cloud B cluster , Update point A cluster_indices_ The value is  B Thread index of the cluster 
  // cluster_indices_  The index is the thread index id,  The value is another thread index in the thread block id,  Indication point A Belongs to point cloud cluster B
  blockLabelling<<<grid_x, block_x>>>(x_, y_, z_, cluster_indices_, size_, threshold_);

  //  scanning cluster_indices_ Array , Set the current point cluster Corresponding cluster_mark Element is marked as 1, If it doesn't exist cluster, So it's cluster_mark by 0
  clusterMark<<<grid_x, block_x>>>(cluster_indices_, cluster_mark, size_);
  //  Calculate the prefix and , Does not include the current value ,cluster_num Indicates the number of point cloud clusters 
  // cluster_mark  The index is the thread index corresponding to the point cloud cluster id, Value indicates the number of point cloud clusters 
  exclusiveScan(cluster_mark, size_ + 1, &cluster_num);

  // int *cluster_mark_host;
  // cluster_mark_host = (int *)malloc(size_ * sizeof(int));
  // CHECK(cudaMemcpy(cluster_mark_host, cluster_mark, size_ * sizeof(int), cudaMemcpyDeviceToHost));
  // for (int i = 0; i < size_; i++)
  // {
    
  // printf(" Thread index :%d %d\n", i, cluster_mark_host[i]);
  // }
  int *cluster_list, *new_cluster_list, *tmp;
  CHECK(cudaMalloc(&cluster_list, cluster_num * sizeof(int)));
  // cluster_list： Index is the subscript of point cloud cluster ( Several point cloud clusters ), The value is the point cloud cluster to which it belongs （ Thread index of point id)
  clusterCollector<<<grid_x, block_x>>>(cluster_indices_, cluster_list, cluster_mark, size_);
  CHECK(cudaDeviceSynchronize());

  int *cluster_matrix, *new_cluster_matrix;
  CHECK(cudaMalloc(&cluster_matrix, cluster_num * cluster_num * sizeof(int)));
  CHECK(cudaMemset(cluster_matrix, 0, cluster_num * cluster_num * sizeof(int)));
  CHECK(cudaMalloc(&new_cluster_list, cluster_num * sizeof(int)));
  // cluster_matrix :  Clustering matrix , Use only the lower half of the matrix , If a pair of clusters x and y Connect , Then the matrix element [x][y]  by 1
  buildClusterMatrix<<<grid_x, block_x>>>(x_, y_, z_, cluster_indices_, cluster_matrix, cluster_mark, size_, cluster_num, threshold_);
  CHECK(cudaDeviceSynchronize());

  int block_x2 = 0, grid_x2 = 0;

  /* Loop until there is no change in the number of clusters */
  do
  {
    
    old_cluster_num = cluster_num;
    //  Divide the number of thread blocks according to the number of point cloud clusters 
    block_x2 = (cluster_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : cluster_num;
    grid_x2 = (cluster_num - 1) / block_x2 + 1;
    //  Merge point cloud clusters by block , to update cluster_list( Index is the subscript of point cloud cluster ( Several point cloud clusters ), The value is the target point cloud cluster to which it belongs （ Thread index of point id))
    // cluster_list Value , That is, the target point cloud cluster （ Thread index of point id) Changed 
    mergeClusters<<<grid_x2, block_x2>>>(cluster_matrix, cluster_list, cluster_num);
    //  Update the target cluster of the corresponding point cloud of each thread index ( Thread index id), That is, update cluster_indices_
    reflexClusterChanges<<<grid_x, block_x>>>(cluster_indices_, cluster_mark, cluster_list, size_);

    CHECK(cudaMemset(cluster_mark, 0, (size_ + 1) * sizeof(int)));
    //  Existing target clusters ( Thread index of point id),cluster_mark Set up 1
    clusterMark<<<grid_x2, block_x2>>>(cluster_list, cluster_mark, cluster_num);
    //  Calculate the prefix and , Does not include the current value ,cluster_num Indicates the number of point cloud clusters after merging 
    // cluster_mark  After the merger , The index is the target cluster that exists ( Thread index of point id), Value indicates the number of point cloud clusters 
    exclusiveScan(cluster_mark, size_ + 1, &cluster_num);

    if (grid_x2 == 1 && cluster_num == old_cluster_num)
      break;
    CHECK(cudaDeviceSynchronize());
    //  The point cloud clusters in each thread block are merged , The thread index of the target point cloud cluster id Changed , The total number of point cloud clusters has changed (cluster_mark Changed ),
    // cluster_list You can get the target point cloud cluster （ Thread index of point id) ,cluster_mar according to cluster_list Is worth the subscript of the point cloud cluster ( Several point cloud clusters ), This subscript is used as new_cluster_list The index of , The value is the target point cloud cluster to which the value belongs （ Thread index of point id)
    //  You need to get a new array new_cluster_list, The storage takes the number of point cloud clusters as the subscript , The value is the target point cloud cluster to which it belongs （ Thread index of point id)
    clusterCollector<<<grid_x2, block_x2>>>(cluster_list, new_cluster_list, cluster_mark, old_cluster_num);

    CHECK(cudaMalloc(&new_cluster_matrix, cluster_num * cluster_num * sizeof(int)));
    CHECK(cudaMemset(new_cluster_matrix, 0, cluster_num * cluster_num * sizeof(int)));
    CHECK(cudaDeviceSynchronize());
    //  Divide the number of merged point cloud clusters into thread blocks and threads , Get a new clustering matrix 
    rebuildClusterMatrix<<<grid_x2, block_x2>>>(cluster_matrix, cluster_list, new_cluster_matrix, cluster_mark, old_cluster_num, cluster_num);

    CHECK(cudaFree(cluster_matrix));
    cluster_matrix = new_cluster_matrix;
    //  In exchange for cluster_list And new_cluster_list Value 
    tmp = cluster_list;
    cluster_list = new_cluster_list;
    new_cluster_list = tmp;
  } while (1);

  cluster_num_ = cluster_num;

  //  After the merger , Each point cloud ( Thread index ) The target cluster corresponding to the point cloud ( Another thread index id), Thread index id It's big , to update cluster_indices_ Value , Index the point cloud cluster in the point cloud from  0  Reset 
  resetClusterIndexes<<<grid_x, block_x>>>(cluster_indices_, cluster_mark, size_);
  CHECK(cudaDeviceSynchronize());
  CHECK(cudaMemcpy(cluster_indices_host_, cluster_indices_, size_ * sizeof(int), cudaMemcpyDeviceToHost));

  CHECK(cudaFree(cluster_matrix));
  CHECK(cudaFree(cluster_list));
  CHECK(cudaFree(new_cluster_list));
  CHECK(cudaFree(cluster_mark));
}

std::vector<GpuEuclideanCluster::GClusterIndex> GpuEuclideanCluster::getOutput()
{
    
  std::vector<GClusterIndex> cluster_indices(cluster_num_);

  for (unsigned int i = 0; i < cluster_indices.size(); i++)
    cluster_indices[i].index_value = -1;

  for (int i = 0; i < size_; i++)
  {
    
    cluster_indices[cluster_indices_host_[i]].points_in_cluster.push_back(i);
    cluster_indices[cluster_indices_host_[i]].index_value = cluster_indices_host_[i];
  }

  for (unsigned int i = 0; i < cluster_indices.size();)
  {
    
    //  The number of points in the point cloud cluster 
    int number_of_pts = cluster_indices[i].points_in_cluster.size();

    if (number_of_pts < min_cluster_pts_ || number_of_pts > max_cluster_pts_)
      cluster_indices.erase(cluster_indices.begin() + i);
    else
      i++;
  }

  return cluster_indices;
}

I actually tested ,cuda This version of European clustering takes time 3ms about , Speed up 10 About times ！

cuda Edition and non Edition cuda Version of European clustering I put csdn link ：https://download.csdn.net/download/weixin_42905141/86226246, You can call directly if you need