拆解KinectFusion算法之TSDF

TSDF的全称是truncated signed distance function，这个算法的主要功能是为了融合(fusion)相机新观测到的深度信息或者说几何信息，当然也可以融合颜色信息甚至是其他的信息比如法向量和材质等等，总的来讲就是把时序上不断观测到的信息加权平均。

首先，要定义一个体素块的集合，这个体素就类比像素，体素是三维空间中承载一些数量值的最小单元。比如说每个体素块的尺寸选择为4mm，这里假定体素块是立方体，当然也可以是小长方体，另外假定每个坐标轴方向都是256个体素块，那么所有体素块就构成一个1.024m的大立方体。之所以选择256这样的数值是为了方便CUDA加速，一般是32的整数倍，32是一个Warp里面的线程数。常见的体素的维度有128，256，512，具体要看下显卡的显存能支持多大。

接下来我们还是从一段CPU代码来具体了解下TSDF算法的执行过程，实际上一个256x256x256的TSDF融合在CUDA加速下可能不到1ms就算完了，CPU代码写个三重的for循环大概可能是秒级算完，不做realtime应用也还可以接受。这段tsdf的代码同样是参考自Intrinsic3D，稍有改动。

for(int z = 0; z < grid_dim; z++){
    for(int y = 0; y < grid_dim; y++){
        for(int x = 0; x < grid_dim; x++){
            int idx = z * grid_dim * grid_dim + y * grid_dim + x;
            Vec3f pos_world(x * voxel_size, y * voxel_size, z * voxel_size);
            //这里的grid_pose实际上是一个平移，就是把位于第一象限的体素栅格平移到第一帧相机的坐标系下。
            //具体地是分别沿x轴和y轴负向移动一段距离，一般就选择0.5 * grid_dim * voxel_size就可以。
            //另外向z轴正向移动一点距离，这个距离具体看你重建的场景在相机z轴正向的距离，以保证大部分场景可以落到体素栅格内部即可。
            pos_world = grid_pose.topLeftCorner(3, 3) * pos_world + grid_pose.topRightCorner(3, 1);
			//这里选择第一帧相机的坐标系为世界坐标系，world_to_cam是把世界坐标系变换到当前相机世界坐标系的刚体变换。
			//这个刚体变换用ICP进行求解得到。
            Vec3f p = world_to_cam.topLeftCorner(3, 3) * pos_world + world_to_cam.topRightCorner(3, 1);
            
            if(p[2] < 0.0f)
                continue;
			//变换到当前相机坐标系下的体素块根据相机内参进行投影；
            int u = (p[0] / p[2]) * fx + cx + 0.5f;
            int v = (p[1] / p[2]) * fy + cy + 0.5f;
            
            //投出图像范围的体素块不更新
            if(u < 0 || v < 0 || u >= depth_imgae.cols || v >= depth_imgae.rows)
                continue;
            
            const float d = depth_imgae.at<float>(v, u);
            if (d <= 0.0f)
                continue;

            // compute signed distance; positive in front of the observation
            const float sdf = d - p[2];
			
			//体素块相比相机观测到的深度值更靠近相机，sdf是正的，否则就是负的
            if (sdf <= -truncation)
                continue;
            if (sdf >= truncation)
                continue;
            valid_voxel_num++;
            
            const float tsdf = sdf >= 0.0f ? std::min(truncation, sdf) : std::max(-truncation, sdf);

            float weight_update = 1.0f;
           
            // update sdf
            const float w_old = grid_weight[idx];
            const float w_new = w_old + weight_update;
            grid_tsdf[idx] = (grid_tsdf[idx] * w_old + tsdf * weight_update) / w_new;
            // update weight
            grid_weight[idx] = w_new;
        }
    }
}

TSDF就写这么多，下一节写MarchingCubes算法。