从零开始学自动驾驶(7)——后端优化

此文对应任佬代码tag10.0

这部分代码总体分为三部分：一是定义g2o的边，二是g2o优化算法的封装，三是back_end对优化算法的调用。

我的习惯一般是从算法的调用看起：

建图算法的调用

（1）首先看对优化后的信息的接受和发布

先看back_end_node文件是如何调用建图算法，首先用回调函数启动输入ros指令函数，然后通过输入指令的方式打开建图开关。

 ForceOptimize()函数：

用于对优化的关键帧进行输出：

 GetOptimizedKeyFrames()函数：

获取优化后的关键帧：

GetOptimizedPose()函数：

获取优化后的关键帧的位姿：

至此对于优化后信息的接受和发布已经完成。

（2）后端优化算法：

依旧是node节点下的函数Run()启动后端整个优化流程：

_back_end_flow_ptr->Run();

Run()函数：

UpdateBackEnd()函数：

激光里程计初始化，以及更新

 Update函数():

保存激光里程计 和 Gnss 的轨迹信息

如果由新的关键帧，则启动添加图优化顶点和边的节点，在进行优化

AddNodeAndEdge()函数：

添加图优化的顶点和边，顶点是待优化的关键帧位姿，边为待优化的误差

 AddSe3Node()函数：

添加顶点信息：

AddSe3Edge()函数：

 AddSe3Edge()函数:

 添加完顶点和边，进行判断是否进行优化

MaybeOptimized()函数：

 Optimize()函数：

进行优化：

到这里主函数算是完事了。。。

后端优化预处理

在启动node节点的时候，有这样一行代码

_back_end_flow_ptr = std::make_shared<BackEndFlow>(nh);

 这行代码启动了用于接受和发布消息的功能：

 其中，下面这个启动了一些定义参数，选择优化器等功能

//启动了后端代码进行了许许多多的预处理

back_end_ptr_ = std::make_shared<BackEnd>();

具体看一下：

config.yaml参数配置文件：

 到此后端该有的代码应该都有了。。。