karto探秘之open_karto 第四章 --- 回环检测与后端优化

1 Mapper::Process()

主函数中与后端优化和回环检测相关的代码如下所示:

  kt_bool Mapper::Process(LocalizedRangeScan* pScan)
  {
      // 省略...

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

        //边是具有约束关系的两个顶点，在 AddEdges中的操作有： 寻找可以连接的两个帧，计算scanMatch，同时保存scanMatch的结果到Graph之中，然后添加到SPA里面
        m_pGraph->AddEdges(pScan, covariance);

        m_pMapperSensorManager->AddRunningScan(pScan);  // 这里面有RunningScan的维护，即删除不合适的头

        //进行回环检测的步骤
        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);   //每一次的 LastScan都是一个正常的pScan，而不是空的
      return true;
    }
    return false;
  }

进行完扫描匹配之后,得到了当前scan的最优位姿. 之后向图结构中添加顶点和边, 再之后是进行回环检测.

2 MapperGraph::AddVertex()

之后,进行后端的第一步,将当前scan作为节点添加到图结构中.

这里的图结构有2个,
一个是 karto里的Graph类, 用于查找回环时,在这个图上做广度优先搜索.

这里的Graph::AddVertex() ，指的是调用MapperGraph的基类Graph 的AddVertex()函数，进行节点的保存．

第二个就是稀疏位姿图spa中的图结构了, 用于做全局位姿优化.

  /**
   * Adds a vertex representing the given scan to the graph
   * @param pScan
   */
  void MapperGraph::AddVertex(LocalizedRangeScan* pScan)
  {
    assert(pScan);

    if (pScan != NULL)
    {
      // 将当前scan转换成 vertex
      Vertex<LocalizedRangeScan>* pVertex = new Vertex<LocalizedRangeScan>(pScan);

      // 将vertex加入Graph图结构中 
      Graph<LocalizedRangeScan>::AddVertex(pScan->GetSensorName(), pVertex);

      if (m_pMapper->m_pScanOptimizer != NULL)
      {
        //使用SPA进行优化，将节点添加进 <sparse_bundle_adjustment/spa2d.h> 的求解类型中
        m_pMapper->m_pScanOptimizer->AddNode(pVertex);
      }
    }
  }


// spa_solver.cpp
void SpaSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
{
  karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
  Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
  m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
}

3 MapperGraph::AddEdges()

// 向图中添加边结构
void MapperGraph::AddEdges(LocalizedRangeScan *pScan, const Matrix3 &rCovariance)
{
    MapperSensorManager *pSensorManager = m_pMapper->m_pMapperSensorManager;

    const Name &rSensorName = pScan->GetSensorName();

    // link to previous scan
    kt_int32s previousScanNum = pScan->GetStateId() - 1;
    if (pSensorManager->GetLastScan(rSensorName) != NULL)
    {
        assert(previousScanNum >= 0);
        // 添加边的第一种方式，总共有三种方式
        // 第一种 将前一帧雷达数据与当前雷达数据连接，添加边约束，添加1条边
        LinkScans(pSensorManager->GetScan(rSensorName, previousScanNum), pScan, pScan->GetSensorPose(), rCovariance);
    }

    Pose2Vector means;
    std::vector<Matrix3> covariances;

    // first scan (link to first scan of other robots)
    // 如果是第一帧雷达数据
    if (pSensorManager->GetLastScan(rSensorName) == NULL)
    {
        assert(pSensorManager->GetScans(rSensorName).size() == 1);

        std::vector<Name> deviceNames = pSensorManager->GetSensorNames();
        // 看看是不是还有其他的设备在传输 scan，对于单一的雷达，进不去循环
        forEach(std::vector<Name>, &deviceNames)
        {
            const Name &rCandidateSensorName = *iter;

            // skip if candidate device is the same or other device has no scans
            if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).empty()))
            {
                continue;
            }

            Pose2 bestPose;
            Matrix3 covariance;
            kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan,
                                                                                pSensorManager->GetScans(rCandidateSensorName),
                                                                                bestPose, covariance);
            LinkScans(pSensorManager->GetScan(rCandidateSensorName, 0), pScan, bestPose, covariance);

            // only add to means and covariances if response was high "enough"
            if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue())
            {
                means.push_back(bestPose);
                covariances.push_back(covariance);
            }
        }
    }
    else
    {
        // link to running scans
        // 第二种 将当前scan 与 running scans 添加边，running scans 就是最近的70个scan，所有的running scans只添加1条边
        Pose2 scanPose = pScan->GetSensorPose();
        means.push_back(scanPose);
        covariances.push_back(rCovariance);
        LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance);
    }

    // link to other near chains (chains that include new scan are invalid)
    // 第三种 将当前scan 与 NearChains scans 添加边
    LinkNearChains(pScan, means, covariances);

    if (!means.empty())
    {
        pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
    }
}

