（Visual Navigation）深度图像转点云

• 深度图像即图像中包含深度信息（也就是距离信息）的单通道图像，每个像素反应的是图中的一点相对于相机的位置：左右位置 高度位置和深度位置
• 而视觉导航中比较常用的就是深度相机，通过深度相机的深度图像和rgb图像的转化得到地图信息，而深度图像转点云就是很重要的一个过程

坐标系介绍

• 一般分为三种坐标系 ：

  • 世界坐标系：描述全局地图信息的坐标系 ，一般以全局地图左上方为坐标原点
    

  • 相机坐标系：顾名思义就是以相机的中心为原点的坐标轴，会随着robot移动
    

  • 图像坐标系：描述深度图像信息的坐标系 一般位于深度图像的左上方
    

坐标系的转换和平移

具体见机器人导论的“刚体运动描述”

图像坐标系转相机坐标系

• 假设图像坐标系0（左上角为远点）中一点P（Xp0，Yp0）
• ①首先需要把图像坐标系0从图像的左上角转化到以图像中心为原点的另一个图像坐标系1，以便图像坐标系1的Z轴（深度方向）与相机坐标系的Z轴重合，得到P（Xp1，Yp1）
• ②如上图，f为相机内参中的焦距，为已知常量；假设P对应空间中的坐标（相对于相机坐标系）为P（Xc，Yc，Zc）；由相似三角形知识可得 Yc / Yp1 = Zc / f ； 故空间中的Yc = Zc*Yp1 / f ；同理可得Xc = Zc * Xp1 / f ；而Zc为深度图中对应（Xp1，Yp1）的通道值 ；至此得到 相对于相机坐标系的空间坐标P（Xc，Yc，Zc）
• 经过上面一步即可以得到以相机为中心的点云信息，可以以此来构建以自我为中心的局部地图（egocentric map）

代码实现

• tensor实现

        # 交换维度 -> b c h w
        depth = depth.permute(0, 3, 1, 2)
        _, _, imh, imw = depth.shape   # batchsize, 1, imh, imw
        # 相当于给矩阵的每个位置编号
        # eg：左上角的第一个元素标号为（Xp0,Yp0） = （0，0）
        x    = rearrange(torch.arange(0, imw), 'w -> () () () w').to(self.device)
        y    = rearrange(torch.arange(imh, 0, step=-1), 'h -> () () h ()').to(self.device)
        # self.cx 指的是depth图像的一半大小
        # 将（Xp0,Yp0）转化为（Xp1,Yp1）在转化为 （Xc，Yc）
        xx   = (x - self.cx) / self.fx
        yy   = (y - self.cy) / self.fy
		
        # 3D real-world coordinates (in meters)
        #（Xc，Yc）转化为（Xw，Yw）
        Z = depth
        X = xx * Z
        Y = yy * Z

• numpy实现

def get_point_cloud_from_z(Y, camera_matrix):
  """Projects the depth image Y into a 3D point cloud.
  Inputs:
    Y is ...xHxW
    camera_matrix
  Outputs:
    X is positive going right
    Y is positive into the image
    Z is positive up in the image
    XYZ is ...xHxWx3
  """
  # 生成网格点坐标矩阵
  # 1-255 255-1
  x, z = np.meshgrid(np.arange(Y.shape[-1]),
                     np.arange(Y.shape[-2]-1, -1, -1))
  for i in range(Y.ndim-2):
    x = np.expand_dims(x, axis=0)
    z = np.expand_dims(z, axis=0)
  X = (x-camera_matrix.xc) * Y / camera_matrix.f
  Z = (z-camera_matrix.zc) * Y / camera_matrix.f
  newx = X[...,np.newaxis]
  XYZ = np.concatenate((newx, Y[...,np.newaxis],
                        Z[...,np.newaxis]), axis=X.ndim)
  return XYZ

相机坐标系转世界坐标系

• 相机坐标系与世界坐标系的关系会因为robot在空间中的位置和朝向不同而变化，朝向影响相机的课室范围；所以相机坐标系转换到世界坐标系只需要知道robot的朝向以及robot在预先定义好的全局地图中的位置（一般设定robot起始位置为全局地图的中心）即可得到转化的旋转矩阵（robot朝向）和平移矩阵（robot位置）
• 旋转的实现可以参考仿射变换（pytorch实现）或者利用上文提到的刚体运动学求解
• 即图中两坐标系的关系
  
• 至此可以得到相对于世界坐标的P（Xw，Yw，Zw），即相对于全局地图左上角的全局点云信息。

点云表示为地图信息请看下一篇文章