这篇博客主要分析这个老哥做的Python实现的基于TSDF地图的三维重建,点击它获取Github链接。三维重建的效果图如下所示。这篇博客主要分析两件事情,一是TSDF地图的原理和融合理论,二是TSDF实现的代码。如果后续有时间,我再去做一个mini版本的三维重建小工程。有关Kinect Fusion相关的知识可以参考这篇知乎笔记。
图1:三维重建效果图
TSDF地图是由一堆体素构成的,每个体素内包含两个变量,一是tsdf值(用于生成重建表面),二是RGB信息(给重建表面贴上彩色纹理)。TSDF地图与实际场景的物理坐标系是可转换的。这是这一节讲解的基本信息。tsdf值和rgb值的计算会在下一节讨论。
相信大家即便是没有玩过“我的世界”,也肯定听说过这款游戏。在这款游戏中,世界是由三维小格子组成的。TSDF地图的原理就和它很像。TSDF是截断符号函数(Truncated Signed Distance Function)的缩写(初学不必去理会这些英文字眼)。一个三维的TSDF地图就是由个三维小方块组成。我们称三维小方块为体素(Voxel)。列举一个例子。给定一个尺寸为的TSDF地图,该地图是有尺寸为的体素构成。那么可以计算出该TSDF地图覆盖一个的真实三维场景。这个地图中有个体素。每一个体素记录着tsdf值和RGB颜色信息(tsdf值范围是,在第三节做介绍)。GPU需要大概2KB存储单个体素的所有信息。因此GPU需要大概2GB左右的显存空间存放这个TSDF地图。从这些计算过程可见,TSDF占据非常大的显存空间,它适合小场景下的三维重建,比如室内环境。
接下来定量地讨论一下TSDF地图下体素坐标系和物理坐标系的变换问题。我们知道TSDF地图是一个大立方块,里面是由很多小立方块(体素)构成的。与此同时,TSDF地图对应着实际的立方块空间。记TSDF地图中处体素对应实际场景坐标为点。那么场景中坐标为的点,对应TSDF地图中处的体素。因为体素坐标系是整数,所以实际点坐标在转化为TSDF地图下体素坐标系的时候需要取整。
TSDF地图由体素构成,每一个体素包含一个的tsdf值,以及包含一个rgb值。因此初始化一个TSDF地图,相当于初始化两个范围内的三维张量,分别存放tsdf值和rgb值。使用numpy就能轻松搞定。把TSDF地图从CPU存储中移动到GPU中需要两步,第一步是调用cuda.mem_alloc在GPU中提前开辟一块内存空间,第二步是调用cuda.memcpy_htod,用这块内存空间存储TSDF地图数据。htod表示host to device,即从CPU存数据到GPU。如果是反过来的话,就使用cuda.memcpy_dtoh。注意,在初始化的时候,TSDF地图内所有体素的tsdf值被填充为1。这个原因会在下一节讲解。
# self._vol_dim 表示 TSDF 地图的尺寸,[L,W,H]
# 使用 np.ones 后,_tsdf_vol_cpu 是一个 L*W*H 的张量,表示 TSDF 地图,
# 填充值为 tsdf值,在初始化阶段,默认为 1
self._tsdf_vol_cpu = np.ones(self._vol_dim).astype(np.float32)
# 对应颜色信息,在初始化阶段,默认为 0
self._color_vol_cpu = np.zeros(self._vol_dim).astype(np.float32)
# 简而言之,TSDF 地图就由 tsdf 值张量和颜色张量组成。
# 把 TSDF 地图存放在 GPU 中
self._tsdf_vol_gpu = cuda.mem_alloc(self._tsdf_vol_cpu.nbytes)
cuda.memcpy_htod(self._tsdf_vol_gpu,self._tsdf_vol_cpu)
self._color_vol_gpu = cuda.mem_alloc(self._color_vol_cpu.nbytes)
cuda.memcpy_htod(self._color_vol_gpu,self._color_vol_cpu)
这一节讨论这样一件事情,在TSDF地图中随便选出一个体素,怎样去计算它的tsdf值和rgb值呢?计算它们的已知条件是需要一张深度图。首先去讲解tsdf值的计算。tsdf值的前身是sdf值。咱们先看下面的经典讲解图。白灰色的小方格表示TSDF地图中的各个体素。蓝色的三角形表示相机的视场范围。图中间有一条绿色的截线,表示一个物体的截面。深度相机的深度图可以显示物体截面的深度信息。我们要去计算体素的sdf值和tsdf值,即和。
图2:讲解tsdf值和sdf值的示意图
首先,需要计算体素在物理坐标下的位置。记体素在TSDF地图上的坐标是。那么该体素在物理世界坐标系下的位置是。然后需要计算体素在相机坐标系下的位置。设相机相对于物理坐标系下的位姿是和。那么体素在相机坐标系下的位置是。根据相机成像模型,。表示相机的内参数矩阵。表示体素投影在相机成像平面下的像素坐标。表示体素相对于相机的深度。
沿着相机的光心和体素做一条直线(图中显示为深蓝色的粗线),这条线会与物体的截面有一个交点,这个交点记为点。点的深度记为。在实际计算中,的获取真的要做直线,计算交点有这么麻烦吗?完全不是,那只是个示意图讲解而已。记当前的深度图为。在实际计算中。那么体素的sdf值就可以计算出来了:
可以发现:表示体素处于相机和物体表面之间;表示体素处于物体表面之后。根据体素的sdf值,可以计算它的的tsdf值:
tsdf的计算表达式看上去有些古怪。首先是要做一个除法操作,即,这就是截断(Truncated)的含义。当时,。当或者时,或者。其实这个表达式是有物理意义的。可以看作是体素和截面对应点深度差值的阈值。当体素距离截面对应点非常远的时候,它的tsdf值等于正负一。当体素距离截面对应点比较近的时候,它的tsdf值之间,是有意义的。用更为通俗的话说,当体素离表面非常近的时候,它的tsdf值接近于零;当体素离表面非常远的时候,它的tsdf值趋于正一或者负一。这正好对应于图2。
还记不记得,在初始化的时候,我们设所有体素的tsdf值为1,相当于这个TSDF地图中没有任何表面。所以这个初始化是有意义的。
最后再去讨论体素的rgb值计算方法。其实很简单,记深度图对应的RGB图像为。那么。实际代码实现的时候会稍稍跟它不太一样,这个放在后面去讲。对每一个体素,都是需要重复进行上述的过程,所以代码实现利用并行计算GPU会非常高效。这一节的计算代码我会放在第四节去讲。
在第三节,咱们讨论“给单张深度图和RGB图像,计算各个体素的tsdf值和rgb值”的过程。这个过程用数学语言描述就是:。表示在时刻下相机相对于物理坐标系下的位姿。由两个三维张量组成,分别是和。整型三维向量是TSDF地图中的体素索引。算子就是第三节的计算过程。那么对于一个完整的RGB-D图序列,怎样去处理呢?势必希望设计一个算子,使得,把各个时刻计算的tsdf值做个融合。
其实是离线三维重建的表述,因为它把之前时刻的结果一古脑地塞进中,并不适用于实时三维重建。实时三维重建的数学表达式应该是这样的:
写成一般表达式就是:
在Kinect Fusion甚至是更早期的三维重建算法中,把设计为一种加权平均的形式:
注意和都是三维张量,表示的都是TSDF中每一个体素的权值。上述公式中的操作符"","",“/”均表示张量间点对点的相乘相加和相除。初始化为零值张量。则是常数值张量。为什么会设计为常数值张量呢?细品一下,这是有一种时间累计效应的感觉。累计了时刻所有的三维重建结果,它的重要性自然比高出很多。因为也是三维张量,所以也要放在一开始做个初始化哈。
self._weight_vol_cpu = np.zeros(self._vol_dim).astype(np.float32)
self._weight_vol_gpu = cuda.mem_alloc(self._weight_vol_cpu.nbytes)
cuda.memcpy_htod(self._weight_vol_gpu,self._weight_vol_cpu)
此外,从这个公式看出,体素的tsdf值的计算和tsdf值的融合可以放在一块进行。下面会缓缓给出TSDF融合的代码,在这个Python实现的TSDF的三维重建中,这段代码是用Cuda写的,通过PyCuda的方式编译这段Cuda代码,让Python主程序能够调用这个Cuda API。
对了,对于RGB信息的融合,也是采取了类似的形式:
上述公式中的操作符"","",“/”同样表示张量间点对点的相乘相加和相除。
在体素的tsdf值的计算和tsdf值的融合的过程中,需要遍历TSDF中每一个体素。对于一个的TSDF地图,就自然要遍历数量个体素,数量级大概在GB级或者说是百万级。GPU虽然是多线程并行运算的利器,但是也不可能同时开百万级的线程。GPU一次性可以开几十万个线程,数量记为。简单地讲,它需要重复地运作次(流水线操作可以加速这个过程,我是指微机原理中处理指令的那个流水线),可以遍历完全部的体素。这里不去细述GPU的更深的原理(我自己也不是太懂)。总之,GPU需要用到线程块Block和线程网格Grid完成对线程的调度。
对GPU来说,Block和Grid的选取非常重要。只有选取对了,GPU才能遍历完TSDF中全部的体素,于此同时不会浪费太多的多余线程。先看看Python中调用tsdf融合的_cuda_integrate的代码。这里有关PyCuda的知识不去做介绍。咱们比较关注代码中block和grid的设定。
self._cuda_integrate = self._cuda_src_mod.get_function("integrate")
self._cuda_integrate(self._tsdf_vol_gpu,
self._weight_vol_gpu,
self._color_vol_gpu,
cuda.InOut(self._vol_dim.astype(np.float32)),
cuda.InOut(self._vol_origin.astype(np.float32)),
cuda.InOut(cam_intr.reshape(-1).astype(np.float32)),
cuda.InOut(cam_pose.reshape(-1).astype(np.float32)),
cuda.InOut(np.asarray([
gpu_loop_idx,
self._voxel_size,
im_h,
im_w,
self._trunc_margin,
obs_weight
], np.float32)),
cuda.InOut(color_im.reshape(-1).astype(np.float32)),
cuda.InOut(depth_im.reshape(-1).astype(np.float32)),
block=(self._max_gpu_threads_per_block,1,1),
grid=(
int(self._max_gpu_grid_dim[0]),
int(self._max_gpu_grid_dim[1]),
int(self._max_gpu_grid_dim[2]),
)
上述代码中,block=(self._max_gpu_threads_per_block,1,1)定义了一个线程块。block是一个三元组,其中block.x=_max_gpu_threads_per_block,block.y=1,block.z=1。这个线程块可以一次性地打开block.x*block.y*block.z个线程。grid表示线程网格,它也是一个三元组。线程网格表示它来调度grid.x*grid.y*grid.z个线程块。我们可以看到一个上下级的关系:线程网格调度线程块,线程块则控制线程。为了让GPU能够遍历到所有的体素,线程网格和线程块的设计必须满足下面的式子:
说句题外话。细心一点可以注意到,block和grid的设定就是一个排列组合的问题,似乎block和grid的选型方案有很多组。但是在实际应用中,block和grid设置地过高和过低都会对并行计算产生影响,甚至GPU会罢工,这个原因可以追溯到GPU的硬件设计上。有兴趣可以看看GPU高性能编程相关书籍。在这里,我们就随便的设计一下就行啦,只要满足上面的式子即可。所以block和grid的设定代码如下所示:
self._max_gpu_threads_per_block = gpu_dev.MAX_THREADS_PER_BLOCK
n_blocks = int(np.ceil(float(np.prod(self._vol_dim))/float(self._max_gpu_threads_per_block)))
grid_dim_x = min(gpu_dev.MAX_GRID_DIM_X,int(np.floor(np.cbrt(n_blocks))))
grid_dim_y = min(gpu_dev.MAX_GRID_DIM_Y,int(np.floor(np.sqrt(n_blocks/grid_dim_x))))
grid_dim_z = min(gpu_dev.MAX_GRID_DIM_Z,int(np.ceil(float(n_blocks)/float(grid_dim_x*grid_dim_y))))
self._max_gpu_grid_dim = np.array([grid_dim_x,grid_dim_y,grid_dim_z]).astype(int)
假设一个极端地情况,grid_dim_x到达MAX_GRID_DIM_X,grid_dim_y到达MAX_GRID_DIM_Y,grid_dim_z到达MAX_GRID_DIM_Z,但是线程数还是小于体素的数量,这该怎么办呢?即如下的数学表达式:
一个简单的办法就是多次遍历,我遍历个次就够啦!
# 一般情况下,如果线程够用,_n_gpu_loops 就等于 1
self._n_gpu_loops = int(np.ceil(float(np.prod(self._vol_dim))/float(np.prod(self._max_gpu_grid_dim)*self._max_gpu_threads_per_block)))
前文已经讲了GPU中的上下级的关系:线程网格调度线程块,线程块则控制线程。直接上代码,就不难理解了,可以这样得到体素的索引voxel_idx:
int gpu_loop_idx = (int) other_params[0]; # gpu_loop_idx = 1
int max_threads_per_block = blockDim.x;
int block_idx = blockIdx.z*gridDim.y*gridDim.x+blockIdx.y*gridDim.x+blockIdx.x;
int voxel_idx = gpu_loop_idx*gridDim.x*gridDim.y*gridDim.z*max_threads_per_block+block_idx*max_threads_per_block+threadIdx.x;
然而体素的三维张量在GPU中存储是一维张量。所以要想得到该体素的体素坐标,需要如下的操作:
// Get voxel grid coordinates (note: be careful when casting)
float voxel_x = floorf(((float)voxel_idx)/((float)(vol_dim_y*vol_dim_z)));
float voxel_y = floorf(((float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z))/((float)vol_dim_z));
float voxel_z = (float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z-((int)voxel_y)*vol_dim_z);
理解了上述两个编程细节,剩下的过程就很好理解了:
# Cuda kernel function (C++)
self._cuda_src_mod = SourceModule("""
__global__ void integrate(float * tsdf_vol,
float * weight_vol,
float * color_vol,
float * vol_dim,
float * vol_origin,
float * cam_intr,
float * cam_pose,
float * other_params,
float * color_im,
float * depth_im) {
// Get voxel index
int gpu_loop_idx = (int) other_params[0];
int max_threads_per_block = blockDim.x;
int block_idx = blockIdx.z*gridDim.y*gridDim.x+blockIdx.y*gridDim.x+blockIdx.x;
int voxel_idx = gpu_loop_idx*gridDim.x*gridDim.y*gridDim.z*max_threads_per_block+block_idx*max_threads_per_block+threadIdx.x;
int vol_dim_x = (int) vol_dim[0];
int vol_dim_y = (int) vol_dim[1];
int vol_dim_z = (int) vol_dim[2];
if (voxel_idx > vol_dim_x*vol_dim_y*vol_dim_z)
return;
// Get voxel grid coordinates (note: be careful when casting)
float voxel_x = floorf(((float)voxel_idx)/((float)(vol_dim_y*vol_dim_z)));
float voxel_y = floorf(((float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z))/((float)vol_dim_z));
float voxel_z = (float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z-((int)voxel_y)*vol_dim_z);
// Voxel grid coordinates to world coordinates
float voxel_size = other_params[1];
float pt_x = vol_origin[0]+voxel_x*voxel_size;
float pt_y = vol_origin[1]+voxel_y*voxel_size;
float pt_z = vol_origin[2]+voxel_z*voxel_size;
// World coordinates to camera coordinates
float tmp_pt_x = pt_x-cam_pose[0*4+3];
float tmp_pt_y = pt_y-cam_pose[1*4+3];
float tmp_pt_z = pt_z-cam_pose[2*4+3];
float cam_pt_x = cam_pose[0*4+0]*tmp_pt_x+cam_pose[1*4+0]*tmp_pt_y+cam_pose[2*4+0]*tmp_pt_z;
float cam_pt_y = cam_pose[0*4+1]*tmp_pt_x+cam_pose[1*4+1]*tmp_pt_y+cam_pose[2*4+1]*tmp_pt_z;
float cam_pt_z = cam_pose[0*4+2]*tmp_pt_x+cam_pose[1*4+2]*tmp_pt_y+cam_pose[2*4+2]*tmp_pt_z;
// Camera coordinates to image pixels
int pixel_x = (int) roundf(cam_intr[0*3+0]*(cam_pt_x/cam_pt_z)+cam_intr[0*3+2]);
int pixel_y = (int) roundf(cam_intr[1*3+1]*(cam_pt_y/cam_pt_z)+cam_intr[1*3+2]);
// Skip if outside view frustum
int im_h = (int) other_params[2];
int im_w = (int) other_params[3];
if (pixel_x < 0 || pixel_x >= im_w || pixel_y < 0 || pixel_y >= im_h || cam_pt_z<0)
return;
// Skip invalid depth
float depth_value = depth_im[pixel_y*im_w+pixel_x];
if (depth_value == 0)
return;
// Integrate TSDF
float trunc_margin = other_params[4];
float depth_diff = depth_value-cam_pt_z;
if (depth_diff < -trunc_margin)
return;
float dist = fmin(1.0f,depth_diff/trunc_margin);
float w_old = weight_vol[voxel_idx];
float obs_weight = other_params[5];
float w_new = w_old + obs_weight;
weight_vol[voxel_idx] = w_new;
tsdf_vol[voxel_idx] = (tsdf_vol[voxel_idx]*w_old+obs_weight*dist)/w_new;
// Integrate color
float old_color = color_vol[voxel_idx];
float old_b = floorf(old_color/(256*256));
float old_g = floorf((old_color-old_b*256*256)/256);
float old_r = old_color-old_b*256*256-old_g*256;
float new_color = color_im[pixel_y*im_w+pixel_x];
float new_b = floorf(new_color/(256*256));
float new_g = floorf((new_color-new_b*256*256)/256);
float new_r = new_color-new_b*256*256-new_g*256;
new_b = fmin(roundf((old_b*w_old+obs_weight*new_b)/w_new),255.0f);
new_g = fmin(roundf((old_g*w_old+obs_weight*new_g)/w_new),255.0f);
new_r = fmin(roundf((old_r*w_old+obs_weight*new_r)/w_new),255.0f);
color_vol[voxel_idx] = new_b*256*256+new_g*256+new_r;
}""")
需要用到ICP去计算,在这个Github开源项目中没有去写,而是利用了数据集的真值。不管怎样,这个小哥写的TSDF地图融合代码非常清楚,值得一读 😃