SLAM_Karto 学习（四） 深入理解 ScanMatch 过程

SLAM_Karto 学习（四） 深入理解 ScanMatch 过程

ScanMatch 调用过程

process函数UML

  process 函数是整个图优化的入口函数，包括激光的处理，图的构建及优化，其工作流程如下：

Created with Raphaël 2.1.2 输入激光数据 激光 Validate？ 地图初始化 ？ 获得lastScan ? 利用LastScan和ODOM里程计初始化scan位姿 HaveMovedEnough? 第一束激光? 将scan 加入 scanBuffer， 分配id UseScanMatching 参数为true？ 将scan加入GraphAddVertex, AddEdges, AddRunningScan DoLoopClosing 参数为true？ TryCloseLoop 回环检测优化 将 scan 设置为 lastScan 结束 MatchScan利用RunningScan纠正位姿 结束 yes no yes no yes no yes no yes no yes no yes no

process函数源码

  kt_bool Mapper::Process(LocalizedRangeScan* pScan)
  {
    if (pScan != NULL)
    {
      karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();

      // validate scan
      if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false)
      {
        return false;
      }

      if (m_Initialized == false)
      {
        // initialize mapper with range threshold from device 地图的范围
        Initialize(pLaserRangeFinder->GetRangeThreshold());
      }

      // get last scan
      LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName());

      // update scans corrected pose based on last correction
      if (pLastScan != NULL)
      {
        Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
        pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose()));
      }

      // test if scan is outside minimum boundary or if heading is larger then minimum heading
      if (!HasMovedEnough(pScan, pLastScan))
      {
        return false;
      }

      Matrix3 covariance;
      covariance.SetToIdentity();

      // correct scan (if not first scan) 从running scan 上开始增加 MatchScan
      if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
      {
        Pose2 bestPose;
        m_pSequentialScanMatcher->MatchScan(pScan,
                                            m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
                                                                                    bestPose,
                                                                                    covariance);
        pScan->SetSensorPose(bestPose);
      }

      // add scan to buffer and assign id
      m_pMapperSensorManager->AddScan(pScan);

      if (m_pUseScanMatching->GetValue())
      {
        // add to graph
        m_pGraph->AddVertex(pScan);
        m_pGraph->AddEdges(pScan, covariance);

        m_pMapperSensorManager->AddRunningScan(pScan);

        if (m_pDoLoopClosing->GetValue())
        {
          std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
          const_forEach(std::vector<Name>, &deviceNames)
          {
            m_pGraph->TryCloseLoop(pScan, *iter);
          }
        }
      }

      m_pMapperSensorManager->SetLastScan(pScan);

      return true;
    }

    return false;
  }

 这就是 Process 的主要流程，对应于源代码的 2217-2291行. 其中最主要的计算步骤时 ScanMatch 和 TryCloseLoop 两个过程。
接下来分析 ScanMatch 过程

ScanMatch 过程UML

  在ScanMatch类中， MatchScan（363） 时整个处理过程的入口函数，其中引入的参量是

实例化 2258：
m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, covariance);

定义处 363：
kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const LocalizedRangeScanVector& rBaseScans, Pose2& rMean, Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
* Match given scan against set of scans
* @param pScan scan being scan-matched 此次激光数据
* @param rBaseScans set of scans whose points will mark cells in grid as being occupied 使用runningScan的device
* @param rMean output parameter of mean (best pose) of match 最好的位姿（刚初始化）
* @param rCovariance output parameter of covariance of match 协方差矩阵（刚初始化）
*
* @param doPenalize whether to penalize matches further from the search center 是否做补偿
* @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true) 重新做精匹配
* @return strength of response 返回响应值
//1、 新建了一个矫正位姿用的栅格图，以当前激光位姿为中心，根据offset设置的值计算出这个栅格图的边界
///其中， 栅格图的大小是 rangeThreshold / resolution， 即激光的范围除以解析度。

