应用于三维重建的TSDF(一)原理与代码解析_人工智能

一些背景知识

TSDF的主要作用是进行三维场景在计算机中的重建。目前的那些中文博客与成熟的TSDF应用其实还有差距，故写此文。视觉SLAM应用的一个分支为Dense SLAM。简单来说就是在定位机器人的同时对周围的环境进行(近乎)实时的3维重建，比如下图[1]

应用于三维重建的TSDF(一)原理与代码解析

比较出名的有开源代码的代表作为15年之前的Kinect Fusion, Dense Visual SLAM for RGB-D Cameras[2]，2015年之后有比较优秀的代表作为Elastic Fusion, Infinitam V3, 2019年的BAD SLAM 以及2020年的基于Voxblox的kimera VINS+Kimera Semantics等。一般的Dense SLAM主要面临的局限性是场景重建地很精密，对硬件的要求高，比如上面的Elastic Fusion的重建场景可能就包含数百万个点，一般需要较强的GPU辅助。而且比较难应用大场景的三维重建。Elastic Fusion, BAD SLAM都只能用于室内小范围的3D重建。 Infinitam V3号称可以进行大范围的3D重建，但是论文最后的limitation说到 Firstly, the number of points that a surfel scene can contain is currently limited to 5 million 5百万点并不适用于大型的场景，当然这个值可以改变，至于具体的效果我并没有试过。读者可自行去尝试。基于CPU的dense SLAM一般来说重建的效果会差一些，但是方便应用于大型室外场景。比如我们在本文或者接下来的文章中会详细介绍的voxblox(又开始画饼，毕竟SLAM introdution都没写了。。。哎主要是简书老吞我辛辛苦苦latex码出来的公式。。。就不想写了= = ) voxblox严格来说并不算是SLAM系统，因为这个系统并不带有定位的功能，它只负责进行三维重建，即只有mapping没有localization。它需要一个具有定位功能的SLAM系统输入当前机器人的位姿估计。这既是缺点也是优点。因为我们可以给这个系统搭配任意一个SLAM系统。像上文提到的Elastic Fusion, BAD SLAM的另一个问题就是因为他们是基于纯视觉的系统，当你的相机有比较大的抖动或者转弯等情况时，追踪就容易丢失。而对于voxblox，我们可以给它提供一个鲁棒性很强的SLAM系统提供位姿估计，一般来说就是包含IMU的比如港科大的VINS，Kimera-VIO 或者ORBSLAM 3(不是2是3哦，在2020 年7月左右开源，增加了对IMU等各种支持)。下图就是我利用港科大的VINS+Voxblox，硬件上利用了Realsense相机+IMU，在室外进行的CPU-Based的3维场景重建。

(场景拉大了看得不是很清楚，有兴趣的同学我可以截取一段视频发送) 我们最容易直接想到的在计算机中进行3维重建的就是点。一个场景可以由无数个点构成点云，参考2的Dense Visual SLAM即是如此。但是这种方法效率并不高。目前的Dense系统最常用的基本单元就是TSDF和Surfel(面片)。 Elastic Fusion，Infinitam v3等是基于surfel的。我不会介绍surfel的相关内容，感兴趣的同学可以取查看Elastic Fusion等论文。voxblox就是基于TSDF的3维场景重建。刚才我们提到直接利用点云进行3维重建效率并不高，原因的话举个简单的栗子。假设我们要重构一个平面，平面上有10万个点，我们要储存些点不仅需要相当的内存，绘制的时候还很费事，毕竟点这么多。然而对于这个平面，其实我们只需要知道四个顶点就可以了。省时又省事。无论TSDF还是surfel，都是以能观测到的表面为基本进行重建，而不是点云。可能你说又不是每个物体表面都是平的，是否忘了曲线的小段都可线性近似(手动滑稽)。这篇文章的后续，会以github上最简单的开源TSDF代码为例讲解其重构的过程，相对简单，在下篇文章，我们会追溯voxblox的代码，来了解tsdf在比较成熟系统中的实际应用。基本的TSDF 我们把一个有限的三维空间想象成由M个小方块堆叠而成，假设每个小方块长宽高都为

