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的東西，現在感覺也可能最後不做科研去打工了，前途無望。。。