深度图转点云

使用RGBD相机可以获得一幅depth和RGB,为了获得更加丰富的几何信息，我们想把depth转换成点云。

• 需要准备的数据
  １．相机内参
  ２．深度图

• 具体计算
  １．首先我们可以根据深度相机内参$scale$和$depth$图像得到$Z$坐标
  ２．根据内参和Z，计算$XY$

  从相机内参可以得知焦距$f_x$和$f_y$,这里只介绍$x$方向的计算，$y$方向同理。记$x、y$为世界坐标系中的坐标值。

  假设要把depth图中的一个像素转换为世界坐标中的点，根据相似三角形：
  $$\frac{depth}{f_x}=\frac{X}{x-c_x}$$

  也就得到了$X$。同理可以得到$Y$。

• 一个高效计算的demo

# xy像素坐标生成  但是还可以用np.mgrid提速
self.xmap = np.array([[j for i in range(640)] for j in range(480)])
self.ymap = np.array([[i for i in range(640)] for j in range(480)])


cam_scale = 1.0  # 深度比例尺
pt2 = depth_masked / cam_scale  # z轴坐标
pt0 = (ymap_masked - self.cam_cx) * pt2 / self.cam_fx  # 减掉cx cy偏移量，再根据深度信息确定世界坐标系中x y的坐标
pt1 = (xmap_masked - self.cam_cy) * pt2 / self.cam_fy
cloud = np.concatenate((pt0, pt1, pt2), axis=1)
cloud = cloud / 1000.0

• 番外，如果要得到$xyz+rgb$，可以先把$depth$图像与$RGB$图像对齐，每个像素就可以进行“着色”了