hdl_graph_slam源码解析（六）

6. 后端优化

6.1 pose graph

hdl_graph_slam采用g2o来进行后端优化，也就是对优化目标函数的工作了。再解析这部分源码之前，我们首先来看下hdl_graph_slam对应的位姿图(pose graph)是什么样子的呢？首先肯定有由激光里程计构成的运动约束，然后还有地面检测和回环检测构成的测量约束，整个pose graph可以用下图来表示：

图中黑色直线代表的是激光里程计构成的运动约束，从数学的角度来讲是两个SE(3)节点之间的约束，紫色直线是地面检测的约束，是SE(3)到平面的约束，而虚线是回环检测约束，同样也是SE(3)之间的约束。接下来，一起看下如何利用g2o表示上图所示的pose graph并采用非线性优化算法进行优化。

6.2 hdl_graph_slam

hdl_graph_slam算法中的后端优化部分主要定义并实现在hdl_graph_slam_nodelet.cpp源码中，由于这部分的源码较多，我们挑比较核心的部分进行解析。首先来看下如何添加激光里程计构成的运动约束：

bool flush_keyframe_queue(){
   
	for(int i=0; i<std::min<int>(keyframe_queue.size(), max_keyframes_per_update); i++) {
   
      num_processed = i;

      const auto& keyframe = keyframe_queue[i];
      // new_keyframes will be tested later for loop closure
      new_keyframes.push_back(keyframe);

      // add pose node
      Eigen::Isometry3d odom = odom2map * keyframe->odom;
      keyframe->node = graph_slam->add_se3_node(odom);
      keyframe_hash[keyframe->stamp] = keyframe;

      // fix the first node
      if(keyframes.empty() && new_keyframes.size() == 1) {
   
        if(private_nh.param<bool>("fix_first_node", false)) {
   
          anchor_node = graph_slam->add_se3