截断符号距离场TSDF

1. 算法简介

截断符号距离函数(Truncated Signed Distance Function, TSDF)是一种用于三维重建的隐式表面表示方法，广泛应用于机器人导航、SLAM 和三维建模等领域。TSDF 核心思想是通过体素(Voxel)网格存储每个点到物体表面的符号距离，并通过截断和多帧融合提升重建精度。
TSDF 中的 SDF 表示符号距离函数(Signed Distance Function)。对于三维空间中的每个点（体素），SDF 表示该点到最近物体表面的最短距离，规定如下：
1）距离为正数：点位于物体表面前方
2）距离为负数：点位于物体表面后方
3）距离为零值：点正好位于物体表面
TSDF 中的 T 表示截断处理(Truncated)。TSDF 仅保留距离物体表面一定范围内的体素距离值（如体素尺寸的 1~2 倍），超出范围的值被截断为固定值（如 1 或 -1 ）。这样做的目的是：减少计算和存储开销，同时忽略远离表面的冗余信息。
TSDF 的流程如下：

如下图所示，$d_v$ 表示体素在相机坐标系下的深度（即坐标 $z$ 的值），而 $d_s$ 是体素根据相机内参在像素坐标系上投影得到的点的深度值（依赖传感器获取）。假设 $d=d_s-d_v$ 表示上述两个距离的差，当 $d>0$ 时，说明体素在表面的前方，当 $d<0$ 时，说明体素在表面的后方，并且 $d\in [-1,1]$。
值得注意的是：这里的表面指的是深度传感器（如 RGB-D 相机）获取的深度图像中的每个有效像素点对应的三维空间点。事实上，如果深度图像中某一个像素点对应的是空位置，而不是具体的某一个物体的话，该像素点的深度值为最大深度 65535（$2^{16}-1$），这一类像素点定义为无效像素点，在 tsdf-fusion-python 代码中是这样处理的：

depth_im /= 1000.
depth_im[depth_im == 65.535] = 0

直接将无效像素点的深度置为 0，在后续计算中会被忽略不计。

实际效果图如下：

2. 计算 TSDF 的两种方式

计算 TSDF 的两种方式为：射线投射(Raycasting) 和投影映射(Projection Mapping)。

2.1. 射线投射

Raycasting 从相机的光学中心向每个观测到的点发射一条射线，并沿着这条射线更新从观测点中心到截断距离(Truncation Distance, $\delta$)后方的所有体素。具体操作为：

• 对于每个观测点，从相机中心向该点发射一条光线
• 沿着这条光线，从观测点的中心开始，向后（即远离相机方向）更新所有体素，直到达到预设的截断距离 $\delta$

这里的观测到的点就是前面所说的深度传感器（如 RGB-D 相机）获取的深度图像中的每个有效像素点对应的三维空间点。
Raycasting 的特点是：
1）精度高：通过逐点处理，能够精确捕捉观测点附近的几何细节；
2）计算成本高：需要遍历每个观测点的光线路径上的所有体素，计算复杂度较高。
适用场景：当需要高精度的体素地图（例如小体素尺寸）时，Raycasting 是更优选择，但会牺牲计算效率。
现在对射线投射进一步解释：
1）获取观测到的点对应的三维点
假设一个 RGB-D 相机捕获了一张深度图像，其中某个像素的深度值为 $d$，则该像素对应的观测到的点为：
$$(x,y,z)=(\frac{(u-c_x)d}{f_x},\frac{(v-c_y)d}{f_y},d)$$其中：

• $(u,v)$ 是像素坐标
• $(c_x,c_y)$ 是相机中心坐标
• $f_x$，$f_y$ 是相机焦距
• $d$ 是该像素的深度值

2）射线生成
对于每个观测到的点，Raycasting 会生成一条从相机光学中心出发、指向该点的射线，公式如下：
$$Ray=Origin+t\cdot \vec{d}$$其中：

• Origin 是相机光学中心
• $\vec{d}$ 是从相机中心指向观测到的点的方向向量
• $t$ 是射线的参数（从 0 到 $\delta$）

3）体素更新
沿着射线路径，从观测到的点向后（远离相机方向）更新所有体素，直到达到截断距离 $\delta$。

