KinectFusion 中的 ICP 算法 GPU 代碼解讀

簡介

本篇博客介紹 KinectFusion 中的 ICP 算法的代碼，代碼實現是 PCL 工程 pcl_gpu_kinfu_large_scale 工程中的文件 estimate_combined.cu。

ICP 算法實現用 GPU 做並行計算可以極大的提高運算效率。

GPU 極小化 ICP 算法中的目標函數

KinectFusion 中 ICP 採用極小化點到平面的距離，計算兩幀之間的位姿變換，ICP 算法理論介紹見博客：
http://blog.csdn.net/fuxingyin/article/details/51425721

ICP 算法通過求解 Cx=b 計算兩幀之間的位姿變換，其中 C 和 b 的表達式爲：

C=∑i=1k(pi×nin1)((pi×ni)⊤n⊤i).

b=∑i=1k((pi×ni)τ, ni)τ⋅((pi−qi)τ⋅ni)

其中 pi 是 current 點雲，qi 是 previous 點雲，ni 是 previous 點雲法向量，pi 和 qi 是匹配點，極小化目標函數前尋找匹配點的操作也在 GPU 中進行。

上式中對於每個點的累加操作採用 GPU 做並行計算。

代碼結構

GPU 和 CPU 接口：

// Rcurr 和 tcurr 是估計的當前幀的位姿
// vmap_curr 和 nmap_curr 由當前幀的深度圖像計算得到的點雲
// Rprev_inv 和 tprev 上一幀相機的位姿 Intr 深度相機內參
// vmap_g_prev 和 nmap_g_prev 上一幀通過光線投影計算得到的點座標和法向量
// distThres 和 angleThres 投影算法計算匹配點時的閾值
// gbuf 和 mbuf GPU 中開闢的顯存，存儲累積和
// matrixA_host 和 vectorB_host GPU 累加之後最終的輸出
void
estimateCombined (const Mat33& Rcurr, const float3& tcurr, 
            const MapArr& vmap_curr, const MapArr& nmap_curr, 
            const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
            const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, 
            float distThres, float angleThres,
            DeviceArray2D<float_type>& gbuf, DeviceArray<float_type>& mbuf, 
            float_type* matrixA_host, float_type* vectorB_host)
      {

GPU 並行計算分配核心分配
如上圖 GPU 線程的框架結構，可以把 GPU 分成 grid.x 乘以 grid.y 個 block， 每個 block 中有 block.x 乘以 block.y 個線程，每個線程的計算獨立。

// 採用 grid.x 乘以 grid.y 個 block，
// 每個 block 中有 block.x 乘以 block.y 個線程，

dim3 block (Combined::CTA_SIZE_X, Combined::CTA_SIZE_Y);
dim3 grid (1, 1, 1);

//  rows 和 cols 分別是圖像行和列的維度
grid.x = divUp (cols, block.x);
grid.y = divUp (rows, block.y);

// 上述操作剛好分配了 rows 乘以 cols 個線程，每個線程處理圖像中的單個像素點，
combinedKernel<<<grid, block>>>(cs);

// 第一次在 GPU 中做累加，每個 block 的累加結果存儲到 gbuf 對應的顯存位置
mbuf.create (TranformReduction::TOTAL);
if (gbuf.rows () != TranformReduction::TOTAL || gbuf.cols () < (int)(grid.x * grid.y))
  gbuf.create (TranformReduction::TOTAL, grid.x * grid.y);

cs.gbuf = gbuf;

combinedKernel<<<grid, block>>>(cs);
cudaSafeCall ( cudaGetLastError () );
cudaSafeCall(cudaDeviceSynchronize());

// 第一次做累加實現
struct Combined
{
  enum
  {
    CTA_SIZE_X = ESTIMATE_COMBINED_CUDA_GRID_X,
    CTA_SIZE_Y = ESTIMATE_COMBINED_CUDA_GRID_Y,
    CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y
  };

  Mat33 Rcurr;
  float3 tcurr;

  PtrStep<float> vmap_curr;
  PtrStep<float> nmap_curr;

  Mat33 Rprev_inv;
  float3 tprev;

  Intr intr;

  PtrStep<float> vmap_g_prev;
  PtrStep<float> nmap_g_prev;

  float distThres;
  float angleThres;

  int cols;
  int rows;

  mutable PtrStep<float_type> gbuf;

  __device__ __forceinline__ bool
  search (int x, int y, float3& n, float3& d, float3& s) const
  {
    float3 ncurr;
    ncurr.x = nmap_curr.ptr (y)[x];

    if (isnan (ncurr.x))
      return (false);

    float3 vcurr;
    vcurr.x = vmap_curr.ptr (y       )[x];
    vcurr.y = vmap_curr.ptr (y + rows)[x];
    vcurr.z = vmap_curr.ptr (y + 2 * rows)[x];

    float3 vcurr_g = Rcurr * vcurr + tcurr;

    float3 vcurr_cp = Rprev_inv * (vcurr_g - tprev); // prev camera coo space

    int2 ukr;         //projection
    ukr.x = __float2int_rn (vcurr_cp.x * intr.fx / vcurr_cp.z + intr.cx);      //4
    ukr.y = __float2int_rn (vcurr_cp.y * intr.fy / vcurr_cp.z + intr.cy);                      //4

    if (ukr.x < 0 || ukr.y < 0 || ukr.x >= cols || ukr.y >= rows || vcurr_cp.z < 0)
      return (false);

    float3 nprev_g;
    nprev_g.x = nmap_g_prev.ptr (ukr.y)[ukr.x];

    if (isnan (nprev_g.x))
      return (false);

    float3 vprev_g;
    vprev_g.x = vmap_g_prev.ptr (ukr.y       )[ukr.x];
    vprev_g.y = vmap_g_prev.ptr (ukr.y + rows)[ukr.x];
    vprev_g.z = vmap_g_prev.ptr (ukr.y + 2 * rows)[ukr.x];

    float dist = norm (vprev_g - vcurr_g);
    if (dist > distThres)
      return (false);

    ncurr.y = nmap_curr.ptr (y + rows)[x];
    ncurr.z = nmap_curr.ptr (y + 2 * rows)[x];

    float3 ncurr_g = Rcurr * ncurr;

    nprev_g.y = nmap_g_prev.ptr (ukr.y + rows)[ukr.x];
    nprev_g.z = nmap_g_prev.ptr (ukr.y + 2 * rows)[ukr.x];

    float sine = norm (cross (ncurr_g, nprev_g));

    if (sine >= angleThres)
      return (false);
    n = nprev_g;
    d = vprev_g;
    s = vcurr_g;
    return (true);
  }

  __device__ __forceinline__ void
  operator () () const
  {
    // 計算線程對應的像素座標，每個線程處理單個像素座標
    int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
    int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;

    float3 n, d, s;
    bool found_coresp = false;

    // 用投影算法尋找匹配點
    if (x < cols && y < rows)
      found_coresp = search (x, y, n, d, s);

    float row[7];

    //如果找到匹配點構建矩陣 A 和 b
    if (found_coresp)
    {
      *(float3*)&row[0] = cross (s, n);
      *(float3*)&row[3] = n;
      row[6] = dot (n, d - s);
    }
    else
      row[0] = row[1] = row[2] = row[3] = row[4] = row[5] = row[6] = 0.f;

    __shared__ float_type smem[CTA_SIZE];
    int tid = Block::flattenedThreadId ();

    int shift = 0;
    for (int i = 0; i < 6; ++i)        //rows
    {
      #pragma unroll
      for (int j = i; j < 7; ++j)          // cols + b
      {
        // 線程同步，只有全部的線程都執行到這一步，才能執行後續操作
        __syncthreads ();
        smem[tid] = row[i] * row[j];
        __syncthreads ();

        // 將單個 block 中的數據做累加
        reduce<CTA_SIZE>(smem);

        // 如果是 block 中的第一個線程，則將單個 block 中的累加和存到顯存中
        if (tid == 0)
          gbuf.ptr (shift++)[blockIdx.x + gridDim.x * blockIdx.y] = smem[0];
      }
    }
  }
};

// GPU 和 CPU 接口
__global__ void
combinedKernel (const Combined cs) 
{
  cs (); // 調用函數 operator () () const
}

// 第二次做累加，將每個 block 的輸出累加
 TranformReduction tr;
 tr.gbuf = gbuf;
 tr.length = grid.x * grid.y;
 tr.output = mbuf;

// 第二次做累加，採用 A 和 b 中元素個數和作爲 block 的個數
// 每個 block 處理單個元素，每個 block 的線程個數爲 512
 TransformEstimatorKernel2<<<TranformReduction::TOTAL, TranformReduction::CTA_SIZE>>>(tr);
 cudaSafeCall (cudaGetLastError ());
 cudaSafeCall (cudaDeviceSynchronize ());

 // 將累加結果從 GPU 顯存轉移到 CPU 內存
 float_type host_data[TranformReduction::TOTAL];
 mbuf.download (host_data);

// 構建矩陣 A 和 b
 int shift = 0;
 for (int i = 0; i < 6; ++i)  //rows
   for (int j = i; j < 7; ++j)    // cols + b
   {
     float_type value = host_data[shift++];
     if (j == 6)       // vector b
       vectorB_host[i] = value;
     else
       matrixA_host[j * 6 + i] = matrixA_host[i * 6 + j] = value;
   }
}