c

cm，我们称他们为voxel(体素)。这个三维空间在X,Y,Z维度上分别有

w,h,l

个小方块。三维空间的体素类比二维空间照片的像素。

应用于三维重建的TSDF(一)原理与代码解析

(图片来源于网络，侵删) 我们不仅用直观的x,y,z坐标来表示一个体素，我们用另外两个值来表示体素 1：体素到最近表面的距离。如果体素在表面之外(更靠近相机一侧)，则为正值，体素在表面之内，则为负值。这也是SDF(signed distance function)名字的由来。下图[3]更好地体现了这点

应用于三维重建的TSDF(一)原理与代码解析

的sdf。我们的主题是tsdf(truncated sdf)，这个’truncated’体现在哪里呢？体现在我们会把具体最近表现太远或者距离相机太近的体素点的sdf给强行赋值为一个确定值。数学表达为，我们人为定义一个截断距离

d

if(sdf > d) \ \ sdf = 1 \\ else \ \ if(sdf<-d ) \ \ sdf = -1

参考下图：

同样是俯视图。我们可以看到，具体表面（红线部分）远的值被赋值sdf如果为正，则被赋值为了1，如果为负则为-1. 2：sdf更新时的权重也被储存在体素内。计算并更新sdf的方法目前主要分两大类[4]，本文先介绍第一类。把位于全局坐标系的体素根据变换矩阵转换到当前相机所在的坐标系，再投影到2d的当前图像，获得与之匹配的像素点。比较体素的深度

z_v

与像素在3维坐标下的深度点

z_p

。像素的深度值即是相机中心到该像素点对应的表面的距离，而体素转换到相机坐标系后的深度值是体素到相机中心的距离，所以

z_p - z_v

就可以得到体素与像素对应的3d点到相机中心的距离差，即sdf.png里的

|\mathbf{p}\mathbf{x}|

了。我们记某个体素的tsdf为

D(\mathbf{x})

，其权重为

W(\mathbf{x})

，计算它的tsdf与权重的方法其实too simple (sometimes naive)。如下[5]

\begin{aligned} &D_i(\mathbf{x}) = \frac{\Sigma\omega_i(\mathbf{x})d_i(\mathbf{x})}{\Sigma \omega_i(\mathbf{x})} \\ & W_i(\mathbf{x}) = \Sigma \omega_i(\mathbf{x}) \end{aligned} \tag{1}

上面的式子中

i

表示相机的帧数，

\omega_i

表示当前帧数的该体素的权重，而

d_i

表示的是当前帧数的tsdf。可以看到体素的权重仅仅是把之前所有帧得到的权重加起来，而体素的tsdf只是把所有帧的权重乘以对应帧得到的tsdf再除以权重和做平均而已。实际操作中，我们不可能等到相机运行了100帧之后再做计算，而是第二帧的体素由第一帧得到，第三帧的由第二帧得到，如此增量计算。所以实际运用中的体素更新公式为(为了简便我省去了(

\mathbf{x}

)，大家知道是针对单个体素即可)

\begin{aligned} &D_{i+1} = \frac{W_{i}D_{i} + \omega_{i+1}d_{i+1}}{W_i + \omega_i} \\ & W_{i+1}(\mathbf{x}) = W_i + \omega_{i+1} \end{aligned} \tag{2}

式2只是写成了了式1的增量形式而已，实际是一个意思。其中每一帧的当前权重很粗暴的直接设置成了1。即

\omega_0 = ... = \omega_i = 1

。相关论文也提到了虽然

\omega_i

