Karto 后端

后端使用的Sparse Bunble Adjustment优化库:

-https://github.com/ros-perception/sparse_bundle_adjustment+三

前面的前端相关匹配完成后，更新一次当前帧激光器的位姿，这是先验。

后面接着先把当前激光帧加入到该激光器的激光容器中。

1.添加顶点Vertex m_pGraph->AddVertex(pScan)

用当前激光帧添加一个顶点

2.添加边

边之间的约束即位姿变换说明， 上一帧的雷达后验或前端匹配的位姿与当前帧前端匹配位姿之间的变换， 就是把当前帧前端匹配位姿转到上一帧的雷达后验或前端匹配的位姿下。当前不仅仅是前一帧，也可以是离得足够近得帧等其他。

可以构成边Karto中有三种情况：

1.当前帧与其上一帧

2.本帧与滑动窗口中距离最近的帧果构成的边

3.找到与当前激光帧直接连接或间接连接的激光帧构成的链；
这条链中的每一个激光帧所在激光序列，时间上连续的，其前面和后面
激光帧构成的激光链，这样就有一些激光链了，找每条激光帧与当前激光帧最近的
激光帧，如果该链的激光帧数大于回环的最小帧数，且当前帧与该链进行精匹配的匹配值
大于最小关联精匹配的匹配值时，构成一个条边

会添加这三种边约束

4.将当前激光帧加入到滑动窗口 AddRunningScan(pScan)

5.回环检测及优化 默认是LM算法

 const_forEach(std::vector<Name>, &deviceNames)//多个激光器时，一般就一个

          {

            m_pGraph->TryCloseLoop(pScan, *iter);//每个激光器，都会进行一次全局的回环优化，如果找到回环边的话

          }

这个回环是针对每个激光器的激光帧序列进行的

进入TryCloseLoop 首先是函数FindPossibleLoopClosure，这个用来找回环的，首先搜索当前激光帧附近的有关联的激光链，与当前帧距离小于最大回环搜索距离，这个关联是指与当前帧构成边或是与当前帧够成边的激光帧之间构成边，这就构成一帧网了，但是这些帧与当前帧的距离要小于最大回环搜索距离。

然后根据传入的激光器名字，开始遍历这个激光器到目前时刻所有的激光关键帧，当然遍历其中与当前激光帧距离小于最大回环搜索距离的激光帧，如果满足这个要求则进一步考虑，这个激光帧不能在上面得到与当前激光帧关联的激光帧序列里，而且要连续直到距离大于了最大回环搜索距离，因为程序中，需要有一帧在上面计算的关联序列里就会情况可能回环激光帧集合chain。最后返回这个chain.

然后开始回环检测了，先进行一次回环的粗匹配，如果匹配得分大于最小回环粗匹配得分且匹配后的协方差小于回环最大协方差，则开始回环精匹配，如果匹配得分大于回环精匹配得分，则先用本次匹配得到当前激光帧位姿更新下当前激光帧位姿，然后添加这条回环边，即将当前帧与上面找的可能回环帧集合中的最近的激光帧之间构成一条边就是回环边了。

然后就是全局优化了：

 //SPA全局优化  优化加入的SPA优化器节点node中的每个激光时刻的位姿

   CorrectPoses();

来看下

void MapperGraph::CorrectPoses()

  {

    // optimize scans!

    ScanSolver* pSolver = m_pMapper->m_pScanOptimizer;

    if (pSolver != NULL)

    {

      pSolver->Compute();//调用SPA库优化  会优化每个激光位姿

      //将之前各个激光器位姿更新为优化后的位姿, 这里是更新每个激光帧时刻的位姿

      const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())

      {

        m_pMapper->m_pMapperSensorManager->GetScan(iter->first)->SetSensorPose(iter->second);

      }

      pSolver->Clear();

    }

  }

  pSolver->Compute();//调用SPA库优化  会优化每个激光位姿

这个函数里会调用sparse_bundle_adjustment库的doSPA()函数，会对所有激光帧位姿进行一次全局优化。都存入

 corrections.push_back(std::make_pair(iter->nodeId, pose));

然后设置下这个激光器目前所有的激光帧位姿

const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())

      {

        m_pMapper->m_pMapperSensorManager->GetScan(iter->first)->SetSensorPose(iter->second);

      }

首先会搜索当前附近的激光帧， 当前帧之前的激光帧中与当前帧之间构成直接或间接约束的激光帧，且距离满足小于回环最大值，构成一个激光链

然后将当前帧与该激光链进行精匹配，若匹配值大于最小回环匹配值，构建一个边约束，就第2种构成边的约束方式

最后更新位姿 CorrectPoses(), 这里调用了SBA库实现。然后更新优化后的位姿

http://www2.imm.dtu.dk/pubdb/edoc/imm3215.pdf

LM的算法伪代码：

 所用的库为sparse bundle adjustment，地址：
https://github.com/ros-perception/sparse_bundle_adjustment

其论文中称其为 SPA Sparse Pose Adjustment (SPA),论文;

《Efficient Sparse Pose Adjustment for 2D Mapping》