2.2. 投影映射

Projection Mapping 将视野内的体素投影到深度图像中，并通过比较体素中心与图像中对应像素的深度值，计算距离差异来更新体素。tsdf-fusion-python 代码实现采用的就是投影映射。
Projection Mapping 的特点：
1）速度快：直接通过投影和像素比较进行更新，计算效率显著高于 Raycasting；
2）走样效应(Aliasing Artifacts)：较大的体素尺寸会导致投影时每个体素覆盖多个像素，而深度图像的分辨率有限，无法精确匹配体素中心与像素深度值。在大体素尺寸下，地图会出现明显的锯齿状边缘或模糊现象（Aliasing），细节丢失。
适用场景：在需要快速构建地图的实时应用（例如 MAV 的在线路径规划）中，Projection Mapping 是更优选择，但需要牺牲一定的精度。

2.3. 射线投射和投影映射对比

3. 代码解读

选择代码库 tsdf-fusion-python 进行解读，链接：https://github.com/andyzeng/tsdf-fusion-python?tab=readme-ov-file。代码支持 CPU 和 GPU 两种运行方式，代码解读只针对 CPU 部分。

3.1. get_view_frustum 函数

代码实现如下：

def get_view_frustum(depth_im, cam_intr, cam_pose):
    """ Get corners of 3D camera view frustum of depth image """
    im_h = depth_im.shape[0]
    im_w = depth_im.shape[1]
    max_depth = np.max(depth_im)  # 深度图像中最大的深度值，即视锥体的远裁剪面距离

    # 构建视锥体的五个顶点，其中 cx = cam_intr[0, 2], cy = cam_intr[1, 2], fx = cam_intr[0, 0], fy = cam_intr[1, 1]
    # x = (u - cx) · z / fx, y = (v - cy) · z / fy
    view_frust_pts = np.array([
        # 坐标 x
        (np.array([0, 0, 0, im_w, im_w]) - cam_intr[0, 2]) * np.array([0, max_depth, max_depth, max_depth, max_depth]) /
        cam_intr[0, 0],

        # 坐标 y
        (np.array([0, 0, im_h, 0, im_h]) - cam_intr[1, 2]) * np.array([0, max_depth, max_depth, max_depth, max_depth]) /
        cam_intr[1, 1],

        # 深度 z
        np.array([0, max_depth, max_depth, max_depth, max_depth])
    ])
    view_frust_pts = rigid_transform(view_frust_pts.T, cam_pose).T
    return view_frust_pts

上述代码获取在相机坐标系下五个点的坐标，分别是$(0,0,0)，(0,0,z_{max})，(0,h,z_{max}),(w,0,z_{max})，(w,h,z_{max})$，其中 $w$，$h$ 和 $z_{max}$ 分别表示图像的宽、高和最大深度。然后，通过 rigid_transform 将相机坐标系下的点转换到世界坐标系下。
这一步的目的是为了计算所有三维点云在 $X$，$Y$ 和 $Z$ 上的最小值和最大值，代码如下：

# 坐标 x、y 和 z 的最小值和最大值
vol_bnds[:, 0] = np.minimum(vol_bnds[:, 0], np.amin(view_frust_pts, axis=1))
vol_bnds[:, 1] = np.maximum(vol_bnds[:, 1], np.amax(view_frust_pts, axis=1))

3.2. 颜色存储方式

代码采取的 RGB 图像编码是：将三个 8 位通道(R、G、B)按低字节 → 中字节 → 高字节的顺序，组合成一个 24 位(3 × 8)无符号整数。
这样做的优势主要有两点：
1）紧凑存储：用单一 32 位（或更大）整数即可存储一个像素的完整颜色信息，无需 3 个独立变量，便于在内存中连续排列和快速读写。
2）位运算高效：提取某一通道时，只需按位与（&）或移位（>>）即可得到对应的 8 位数值，速度极快。
将三通道 RGB 图像编码成单通道图像的代码如下：

# 将 RGB 图像 color_im 压缩为单通道整数，便于存储. color = R + G · 256 + B · 256^2
color_im = np.floor(color_im[..., 2] * 256^2 + color_im[..., 1] * 256 + color_im[..., 0])

