一些背景知識(shí)

TSDF的主要作用是進(jìn)行三維場(chǎng)景在計(jì)算機(jī)中的重建。目前的那些中文博客與成熟的TSDF應(yīng)用其實(shí)還有差距滑沧，故寫(xiě)此文。
視覺(jué)SLAM應(yīng)用的一個(gè)分支為Dense SLAM巍实。簡(jiǎn)單來(lái)說(shuō)就是在定位機(jī)器人的同時(shí)對(duì)周?chē)沫h(huán)境進(jìn)行(近乎)實(shí)時(shí)的3維重建滓技，比如下圖[1]

tmp_elastic.jpg

image.png

我們最容易直接想到的在計(jì)算機(jī)中進(jìn)行3維重建的就是點(diǎn)。一個(gè)場(chǎng)景可以由無(wú)數(shù)個(gè)點(diǎn)構(gòu)成點(diǎn)云书劝，參考2的Dense Visual SLAM即是如此瘫证。但是這種方法效率并不高。目前的Dense系統(tǒng)最常用的基本單元就是TSDF和Surfel(面片)庄撮。
Elastic Fusion背捌，Infinitam v3等是基于surfel的。我不會(huì)介紹surfel的相關(guān)內(nèi)容洞斯，感興趣的同學(xué)可以取查看Elastic Fusion等論文毡庆。voxblox就是基于TSDF的3維場(chǎng)景重建。
剛才我們提到直接利用點(diǎn)云進(jìn)行3維重建效率并不高烙如，原因的話(huà)舉個(gè)簡(jiǎn)單的栗子么抗。假設(shè)我們要重構(gòu)一個(gè)平面，平面上有10萬(wàn)個(gè)點(diǎn)亚铁，我們要儲(chǔ)存些點(diǎn)不僅需要相當(dāng)?shù)膬?nèi)存蝇刀，繪制的時(shí)候還很費(fèi)事，畢竟點(diǎn)這么多徘溢。然而對(duì)于這個(gè)平面吞琐，其實(shí)我們只需要知道四個(gè)頂點(diǎn)就可以了。省時(shí)又省事然爆。無(wú)論TSDF還是surfel站粟，都是以能觀測(cè)到的表面為基本進(jìn)行重建，而不是點(diǎn)云曾雕∨樱可能你說(shuō)又不是每個(gè)物體表面都是平的，是否忘了曲線(xiàn)的小段都可線(xiàn)性近似(手動(dòng)滑稽)。
這篇文章的后續(xù)切诀，會(huì)以github上最簡(jiǎn)單的開(kāi)源TSDF代碼為例講解其重構(gòu)的過(guò)程揩环，相對(duì)簡(jiǎn)單，在下篇文章幅虑，我們會(huì)追溯voxblox的代碼丰滑，來(lái)了解tsdf在比較成熟系統(tǒng)中的實(shí)際應(yīng)用。

基本的TSDF

我們把一個(gè)有限的三維空間想象成由M個(gè)小方塊堆疊而成翘单，假設(shè)每個(gè)小方塊長(zhǎng)寬高都為 cm吨枉，我們稱(chēng)他們?yōu)関oxel(體素)。這個(gè)三維空間在X,Y,Z維度上分別有個(gè)小方塊哄芜。三維空間的體素類(lèi)比二維空間照片的像素貌亭。

三維空間的體素

sdf.png

tsdf

計(jì)算并更新sdf的方法目前主要分兩大類(lèi)[4]，本文先介紹第一類(lèi)丈屹。
把位于全局坐標(biāo)系的體素根據(jù)變換矩陣轉(zhuǎn)換到當(dāng)前相機(jī)所在的坐標(biāo)系，再投影到2d的當(dāng)前圖像，獲得與之匹配的像素點(diǎn)旺垒。比較體素的深度與像素在3維坐標(biāo)下的深度點(diǎn)彩库。像素的深度值即是相機(jī)中心到該像素點(diǎn)對(duì)應(yīng)的表面的距離，而體素轉(zhuǎn)換到相機(jī)坐標(biāo)系后的深度值是體素到相機(jī)中心的距離先蒋，所以就可以得到體素與像素對(duì)應(yīng)的3d點(diǎn)到相機(jī)中心的距離差骇钦，即sdf.png里的了。
我們記某個(gè)體素的tsdf為 竞漾，其權(quán)重為眯搭，計(jì)算它的tsdf與權(quán)重的方法其實(shí)too simple (sometimes naive)。如下[5]

上面的式子中表示相機(jī)的幀數(shù)业岁，表示當(dāng)前幀數(shù)的該體素的權(quán)重鳞仙，而表示的是當(dāng)前幀數(shù)的tsdf”适保可以看到體素的權(quán)重僅僅是把之前所有幀得到的權(quán)重加起來(lái)棍好，而體素的tsdf只是把所有幀的權(quán)重乘以對(duì)應(yīng)幀得到的tsdf再除以權(quán)重和做平均而已。
實(shí)際操作中允耿，我們不可能等到相機(jī)運(yùn)行了100幀之后再做計(jì)算借笙，而是第二幀的體素由第一幀得到，第三幀的由第二幀得到较锡，如此增量計(jì)算业稼。所以實(shí)際運(yùn)用中的體素更新公式為(為了簡(jiǎn)便我省去了()，大家知道是針對(duì)單個(gè)體素即可)

