0. 简介

对于激光雷达而言,玻璃等场景一直是漏检的主要问题,而如何去采用一种有效地方法能够完成激光雷达对室内场景的玻璃物体的检测和包含,这一直是研究重点。当LiDAR数据是主要的外部输入时,玻璃对象不能正确配准。这是因为入射光主要穿过玻璃对象或从光源反射,导致玻璃表面的距离测量不准确。而文章《Cartographer_glass: 2D Graph SLAM Framework using LiDAR for Glass Environments》就初步提出了这个解决方法。目前代码还未开源,可以等一波顶会的收入。

1. 文章贡献

在本文中,我们利用一种简单且计算成本低的玻璃检测方案来检测玻璃物体,并提出了将识别出的物体合并到由这种算法(Google Cartograph)维护的占用网格中的方法。为了实现上述目标,我们开发了局部(子图级别)和全局算法,并将我们的方法生成的地图与使用基于SLAM的粒子过滤器的现有算法生成的地图进行了比较。本文贡献如下:

  1. 在基于优化的SLAM算法中实施玻璃检测和建图
  2. 提高了基于GMapping的玻璃检测和建图方案生成的地图的准确性
  3. 使用配备LiDAR的移动平台在玻璃环境中进行了广泛的测试。

2. 玻璃检测

本文仅使用LiDAR光束反射的强度数据来检测透明表面(玻璃),正如作者在 [3] 中介绍的那样。由于玻璃表面具有高度光滑的表面,因此这些镜面反射激光光束。因此,当机器人从法线入射角接收激光射线时,接收的光线强度将具有最高强度。因此,通过检查激光扫描数据的强度配置文件并识别强度峰值,可以检测到玻璃表面。为了防止误报,该算法使用强度阈值(thresh)、强度梯度(grad)和配置文件宽度(width)作为可调参数。玻璃检测使用的方法如算法 1 所示。尽管此检测方法仅允许很小的视角范围,但这在最近的文献 [2],[3] 中已被证明是玻璃检测的一种有效方法。

这里我们展示了[3]当中的代码即:https://github.com/uts-magic-lab/slam_glass

void GridSlamProcessor::glassMatch(ScanMatcherMap& map, TNodeVector & bestTroj)
{
  // going backwards through the bestTroj vector
  size_t csize = m_glassCache.size();
  unsigned int nextIndex = 0;

  if (csize==0)
    return;

  TNodeVector::reverse_iterator riter = bestTroj.rbegin();
  double prevTimestamp = start_up_time_;
  OrientedPoint  m_g_drift;

  while (riter != bestTroj.rend())
  {
    OrientedPoint pose_diff = (*riter)->pose - (*riter)->reading->getPose();

    if (pose_diff.theta > M_PI )
      pose_diff.theta -= 2*M_PI;
    else if (pose_diff.theta < -M_PI)
      pose_diff.theta += 2*M_PI;

    OrientedPoint v =  pose_diff - m_g_drift;
    double timestamp = (*riter)->reading->getTime();
    double timediff = timestamp - prevTimestamp;

    v.x = v.x / timediff;v.y = v.y / timediff;
    v.theta = v.theta / timediff;// drift rate del pose / del time.

    unsigned int i = nextIndex;
    while (i < csize && m_glassCache[i].timestamp <= timestamp) {
      i++;
    }
    for (unsigned int j = nextIndex; j < i; j++) {
      OrientedPoint cp = m_glassCache[j].pose + v * (m_glassCache[j].timestamp - prevTimestamp) + m_g_drift; // correct for pose stored in glass cache
      Point phit = cp;
      phit.x += m_glassCache[j].radius * cos(cp.theta + m_glassCache[j].phi);
      phit.y += m_glassCache[j].radius * sin(cp.theta + m_glassCache[j].phi);
      //TODO: Check neighbour points are also required to be marked?
      IntPoint p1 = map.world2map( phit );
      if (p1.x>=0 && p1.y>=0 && map.isInside( p1 ))
        map.cell(p1).updateGlass();
      else {
        printf( "glassmatch hit invalid point (%d,%d) phit (%f,%f) r= %f angle %f, cptheta = %f\n",
            p1.x, p1.y, phit.x, phit.y, m_glassCache[j].radius, m_glassCache[j].phi, cp.theta );
      }
    }
    nextIndex = i;
    m_g_drift = pose_diff;
    prevTimestamp = timestamp;
    ++riter;
  }
}

