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的东西,现在感觉也可能最后不做科研去打工了,前途无望。。。

posted @ 2024-04-24 01:29  lcyfrog  阅读(58)  评论(0编辑  收藏  举报