使用深度学习进行点云匹配（二）

本文承接上一篇《使用深度学习进行点云匹配（一）》。

在上一篇研究了Demo代码如何实现根据3Dmatch描述子实现点云配对以后，接下来的问题就是如何训练出3Dmatch描述子。在此之前，作者先将数据转换为了TDF体素网络，今天便来解析这部分是怎么做的。但是我昨天研究了一天，有一些地方还是没有搞懂，也只能先记录下来，如果有人明白，可以评论告诉我。

先看github中给出的操作步骤：

翻译一下，这一部分大标题便是如何将3D数据转变成TDF体素网格。接下来3部分分别介绍了点云图，网格，深度图分别如何转化为TDF体素网络。这里我们先介绍点云图的转换。下面写了三种方案，分别对应3份代码，第一份是C++/CUDA 写的代码，也就是上次运行的demo文件的一个支持文件，使用占用体素网格可以快速的计算TDF但是是一个近似的TDF值。（occupancy在这里翻译为占用不知道对不对，是什么意思我没理解，我认为是指将点云包裹起来的立方体网格。下面代码也有提到这个）第二份替代方案是Matlab/CUDA 代码，它可以精确地计算TDF但是很慢。第三个替代方案是纯matlab写的代码，这也可以精确计算TDF，并可以在matlab独立运行，如果点云比较小，就不会有内存问题。这里我们就先介绍第一个方案，因为它也是和Demo相关的方案。而2和3都是函数，参数含义没有具体写，需要结合别的文件进行分析，之后再说。

打开demo.cu，可以看到这是基于C++的CUDA代码，简单说一下CUDA，就是让你可以在英伟达显卡上（GPU）进行编程，从而实现比如加速计算的目的。

#include <iostream>
#include <fstream>     //ifstream是从硬盘到内存
#include <iomanip>    //主要是对cin,cout之类的一些操纵运算子，它是I/O流控制头文件,就像C里面的格式化输出一样
#include <sstream>    //c++ 字符串流 sstream(常用于格式转换)
#include <string>
#include "utils.hpp"  
#include "marvin.hpp"

#define CUDA_NUM_THREADS 512   //定义block内的线程个数为512
#define CUDA_MAX_NUM_BLOCKS 2880   //CUDA最大的blocks是2880

这些是一些头文件，以及CUDA编程要定义的black和线程，这些都是CUDA编程需要的东西，与分析文件无关。

接下来是ComputeTDF函数定义，我们先看主函数，这部分跳过。