3. 在2D GRAPH SLAM中的玻璃检测

我们将讨论如何将检测到的玻璃点整合到二维图形SLAM框架(Google Cartographer)中。玻璃映射方案的主要目的是将检测到的玻璃点稳健地加入地图。理想情况下,可以通过识别检测到的玻璃点在概率网格中的单元格索引,并命令将该单元格的网格概率增加到最大允许概率。然而,只有当激光扫描光线是玻璃表面的法线时,玻璃点才能被识别并被检测为命中点。因此,当激光通过玻璃或从LiDAR外向进行镜面反射时,该特定点将被视为未命中点而不是命中点。因此,即使在检测时将检测到的点的网格概率设置为高值,当相同的玻璃点被注册为未命中点时,Cartographer算法仍将其后续降低概率。这是由于Cartographer算法的固有性质,因为它通过考虑命中点和未命中点来更新概率网格,如II节所述。因此,为了稳妥地将检测到的点纳入地图,我们提出了Cartographer glass框架,它使用两个子方案:1)局部玻璃映射方案将检测到的玻璃点映射到当前的子地图上;2)全局玻璃映射方案在全局地图中保留检测到的玻璃点。

本地玻璃映射:在这个方案中,首先确定当前子图的概率网格中检测到的玻璃点的单元格索引。接下来,确定的单元格的网格概率被人为地设置为最大概率值。因此,检测到的玻璃点现在作为具有最大网格概率的命中点被添加到本地子图中,以编码所观察到的检测到的玻璃点的识别,这将在识别后续出现的相同点时有用。如果已经观察到的玻璃点再次被观察到,作为命中点或未命中点,我们将不会更新其占用率从已设置的最大值。这个过程确保了子图上的检测到的和映射的玻璃点不会逐渐消失。此外,检测到的玻璃点的全局坐标以初始全局坐标系为参考,通过使用同一变换矩阵 H_k 将在当前全局坐标系中给出的坐标转换为全局坐标。这里的 H_k 是从初始全局坐标系到第k个(当前)全局坐标系的同一变换,在运行闭环优化(Cartographer的后端)时计算。算法2显示了本地玻璃映射方案,它将返回第k个子图的概率网格。

全局玻璃映射:本地玻璃映射方案仅适用于子图层次,因为在每个子图的开头时,计算的概率网格将被重新初始化。因此,先前观察到的玻璃点的标识符在新初始化的概率网格中不再存在。因此,如果先前观察到的玻璃点现在注册为miss,则根据p_{miss}的概率更新,已经在先前的子图中添加的检测到的玻璃点将消失。因此,在保留全局地图中的检测到的表面时,必须将全局Glass点数组中保存的点添加到每个后续子图中。然而,在添加它们之前,这些点需要被转换到当前全局坐标系中。接下来,这些添加的点的网格概率将被设置为最大概率(带有编码的标识符)。这将使得在演变子图时,本地玻璃映射方案能够识别新添加的点作为玻璃点。算法3显示了全局玻璃映射方案。

然而,实现全局玻璃方案的一个重要挑战是准确估计检测到的玻璃点的同构变换。此外,由于在观察多次后,地图的一些部分发生回路关闭,因此当前子图可能跨越尚未通过添加回路关闭约束进行优化的区域。在这些情况下,添加的检测点可能对应于机器人实际和估计的轨迹/姿态的误差。这在绘制大空间地图的情形下更为明显,因为在这样的情形中引入偏差的可能性更大。因此,在这种情况下,上述全局玻璃映射方案是无效的。然而,作为克服这一挑战的替代方法,仅使用局部玻璃映射算法并增加p_{miss}值,以便以前的点不会经常消失。我们将称之为仅使用增加p_{miss}的局部映射方案的方法为Cartographer glass lite。

4. 参考链接

https://mp.weixin.qq.com/s/MTT_jFKvx566r4LxA0DZeA