向位姿图中添加边一共存在三种方式:

第一种: LinkScans()

将前一帧雷达数据与当前雷达数据连接，添加边约束. 调用一次SpaSolver::AddConstraint().

第二种 LinkChainToScan()

karto中将 一定范围内的 多个连续的 scan 称为一条chain, 中文不好翻译…就直接这么叫吧.

running scans 是在当前scan之前的连续的20m范围内的scan.

将running scans 作为一条chain, 将当前scan与这个chain添加一条边, 在所有的running scans中找到距离当前scan最近的一个scan, 再调用 LinkScans() 将这两个scan添加一条边.

第三种 LinkNearChains()

首先Graph图结构中搜索与当前scan的距离在一定阈值范围内的所有的scan, 这些scan被karto称为near scan .

然后根据near scan的index, 为每一个near scan 扩充成为一条chain, 以index增加 和 index减小 两个方向将 near scan 扩充成为一条chain. 同时, 这条chain 不可以包含当前的scan, 这种chain被成为 near chain.

3.1 将当前scan与前一帧scan添加边

3.1.1 MapperGraph::LinkScans()

// 将前一帧雷达数据与当前雷达数据连接，添加边约束，添加1条边
void MapperGraph::LinkScans(LocalizedRangeScan *pFromScan, LocalizedRangeScan *pToScan,
                            const Pose2 &rMean, const Matrix3 &rCovariance)
{
    kt_bool isNewEdge = true;
    // 将source与target连接起来，或者说将pFromScan与pToScan连接起来
    Edge<LocalizedRangeScan> *pEdge = AddEdge(pFromScan, pToScan, isNewEdge);

    // only attach link information if the edge is new
    if (isNewEdge == true)
    {
        pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance));
        if (m_pMapper->m_pScanOptimizer != NULL)
        {
            m_pMapper->m_pScanOptimizer->AddConstraint(pEdge);
        }
    }
}

3.1.2 SpaSolver::AddConstraint()

void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge)
{
    karto::LocalizedRangeScan *pSource = pEdge->GetSource()->GetObject();
    karto::LocalizedRangeScan *pTarget = pEdge->GetTarget()->GetObject();
    karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());

    // 两个位姿间的坐标变换
    karto::Pose2 diff = pLinkInfo->GetPoseDifference();
    Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());

    // 信息矩阵, 协方差矩阵的逆
    karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
    Eigen::Matrix<double, 3, 3> m;
    m(0, 0) = precisionMatrix(0, 0);
    m(0, 1) = m(1, 0) = precisionMatrix(0, 1);
    m(0, 2) = m(2, 0) = precisionMatrix(0, 2);
    m(1, 1) = precisionMatrix(1, 1);
    m(1, 2) = m(2, 1) = precisionMatrix(1, 2);
    m(2, 2) = precisionMatrix(2, 2);

    m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
}

3.2 将当前scan与 running scans 添加边：MapperGraph::LinkChainToScan()

// 将当前scan 与 scanchain 添加边，在所有的数据中找到和当前scan距离最近的一个scan,与当前scan添加一条边
void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector &rChain, LocalizedRangeScan *pScan,
                                  const Pose2 &rMean, const Matrix3 &rCovariance)
{
    Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());

    LocalizedRangeScan *pClosestScan = GetClosestScanToPose(rChain, pose);
    assert(pClosestScan != NULL);

    Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());

    kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());
    if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
    {
        LinkScans(pClosestScan, pScan, rMean, rCovariance);
    }
}