的选取很重要，但是大多数时候另它等于1都能得到不错的效果，在一些著名的应用中比如Kinect Fusion就是如此设置的(在voxblox中进行了一些改进，我们在分析voxblox代码时会讲到)。 TSDF的基本更新方法就是如此。相机的每一帧到来时，都会尝试更新每个体素的值，每个体素都可以独立更新，所以理所当然地应该应用于GPU上。下面我们参考一段tsdf重构的基本代码[6]tsdf代码。代码是应用于GPU上的CUDA代码，不了解CUDA也没有关系，我们会稍加解释。主代码只有一个文件，实现tsdf基本功能，非常简单。在代码的demo.cu的主函数中，首先定义了输入文件的位置

  // Location of camera intrinsic file
  std::string cam_K_file = "data/camera-intrinsics.txt";

  // Location of folder containing RGB-D frames and camera pose files
  std::string data_path = "data/rgbd-frames";
  int base_frame_idx = 150;
  int first_frame_idx = 150;
  float num_frames = 50;

  float cam_K[3 * 3];
  float base2world[4 * 4];
  float cam2base[4 * 4];
  float cam2world[4 * 4];
  int im_width = 640;
  int im_height = 480;
  float depth_im[im_height * im_width];

输入有:每一帧2D图像以及对应的深度图像，相机内参，每一帧的位姿(TSDF的应用比如Kinect Fusion一般会根据图片的像素的匹配来估计位姿，实现SLAM的功能，但这里的应用我们只需要了解到tsdf本身怎样使用就可以了，所以就使用已知位姿了)。接下来设置我们一共有多少体素点，以及每个体素大小是多少。

  // Voxel grid parameters (change these to change voxel grid resolution, etc.)
  float voxel_grid_origin_x = -1.5f; // Location of voxel grid origin in base frame camera coordinates
  float voxel_grid_origin_y = -1.5f;
  float voxel_grid_origin_z = 0.5f;
  float voxel_size = 0.006f;
  float trunc_margin = voxel_size * 5;
  int voxel_grid_dim_x = 500;
  int voxel_grid_dim_y = 500;
  int voxel_grid_dim_z = 500;

可以看到体素起点为(-1.5,-1.5,0.5)，每个体素的大小为0.006(立方米)，体素总数为长×宽x高(500x500x500)。 看到这里我很自然地想到这个程序的tsdf应用的一些限制，体素所能表示的环境的大小看来已经被限制，一个体素一个维度的长度为0.006m，一共有500x500x500个体素，也就是我们能map的3D环境大小不能超过(0.006^3)X500X500X500 = 27立方米，每个维度不能超过0.006X500 = 3米。这对于稍微大一点的室内环境都不够，更别说室外大场景了。我们可以增大voxel_size，但这会意味着每一个体素所表示的空间体积增大，精度降低。我们可以增大体素的个数比如500改为1000甚至更大,这对于室内场景可行。但是我们需要把这些体素拷贝到GPU中，小的GPU存量几个G，大的十几个G，体素过多，对于室外场景，如果还保持高精度，GPU的是难以储存并计算的。另外一个维度的长度被固定地死死的，超一丢丢都不行，能不能让这个尺寸能动态变化呢?这些问题放到第二讲讨论。这一讲我们只走完这个基本程序。定义完体素的长宽高后，把所有体素的tsdf,weight都初始化为0并拷贝到GPU里。

// Initialize voxel grid
...
 memset(voxel_grid_weight, 0, sizeof(float) * voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z);
...
  cudaMalloc(&gpu_depth_im, im_height * im_width * sizeof(float));
  checkCUDA(__LINE__, cudaGetLastError());

对于不太了解CUDA的朋友可以省去看这段。简要理解为，CPU和GPU作为两个设备，我们要使用GPU计算就需要把CPU的东西拷贝到GPU里面去。之后进入for循环，读取每一帧的rgbd图像以及位姿，更新tsdf

// Loop through each depth frame and integrate TSDF voxel grid
  for (int frame_idx = first_frame_idx; frame_idx < first_frame_idx + (int)num_frames; ++frame_idx){
...
}

for循环里会调用函数

