TSDF Fusion实现
好久没看博客了，突然想发点东西，遂诈尸
想来想去不知道发什么，发个作业报告吧
Task1: 从深度图生成点云（4 pts.）
已知相机内外参和深度图，可以根据以下公式将深度图的像素坐标\((u, v)\)变换到世界坐标系下的坐标\((x, y, z)\)得到点云。
\[\left [
\begin{matrix}
x \\
y \\
z
\end{matrix}
\right ] = z
\left [
\begin{matrix}
1 / f_x & 0 & -c_x/f_x \\
0 & 1 / f_y & -c_y/f_y \\
0 & 0 & 1
\end{matrix}
\right ]
\left [
\begin{matrix}
u \\
v \\
1
\end{matrix}
\right ]
\]
本步骤需要完成：
- 实现
fusion.cam_to_world函数，将深度图转为世界坐标系下的点云 - 对点云进行可视化（可以用
trimesh.PointCloud输出为.ply文件并使用MeshLab打开） - 根据点云坐标的最大最小值，确定TSDF的体素场范围
vol_bnds - 每个体素小立方体尺寸为
vol_size，按照vol_bnds / vol_size确定体素场的整数坐标，在该体素场内划分格点，作为TSDF的采样点。
Task2: 从深度图采样（3 pts. ）
与上一步相反，这一步我们需要把TSDF体素场的采样点向深度图上投影，然后从深度图上采样深度，为计算TSDF做准备。
本步骤需要完成：
- TSDF采样点的整数坐标依次转换到世界坐标系、相机坐标系、像素坐标
- 为每个TSDF采样点从深度图上采样深度
Task3: 计算单帧TSDF（3 pts.）
根据每个采样点的深度，可以近似计算得到该点的TSDF值。
\[TSDF = \min \{1.0, (depth - z) / t \}
\]
本步骤需要完成：
- 根据深度计算单帧的TSDF值
Task4: 融合多帧TSDF（3 pts.）
已知单帧的TSDF数据，我们需要将其通过加权平均的方式融合得到整体的TSDF场。
\[D_{i+1}(x) = \frac{W_i(x)D_i(x) + w_{i+1}d_{i+1}(x)}{W_i(x) + w_{i+1}(x)}
\]
\[W_{i+1}(x) = W_i(x) + w_{i+1}(x)
\]
本步骤需要完成：
- 融合多帧TSDF得到TSDF场
- 在TSDF场上使用Marching Cubes算法得到mesh（Marching Cubes可以调包）。
附加分（2 pts.）
任选一项完成即可：
- 手动实现Marching Cubes算法
- 使用RGB图片数据，实现带颜色的TSDF Fusion
第一次作业报告
Task1
def cam_to_world(depth_im, cam_intr, cam_pose):
"""Get 3D point cloud from depth image and camera pose
"""
world_pts = np.zeros((3, depth_im.shape[0] * depth_im.shape[1]))
cam_coords = np.zeros((depth_im.shape[0], depth_im.shape[1], 3))
# 运用np.arrange函数，得到在每个维度上递增的下标数组：
cam_coords[:,:,0] = np.tile(np.arange(depth_im.shape[1]), (depth_im.shape[0], 1))
cam_coords[:,:,1] = np.tile(np.arange(depth_im.shape[0])[:, np.newaxis], (1, depth_im.shape[1]))
# 运用相机内参和深度图，把像素坐标转换为相机坐标系坐标
cam_coords[:,:,0] = (cam_coords[:,:,0] - cam_intr[0, 2]) * depth_im / cam_intr[0, 0]
cam_coords[:,:,1] = (cam_coords[:,:,1] - cam_intr[1, 2]) * depth_im / cam_intr[1, 1]
cam_coords[:,:,2] = depth_im
cam_coords = cam_coords.reshape(-1, 3).T
# concat一个1后再左乘相机外参矩阵，得到世界坐标系坐标，导出为ply
world_pts = np.dot(cam_pose, np.vstack((cam_coords, np.ones(cam_coords.shape[1]))))[:3]
pointcloud = trimesh.PointCloud(world_pts.T)
pointcloud.export("pointcloud.ply")
return world_pts
确定边界，在该体素场内划分格点，作为TSDF的采样点
# 确定边界
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))
# 划分格点
self.ranges = np.ceil((self.vol_bnds[:, 1] - self.vol_bnds[:, 0]) / self.voxel_size )
self.ranges = self.ranges.astype(int)
一开始写的for循环，后改成numpy快很多
Task2
TSDF采样点的整数坐标依次转换到世界坐标系、相机坐标系、像素坐标：
# 用arrange得到下标数组
vox_coords = np.zeros((self.ranges[0], self.ranges[1], self.ranges[2], 3))
vox_coords[:,:,:,0] = np.tile(np.arange(self.ranges[0])[:, np.newaxis, np.newaxis], (1, self.ranges[1], self.ranges[2]))
vox_coords[:,:,:,1] = np.tile(np.arange(self.ranges[1])[np.newaxis, :, np.newaxis], (self.ranges[0], 1, self.ranges[2]))
vox_coords[:,:,:,2] = np.tile(np.arange(self.ranges[2])[np.newaxis, np.newaxis, :], (self.ranges[0], self.ranges[1], 1))
vox_coords = vox_coords.reshape(-1, 3)
# 世界坐标系
vox_coords = (vox_coords * self.voxel_size) + self.vol_bnds[:, 0]
# 相机坐标系
vox_coords = np.dot(np.linalg.inv(cam_pose), np.vstack((vox_coords.T, np.ones(vox_coords.shape[0]))))[:3]
# 像素坐标，先乘内参矩阵并除以z，再取整
pix_coords = np.dot(cam_intr, vox_coords)
pix_coords[0, :] /= pix_coords[2, :]
pix_coords[1, :] /= pix_coords[2, :]
pix_coords = np.round(pix_coords[:2, :]).astype(int)
为每个TSDF采样点从深度图上采样深度：
# 得到在图像范围内的点
valid_pix = np.logical_and(pix_coords[0, :] >= 0, np.logical_and(pix_coords[0, :] < depth_im.shape[1], np.logical_and(pix_coords[1, :] >= 0, pix_coords[1, :] < depth_im.shape[0])))
# 采样，图片深度减去体素对应的深度，得到深度差
depth_val = np.zeros(pix_coords.shape[1], dtype=float)
depth_val[valid_pix] = depth_im[pix_coords[1, valid_pix], pix_coords[0, valid_pix]] - vox_coords[2, valid_pix]
# 截断
valid_pix = np.logical_and(valid_pix, depth_val >= -self.trunc_margin)
Task3
计算TSDF值
dist = np.minimum(1., depth_val / self.trunc_margin)
Task4
融合得到整体的TSDF场
# 加权平均
self.tsdf_vol[valid_pix] = (self.tsdf_vol[valid_pix] * self.weight_vol[valid_pix] + dist[valid_pix]) / (self.weight_vol[valid_pix] + 1)
self.weight_vol[valid_pix] += 1
调用marching cube：
def get_mesh(self):
tsdf_vol = self.tsdf_vol
tsdf_vol = tsdf_vol.reshape(self.ranges[0], self.ranges[1], self.ranges[2])
# Extract a mesh
verts, faces, norms, vals = measure.marching_cubes(tsdf_vol, level=0)
verts_val = verts * self.voxel_size + self.vol_bnds[:, 0]
# export to ply
trimesh.Trimesh(verts_val, faces).export("mesh.ply")
处理的 Average FPS: 1.26
附加分：带颜色的TSDF fusion
我的想法是把图片的颜色加到距离<0.1的体素上，加权平均得到体素颜色
# 判断体素到表面距离，获得可以加颜色的体素（valid_pix），后续只对这些体素操作
valid_pix = np.logical_and(valid_pix, np.logical_and(depth_val < 0.1 , depth_val > -0.1))
color_val = np.zeros((pix_coords.shape[1], 3), dtype=float)
# 得到颜色
color_val[valid_pix] = color_im[pix_coords[1, valid_pix], pix_coords[0, valid_pix]]
# 加权平均
self.color_vol[valid_pix] = (self.color_vol[valid_pix] * self.color_weight_vol[valid_pix] + color_val[valid_pix]) / (self.color_weight_vol[valid_pix] + 1)
self.color_weight_vol[valid_pix] += 1
导出的时候对mesh顶点查询颜色：
# Get vertex colors
colors = self.color_vol.reshape(self.ranges[0], self.ranges[1], self.ranges[2], 3)
verts = verts.astype(int)
colors = colors[verts[:, 0], verts[:, 1], verts[:, 2]]
# export to ply
trimesh.Trimesh(verts_val, faces, vertex_colors=colors).export("mesh.ply")
处理的 Average FPS: 0.93
因为颜色初值为0，边缘是黑色的
end
最近越来越颓废了，之前跟着做gnn感觉没意思，又学了点graphics的东西，现在感觉也可能最后不做科研去打工了，前途无望。。。
