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节建立的体素栅格所在的坐标系为世界坐标系，体素坐标为(p0w,p1w,...,pmw)(\mathbf{p}_{0}^{w}, \mathbf{p}_{1}^{w}, ...,\mathbf{p}_{m}^{w})(p0w,p1w,...,pmw)，相机的位姿为(Tc0w,Tc1w...,Tcnw)(\mathbf{T}_{c_{0}}^{w}, \mathbf{T}_{c_{1}}^{w} ..., \mathbf{T}_{c_{n}}^{w})(Tc0w,Tc1w...,Tcnw)，相机的内参为K\mathbf{K}K

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

Iij=[uijvij]=KTciwpjw(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=[uijvij]=KTciwpjw(1)
式中i∈(0,1,....,n)i\in(0, 1, ...., n)i∈(0,1,....,n)，j∈(0,1,...,m)j\in(0, 1, ..., m)j∈(0,1,...,m)．

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

首先计算每个体素sdf值:
sdfj=∥tpjw−tciw∥−dep(Iij)(2)sdf_{j} = \left \| \mathbf{t}_{pj}^{w} - \mathbf{t}_{ci}^{w} \right \| - \mathbf{dep}(\mathbf{I}_{ij}) \tag{2} sdfj=∥∥tpjw−tciw∥∥−dep(Iij)(2)
式中tpjw\mathbf{t}_{pj}^{w}tpjw表示第jjj个体素在世界坐标系下的位置信息，tciw\mathbf{t}_{ci}^{w}tciw表示第iii个相机位姿在世界坐标系下的位置信息，dep(Iij)\mathbf{dep}(\mathbf{I}_{ij})dep(Iij)表示第jjj个体素在第iii个相机深度图像坐标系下的深度值．

截断每个体素的sdf值:
ifsdfj>0tsdfj=min(1,sdfj/trunc)elsetsdfj=max(−1,sdfj/trunc)(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)
式中 trunctrunctrunc 表示截断距离，人为设定，可理解为相机深度信息的误差值，如果误差越大，建议该值设置大一些，否则会丢掉很多深度相机获取的信息．

计算每个体素的tsdf值:
tsdfj=tsdfj−1⋅wj−1+tsdfj⋅wjwj−1+wj(4)tsdf_{j} = \frac{tsdf_{j-1}\cdot w_{j-1} + tsdf_{j}\cdot w_{j}}{w_{j-1}+w_{j}} \tag{4} tsdfj=wj−1+wjtsdfj−1⋅wj−1+tsdfj⋅wj(4)
式中初始wjw_{j}wj一般默认设置为1．

计算每个体素的权值:
wj=wj−1+wj(5)w_{j} = w_{j-1} + w_{j} \tag{5} wj=wj−1+wj(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

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_1tsdf_vals = self._tsdf_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] # 提取上个循环对应的tsdf值，对应公式(4)中的tsdf_j_1valid_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*256new_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*256new_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

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*256colors = np.floor(np.asarray([colors_r,colors_g,colors_b])).Tcolors = colors.astype(np.uint8)return verts, faces, norms, colors

TSDF算法原理及源码解析相关推荐

Sentinel滑动时间窗限流算法原理及源码解析（上）
文章目录时间窗限流算法滑动时间窗口滑动时间窗口算法改进滑动时间窗口源码解析时间窗限流算法 10t到16t 10个请求 16t-20t 50个请求 20t-26t 60个请求 26t到30t ...
Sentinel滑动时间窗限流算法原理及源码解析（中）
文章目录 MetricBucket MetricEvent数据统计的维度 WindowWrap样本窗口实例范型T为MetricBucket windowLengthInMs 样本窗口长度 windo ...
Sentinel滑动时间窗限流算法原理及源码解析（下）
文章目录对统计数据如何使用获取之前统计好的数据对统计数据如何使用流控快速失败获取之前统计好的数据
【特征匹配】ORB原理与源码解析
相关 : Fast原理与源码解析 Brief描述子原理与源码解析 Harris原理与源码解析 http://blog.csdn.net/luoshixian099/article/details/48 ...
PCA-SIFT原理及源码解析
相关: SIFT原理与源码解析 SURF原理与源码解析 ORB原理与源码解析 FAST原理与源码解析 BRIEF描述子原理与源码解析 Harris原理与源码解析转载请注明出处:http://blog ...
【特征匹配】BRIEF特征描述子原理及源码解析
相关:Fast原理及源码解析 Harris原理及源码解析 SIFT原理及源码解析 SURF原理及源码解析转载请注明出处: http://blog.csdn.net/luoshixian099/art ...
视频教程-YOLOv3目标检测：原理与源码解析-计算机视觉
YOLOv3目标检测:原理与源码解析大学教授,美国归国博士.博士生导师:人工智能公司专家顾问:长期从事人工智能.物联网.大数据研究:已发表学术论文100多篇,授权发明专利10多项白勇 ¥78.00 ...
Redis进阶- Redisson分布式锁实现原理及源码解析
文章目录 Pre 用法 Redisson分布式锁实现原理 Redisson分布式锁源码分析 redisson.getLock(lockKey) 的逻辑 redissonLock.lock()的逻辑 r ...
Spring Boot 核心原理与源码解析 - 目录
准备重新写 SpringBoot 配置文件解析原理 , 先在这里把要写的内容记下来 Spring Boot 核心原理与源码解析 - 目录 1\何时解析\如何解析 application.propert ...

TSDF算法原理及源码解析