TSDF算法原理及源码解析

1. 先看效果

参考源码: https://github.com/andyzeng/tsdf-fusion-python

从图中可以看出，tsdf算法的重建效果还是不错的．该算法是很多牛掰3D重建算法的基础，例如:KinectFusion、InfiniTAM等.

2. 原理解析

2.1 体素栅格建立

根据环境待重建点云分布情况，确定包围所有点云的边界大小，然后再将边界内的空间根据设定的尺寸大小进行体素化，这样就将空间划分成一个个下图所示的小立方体(立方体的尺寸由用户自行定义，越小建模精度越高，越耗时)，图像来源https://blog.csdn.net/qq_30339595/article/details/86103576

2.2 体素栅格更新

定义2.1节建立的体素栅格所在的坐标系为世界坐标系，体素坐标为$({\mathbf{p}}_0^w,{\mathbf{p}}_1^w,...,{\mathbf{p}}_m^w)$，相机的位姿为$({\mathbf{T}}_{c_0}^w,{\mathbf{T}}_{c_1}^w...,{\mathbf{T}}_{c_n}^w)$，相机的内参为$\mathbf{K}$

2.2.1 投影世界坐标系下的体素栅格到图像坐标系

I i j = [ u i j v i j ] = K T c i w p j w (1) \mathbf{I}_{ij} =

$$\begin{bmatrix}\mathbf{u}_{ij} \\ \mathbf{v}_{ij}\end{bmatrix}$$ = \mathbf{K}\mathbf{T}_{c_{i}}^{w}\mathbf{p}_{j}^{w} \tag{1} Iij​=[uij​vij​​]=KTci​w​pjw​(1)
式中$i\in (0,1,....,n)$，$j\in (0,1,...,m)$．

2.2.2 更新栅格tsdf值及对应的权重

首先计算每个体素sdf值:
$$sd{f}_j=\Vert {\mathbf{t}}_{pj}^w-{\mathbf{t}}_{ci}^w\Vert -\mathbf{d}\mathbf{e}\mathbf{p}({\mathbf{I}}_{ij})\text{\hspace{1em}(2)}$$
式中${\mathbf{t}}_{pj}^w$表示第$j$个体素在世界坐标系下的位置信息，${\mathbf{t}}_{ci}^w$表示第$i$个相机位姿在世界坐标系下的位置信息，$\mathbf{d}\mathbf{e}\mathbf{p}({\mathbf{I}}_{ij})$表示第$j$个体素在第$i$个相机深度图像坐标系下的深度值．

截断每个体素的sdf值:
i f 　 s d f j > 0 　 t s d f j = m i n ( 1 , s d f j / t r u n c ) e l s e 　 t s d f j = m a x ( − 1 , s d f j / t r u n c ) (3)

$$\begin{aligned} &if　sdf_{j} > 0　 tsdf_{j} = min(1, sdf_{j}/trunc) \\ &else　tsdf_{j} = max(-1, sdf_{j}/trunc) \end{aligned}$$ \tag{3} ​if　sdfj​>0　tsdfj​=min(1,sdfj​/trunc)else　tsdfj​=max(−1,sdfj​/trunc)​(3)
式中 $trunc$ 表示截断距离，人为设定，可理解为相机深度信息的误差值，如果误差越大，建议该值设置大一些，否则会丢掉很多深度相机获取的信息．

计算每个体素的tsdf值:
$$tsd{f}_j=\frac{tsd{f}_{j-1}\cdot w_{j-1}+tsd{f}_j\cdot w_j}{w_{j-1}+w_j}\text{\hspace{1em}(4)}$$
式中初始$w_j$一般默认设置为1．

计算每个体素的权值:
$$w_j=w_{j-1}+w_j\text{\hspace{1em}(5)}$$

2.3 找等值面

通过2.2节更新完每个体素的tsdf值之后，通过marching cubes算法寻找体素中的等值面作为重构曲面，算法示意图如下:

其中每个栅格里面的值为对应体素的tsdf值.

3. 源码解析

参考源码: https://github.com/andyzeng/tsdf-fusion-python
注: 这里仅分析cpu版本源码，对于深入理解tsdf原理足够．

3.1 体素栅格建立

 def __init__(self, vol_bnds, voxel_size, use_gpu=True):
 
   # 将点云分布边界转换成numpy数组
   vol_bnds = np.asarray(vol_bnds) 
   assert vol_bnds.shape == (3, 2), "[!] `vol_bnds` should be of shape (3, 2)."

   # 定义体素体参数
   self._vol_bnds = vol_bnds # 体素体边界
   self._voxel_size = float(voxel_size) # 体素体每个立方体边长
   self._trunc_margin = 5 * self._voxel_size  # truncation on SDF # 截断距离，设置为体素边长的5倍
   self._color_const = 256 * 256 # 辅助参数，用于提取rgb值

   # 调整体素体边界确保c连续
   self._vol_dim = np.ceil((self._vol_bnds[:,1]-self._vol_bnds[:,0])/self._voxel_size).copy(order='C').astype(int) # 计算体素体每个维度方向上的栅格数量
   self._vol_bnds[:,1] = self._vol_bnds[:,0]+self._vol_dim*self._voxel_size # 根据各个维度栅格数量，计算体素体边界
   self._vol_origin = self._vol_bnds[:,0].copy(order='C').astype(np.float32) # 使体素体原点为c有序

   # 初始化保存体素体信息的容器
   self._tsdf_vol_cpu = np.ones(self._vol_dim).astype(np.float32) # 用于保存每个体素栅格的tsdf值
   self._weight_vol_cpu = np.zeros(self._vol_dim).astype(np.float32) # 用于保存每个体素栅格的权重值
   self._color_vol_cpu = np.zeros(self._vol_dim).astype(np.float32) # 用于保存每个体素栅格的颜色值(将rgb三个值压缩成一个float32值表示)
     
   # 获取每个体素栅格的坐标
   xv, yv, zv = np.meshgrid(
     range(self._vol_dim[0]),
     range(self._vol_dim[1]),
     range(self._vol_dim[2]),
     indexing='ij'
   )
   self.vox_coords = np.concatenate([
     xv.reshape(1,-1),
     yv.reshape(1,-1),
     zv.reshape(1,-1)
   ], axis=0).astype(int).T
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