Integrate <<< voxel_grid_dim_z, voxel_grid_dim_y >>>...

这个函数会在GPU里执行对应代码开头的Integrate函数，用来对tsdf累积更新

__global__
void Integrate(...){
...}

下面我们继续对integrate函数进行解读，函数的前两行

int pt_grid_z = blockIdx.x;
  int pt_grid_y = threadIdx.x;

把block和thread id赋值给了pt_grid_z和pt_grid_y。接着可以看到for循环针对x维度进行循环

for (int pt_grid_x = 0; pt_grid_x < voxel_grid_dim_x; ++pt_grid_x)

这表示作者想用GPU里的每一个线程，去处理y,z固定，仅仅有x变化的一条线上的体素，即500个体素。进入for循环后，可以看到针对一条线上的每个体素

    // Convert voxel center from grid coordinates to base frame camera coordinates
    float pt_base_x = voxel_grid_origin_x + pt_grid_x * voxel_size;
    float pt_base_y = voxel_grid_origin_y + pt_grid_y * voxel_size;
    float pt_base_z = voxel_grid_origin_z + pt_grid_z * voxel_size;
    // Convert from base frame camera coordinates to current frame camera coordinates
       float tmp_pt[3] = {0};
    tmp_pt[0] = pt_base_x - cam2base[0 * 4 + 3];
    tmp_pt[1] = pt_base_y - cam2base[1 * 4 + 3];
    tmp_pt[2] = pt_base_z - cam2base[2 * 4 + 3];
    float pt_cam_x = cam2base[0 * 4 + 0] * tmp_pt[0] + cam2base[1 * 4 + 0] * tmp_pt[1] + cam2base[2 * 4 + 0] * tmp_pt[2];
    float pt_cam_y = cam2base[0 * 4 + 1] * tmp_pt[0] + cam2base[1 * 4 + 1] * tmp_pt[1] + cam2base[2 * 4 + 1] * tmp_pt[2];
    float pt_cam_z = cam2base[0 * 4 + 2] * tmp_pt[0] + cam2base[1 * 4 + 2] * tmp_pt[1] + cam2base[2 * 4 + 2] * tmp_pt[2];

会先把它投影到base frame，这个base frame是相机的初始坐标。再根据相机的当前位姿，投影到相机的当前坐标。此时，就可以得到该体素到相机的距离pt_cam_z了。之后会再将体素投影到当前相机的二维平面，找到与之对应的pixel，获取那个pixel的深度值depth_val

    int pt_pix_x = roundf(cam_K[0 * 3 + 0] * (pt_cam_x / pt_cam_z) + cam_K[0 * 3 + 2]);
    int pt_pix_y = roundf(cam_K[1 * 3 + 1] * (pt_cam_y / pt_cam_z) + cam_K[1 * 3 + 2]);
    if (pt_pix_x < 0 || pt_pix_x >= im_width || pt_pix_y < 0 || pt_pix_y >= im_height)
      continue;

    float depth_val = depth_im[pt_pix_y * im_width + pt_pix_x];

depth_val和pt_cam_z只差即是我们之前提到的sdf

|\mathbf{p}\mathbf{x}|

了。小于某个值则忽略它(这和之前直接赋值为+-1有些不一样，但是)

if (diff <= -trunc_margin)
      continue;

之后对sdf进行截断

float dist = fmin(1.0f, diff / trunc_margin);

diff呢就是咱们的TSDF了。下面几行代码就妥妥的对应式2了

    float weight_old = voxel_grid_weight[volume_idx];
    float weight_new = weight_old + 1.0f;
    voxel_grid_weight[volume_idx] = weight_new;
    voxel_grid_TSDF[volume_idx] = (voxel_grid_TSDF[volume_idx] * weight_old + dist) / weight_new;

