（Visual Navigation）点云转地图信息

• 上一次总结了深度图转点云的详细过程 这一次总结一下点云怎么转化为机器人能理解的地图信息
  

Bin_Point

效果

• 此时Z轴代表高度 Y代表深度 X代表左右
• 使用的是Facebook的开源仿真平台Habitat
• baseline参考/depth_utils.py
  
  

基本思路

• 按照高度信息将点云分为三类
• 将每一类都放到一个通道
• 获取到一个点则相应三通道矩阵的位置++
• 所以得到一个三通道的矩阵 包含空间位置信息和在高度三个层出现的次数
  

实现流程

• 输入（256，256，1）的depth img
• 分割出空间点云矩阵（256，256，3）三个通道分别代表X Y Z信息
• 根据高度信息将Z的数据归类到0 1 2 0代表底层 1代表中间层 2代表高层
• 将（256，256，3）转化为（1201，1201，3）的全局地图信息 转换过程使用np.bincount
  

代码实现

def bin_points(XYZ_cms, map_size, z_bins, xy_resolution):
  """Bins points into xy-z bins
  XYZ_cms is ... x H x W x3
  Outputs is ... x map_size x map_size x (len(z_bins)+1)
  """
  sh = XYZ_cms.shape
  # （1*256*256*3）
  XYZ_cms = XYZ_cms.reshape([-1, sh[-3], sh[-2], sh[-1]])
  n_z_bins = len(z_bins)+1  #3
  counts = []
  isvalids = []
  for XYZ_cm in XYZ_cms:
    isnotnan = np.logical_not(np.isnan(XYZ_cm[:,:,0]))
    # np.round 四舍五入
    X_bin = np.round(XYZ_cm[:,:,0] / xy_resolution).astype(np.int32)
    Y_bin = np.round(XYZ_cm[:,:,1] / xy_resolution).astype(np.int32)
    # np.digitize 有点归一化的意思 大概就是将数值映射到特定的范围里
    # 这里z_bins = 【20，150】所以小于20为0 20-150为1 大于150为2
    Z_bin = np.digitize(XYZ_cm[:,:,2], bins=z_bins).astype(np.int32)
 	# 设置点云的范围
    # （256，256，7）
    isvalid = np.array([X_bin >= 0, X_bin < map_size, Y_bin >= 0, Y_bin < map_size,
                        Z_bin >= 0, Z_bin < n_z_bins, isnotnan])
    # np.all()判断给定轴向上的所有元素是否都为True
    # np.any()判断给定轴向上是否有一个元素为True
    # （256，256）
    isvalid = np.all(isvalid, axis=0)
	# 编码
    ind = (Y_bin * map_size + X_bin) * n_z_bins + Z_bin
    ind[np.logical_not(isvalid)] = 0
    # np.bincount（x）:返回一个数组，元数个数为x中最大的数 每个bin给出了它的索引值在x中出现的次数
    '''
    我们可以看到x中最大的数为7，因此bin的数量为8，那么它的索引值为0->7
	x = np.array([0, 1, 1, 3, 2, 1, 7])
	索引0出现了1次，索引1出现了3次......索引5出现了0次......
	np.bincount(x)
	因此，输出结果为：array([1, 3, 1, 1, 0, 0, 0, 1])
	如果minlength被指定，那么输出数组中bin的数量至少为它指定的数
    '''
    # ravel()方法将数组维度拉成一维数组
    count = np.bincount(ind.ravel(), isvalid.ravel().astype(np.int32),
                         minlength=map_size*map_size*n_z_bins)
    count = np.reshape(count, [map_size, map_size, n_z_bins])
    counts.append(count)
    isvalids.append(isvalid)
  counts = np.array(counts).reshape(list(sh[:-3]) + [map_size, map_size, n_z_bins])
  isvalids = np.array(isvalids).reshape(list(sh[:-3]) + [sh[-3], sh[-2], 1])
  return counts, isvalids

函数讲解

• np.digitize()

    # np.digitize 有点归一化的意思 大概就是将数值映射到特定的范围里
    # 这里z_bins = 【20，150】所以小于20为0 20-150为1 大于150为2
    Z_bin = np.digitize(XYZ_cm[:,:,2], bins=z_bins).astype(np.int32)

• np.bincount()

    # np.bincount（x）:返回一个数组，元数个数为x中最大的数 每个bin给出了它的索引值在x中出现的次数
    '''
    我们可以看到x中最大的数为7，因此bin的数量为8，那么它的索引值为0->7
	x = np.array([0, 1, 1, 3, 2, 1, 7])
	索引0出现了1次，索引1出现了3次......索引5出现了0次......
	np.bincount(x)
	因此，输出结果为：array([1, 3, 1, 1, 0, 0, 0, 1])
	如果minlength被指定，那么输出数组中bin的数量至少为它指定的数
    '''
    # ravel()方法将数组维度拉成一维数组
    count = np.bincount(ind.ravel(), isvalid.ravel().astype(np.int32),
                         minlength=map_size*map_size*n_z_bins)

障碍判断

• obstacle 会在转化回来的Map（即count）的第一高度层判断是否叠加大于2

self.map = self.map + count(上文所得)
obstacle = self.map[:,:,1] >= 2

torch.scatter

基本思路

• 此处Z指向深度 Y指向高度 X指向左右
• 将rgb-d图像放进一个cnn得到 img-feature
• 将原来depth img下采样对应 img-feature的大小
• 将subsample depth img 转化为全局地图的点云
• 故地图信息为img-feature

代码实现

• 参考habitat_baselines/rl/models/projection.py
• 代码每一段都配上了图解
  • _ss后缀为下采样后矩阵
  • spatial_loc为点云信息
    
    
    
    
    
    
    
    

函数详解

• torch.scatter.scatter_max

有了地图信息机器人就需要路径规划到达目标点