// 第二次做累加實現
struct TranformReduction
{
  enum
  {
    CTA_SIZE = 512,
    STRIDE = CTA_SIZE,

    B = 6, COLS = 6, ROWS = 6, DIAG = 6,
    UPPER_DIAG_MAT = (COLS * ROWS - DIAG) / 2 + DIAG,
    TOTAL = UPPER_DIAG_MAT + B,

    GRID_X = TOTAL
  };

  PtrStep<float_type> gbuf;
  int length;
  mutable float_type* output;

  __device__ __forceinline__ void
  operator () () const
  {
    const float_type *beg = gbuf.ptr (blockIdx.x);
    const float_type *end = beg + length;

    int tid = threadIdx.x;

    float_type sum = 0.f;

    // 每個線程對 A 或者 b 中的單個元素做累積
    for (const float_type *t = beg + tid; t < end; t += STRIDE)
      sum += *t;

    // 每個 block 中有 CTA_SIZE 個線程
    __shared__ float_type smem[CTA_SIZE];
    smem[tid] = sum;
    __syncthreads ();

    // 將每個 block 中的單個元素的值求和
    reduce<CTA_SIZE>(smem);

    // 當在 block 中線程編號爲 0 時將累加結果輸出
    if (tid == 0)
      output[blockIdx.x] = smem[0];
  }
};