3.3 将当前scan与 near chains 添加边：MapperGraph::LinkNearChains()

3.3.1 LinkNearChains()

LinkNearChains() 函数首先调用 FindNearChains() 获取 near chain .

之后再对每一个有效的near chain 与 当前scan 添加边, 每一个near chain 会调用一次 LinkChainToScan().

这其实也算是个回环检测, 只不过这个搜索的范围会小一些.

// 将满足要求的near chain在图结构中添加一条边
void MapperGraph::LinkNearChains(LocalizedRangeScan *pScan, Pose2Vector &rMeans, std::vector<Matrix3> &rCovariances)
{
    // 找到满足要求的 near chain
    const std::vector<LocalizedRangeScanVector> nearChains = FindNearChains(pScan);
    const_forEach(std::vector<LocalizedRangeScanVector>, &nearChains)
    {
        // chain中scan的个数要满足要求
        if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
        {
            continue;
        }

        Pose2 mean;
        Matrix3 covariance;
        // match scan against "near" chain
        // 将当前scan 对 所有的chain 进行匹配,匹配得分大于阈值就将这条chain与scan添加一个边
        kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false);
        if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE)
        {
            rMeans.push_back(mean);
            rCovariances.push_back(covariance);
            // 得分满足要求就在图结构中 将这个chain添加一条边
            LinkChainToScan(*iter, pScan, mean, covariance);
        }
    }
}

3.3.2 FindNearChains()

FindNearChains() 函数调用FindNearLinkedScans() 来获取 near scan, 再对所有的near scan 扩充成为 near chain , 就会得到很多个 near chain , 之后通过将当前scan与这个chain进行扫描匹配, 计算下匹配得分, 如果匹配得分高于阈值就说明是个有效的 near chain.

// 找到不包含当前scan的一系列near chain
std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRangeScan *pScan)
{
    std::vector<LocalizedRangeScanVector> nearChains;

    Pose2 scanPose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());

    // to keep track of which scans have been added to a chain
    LocalizedRangeScanVector processed;

    // 在pscan周围查找可以链接的scan
    // m_pMapper->m_pLinkScanMaximumDistance 为1.5m
    const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan,
                                                                         m_pMapper->m_pLinkScanMaximumDistance->GetValue());
    // 利用每一个 LinkedScans 来扩充成一个chain， chain要满足不包含pscan的条件
    const_forEach(LocalizedRangeScanVector, &nearLinkedScans)
    {
        LocalizedRangeScan *pNearScan = *iter;

        if (pNearScan == pScan)
        {
            continue;
        }

        // scan has already been processed, skip
        // scan已经被添加过了就跳过
        if (find(processed.begin(), processed.end(), pNearScan) != processed.end())
        {
            continue;
        }

        processed.push_back(pNearScan);

        // build up chain
        kt_bool isValidChain = true;
        std::list<LocalizedRangeScan *> chain;

        // add scans before current scan being processed
        // 从当前scan的index-1开始遍历,直到index=0
        for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; candidateScanNum--)
        {
            LocalizedRangeScan *pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
                                                                                            candidateScanNum);

            // chain is invalid--contains scan being added
            // 包含当前scan的chain不能用
            if (pCandidateScan == pScan)
            {
                isValidChain = false;
            }

            Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
            kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());

            if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
            {
                chain.push_front(pCandidateScan);
                processed.push_back(pCandidateScan);
            }
            else
            {
                break;
            }
        }

        // 将当前scan加入到chain中
        chain.push_back(pNearScan);

        // add scans after current scan being processed
        // 从当前scan的index+1开始遍历,直到index=end
        kt_int32u end = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size());
        for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; candidateScanNum++)
        {
            LocalizedRangeScan *pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
                                                                                            candidateScanNum);
            // 包含当前scan的chain不能用
            if (pCandidateScan == pScan)
            {
                isValidChain = false;
            }

            Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
            kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());

            if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
            {
                chain.push_back(pCandidateScan);
                processed.push_back(pCandidateScan);
            }
            else
            {
                break;
            }
        }

        if (isValidChain)
        {
            // change list to vector
            LocalizedRangeScanVector tempChain;
            std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin()));
            // add chain to collection
            nearChains.push_back(tempChain);
        }
    }

    return nearChains;
}