int main(int argc, char * argv[]) {

  std::string pointcloud_filename(argv[1]);                     //输入的点云文件
  std::string out_prefix_filename(argv[2]);                     //保存文件

  // Super hacky code to read a point cloud file (replace this...)           超级简洁的代码去读取点云文件
  std::ifstream pointcloud_file(pointcloud_filename.c_str());
  if (!pointcloud_file) {
    std::cerr << "Point cloud file not found." << std::endl;
    return -1;
  }
  int num_pts = 0;
  for (int line_idx = 0; line_idx < 7; ++line_idx) {
    std::string line_str;
    std::getline(pointcloud_file, line_str);
    if (line_idx == 2) {
      std::istringstream tmp_line(line_str);                 //istringstream 是从字符串读   https://blog.csdn.net/sinat_30071459/article/details/50755710
      std::string tmp_line_prefix;                            //输出前面的两部分 
      tmp_line >> tmp_line_prefix;                            //输出element              
      tmp_line >> tmp_line_prefix;                            //输出vertex
      tmp_line >> num_pts;                                  //点云的数目（对于点云1是257179）
    }
  }
if (num_pts == 0) {   //点云文件的第三行没有告诉我点云的数目
    std::cerr << "Third line of .ply file does not tell me number of points. Double check format of point cloud file (or change .ply file reader code)." << std::endl;
    return 0;
  }
float * pts = new float[num_pts * 3]; // Nx3 matrix saved as float array (row-major order)   Nx3矩阵保存为float数组（行主顺序）
  pointcloud_file.read((char*)pts, sizeof(float) * num_pts * 3);
  pointcloud_file.close();

  std::cout << "Loaded point cloud with " << num_pts << " points!" << std::endl;   //导入了（对点云1来说是）252179个点

首先定义了读取的点云文件和结果存放文件。然后去读取点云数据，保存到line_str字符串中，下面部分是为了求出点云数目，在line_idx==2的时候，先把前面的element和vertex输出到tmp_line_prefix，之后下一个就是点云数目，保存到num_pts中，对于点云1数目是2570179.如果得不到，就返回一个没有的提示。之后把所有点云读取到pts之中。它的大小为2571019*3.

 float voxel_size = 0.01;                                                  //体素网络尺寸是0.01
  float trunc_margin = voxel_size * 5;
  int voxel_grid_padding = 15; // in voxels

  // Compute point cloud coordinates of the origin voxel (0,0,0) of the voxel grid   计算体素网格的原点体素（0,0,0）的点云坐标
  float voxel_grid_origin_x, voxel_grid_origin_y, voxel_grid_origin_z;
  float voxel_grid_max_x, voxel_grid_max_y, voxel_grid_max_z;
  voxel_grid_origin_x = pts[0]; voxel_grid_max_x = pts[0];
  voxel_grid_origin_y = pts[1]; voxel_grid_max_y = pts[1];
  voxel_grid_origin_z = pts[2]; voxel_grid_max_z = pts[2];
  for (int pt_idx = 0; pt_idx < num_pts; ++pt_idx) {
    voxel_grid_origin_x = min(voxel_grid_origin_x, pts[pt_idx * 3 + 0]);
    voxel_grid_origin_y = min(voxel_grid_origin_y, pts[pt_idx * 3 + 1]);
    voxel_grid_origin_z = min(voxel_grid_origin_z, pts[pt_idx * 3 + 2]);
    voxel_grid_max_x = max(voxel_grid_max_x, pts[pt_idx * 3 + 0]);
    voxel_grid_max_y = max(voxel_grid_max_y, pts[pt_idx * 3 + 1]);
    voxel_grid_max_z = max(voxel_grid_max_z, pts[pt_idx * 3 + 2]);
  }

接下来是正式的东西，如何计算TDF，TDF是什么呢，我们一直没有详细介绍，但是你应该听说过TSDF，它可以将点云的深度估计值和深度相机的测量值进行融合，提高深度估计的精度，可以参照这篇文章介绍：https://zhuanlan.zhihu.com/p/42112101。TDF借鉴了TSDF的思想，但是与TSDF也有不同，作者在论文的4.1节是这么解释的：The TDF representation holds several advantages over its signed alternative TSDF [7], which encodes occluded space (values near -1) in addition to the surface (values near 0) and free space (values near 1). By removing the sign, the TDF loses the distinction between free space and occluded space, but gains a new property that is crucial to the robustness of our descriptor on partial data: the largest gradients between voxel values are concentrated around the surfaces rather than in the shadow boundaries between free space and occluded space. Furthermore, the TDF representation reduces the ambiguity of determining what is occluded space on 3D data where camera view is unavailable.翻译一下就是：TDF表示与其带替代的TSDF相比具有若干优点，TSDF除了表面（接近0的值）和自由空间（接近1的值）之外还编码被遮挡的空间（值接近-1）。通过删除符号，TDF失去了自由空间和被遮挡空间之间的区别，但在部分数据上获得了一个对我们的描述子的鲁棒性至关重要的新属性：体素值之间的最大梯度集中在表面周围，而不是自由空间和遮挡空间之间的阴影边界。此外，TDF表示减少了确定摄像机视图不可用的3D数据上的遮挡空间是什么的模糊性。

也就是说每个体素的TDF值表示该体素的中心与最近的3D表面之间的距离。这些TDF值被截断，归一化，然后翻转到介于1（表面上）和0（远离表面）之间。回到代码，以上代码求了点云图的总体范围，x,y,z分别的最大，最小值。

int voxel_grid_dim_x = round((voxel_grid_max_x - voxel_grid_origin_x) / voxel_size) + 1 + voxel_grid_padding * 2;
    int voxel_grid_dim_y = round((voxel_grid_max_y - voxel_grid_origin_y) / voxel_size) + 1 + voxel_grid_padding * 2;
    int voxel_grid_dim_z = round((voxel_grid_max_z - voxel_grid_origin_z) / voxel_size) + 1 + voxel_grid_padding * 2;
    std::cout <<voxel_grid_dim_x<<std::endl;  //370
    std::cout <<voxel_grid_dim_y<<std::endl;  //253
    std::cout <<voxel_grid_dim_z<<std::endl;  //324

这部分代码就说明我们如果按照体素voxel_size=0.01来计算，我们可以得到的是一个x轴占370，y轴占253，z轴占324的立方体。（这个表达很怪，不过我也不知道怎么表达为好）。这里有一个问题是为什么计算dim的时候要+ 1 + voxel_grid_padding * 2，我的理解是将这个立方体扩大了一下，这样就有很多表示体外的体素了，它们中有很多可以在表面（TDF=1），这样更容易找到点云表面。

voxel_grid_origin_x = voxel_grid_origin_x - voxel_grid_padding * voxel_size + voxel_size / 2;
    voxel_grid_origin_y = voxel_grid_origin_y - voxel_grid_padding * voxel_size + voxel_size / 2;
    voxel_grid_origin_z = voxel_grid_origin_z - voxel_grid_padding * voxel_size + voxel_size / 2;
    std::cout <<voxel_grid_origin_x<<std::endl;   //-2.31256
    std::cout <<voxel_grid_origin_y<<std::endl;   //-1.54346
    std::cout <<voxel_grid_origin_z<<std::endl;   //0.899

    std::cout << "Size of TDF voxel grid: " << voxel_grid_dim_x << " x " << voxel_grid_dim_y << " x " << voxel_grid_dim_z << std::endl;
    std::cout << "Computing TDF voxel grid..." << std::endl;

这一步也让人费解，xy,z的值等于原来的值减去voxel_grid_padding * voxel_size 再加上 voxel_size / 2，我本来以为应该是把立方体现在的最小的边界点重新定位，但是计算了一下，最小的边界点貌似不应该这么求，应该是减去voxel_grid_padding * voxel_size 再减去 voxel_size / 2。所以我这里也是存疑。

// Compute surface occupancy grid             计算表面占用网格
    float * voxel_grid_occ = new float[voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z];   //30329640
    memset(voxel_grid_occ, 0, sizeof(float) * voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z);
    for (int pt_idx = 0; pt_idx < num_pts; ++pt_idx)
    {
        int pt_grid_x = round((pts[pt_idx * 3 + 0] - voxel_grid_origin_x) / voxel_size);
        int pt_grid_y = round((pts[pt_idx * 3 + 1] - voxel_grid_origin_y) / voxel_size);
        int pt_grid_z = round((pts[pt_idx * 3 + 2] - voxel_grid_origin_z) / voxel_size);
        voxel_grid_occ[pt_grid_z * voxel_grid_dim_y * voxel_grid_dim_x + pt_grid_y * voxel_grid_dim_x + pt_grid_x] = 1.0f;
    }

这一步是计算表面占用网格，按照意思应该是把点云表面的体素值设置为1，但是这个给出的方法让我感到费解。

// Initialize TDF voxel grid             初始化TDF体素网格
  float * voxel_grid_TDF = new float[voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z];
  memset(voxel_grid_TDF, 0, sizeof(float) * voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z);

这个定义的TDF体素网格，就要存储所有体素的值。

// Copy voxel grids to GPU memory     将体素网格复制到GPU内存
  float * gpu_voxel_grid_occ;
  float * gpu_voxel_grid_TDF;
  cudaMalloc(&gpu_voxel_grid_occ, voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z * sizeof(float));
  cudaMalloc(&gpu_voxel_grid_TDF, voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z * sizeof(float));
  marvin::checkCUDA(__LINE__, cudaGetLastError());
  cudaMemcpy(gpu_voxel_grid_occ, voxel_grid_occ, voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z * sizeof(float), cudaMemcpyHostToDevice);
  cudaMemcpy(gpu_voxel_grid_TDF, voxel_grid_TDF, voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z * sizeof(float), cudaMemcpyHostToDevice);
  marvin::checkCUDA(__LINE__, cudaGetLastError());

  int CUDA_NUM_LOOPS = (int)ceil((float)(voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z) / (float)(CUDA_NUM_THREADS * CUDA_MAX_NUM_BLOCKS));

  for (int CUDA_LOOP_IDX = 0; CUDA_LOOP_IDX < CUDA_NUM_LOOPS; ++CUDA_LOOP_IDX) {
    ComputeTDF <<< CUDA_MAX_NUM_BLOCKS, CUDA_NUM_THREADS >>>(CUDA_LOOP_IDX, gpu_voxel_grid_occ, gpu_voxel_grid_TDF,
        voxel_grid_dim_x, voxel_grid_dim_y, voxel_grid_dim_z,
        voxel_grid_origin_x, voxel_grid_origin_y, voxel_grid_origin_z,
        voxel_size, trunc_margin);
  }

因为接下来我们要使用GPU计算TDF体素网格（没错，不是只有训练描述子用到了GPU），所以代码中是一些关于GPU与CPU之间交互的操作，首先在gpu中建立了两个指针，他们对应表面网格和初始化的体素网格，下一步进行复制，然后调用CUDA内核定义的ComputeTDF函数，计算TDF值。

// CUDA kernel function to compute TDF voxel grid values given a point cloud (warning: approximate, but fast)  CUDA内核函数在给定点云的情况下计算TDF体素网格值（警告：近似但快速）
__global__
void ComputeTDF(int CUDA_LOOP_IDX, float * voxel_grid_occ, float * voxel_grid_TDF,
                int voxel_grid_dim_x, int voxel_grid_dim_y, int voxel_grid_dim_z,
                float voxel_grid_origin_x, float voxel_grid_origin_y, float voxel_grid_origin_z,
                float voxel_size, float trunc_margin) {

  int voxel_idx = CUDA_LOOP_IDX * CUDA_NUM_THREADS * CUDA_MAX_NUM_BLOCKS + blockIdx.x * CUDA_NUM_THREADS + threadIdx.x;
  if (voxel_idx > (voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z))
    return;

  int pt_grid_z = (int)floor((float)voxel_idx / ((float)voxel_grid_dim_x * (float)voxel_grid_dim_y));
  int pt_grid_y = (int)floor(((float)voxel_idx - ((float)pt_grid_z * (float)voxel_grid_dim_x * (float)voxel_grid_dim_y)) / (float)voxel_grid_dim_x);
  int pt_grid_x = (int)((float)voxel_idx - ((float)pt_grid_z * (float)voxel_grid_dim_x * (float)voxel_grid_dim_y) - ((float)pt_grid_y * (float)voxel_grid_dim_x));

  int search_radius = (int)round(trunc_margin / voxel_size);

  if (voxel_grid_occ[voxel_idx] > 0) {
    voxel_grid_TDF[voxel_idx] = 1.0f; // on surface
    return;
  }

 // Find closest surface point
  for (int iix = max(0, pt_grid_x - search_radius); iix < min(voxel_grid_dim_x, pt_grid_x + search_radius + 1); ++iix)
    for (int iiy = max(0, pt_grid_y - search_radius); iiy < min(voxel_grid_dim_y, pt_grid_y + search_radius + 1); ++iiy)
      for (int iiz = max(0, pt_grid_z - search_radius); iiz < min(voxel_grid_dim_z, pt_grid_z + search_radius + 1); ++iiz) {
        int iidx = iiz * voxel_grid_dim_x * voxel_grid_dim_y + iiy * voxel_grid_dim_x + iix;
        if (voxel_grid_occ[iidx] > 0) {
          float xd = (float)(pt_grid_x - iix);
          float yd = (float)(pt_grid_y - iiy);
          float zd = (float)(pt_grid_z - iiz);
          float dist = sqrtf(xd * xd + yd * yd + zd * zd) / (float)search_radius;
          if ((1.0f - dist) > voxel_grid_TDF[voxel_idx])
            voxel_grid_TDF[voxel_idx] = 1.0f - dist;
        }
      }
}

这就是计算TDF的核心表达了，首先的__global__ 是在CUDA中表示这是一个内核函数，是一组由GPU执行的并行计算任务，以foo<<>>(a)的形式或者driver API的形式调用。目前__global__函数必须由CPU调用，并将并行计算任务发射到GPU的任务调用单元。随着GPU可编程能力的进一步提高，未来可能可以由GPU调用。下面代码将voxel_grid_TDF中代表表面的体素置为1.接下来寻找每个体素距最近表面的距离，这里的计算方法我还是没有看懂。只能先记录一下。

当所有计算之后，TDF体素中的每一个值范围都在[0,1]之间，作者将surface表示为一，没有surface的地方根据与surface的距离由一向零递减，而且零是截断值，也就是小立方体内的值不会比零小。 

// Load TDF voxel grid from GPU to CPU memory
  cudaMemcpy(voxel_grid_TDF, gpu_voxel_grid_TDF, voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z * sizeof(float), cudaMemcpyDeviceToHost);
  marvin::checkCUDA(__LINE__, cudaGetLastError());

接下来就是将GPU训练好的TDF网格复制回CPU内。

// Compute random surface keypoints in point cloud coordinates and voxel grid coordinates 计算点云坐标和体素网格坐标中的随机表面关键点
  std::cout << "Finding random surface keypoints..." << std::endl;
  int num_keypts = 50 * 10;
  float * keypts = new float[num_keypts * 3];
  float * keypts_grid = new float[num_keypts * 3];
  for (int keypt_idx = 0; keypt_idx < num_keypts; ++keypt_idx) {
    int rand_idx = (int)(GetRandomFloat(0.0f, (float)num_pts));
    keypts[keypt_idx * 3 + 0] = pts[rand_idx * 3 + 0];
    keypts[keypt_idx * 3 + 1] = pts[rand_idx * 3 + 1];
    keypts[keypt_idx * 3 + 2] = pts[rand_idx * 3 + 2];
    keypts_grid[keypt_idx * 3 + 0] = round((pts[rand_idx * 3 + 0] - voxel_grid_origin_x) / voxel_size);
    keypts_grid[keypt_idx * 3 + 1] = round((pts[rand_idx * 3 + 1] - voxel_grid_origin_y) / voxel_size);
    keypts_grid[keypt_idx * 3 + 2] = round((pts[rand_idx * 3 + 2] - voxel_grid_origin_z) / voxel_size);
  }

接下来是随机在表面选一些兴趣点，因为点云的数据太多了，不可能给每一个点云都建立3DMatch描述子，作者在这里选取了500个点，这里有别的人提到了，相邻帧每一帧对应的点云中点的个数在10万这个数量级，源码中随机选择了500个，仍然根据匹配结果可以计算出一个相对比较的变换。这一部分代码就是选取兴趣点和它周围的TDF体素值，其中兴趣点的选择是随机的。

这次分析就到这里，接下来一部分就是神经网络的架构了，因为硬件原因，我没办法去运行这个这个程序，但是前面这一部分我直接改成了C++代码进行运行，协助分析。总结来说，这一部分主要讲了如何从点云数据转化成TDF体素网格，至于为什么使用TDF而不是使用2D深度图，作者在论文中也提到了：原因有两个：TDF可以容易的从mesh和点云转换；this 3D representation allows reasoning over real-world spatial scale and occluded regions, which cannot be directly encoded in 2D depth patches。而且作者通过实验证明了TDF的效果要比直接使用深度图效果好 。