Created with Raphaël 2.1.2 开始 得到scan的位置 存在激光数据 获得矫正位姿栅格图的size CorrelationGrid( rangeThreshold / resolution) 考虑补偿，计算出一个有效范围（将激光位置放在 CorrelationGrid的中心，加入补偿,计算出grid左下角位置） 设置矫正位姿图（加入runningScan，此次Scan的中心） 设置粗匹配在 x，y方向上的范围和解析度 调用 CorrelateScan（ ） 函数进行 scan-matching, 得到 bestResponse. 参数UseResponseExpansion 为True？ 如果 bestResponse == 0.0 ？ 扩大匹配角度补偿20度，在60度范围内进行搜索CorrelateScan() 参数doRefineMatch 为 True？ 解析度设置为粗匹配的 0.5， 再次进行CorrelateScan() 返回bestResponse 结束 rCovariance(0, 0) rCovariance(1, 1)设置为最大500， rCovariance(2, 2) =4 * math::Square（CoarseAngleResolution） 返回 response=0.0 结束 yes no yes no yes no yes no

ScanMatch函数源码

源码 363：
kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const LocalizedRangeScanVector& rBaseScans, Pose2& rMean,
                                   Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
  {
    ///////////////////////////////////////
    // set scan pose to be center of grid

    // 1. get scan position
    Pose2 scanPose = pScan->GetSensorPose();

    // scan has no readings; cannot do scan matching
    // best guess of pose is based off of adjusted odometer reading
    if (pScan->GetNumberOfRangeReadings() == 0)
    {
      rMean = scanPose;

      // maximum covariance
      rCovariance(0, 0) = MAX_VARIANCE;  // XX
      rCovariance(1, 1) = MAX_VARIANCE;  // YY
      rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue());  // TH*TH

      return 0.0;
    }

    // 2. get size of grid
    Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();

    // 3. compute offset (in meters - lower left corner)
    Vector2<kt_double> offset;
    offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution()));
    offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution()));

    // 4. set offset
    m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);

    ///////////////////////////////////////

    // set up correlation grid
    AddScans(rBaseScans, scanPose.GetPosition());

    // compute how far to search in each direction
    Vector2<kt_double> searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight());
    Vector2<kt_double> coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(),
                                          0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution());

    // a coarse search only checks half the cells in each dimension
    Vector2<kt_double> coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(),
                                              2 * m_pCorrelationGrid->GetResolution());

    // actual scan-matching
    kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
                                           m_pMapper->m_pCoarseSearchAngleOffset->GetValue(),
                                           m_pMapper->m_pCoarseAngleResolution->GetValue(),
                                           doPenalize, rMean, rCovariance, false);

    if (m_pMapper->m_pUseResponseExpansion->GetValue() == true)
    {
      if (math::DoubleEqual(bestResponse, 0.0))
      {
#ifdef KARTO_DEBUG
        std::cout << "Mapper Info: Expanding response search space!" << std::endl;
#endif
        // try and increase search angle offset with 20 degrees and do another match
        kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue();
        for (kt_int32u i = 0; i < 3; i++)
        {
          newSearchAngleOffset += math::DegreesToRadians(20);

          bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
                                       newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
                                       doPenalize, rMean, rCovariance, false);

          if (math::DoubleEqual(bestResponse, 0.0) == false)
          {
            break;
          }
        }

#ifdef KARTO_DEBUG
        if (math::DoubleEqual(bestResponse, 0.0))
        {
          std::cout << "Mapper Warning: Unable to calculate response!" << std::endl;
        }
#endif
      }
    }

    if (doRefineMatch)
    {
      Vector2<kt_double> fineSearchOffset(coarseSearchResolution * 0.5);
      Vector2<kt_double> fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution());
      bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
                                   0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
                                   m_pMapper->m_pFineSearchAngleOffset->GetValue(),
                                   doPenalize, rMean, rCovariance, true);
    }