3.3.3 FindNearLinkedScans()

// 根据传入不同的搜索距离,在一定范围内来寻找 NearLinkedScans
LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(LocalizedRangeScan *pScan, kt_double maxDistance)
{
    // 在pScan周围寻找 NearLinkedScans
    NearScanVisitor *pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue());
    LocalizedRangeScanVector nearLinkedScans = m_pTraversal->Traverse(GetVertex(pScan), pVisitor);
    delete pVisitor;

    return nearLinkedScans;
}

3.3.4 广度优先搜索算法 Traverse()

添加边时的LinkNearChains()函数 与 回环检测时的 FindPossibleLoopClosure()函数 都会调用 FindNearLinkedScans() 函数.

只不过前者是使用 FindNearLinkedScans() 的结果, 后者是将 FindNearLinkedScans() 的结果排除掉.

FindNearLinkedScans() 函数 是使用广度优先搜索算法, 在图结构中找到与当前节点距离在一定范围内的所有节点.

经常刷 leetcode 的同学可能会对这个算法很熟悉, 但是在slam中挺少遇到的, 所以这里将这一块拿出来着重讲一下.

所以, 同学们, 刷 leetcode 不是毫无作用的, 那里的算法是真正可以用到实际项目中的. slam 里就用到了.

    /**
     * Traverse the graph starting with the given vertex; applies the visitor to visited nodes
     * 广度优先搜索算法,查找给定范围内的所有的节点
     * @param pStartVertex
     * @param pVisitor
     * @return visited vertices
     */
    virtual std::vector<T *> Traverse(Vertex<T> *pStartVertex, Visitor<T> *pVisitor)
    {
        std::queue<Vertex<T> *> toVisit;
        std::set<Vertex<T> *> seenVertices;
        std::vector<Vertex<T> *> validVertices;

        toVisit.push(pStartVertex);
        seenVertices.insert(pStartVertex);

        do
        {
            Vertex<T> *pNext = toVisit.front();
            toVisit.pop();
            // 距离小于阈值就加入到validVertices中
            if (pVisitor->Visit(pNext))
            {
                // vertex is valid, explore neighbors
                validVertices.push_back(pNext);

                // 获取与这个节点相连的所有节点
                std::vector<Vertex<T> *> adjacentVertices = pNext->GetAdjacentVertices();
                forEach(typename std::vector<Vertex<T> *>, &adjacentVertices)
                {
                    Vertex<T> *pAdjacent = *iter;

                    // adjacent vertex has not yet been seen, add to queue for processing
                    if (seenVertices.find(pAdjacent) == seenVertices.end())
                    {
                        // 如果没有使用过,就加入到toVisit中,同时加入seenVertices以防止重复使用
                        toVisit.push(pAdjacent);
                        seenVertices.insert(pAdjacent);
                    }
                }
            }
        } while (toVisit.empty() == false);

        // 将结果保存成vector
        std::vector<T *> objects;
        forEach(typename std::vector<Vertex<T> *>, &validVertices)
        {
            objects.push_back((*iter)->GetObject());
        }

        return objects;
    }

3.3.6 GetAdjacentVertices()

    /**
     * Gets a vector of the vertices adjacent to this vertex
     * @return adjacent vertices
     */
    std::vector<Vertex<T> *> GetAdjacentVertices() const
    {
        std::vector<Vertex<T> *> vertices;

        const_forEach(typename std::vector<Edge<T> *>, &m_Edges)
        {
            Edge<T> *pEdge = *iter;

            // check both source and target because we have a undirected graph
            if (pEdge->GetSource() != this)
            {
                vertices.push_back(pEdge->GetSource());
            }

            if (pEdge->GetTarget() != this)
            {
                vertices.push_back(pEdge->GetTarget());
            }
        }

        return vertices;
    }

4 回环检测与全局优化

4.1 TryCloseLoop()

