TSDF 体素模型与光线投射
目录
一、TSDF 体素模型简介
二、光线投射
🔷 TSDF 中“Compute Surface Prediction”步骤总结
① 从像素确定射线
② 沿射线穿过 TSDF 体素
③ 检测“零交叉”
④ 计算法向量
光线投射代码演示
一、TSDF 体素模型简介
TSDF体素模型 （Truncated Signed Distance Function，截断符号距离函数）是一种在计算机视觉中常用的3D场景表示方法。TSDF模型利用每个体素中存储的距离信息来表示点云数据与已知模型之间的距离关系。与纯粹的点云数据相比，TSDF体素模型的优势在于能够提供更为密集和平滑的表面表示，这对于重建场景中的细节非常重要。
二、光线投射
🔷 TSDF 中“Compute Surface Prediction”步骤总结
目标是从 TSDF 体素场中 恢复物体表面的真实位置（点）与法向量 。
① 从像素确定射线
在成像平面选择一个像素 (u, v)，结合相机内参与姿态，得到一条从相机出发的射线 (Ray)。
② 沿射线穿过 TSDF 体素
沿着射线依次访问经过的体素，读取这些体素的 TSDF 值。
-
TSDF > 0 ：空间还在表面外部（free space）
-
TSDF < 0 ：已经进入物体内部
-
TSDF = 0 ：刚好在物体表面
③ 检测“零交叉”
当 TSDF 值第一次出现 由正变负 时，
说明射线穿过了表面 → 找到了表面位置。
这个位置通过对正负两个 TSDF 值进行线性插值 可得到更精确的表面点坐标 V^ 。
④ 计算法向量
表面法向量 N^ 由 TSDF 在该点处的梯度求得：
N^ = ∇TSDF / ||∇TSDF||其中梯度 ∇TSDF 通常用三点差分计算：
nx = (TSDF(x+1) - TSDF(x-1)) / 2
ny = (TSDF(y+1) - TSDF(y-1)) / 2
nz = (TSDF(z+1) - TSDF(z-1)) / 2✅ 最终得到的结果
| 输出量 | 含义 |
|---|---|
| V^ | 从该像素 ray 投射到 TSDF 中找到的真实表面点 |
| N^ | 该表面点的法向量（指向 free space） |
光线投射代码演示
import numpy as np# =========================
# 基础工具：体素场与采样
# =========================
def make_sphere_tsdf(nx, ny, nz, voxel_size, origin, sphere_center, sphere_radius, trunc_dist):"""构造一个球体的 TSDF 体素：- 先计算真实的有符号距离 SDF(p) = ||p - c|| - R- 再截断并归一化：TSDF = clip(SDF, [-trunc_dist, +trunc_dist]) / trunc_dist ∈ [-1, 1]返回：tsdf_vol: (nx, ny, nz) 的 TSDFparams: 体素参数字典"""# 体素中心坐标网格（世界坐标）xs = origin[0] + (np.arange(nx) + 0.5) * voxel_sizeys = origin[1] + (np.arange(ny) + 0.5) * voxel_sizezs = origin[2] + (np.arange(nz) + 0.5) * voxel_sizeX, Y, Z = np.meshgrid(xs, ys, zs, indexing='ij')# SDFdist = np.sqrt((X - sphere_center[0])**2+(Y-sphere_center[1])** 2 + (Z - sphere_center[2])**2)sdf = dist - sphere_radius# 截断并归一化sdf_clamped = np.clip(sdf, -trunc_dist, trunc_dist)tsdf = sdf_clamped / trunc_distparams = {"nx": nx, "ny": ny, "nz": nz,"voxel_size": voxel_size,"origin": np.array(origin, dtype=np.float32),"trunc": float(trunc_dist)}return tsdf.astype(np.float32), paramsdef trilinear_sample_tsdf(tsdf, params, p_world):"""在任意世界坐标点 p_world 对 TSDF 三线性插值采样。超出边界时返回 +1（视为空域）。"""nx, ny, nz = params["nx"], params["ny"], params["nz"]vs = params["voxel_size"]org = params["origin"]# 连续体素坐标（以体素中心为格点）g = (p_world - org) / vs - 0.5 # 让整数坐标对齐体素中心x, y, z = g[0], g[1], g[2]i0, j0, k0 = int(np.floor(x)), int(np.floor(y)), int(np.floor(z))i1, j1, k1 = i0 + 1, j0 + 1, k0 + 1# 边界检查：若有一个角超界则简单返回 +1if (i0 < 0 or j0 < 0 or k0 < 0 ori1 >= nx or j1 >= ny or k1 >= nz):return 1.0xd = x - i0yd = y - j0zd = z - k0# 8 顶点c000 = tsdf[i0, j0, k0]; c100 = tsdf[i1, j0, k0]c010 = tsdf[i0, j1, k0]; c110 = tsdf[i1, j1, k0]c001 = tsdf[i0, j0, k1]; c101 = tsdf[i1, j0, k1]c011 = tsdf[i0, j1, k1]; c111 = tsdf[i1, j1, k1]c00 = c000*(1-xd) + c100*xdc01 = c001*(1-xd) + c101*xdc10 = c010*(1-xd) + c110*xdc11 = c011*(1-xd) + c111*xdc0 = c00*(1-yd) + c10*ydc1 = c01*(1-yd) + c11*ydc = c0*(1-zd) + c1*zdreturn float(c)def tsdf_gradient(tsdf, params, p_world, h=None):"""用中心差分近似 ∇TSDF(p)。步长 h 默认取 1 个体素大小。结果会被归一化成单位法向。"""if h is None:h = params["voxel_size"]ex = np.array([h, 0, 0], dtype=np.float32)ey = np.array([0, h, 0], dtype=np.float32)ez = np.array([0, 0, h], dtype=np.float32)fx = (trilinear_sample_tsdf(tsdf, params, p_world + ex) -trilinear_sample_tsdf(tsdf, params, p_world - ex)) / (2*h)fy = (trilinear_sample_tsdf(tsdf, params, p_world + ey) -trilinear_sample_tsdf(tsdf, params, p_world - ey)) / (2*h)fz = (trilinear_sample_tsdf(tsdf, params, p_world + ez) -trilinear_sample_tsdf(tsdf, params, p_world - ez)) / (2*h)g = np.array([fx, fy, fz], dtype=np.float32)n = g / (np.linalg.norm(g) + 1e-9)return n# =========================
# 相机模型：像素 → 射线
# =========================
def pixel_to_ray_dir(u, v, K, R_wc):"""输入像素 (u,v)、内参 K（fx,fy,cx,cy），相机外参 R_wc（3x3，世界←相机）返回：相机在世界坐标的光线方向（单位向量）"""fx, fy, cx, cy = K# 相机坐标系下的方向d_cam = np.array([(u - cx) / fx, (v - cy) / fy, 1.0], dtype=np.float32)d_cam /= np.linalg.norm(d_cam)# 旋转到世界坐标d_world = R_wc @ d_camd_world /= np.linalg.norm(d_world)return d_world# =========================
# Ray marching：找零交叉
# =========================
def march_and_intersect(tsdf, params, cam_o, ray_dir,t_min=0.1, t_max=3.0, step_ratio=0.75):"""从相机原点 cam_o 沿 ray_dir 以固定步长前进，直到 TSDF 由正→负，用线性插值求交点坐标与 TSDF 值（≈0）。返回：hit (bool), p_hit(3,), t_hit, tsdf_at_hit"""vs = params["voxel_size"]step = vs * step_ratiot_prev = t_minp_prev = cam_o + t_prev * ray_dirval_prev = trilinear_sample_tsdf(tsdf, params, p_prev)t = t_prevwhile t <= t_max:t += stepp = cam_o + t * ray_dirval = trilinear_sample_tsdf(tsdf, params, p)# 检测正->负的首次零交叉if val_prev > 0.0 and val < 0.0:# 线性插值：val_prev + alpha*(val - val_prev) = 0alpha = val_prev / (val_prev - val + 1e-12)t_hit = t_prev + alpha * (t - t_prev)p_hit = cam_o + t_hit * ray_dirtsdf_at_hit = trilinear_sample_tsdf(tsdf, params, p_hit)return True, p_hit, t_hit, tsdf_at_hitt_prev, p_prev, val_prev = t, p, valreturn False, None, None, None# =========================
# Demo 主程序
# =========================
def main():np.set_printoptions(precision=5, suppress=True)# ---- 1) 构造一个合成 TSDF（球体） ----nx, ny, nz = 160, 160, 200 # 体素分辨率voxel_size = 0.01 # 1 cm/voxelorigin = np.array([-0.8, -0.8, 0.0], dtype=np.float32) # 体素最小角点（世界坐标）sphere_center = np.array([0.0, 0.0, 1.2], dtype=np.float32)sphere_radius = 0.35trunc = 3 * voxel_size # 截断距离（经验：2~5 个体素）tsdf, params = make_sphere_tsdf(nx, ny, nz, voxel_size, origin, sphere_center, sphere_radius, trunc)# ---- 2) 相机参数与姿态（看向 +Z） ----fx = fy = 600.0cx, cy = 320.0, 240.0K = (fx, fy, cx, cy)cam_pos = np.array([0.0, 0.0, 0.0], dtype=np.float32) # 世界坐标R_wc = np.eye(3, dtype=np.float32) # 相机坐标与世界坐标对齐# ---- 3) 选一个像素，生成射线 ----u, v = 320.0, 240.0 # 光轴中心像素ray_dir = pixel_to_ray_dir(u, v, K, R_wc)# ---- 4) Ray marching 寻找零交叉 ----hit, p_hit, t_hit, tsdf_at_hit = march_and_intersect(tsdf, params, cam_pos, ray_dir, t_min=0.1, t_max=3.0, step_ratio=0.75)if not hit:print("没有击中表面（可能视锥外/步长过大/体素覆盖范围不足）。")return# ---- 5) 计算法向（梯度方向） ----n = tsdf_gradient(tsdf, params, p_hit, h=None) # 默认 h = 1 voxel# ---- 6) 打印结果 ----print("=== 命中结果 ===")print(f"像素 (u,v) = ({u:.1f}, {v:.1f})")print(f"射线方向 ray_dir = {ray_dir}")print(f"命中距离 t_hit = {t_hit:.4f} m")print(f"表面点 V^ = {p_hit}")print(f"TSDF(V^) ≈ {tsdf_at_hit:.5f} (应接近 0)")print(f"法向量 N^ = {n} (单位向量)")# ---- 7) 你可以进一步：遍历像素生成稀疏点云 ----# 例如每隔 20 像素取一个点，收集 (V^, N^) 列表# points, normals = [], []# for v in range(120, 361, 20):# for u in range(220, 421, 20):# d = pixel_to_ray_dir(u, v, K, R_wc)# hit, p, t, _ = march_and_intersect(tsdf, params, cam_pos, d)# if hit:# n = tsdf_gradient(tsdf, params, p)# points.append(p); normals.append(n)# print(f"采到点数：{len(points)}")if __name__ == "__main__":main()