#ifdef KARTO_DEBUG
    std::cout << "  BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ",  VARIANCE = "
              << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl;
#endif
    assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));

    return bestResponse;
  }

FindPossibleLoopClosure函数 UML

Mapper.cpp 第1552行
函数定义
LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan* pScan,const Name& rSensorName,
kt_int32u& rStartNum)

函数调用： tryClosedLoop 中
FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
//其中 scanIndex = 0
//函数主要功能是在当前节点周围寻找到可以可能的回环点
// LoopMatchMinimumChainSize 参数不是对 candidate的要求，而是对结果的限制，防止candidate太多，而我们就需要很多candidate，不然没法做，所以应该增大这个参数，保留更多的可能性。

Created with Raphaël 2.1.2 输入激光数据，device，激光ID 调用FindNearLinkedScans 函数，找到相邻的nearLinkedScans 得到所有的 scans数目 nScans rStartNum < nScans ? 是否遍历完所有scans 结束 返回chain 找到激光，求出与当前scan几何距离 在LoopSearchMaximumDistance范围内且足够长 将CandidateScan推到chain中 CandatecChain要大于LoopMatchMinimumChainSize参数 清空该数据 chain.clear（） yes no yes no yes no

FindPossibleLoopClosure 函数源码

Mapper.cpp 第1552行

 LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan* pScan,
                                                                const Name& rSensorName,
                                                                kt_int32u& rStartNum)
  {
    LocalizedRangeScanVector chain;  // return value 想得到的 chain

    Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); // 应该是得到参考点的中心位置坐标

    // possible loop closure chain should not include close scans that have a
    // path of links to the scan of interest
    const LocalizedRangeScanVector nearLinkedScans =
          FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue()); // 简单的 VISIT 周围的节点

    kt_int32u nScans = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size()); // 得到目前节点的总数目
    for (; rStartNum < nScans; rStartNum++) // rStartNum = 0 就是说这里会从序号0开始遍历所有的节点
    {
      LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum); // 得到 参考点对象

      Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); // 得到参考点的位置

      kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition()); // 查找两者的距离
      if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE) // 是否在规定距离之内
      {
        // a linked scan cannot be in the chain
        if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end()) // find 是 std 标准函数， 在 nearlinkedScan 中查找是否有这个scan
        {
          chain.clear(); // 如果存在清空这个
        }
        else
        {
          chain.push_back(pCandidateScan); // 推入
        }
      }
      else // 不在规定的距离内
      {
        // return chain if it is long "enough"
        if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) // 大于最大的 candidate 数目
        {
          return chain;
        }
        else
        {
          chain.clear();
        }
      }
    }

    return chain;
  }

TryCloseLoop 函数 UML

• 函数定义：1228
  kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
• 函数调用部分
  m_pGraph->TryCloseLoop(pScan, *iter)
  // 其中iter是在 Macros.h中定义的迭代器。
  输入 ： 激光数据， 此次的device

Created with Raphaël 2.1.2 开始 !candidateChain.empty() 调用LoopScanMatcher->MatchScan()函数 粗匹配成功协方差够小 精匹配 MatchScan（） 精匹配小于最小值阈值 FireLoopClosureCheck("REJECTED!") 重新查找 FireEndLoopClosure 返回bestResponse 结束 FireBeginLoopClosure("Closing loop...") CorrectPoses() yes no yes no yes no

TryCloseLoop 函数源码