式2只是寫(xiě)成了了式1的增量形式而已蚂蕴，實(shí)際是一個(gè)意思低散。其中每一幀的當(dāng)前權(quán)重很粗暴的直接設(shè)置成了1。即掂墓。相關(guān)論文也提到了雖然的選取很重要谦纱，但是大多數(shù)時(shí)候另它等于1都能得到不錯(cuò)的效果，在一些著名的應(yīng)用中比如Kinect Fusion就是如此設(shè)置的(在voxblox中進(jìn)行了一些改進(jìn)君编，我們?cè)诜治鰒oxblox代碼時(shí)會(huì)講到)跨嘉。
TSDF的基本更新方法就是如此。
相機(jī)的每一幀到來(lái)時(shí)吃嘿，都會(huì)嘗試更新每個(gè)體素的值祠乃，每個(gè)體素都可以獨(dú)立更新，所以理所當(dāng)然地應(yīng)該應(yīng)用于GPU上兑燥。
下面我們參考一段tsdf重構(gòu)的基本代碼[6]tsdf代碼亮瓷。代碼是應(yīng)用于GPU上的CUDA代碼，不了解CUDA也沒(méi)有關(guān)系降瞳，我們會(huì)稍加解釋嘱支。主代碼只有一個(gè)文件蚓胸，實(shí)現(xiàn)tsdf基本功能，非常簡(jiǎn)單除师。
在代碼的demo.cu的主函數(shù)中沛膳，首先定義了輸入文件的位置

  // 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圖像以及對(duì)應(yīng)的深度圖像，相機(jī)內(nèi)參汛聚，每一幀的位姿(TSDF的應(yīng)用比如Kinect Fusion一般會(huì)根據(jù)圖片的像素的匹配來(lái)估計(jì)位姿锹安，實(shí)現(xiàn)SLAM的功能，但這里的應(yīng)用我們只需要了解到tsdf本身怎樣使用就可以了倚舀，所以就使用已知位姿了)叹哭。
接下來(lái)設(shè)置我們一共有多少體素點(diǎn)，以及每個(gè)體素大小是多少痕貌。

  // 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;

可以看到體素起點(diǎn)為(-1.5,-1.5,0.5)风罩，每個(gè)體素的大小為0.006(立方米)，體素總數(shù)為長(zhǎng)×寬x高(500x500x500)芯侥。
看到這里我很自然地想到這個(gè)程序的tsdf應(yīng)用的一些限制泊交，
體素所能表示的環(huán)境的大小看來(lái)已經(jīng)被限制，一個(gè)體素一個(gè)維度的長(zhǎng)度為0.006m柱查，一共有500x500x500個(gè)體素廓俭，也就是我們能map的3D環(huán)境大小不能超過(guò)(0.006^3)X500X500X500 = 27立方米，每個(gè)維度不能超過(guò)0.006X500 = 3米唉工。這對(duì)于稍微大一點(diǎn)的室內(nèi)環(huán)境都不夠研乒，更別說(shuō)室外大場(chǎng)景了。
我們可以增大voxel_size淋硝，但這會(huì)意味著每一個(gè)體素所表示的空間體積增大雹熬，精度降低。我們可以增大體素的個(gè)數(shù)比如500改為1000甚至更大,這對(duì)于室內(nèi)場(chǎng)景可行谣膳。但是我們需要把這些體素拷貝到GPU中竿报，小的GPU存量幾個(gè)G，大的十幾個(gè)G继谚，體素過(guò)多烈菌，對(duì)于室外場(chǎng)景，如果還保持高精度花履，GPU的是難以?xún)?chǔ)存并計(jì)算的芽世。
另外一個(gè)維度的長(zhǎng)度被固定地死死的，超一丟丟都不行诡壁，能不能讓這個(gè)尺寸能動(dòng)態(tài)變化呢?
這些問(wèn)題放到第二講討論济瓢。這一講我們只走完這個(gè)基本程序。

定義完體素的長(zhǎng)寬高后妹卿，把所有體素的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());

對(duì)于不太了解CUDA的朋友可以省去看這段蔑鹦。簡(jiǎn)要理解為，CPU和GPU作為兩個(gè)設(shè)備宠漩，我們要使用GPU計(jì)算就需要把CPU的東西拷貝到GPU里面去举反。
之后進(jìn)入for循環(huán)懊直，讀取每一幀的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循環(huán)里會(huì)調(diào)用函數(shù)

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

這個(gè)函數(shù)會(huì)在GPU里執(zhí)行對(duì)應(yīng)代碼開(kāi)頭的Integrate函數(shù)，用來(lái)對(duì)tsdf累積更新

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

