TSDF地图

TSDF地图构建

What is TSDF Map

• TSDF的主要作用是进行三维场景在计算机中的重建，著名的Kinfusion就是采用TSDF来构建空间体素的，通过求去每个体素的值，然后再使用之前提到的Marching Cube来提取表面的

• 这个地图存储形式在计算是对算力的需求比较大，因为每一次更新地图时都需要对所有地图中的体素点进行一次tsdf函数的运算

• 如果想要创建一个TSDF Map，就一定要弄清楚两个事情：

  • sdf(x)
  • tsdf(x)

• 上面这两个函数在TSDF Map构建的过程中具有至关重要的作用

为了梳理自己关于这部分的学习思路，特此撰写博客。

TSDF地图初始化

TSDF地图的创建由许多个voxel拼接而成，表现形式的话就是下面这幅图

这个图片在网上很多地方都被引用了，在这个地图上的每一个小方块就是我们说的voxel。

下面举个例子来说明整个地图的创建流程：

• 比如现实世界地图大小为$5m \times 5m \times 5m$，我们想要创建一个包含$100 \times 100 \times 100$个voxel的TSDF地图，那么就要求

  • 每一个voxel的size对应为$0.05m \times 0.05m \times 0.05m$

• 在确定好了size之后，和相机模型投影到像素平面需要内参矩阵K一样，TSDF地图原点相对真实世界原点也是存在偏移的，也就说

  • 需要设定TSDF地图中的$(0, 0, 0)$坐标体素对应真实世界坐标中的$(x_0, y_0, z_0)$

• 反过来想，那TSDF地图中一个体素点$(v_x, v_y, v_z)$对应真实世界坐标系中的那个点呢？

  • 很简单啦，其实只要把上面那个投影两个世界的原点过程反过来，再加上考虑一个voxel的size就可以(记得取整就好)

    \begin{equation} \begin{cases} v_x = int( \frac{x - x_0}{0.05m(voxel.size.x)}) \\ v_y = int(\frac{y - y_0}{0.05m(voxel.size.y)}) \\ v_z = int(\frac{z - z_0}{0.05m(voxel.size.z)}) \end{cases} \end{equation}

• 最后重要的一点！！！：比如我们要创建一个$L \times W \times H$的TSDF地图，那我们一定会拿到一个大小为$L \times W \times H \times 2$的地图，这是因为在TSFD地图中我们需要考虑两个值，分别为rgb和tsdf，就是我们要分别为这两个值的存储而创建两个$L \times W \times H$的地图。

  • 剧透一下，tsdf函数其实就是用来衡量物体到一些穿不透的墙面等表面的距离，而rgb就是用来贴图的

初步了解什么是tsdf函数

这里偷懒复制了别人的图，以后想要精致自己的Blog的话会自己在来画一张的哈哈哈

这个tsdf函数其实是有一个祖先的，叫做sdf，那么这里我就先从他的祖先说起吧！

• sdf：用来判断当前位置的voxel在障碍物前后

  • 这个函数的输出是什么呢？不妨看看我下面的推导

  • 还是举个例子吧，在TSFD地图中我们拿到了一个voxel x，现在要计算sdf(x)，应该怎么做呢？

  • 第一步：把voxel x从TSDF地图投影到真实世界中

    P^w= \begin{equation} \begin{cases} x_w=x_0 + (0.05m) \cdot v_x \\ y_w=y_0 + (0.05m) \cdot v_y \\ z_w=z_0 + (0.05m) \cdot v_z \end{cases} \end{equation}

    再转化到相机坐标系下得到

    $\frac{1}{z_c}P^c = K(RP^w + t)$

  • 第二步：假设世界坐标系与相机坐标系的变换关系$R, t$都是已知的，由于是我们拍摄，我们定义地图，相当于$P^c, P^w$对我们来说都是已知的，那么从上面的公式就可以求出这个voxel在相机坐标系的深度$z_c$。从而可以得到**sdf(x)**函数的输出

    \begin{equation} sdf(x) = Depth(P^c) - z_c \end{equation}

    这里的$Depth(P_c)$就是那个voxel在RGBD相机中的真实深度

    • 如果$sdf(x)<0$就相当于这个voxel前面是有东西挡着的，反之就是这个voxel在遮挡物和相机之间夹着。