编码后的颜色恢复 RGB 三个通道的代码如下：

R =  color & 0xFF = color % 256  
G = (color >> 8) & 0xFF = (color / 256) % 256  
B = (color >> 16)& 0xFF = (color / 256^2) % 256  

3.3. TSDFVolume 类

3.3.1. 初始化函数

vol_bnds = np.asarray(vol_bnds)  # 一个形状为 (3, 2) 的数组, 表示体积在 x、y、z 三个维度上的边界(最小值和最大值)
self._vol_bnds = vol_bnds
self._voxel_size = float(voxel_size)       # 体素的边长（单位: 米），决定了体素的分辨率
self._trunc_margin = 5 * self._voxel_size  # 截断边距，用于限制 TSDF 中有效距离的范围(例如只考虑距离相机 5 倍体素大小内的点)
self._color_const = 256 * 256  # 使用 256^2 作为基数，将 RGB 颜色压缩为一个整数（例如 R + G*256 + B*256^2），便于存储和解码

# 计算每个维度的体素数量, ceil((max - min) / voxel_size), 确保边界完全覆盖
self._vol_dim = np.ceil((self._vol_bnds[:, 1] - self._vol_bnds[:, 0]) / self._voxel_size).copy(order='C').astype(int)
# 调整右边界: 确保体积边界与体素网格严格对齐。例如：如果原始边界是 [0, 1.2]，体素大小为 0.5，则右边界会被修正为 0 + 4 * 0.5 = 2.0
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)

self._tsdf_vol_cpu = np.ones(self._vol_dim).astype(np.float32)     # 存储 TSDF 值的 CPU 数组
self._weight_vol_cpu = np.zeros(self._vol_dim).astype(np.float32)  # 存储每个体素的权重（用于计算加权平均）
self._color_vol_cpu = np.zeros(self._vol_dim).astype(np.float32)   # 存储颜色信息的 CPU 数组

# 生成三个 3D 数组 xv, yv, zv, 表示每个体素在 x、y、z 维度的索引
xv, yv, zv = np.meshgrid(
    range(self._vol_dim[0]),
    range(self._vol_dim[1]),
    range(self._vol_dim[2]),
    indexing='ij'
)
# 得到所有体素的 xyz 索引 (N, 3)
self.vox_coords = np.concatenate([
    xv.reshape(1, -1),
    yv.reshape(1, -1),
    zv.reshape(1, -1)
], axis=0).astype(int).T

3.3.2. integrate 函数

im_h, im_w = depth_im.shape
color_im = color_im.astype(np.float32)  # RGB
# 将 RGB 图像压缩为单通道整数，便于存储. color = R + G · 256 + B · 256^2
color_im = np.floor(color_im[..., 2] * self._color_const + color_im[..., 1] * 256 + color_im[..., 0])

# 将体素坐标转换为世界坐标系下的点, x = origin + voxel_coords * voxel_size
cam_pts = self.vox2world(self._vol_origin, self.vox_coords, self._voxel_size)
# 将世界坐标系下的点转换到相机坐标系下, cam_pose 为相机坐标系到世界坐标系的变换矩阵（相机位姿）
cam_pts = rigid_transform(cam_pts, np.linalg.inv(cam_pose))

pix_z = cam_pts[:, 2]
# 使用相机内参矩阵 cam_intr 将 3D 点投影到 2D 像素坐标
pix = self.cam2pix(cam_pts, cam_intr)
pix_x, pix_y = pix[:, 0], pix[:, 1]

# 只保留位于图像范围内的像素, 0 ≤ u < im_w and 0 ≤ v < im_h and z > 0（排除无效深度）
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))))

# 将有效像素的深度值提取出来，与相机坐标系下的 z 值比较
depth_val = np.zeros(pix_x.shape)
depth_val[valid_pix] = depth_im[pix_y[valid_pix], pix_x[valid_pix]]
depth_diff = depth_val - pix_z  # 深度差值 = 图像深度 - 相机坐标系下的 z 值
# depth_val > 0: 深度有效（深度值小于 0，表示体素在传感器后方）. depth_diff ≥ -trunc_margin: 避免过大的负值（防止噪声干扰）
# 在相机坐标系下，深度差值超过截断边距的体素会被忽略，避免远距离噪声干扰
valid_pts = np.logical_and(depth_val > 0, depth_diff >= -self._trunc_margin)
dist = np.minimum(1, depth_diff / self._trunc_margin)  # 归一化距离, 取值范围 [-1, 1]