TryCloseLoop() 函数首先调用 FindPossibleLoopClosure() 获取可能是回环的 chain , 然后使用当前scan与这个chain进行粗扫描匹配，如果响应值大于阈值，协方差小于阈值，再进行精匹配，如果精匹配的得分（响应值）大于阈值，则找到了回环

如果得分大于阈值, 就认为找到了一个回环, 调用LinkChainToScan() 函数添加一条边, 并紧接着调用 CorrectPoses() 函数执行全局优化,重新计算所有位姿.

之后再调用FindPossibleLoopClosure() 函数, 按照上次的index继续进行回环检测与匹配, 如果有回环了再进行扫描匹配. 直到index为最后一个scan的索引为止.

// 进行回环检测,找到回环后再进行全局优化
kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan *pScan, const Name &rSensorName)
{
    kt_bool loopClosed = false;

    kt_int32u scanIndex = 0;
    
    // 通过FindPossibleLoopClosure遍历所有之前的scan,计算其与当前pscan的距离
    // 找到 连续的几帧和当前帧的距离小于阈值，同时不属于 Near Linked Scans的 几个scan,也是一条chain
    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);

        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.

            // 精匹配 这里的这句和上面 不同之处在于 MatchScan的最后一个参数, 这里使用了缺省值 true，因此进行细匹配
            kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain,bestPose, covariance, false);

            if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue())
            {
                // m_pMapper->FireLoopClosureCheck("REJECTED!");
            }
            else
            {
                // 精匹配得分大于阈值,认为找到回环了
                pScan->SetSensorPose(bestPose);

                // 将chain添加边约束，1个candidateChain添加1个边约束，等会要用来优化
                LinkChainToScan(candidateChain, pScan, bestPose, covariance);

                // 找到回环了立刻执行一下全局优化,重新计算所有位姿
                CorrectPoses();
                loopClosed = true;
            }
        }
        // 计算下一个candidateChain, scanIndex会继续增加
        candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
    }

    return loopClosed;
}

4.3 FindPossibleLoopClosure()

FindPossibleLoopClosure() 首先调用 FindNearLinkedScans() 获取当前节点的 near scan, 这些near scan 不可以作为回环.

这块我没搞懂, 可能是因为 near scan 已经在上边的程序中使用过了. 但是这块的搜索距离又和上边的不一样… 不理解.

之后, 对所有之前保存的scan做依次遍历, 计算与当前scan的距离.

如果距离小于m_pLoopSearchMaximumDistance, 那说明这附近可能是个存在回环的区域, 在这个区域范围内时向chain中添加数据.

当距离已经大于 m_pLoopSearchMaximumDistance , 说明已经出了回环可能存在的区域, 当出了这个区域时停止向chain添加scan,并判断这个chain是否是个有效的chain

有效的chain 要满足几个条件:

• 要满足连续的几个候选解都在这个范围内才能是一个有效的chain.
• 如果 pCandidateScan 在 nearLinkedScans 中, 就将这个chain删掉.
• chain中的scan个数大于阈值m_pLoopMatchMinimumChainSize

如果找到了一个有效的chain, 程序就先退出, 并将当前处理到的scan的index返回.

// 遍历之前保存的每个scan,计算与当前pscan的距离,如果能够满足连续几个scan与pscan的距离都在阈值范围内,则认为找到了一个可能的回环
LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan *pScan,
                                                              const Name &rSensorName,
                                                              kt_int32u &rStartNum)
{
    LocalizedRangeScanVector chain; // return value
    // 得到 当前帧的扫描点世界坐标的平均值
    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
    
    // m_pMapper->m_pLoopSearchMaximumDistance可以设置为15m

    // 找到可以作为near linked 的scan,这些scan不可作回环
    const LocalizedRangeScanVector nearLinkedScans =
        FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue());

    kt_int32u nScans = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size());
    for (; rStartNum < nScans; rStartNum++)
    {
        // 遍历每个scan, 每个scan都是可能是回环的候选项
        LocalizedRangeScan *pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum);
        // 获取候选项的位姿
        Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
        // 计算当前scan与候选项的距离的平方
        kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
        // 如果距离小于m_pLoopSearchMaximumDistance, 那说明这附近可能是个存在回环的区域
        // 在这个区域范围内时 向chain中添加数据, 要满足连续的几个候选解都在这个范围内才能是一个有效的chain
        if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE)
        {
            // a linked scan cannot be in the chain
            // 如果 pCandidateScan 在 nearLinkedScans 中,就将这个chain删掉
            if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end())
            {
                chain.clear();
            }
            // 在这个区域范围内时 向chain中添加数据
            else
            {
                chain.push_back(pCandidateScan);
            }
        }
        // 当距离已经大于 m_pLoopSearchMaximumDistance , 说明已经出了回环可能存在的区域
        // 当出了这个区域时停止向chain添加scan,并判断这个chain是否是个有效的chain
        else
        {
            // return chain if it is long "enough"
            // 如果chain中的scan个数大于阈值,说明是个有效的回环chain,提前返回
            if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
            {
                return chain;
            }
            // 个数小于阈值,是个无效的回环chain,删掉
            else
            {
                chain.clear();
            }
        }
    }

    return chain;
}