• 回到tsdf这个进阶版函数，其实就是在**sdf(x)**上加了一个截断函数，具体函数形式为

  \begin{equation} tsdf(x) = max( -1, min(1, \frac{sdf(x)}{t}) ) \end{equation}

  看看这个东西，其实还是很懵b的，那我们直接输出反推，当$tsdf(x)=\frac{sdf(x)}{t}$的时候有

  \begin{equation} \begin{aligned} \because & -1< \frac{sdf(x)}{t} < 1 \\ \therefore & -t< s df(x) < t \end{aligned} \end{equation}

  其实这里就是说$sdf(x)$这个距离在$[-t, t]$区间内的时候，对应的**tsdf(x)**函数值就会在$[-1, 1]$这个区间

• 通过上面的推导我们可以发现，tsdf相较于sdf添加了一个截断效应，就说让这个**sdf(x)**距离在$[-t,t]$区间内时，tsdf的输出就在$[-1,1]$内

  否则，当$sdf(x) > t$的时候，tsdf就会输出1，也就是说这个voxel距离表面很远(表示voxel在障碍物前)

  ​ 当$sdf(x)<-t$的时候，tsdf就会输出-1，也还是在说这个voxel距离表面很远(表示voxel在障碍物后)

  ​ 当$sdf(x) =0$或者接近$0$的时候，tsdf就会输出0，意思很简明，voxel距离这个障碍物很近

至此，我们就已经了解了tsdf和他的祖先sdf，这对我们之后理解TSDF地图会有很大的帮助

开始建立TSFD地图

• 通常来说，对于离线构建地图，那我们的数据应该长成这个样子

\begin{equation} \begin{aligned} tsdf_{i} & =\{ tsdf(x_1) \cdots tsdf(x_n) \} \\ rgb_{i} & =\{ rgb(x_1) \cdots rgb(x_n) \} \\ \end{aligned} \end{equation}

​ 之后有这样一个处理TSDF地图的函数(或者叫算子$g(·)$)来处理上面这么一大包数据。

• 但是系统要求我们达到实时性。。。因此上面这个算子就变成$f(·)$表示的样子

  \begin{equation} \begin{aligned} & f(tsdf(t_i), tsdf(t_{i-1})) \\ & f(rgb(t_i), rgb(t_{i-1})) \end{aligned} \end{equation}

  其实大概的意思就是说，不能把当前时刻以前的数据全部抛弃掉，而是要采用一种加权的方式逐渐丢弃以前的数据。

• 来看看表达式

  \begin{equation} \begin{aligned} f(tsdf(t_i), tsdf(t_{i-1})) & = \frac {W(t-1) tsdf(t -1) + w(t)tsdf(t)}{W(t-1)+w(t)} \\ \end{aligned} \end{equation}

  其中，

  \begin{equation} \begin{aligned} W(t) & = W(t - 1)+ w(t) \\ W(0) & = 0_{L \times W \times H}\\ w(t) & = I_{L \times W \times H} \end{aligned} \end{equation}

  这里提醒一点这里的$tsdf(t)$其实变成了矩阵的形式，你懂我意思吧，就是代表所有的$tsdf$

  • 举例子来说一下吧，就是比如现在$t=1$​，现在有

    \begin{equation} \begin{aligned} W(1) & = W(0) + w(1) = I_{L \times W \times H} \\ f(tsdf(1), tsdf(0)) & = \frac {W(0) tsdf(0) + I_{L \times W \times H}tsdf(1)}{W(0)+I_{L \times W \times H}} \\ \end{aligned} \end{equation}

  • 那这个样子结束后，就相当于一个TSDF地图就建立好了，我们得到了$tsdf(1)$！！！