Mapp.cpp 1228行

 kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
  {
    kt_bool loopClosed = false;

    kt_int32u scanIndex = 0;

    LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);

    while (!candidateChain.empty())
    {
      Pose2 bestPose;
      Matrix3 covariance;
      kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain,
                                                               bestPose, covariance, false, false);

      std::stringstream stream;
      stream << "COARSE RESPONSE: " << coarseResponse
             << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")"
             << std::endl;
      stream << "            var: " << covariance(0, 0) << ",  " << covariance(1, 1)
             << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")";

      m_pMapper->FireLoopClosureCheck(stream.str());

      if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
          (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
          (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
      {
        LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector());
        tmpScan.SetUniqueId(pScan->GetUniqueId());
        tmpScan.SetTime(pScan->GetTime());
        tmpScan.SetStateId(pScan->GetStateId());
        tmpScan.SetCorrectedPose(pScan->GetCorrectedPose());
        tmpScan.SetSensorPose(bestPose);  // This also updates OdometricPose.
        kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain,
                                                                                bestPose, covariance, false);

        std::stringstream stream1;
        stream1 << "FINE RESPONSE: " << fineResponse << " (>"
                << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl;
        m_pMapper->FireLoopClosureCheck(stream1.str());

        if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue())
        {
          m_pMapper->FireLoopClosureCheck("REJECTED!");
        }
        else
        {
          m_pMapper->FireBeginLoopClosure("Closing loop...");

          pScan->SetSensorPose(bestPose);
          LinkChainToScan(candidateChain, pScan, bestPose, covariance);
          CorrectPoses();

          m_pMapper->FireEndLoopClosure("Loop closed!");

      {
        // initialize mapper with range threshold from device 地图的范围
        Initialize(pLaserRangeFinder->GetRangeThreshold());
      }

      // get last scan
      LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName());

      // update scans corrected pose based on last correction
      if (pLastScan != NULL)
      {
        Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
        pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose()));
      }

      // test if scan is outside minimum boundary or if heading is larger then minimum heading
      if (!HasMovedEnough(pScan, pLastScan))
      {
        return false;
      }

      Matrix3 covariance;
      covariance.SetToIdentity();

      // correct scan (if not first scan) 从running scan 上开始增加 MatchScan
      if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
      {
        Pose2 bestPose;
        m_pSequentialScanMatcher->MatchScan(pScan,
                                            m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
                                                                                    bestPose,
                                                                                    covariance);
        pScan->SetSensorPose(bestPose);
      }

      // add scan to buffer and assign id
      m_pMapperSensorManager->AddScan(pScan);

      if (m_pUseScanMatching->GetValue())
      {
        // add to graph
        m_pGraph->AddVertex(pScan);
        m_pGraph->AddEdges(pScan, covariance);

        m_pMapperSensorManager->AddRunningScan(pScan);

        if (m_pDoLoopClosing->GetValue())
        {
          std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
          const_forEach(std::vector<Name>, &deviceNames)
          {
            m_pGraph->TryCloseLoop(pScan, *iter);
          }
        }
      }

      m_pMapperSensorManager->SetLastScan(pScan);

      return true;
    }

    return false;
  }

 这就是 Process 的主要流程，对应于源代码的 2217-2291行. 其中最主要的计算步骤时 ScanMatch 和 TryCloseLoop 两个过程。
接下来分析 ScanMatch 过程

问题与思考

哪些节点可以成为回环检测的 Candidate？

答： Candidate 主要是在 FindPossibleLoopClosure（）中获得的， 在这个函数当中 会从第0个激光数据开始，遍历所有的数据节点。 比较每个数据节点与当前节点之间的距离，在 LoopSearchMaximumDistance 范围内且不在 linked Scan 中的将成为 Candidate。 当遇到节点在节点 LoopSearchMaximumDistance 距离范围外， 且超过了 LoopMatchMinimumChainSize 参数的， 将会返回 chain。

所以范围内的所有非邻点都会成为 candidate， 但这个总数不应该超过最大的 Candidate 数目。

减少 Candidate 数目是否会降低CPU运算量？ 怎样减少Candidate数目？

答： 减少Candidate数目， 可以从以下几个方面入手

1、 减小 LoopSearchMaximumDistance， 这样进入Candidate 范围的数目就少。
2、 增大 minimum_travel_distance 和 minimum_travel_heading 这样的话会减少总体节点数目， 进入范围内的节点密度会减小。