# 更新 TSDF 值
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]
tsdf_vals = self._tsdf_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z]
valid_dist = dist[valid_pts]
tsdf_vol_new, w_new = self.integrate_tsdf(tsdf_vals, valid_dist, w_old, obs_weight)  # obs_weight 默认为 1.
self._weight_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = w_new
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

更新 TSDF 的代码如下：

def integrate_tsdf(tsdf_vol, dist, w_old, obs_weight):
    """ Integrate the TSDF volume. """
    tsdf_vol_int = np.empty_like(tsdf_vol, dtype=np.float32)
    w_new = np.empty_like(w_old, dtype=np.float32)
    for i in prange(len(tsdf_vol)):
        w_new[i] = w_old[i] + obs_weight
        tsdf_vol_int[i] = (w_old[i] * tsdf_vol[i] + obs_weight * dist[i]) / w_new[i]
    return tsdf_vol_int, w_new

更新 TSDF 的公式如下：
$$\begin{array}{rl}{D}_{i+1}& =\frac{W_i{D}_i+{\omega }_{i+1}{d}_{i+1}}{W_i+{\omega }_{i+1}}\\ W_{i+1}& =W_i+{\omega }_{i+1}\end{array}$$其中，$i$ 表示第 $i$ 帧图像，${\omega }_i$ 表示第 $i$ 帧图像中体素的权重，$d_i$ 表示第 $i$ 帧图像中体素的 TSDF 值。实际中可以假设 ${\omega }_i=1$（代码中也是这么做的）。
颜色的更新也是采用加权融合的方式，如代码所示。

3.4. Marching Cubes 算法

Marching Cubes(MC) 算法的核心目标是从三维离散数据场中提取等值面(Isosurface)。等值面是三维空间中所有满足 $f(x,y,z)=c$ 的点的集合（其中 $c$ 是用户设定的阈值，论文中将 $c$ 取值为 $0$）。Marching Cubes 算法通过将三维数据场离散化为体素，并逐个处理体素中的等值面提取，实现了高效的三维表面重建。
算法步骤如下：

• 数据场划分：三维数据场划分为许多小立方体（称为体素）。每个体素由 8 个顶点组成，每个顶点的值代表该位置的标量场值。
• 顶点分类：对每个体素的 8 个顶点，根据其标量值与等值面阈值 $c$ 的关系进行分类：
  高于等值面：标记为 1（实点）
  低于等值面：标记为 0（虚点）
  这样，每个体素的 8 个顶点会形成一个 8 位的二进制状态码（共 $2^8=256$ 种可能情况）。
• 查找交点：根据状态码确定哪些边与等值面相交。对于相交边，使用线性插值计算交点坐标，计算方式如下：
  $$交点=\frac{c-f(v_1)}{f(v_2)-f(v_1)}\cdot (v_2-v_1)+v_1$$其中，$v_1$ 和 $v_2$ 是边的两个端点，$f(v_1)$ 和 $f(v_2)$ 是对应的标量值。
  将所有交点连接在一起构成等值面。

详细实现细节可以参考：https://paulbourke.net/geometry/polygonise/。
伪代码实现如下：

for each voxel in 3D data:
# 1. 确定顶点状态
state = 0;
for i in 0..7:
    if vertex_value[i] > iso_value:
        state |= (1 << i);  # 标记顶点为实点

# 2. 查询查找表，获取三角面片连接方式
case = lookup_table[state];
if case == 0: continue;  # 无交点

# 3. 计算交点并生成三角面片
for each edge in case:
    if edge intersects isosurface:
        compute intersection point using linear interpolation
        add point to triangle list

# 4. 根据连接方式生成三角面片
for each triangle in case:
    create triangle from intersection points

具体代码实现如下：

from skimage.measure import marching_cubes
verts, faces, norms, vals = marching_cubes(tsdf_vol, level=0)