4.4 执行位姿图优化

4.4.1 MapperGraph::CorrectPoses()

CorrectPoses() 是进行位姿图优化求解的地方。将这个函数放在了 回环检测的里边，也就是说，只有在回环检测通过（2帧scan匹配得分大于阈值时）的情况下，才进行一次图优化求解。

// 执行后端优化,校正所有的位姿
void MapperGraph::CorrectPoses()
{
    // optimize scans!
    ScanSolver *pSolver = m_pMapper->m_pScanOptimizer;
    if (pSolver != NULL)
    {
        // 执行优化操作
        pSolver->Compute();

        const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())
        {
            // 保存优化后的所有位姿
            m_pMapper->m_pMapperSensorManager->GetScan(iter->first)->SetSensorPose(iter->second);
        }

        pSolver->Clear();
    }
}

4.4.2 SpaSolver::Compute()

// 进行全局优化
void SpaSolver::Compute()
{
    corrections.clear();

    typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d>> NodeVector;

    ROS_INFO("Calling doSPA for loop closure");
    // 进行优化求解
    m_Spa.doSPA(40);
    ROS_INFO("Finished doSPA for loop closure");

    // 将优化后的位姿转换成karto的格式
    NodeVector nodes = m_Spa.getNodes();
    forEach(NodeVector, &nodes)
    {
        karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
        corrections.push_back(std::make_pair(iter->nodeId, pose));
    }
}

图优化求解的结果：

corrections 存储的是图结构 所有的 顶点id 与 顶点
图优化的结果是 更新了所有顶点（也就是有效scan）的坐标值

总结

1. karto的后端优化是和回环检测结合在一起的，首先向SPA中添加顶点，再通过3种方式向SPA中添加边结构，然后试着找回环。

2. 找回环：首先通过坐标距离小于阈值, 并且形成的chain中scan的个数大于阈值，才算是一个候选的回环chain。然后使用当前scan与这个chain进行粗扫描匹配，如果响应值大于阈值，协方差小于阈值，再进行精匹配，如果精匹配的得分（响应值）大于阈值，则找到了回环。

3. 找到了回环则将回环约束作为边结构添加到SPA中，然后进行图优化求解，更新所有scan的坐标。

4. 通过 扫描匹配 与 后端优化环节可知，整个open_karto没有维护地图, 每次扫描匹配的地图都是通过一系列的scan新生成的, 是非常省资源的。
   最后的栅格地图是通过所有优化后的scan进行生成的，在slam_karto进行进行调用，不用维护整个栅格地图，非常的省cpu与内存。

REFERENCES

加入了详细中文解释的open_karto地址: https://github.com/kadn/open_karto

Karto_slam框架与代码解析 https://blog.csdn.net/qq_24893115/article/details/52965410

Karto SLAM之open_karto代码学习笔记（二） https://blog.csdn.net/wphkadn/article/details/90247146

SLAM_Karto 学习（四） 深入理解 ScanMatch 过程 https://blog.csdn.net/Fourier_Legend/article/details/81558886?utm_source=blogxgwz0#process%E5%87%BD%E6%95%B0uml