[人工智能]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算法

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

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

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