__global__ void
TransformEstimatorKernel2 (const TranformReduction tr) 
{
  tr ();
}

// 單個 block 中的元素做累加
template<int CTA_SIZE_, typename T>
static __device__ __forceinline__ void reduce(volatile T* buffer)
{
    int tid = Block::flattenedThreadId();
    T val =  buffer[tid];

    if (CTA_SIZE_ >= 1024) { if (tid < 512) buffer[tid] = val = val + buffer[tid + 512]; __syncthreads(); }
    if (CTA_SIZE_ >=  512) { if (tid < 256) buffer[tid] = val = val + buffer[tid + 256]; __syncthreads(); }
    if (CTA_SIZE_ >=  256) { if (tid < 128) buffer[tid] = val = val + buffer[tid + 128]; __syncthreads(); }
    if (CTA_SIZE_ >=  128) { if (tid <  64) buffer[tid] = val = val + buffer[tid +  64]; __syncthreads(); }

    // 因爲 GPU 並行運算是以 warp 爲單位的，每個 warp 中有 32 個線程，
    //並且每個線程的操作是嚴格同步的，當線程個數小於 32 時，GPU 單個線程做累加時不需要再同步
    if (tid < 32)
    {
      if (CTA_SIZE_ >=   64) { buffer[tid] = val = val + buffer[tid +  32]; }
      if (CTA_SIZE_ >=   32) { buffer[tid] = val = val + buffer[tid +  16]; }
      if (CTA_SIZE_ >=   16) { buffer[tid] = val = val + buffer[tid +   8]; }
      if (CTA_SIZE_ >=    8) { buffer[tid] = val = val + buffer[tid +   4]; }
      if (CTA_SIZE_ >=    4) { buffer[tid] = val = val + buffer[tid +   2]; }
      if (CTA_SIZE_ >=    2) { buffer[tid] = val = val + buffer[tid +   1]; }
    }
}

版權聲明：本文爲博主原創文章，未經博主允許不得轉載。