利用上一帧得到的tsdf的 weight和tsdf的值更新当前tsdf的值。我们可以看到，每一帧图像进来时，我们是对每一个体素都进行了更新的，计算量很大。如果场景小还好，场景如果很大，可能很多体素是不需要再进行更新的，因为他们经过投影不会投影到当前的相机平面的。至此基本的tsdf的更新就讲解完毕。那么有了这些tsdf，我们是如何把3d场景再还原出来的呢？在主函数的for循环结束后，我们会进入下面几行程序

  // Load TSDF voxel grid from GPU to CPU memory
  cudaMemcpy(voxel_grid_TSDF, gpu_voxel_grid_TSDF, voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z * sizeof(float), cudaMemcpyDeviceToHost);
  cudaMemcpy(voxel_grid_weight, gpu_voxel_grid_weight, voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z * sizeof(float), cudaMemcpyDeviceToHost);
  checkCUDA(__LINE__, cudaGetLastError());

把我们所有体素的tsdf从GPU拷贝会CPU。之后根据体素重建表面

  SaveVoxelGrid2SurfacePointCloud("tsdf.ply", voxel_grid_dim_x, voxel_grid_dim_y, voxel_grid_dim_z, 
                                  voxel_size, voxel_grid_origin_x, voxel_grid_origin_y, voxel_grid_origin_z,
                                  voxel_grid_TSDF, voxel_grid_weight, 0.2f, 0.0f);

这个函数在utils.hpp里，下面我们进入这个函数

// Compute surface points from TSDF voxel grid and save points to point cloud file
void SaveVoxelGrid2SurfacePointCloud(const std::string &file_name, int voxel_grid_dim_x, int voxel_grid_dim_y, int voxel_grid_dim_z,
                                     float voxel_size, float voxel_grid_origin_x, float voxel_grid_origin_y, float voxel_grid_origin_z,
                                     float * voxel_grid_TSDF, float * voxel_grid_weight,
                                     float tsdf_thresh, float weight_thresh) {

...
  // Create point cloud content for ply file
  for (int i = 0; i < voxel_grid_dim_x * voxel_grid_dim_y * voxel_grid_dim_z; i++) {

    // If TSDF value of voxel is less than some threshold, add voxel coordinates to point cloud
    if (std::abs(voxel_grid_TSDF[i]) < tsdf_thresh && voxel_grid_weight[i] > weight_thresh) {
}
...

}

其实截出来的两行代码说明了一切。从tsdf再重构表面的方法是 1:遍历所有的体素。 2:如果每个体素的tsdf小于某个数值(代表它足够接近一个表面)并且它的权重大于某个数值(体素的权重每次更新时加1，大于某个数值代表更新次数足够) 那么我们会把该体素对应的3d位置转换为3d点云，最后把点云画出即能得到我们重构的表面。总体来说，最基本的tsdf代码还是非常straigtforward的，但也因此暴露出来了一些的问题。根据前面的讲解我们再总结一下 1:体素的大小需要预先设定并固定。这意味着场景超出体素涵盖的范围时，将不能重建。 2:我们利用体素重构表面时，理论上需要选tsdf为0的点，这些点才在表面上。但这几乎是不能满足的，所以参考代码退而求其次，选取tsdf小于某个数值的体素。这种做法比较粗糙也会损失不少的精度。成熟的tsdf based 3d重建实际不会直接用点云常见。这两个问题我们会在介绍voxblox里说到。但其实只要你想重建的环境足够小，上面的问题都不是问题。因为你的voxel_size只要够小，tsdf也会足够精确，重建也会足够精确。但是我最近一些应用希望把tsdf应用到室内/室外较大的场景，就需要额外考虑很多东西。参考(参考链接可能需要科学上网): (1) Elastic Fusion (2) Dense Visual SLAM (3) Truncated Signed Distance Function: Experiments on Voxel Size (4) voxblox (5) A Volumetric Method for Building Complex Models from Range Images (6) https://github.com/andyzeng/tsdf-fusion

一些背景知识

您必须 登录 才能发表评论！

您必须登录才能发表评论！