3.2 体素栅格更新

def integrate(self, color_im, depth_im, cam_intr, cam_pose, obs_weight=1.):

    im_h, im_w = depth_im.shape # 获取图像尺寸

    # 将rgb三个值表示的颜色通道信息转换成一个用float32表示的单通道信息
    color_im = color_im.astype(np.float32)
    color_im = np.floor(color_im[...,2]*self._color_const + color_im[...,1]*256 + color_im[...,0])

      # 将体素栅格坐标转换成像素坐标，对应2.2节的公式(1)
      cam_pts = self.vox2world(self._vol_origin, self.vox_coords, self._voxel_size) # 体素坐标系转换到世界坐标系
      cam_pts = rigid_transform(cam_pts, np.linalg.inv(cam_pose)) # 世界坐标系转换到相机坐标系
      pix_z = cam_pts[:, 2]
      pix = self.cam2pix(cam_pts, cam_intr) # 相机坐标系转换到像素坐标系
      pix_x, pix_y = pix[:, 0], pix[:, 1]

      # 移除像素边界之外的投影点
      valid_pix = np.logical_and(pix_x >= 0,
                  np.logical_and(pix_x < im_w,
                  np.logical_and(pix_y >= 0,
                  np.logical_and(pix_y < im_h,
                  pix_z > 0))))
      depth_val = np.zeros(pix_x.shape)
      depth_val[valid_pix] = depth_im[pix_y[valid_pix], pix_x[valid_pix]]

      # 更新每个体素栅格的tsdf值及对应的权重
      depth_diff = depth_val - pix_z # 计算sdf值，对应公式(2)
      valid_pts = np.logical_and(depth_val > 0, depth_diff >= -self._trunc_margin) # 确定出有效深度值(即sdf值的值要大于负的截断值)
      dist = np.minimum(1, depth_diff / self._trunc_margin) # 计算截断值，对应公式(3)
      valid_vox_x = self.vox_coords[valid_pts, 0]
      valid_vox_y = self.vox_coords[valid_pts, 1]
      valid_vox_z = self.vox_coords[valid_pts, 2]
      w_old = self._weight_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] # 提取上个循环对应体素的权重，对应公式(4)中的w_j_1
      tsdf_vals = self._tsdf_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] # 提取上个循环对应的tsdf值，对应公式(4)中的tsdf_j_1
      valid_dist = dist[valid_pts]
      tsdf_vol_new, w_new = self.integrate_tsdf(tsdf_vals, valid_dist, w_old, obs_weight) # 计算体素新的权重和tsdf值，对应公式(4)(5)
      self._weight_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = w_new # 将新的权值和tsdf值更新到体素信息容器中
      self._tsdf_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = tsdf_vol_new

      # 更新每个体素栅格的颜色值，其实就是按旧的权重和新的权重加权更新每个体素栅格的rgb值
      old_color = self._color_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z]
      old_b = np.floor(old_color / self._color_const)
      old_g = np.floor((old_color-old_b*self._color_const)/256)
      old_r = old_color - old_b*self._color_const - old_g*256
      new_color = color_im[pix_y[valid_pts],pix_x[valid_pts]]
      new_b = np.floor(new_color / self._color_const)
      new_g = np.floor((new_color - new_b*self._color_const) /256)
      new_r = new_color - new_b*self._color_const - new_g*256
      new_b = np.minimum(255., np.round((w_old*old_b + obs_weight*new_b) / w_new))
      new_g = np.minimum(255., np.round((w_old*old_g + obs_weight*new_g) / w_new))
      new_r = np.minimum(255., np.round((w_old*old_r + obs_weight*new_r) / w_new))
      self._color_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = new_b*self._color_const + new_g*256 + new_r
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

3.3 找等值面

  def get_mesh(self):

	# 获取体素栅格的tsdf值及对应的颜色值
    tsdf_vol, color_vol = self.get_volume()

    # 直接使用scikit-image工具包中封装的Marching cubes算法接口提取等值面
    verts, faces, norms, vals = measure.marching_cubes_lewiner(tsdf_vol, level=0)
    verts_ind = np.round(verts).astype(int)
    verts = verts*self._voxel_size+self._vol_origin  # voxel grid coordinates to world coordinates

    # 为每个体素赋值颜色
    rgb_vals = color_vol[verts_ind[:,0], verts_ind[:,1], verts_ind[:,2]]
    colors_b = np.floor(rgb_vals/self._color_const)
    colors_g = np.floor((rgb_vals-colors_b*self._color_const)/256)
    colors_r = rgb_vals-colors_b*self._color_const-colors_g*256
    colors = np.floor(np.asarray([colors_r,colors_g,colors_b])).T
    colors = colors.astype(np.uint8)
    return verts, faces, norms, colors
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18