经典文献阅读之--Cartographer_glass(激光SLAM中玻璃物体的检测)

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个子图的概率网格。

…详情请参照古月居