下面我們繼續(xù)對(duì)integrate函數(shù)進(jìn)行解讀室囊，函數(shù)的前兩行

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

把block和thread id賦值給了pt_grid_z和pt_grid_y雕崩。接著可以看到for循環(huán)針對(duì)x維度進(jìn)行循環(huán)

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

這表示作者想用GPU里的每一個(gè)線(xiàn)程，去處理y,z固定融撞，僅僅有x變化的一條線(xiàn)上的體素盼铁，即500個(gè)體素。進(jìn)入for循環(huán)后尝偎，可以看到針對(duì)一條線(xiàn)上的每個(gè)體素

    // 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];

會(huì)先把它投影到base frame饶火，這個(gè)base frame是相機(jī)的初始坐標(biāo)。再根據(jù)相機(jī)的當(dāng)前位姿致扯，投影到相機(jī)的當(dāng)前坐標(biāo)肤寝。此時(shí)，就可以得到該體素到相機(jī)的距離pt_cam_z了抖僵。
之后會(huì)再將體素投影到當(dāng)前相機(jī)的二維平面鲤看，找到與之對(duì)應(yīng)的pixel，獲取那個(gè)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 了耍群。小于某個(gè)值則 忽略它(這和之前直接賦值為+-1有些不一樣义桂，但是)

    if (diff <= -trunc_margin)
      continue;

之后對(duì)sdf進(jìn)行截?cái)?/p>

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

diff呢就是咱們的TSDF了。下面幾行代碼就妥妥的對(duì)應(yīng)式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的值更新當(dāng)前tsdf的值蹈垢。
我們可以看到慷吊，每一幀圖像進(jìn)來(lái)時(shí)，我們是對(duì)每一個(gè)體素都進(jìn)行了更新的曹抬，計(jì)算量很大溉瓶。如果場(chǎng)景小還好，場(chǎng)景如果很大沐祷，可能很多體素是不需要再進(jìn)行更新的嚷闭，因?yàn)樗麄兘?jīng)過(guò)投影不會(huì)投影到當(dāng)前的相機(jī)平面的。

至此基本的tsdf的更新就講解完畢赖临。那么有了這些tsdf胞锰，我們是如何把3d場(chǎng)景再還原出來(lái)的呢？
在主函數(shù)的for循環(huán)結(jié)束后兢榨，我們會(huì)進(jìn)入下面幾行程序

  // 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拷貝會(huì)CPU嗅榕。之后根據(jù)體素重建表面

  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);

這個(gè)函數(shù)在utils.hpp里顺饮，下面我們進(jìn)入這個(gè)函數(shù)

// 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) {
}
...

}

其實(shí)截出來(lái)的兩行代碼說(shuō)明了一切。
從tsdf再重構(gòu)表面的方法是
1:遍歷所有的體素凌那。
2:如果每個(gè)體素的tsdf小于某個(gè)數(shù)值(代表它足夠接近一個(gè)表面)并且它的權(quán)重大于某個(gè)數(shù)值(體素的權(quán)重每次更新時(shí)加1兼雄，大于某個(gè)數(shù)值代表更新次數(shù)足夠)
那么我們會(huì)把該體素對(duì)應(yīng)的3d位置轉(zhuǎn)換為3d點(diǎn)云，最后把點(diǎn)云畫(huà)出即能得到我們重構(gòu)的表面帽蝶。

總體來(lái)說(shuō)赦肋，最基本的tsdf代碼還是非常straigtforward的，但也因此暴露出來(lái)了一些的問(wèn)題励稳。根據(jù)前面的講解我們?cè)倏偨Y(jié)一下
1:體素的大小需要預(yù)先設(shè)定并固定佃乘。這意味著場(chǎng)景超出體素涵蓋的范圍時(shí)，將不能重建驹尼。
2:我們利用體素重構(gòu)表面時(shí)趣避，理論上需要選tsdf為0的點(diǎn)，這些點(diǎn)才在表面上新翎。但這幾乎是不能滿(mǎn)足的程帕，所以參考代碼退而求其次，選取tsdf小于某個(gè)數(shù)值的體素地啰。這種做法比較粗糙也會(huì)損失不少的精度愁拭。成熟的tsdf based 3d重建實(shí)際不會(huì)直接用點(diǎn)云常見(jiàn)。
這兩個(gè)問(wèn)題我們會(huì)在介紹voxblox里說(shuō)到髓绽。

但其實(shí)只要你想重建的環(huán)境足夠小敛苇，上面的問(wèn)題都不是問(wèn)題。因?yàn)槟愕膙oxel_size只要夠小顺呕，tsdf也會(huì)足夠精確枫攀，重建也會(huì)足夠精確。但是我最近一些應(yīng)用希望把tsdf應(yīng)用到室內(nèi)/室外較大的場(chǎng)景株茶，就需要額外考慮很多東西来涨。

參考(參考鏈接可能需要科學(xué)上網(wǎng)):
(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