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_node(Eigen::Isometry3d::Identity());
          anchor_node->setFixed(true);
          anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 6));
        }
      }

      if(i==0 && keyframes.empty()) {
        continue;
      }

      // 添加由激光里程计构成的运动约束
      const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1];

      Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom;
      Eigen::MatrixXd information = inf_calclator->calc_information_matrix(prev_keyframe->cloud, keyframe->cloud, relative_pose);
      auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("odometry_edge_robust_kernel", "NONE"), private_nh.param<double>("odometry_edge_robust_kernel_size", 1.0));
    }
}

然后是地面检测对应的测量约束：

bool flush_floor_queue(){
	for(const auto& floor_coeffs : floor_coeffs_queue) {
      if(floor_coeffs->header.stamp > latest_keyframe_stamp) {
        break;
      }

      auto found = keyframe_hash.find(floor_coeffs->header.stamp);
      if(found == keyframe_hash.end()) {
        continue;
      }

      if(!floor_plane_node) {
        floor_plane_node = graph_slam->add_plane_node(Eigen::Vector4d(0.0, 0.0, 1.0, 0.0));
        floor_plane_node->setFixed(true);
      }

      const auto& keyframe = found->second;
	  //添加地面检测的测量约束
      Eigen::Vector4d coeffs(floor_coeffs->coeffs[0], floor_coeffs->coeffs[1], floor_coeffs->coeffs[2], floor_coeffs->coeffs[3]);
      Eigen::Matrix3d information = Eigen::Matrix3d::Identity() * (1.0 / floor_edge_stddev);
      auto edge = graph_slam->add_se3_plane_edge(keyframe->node, floor_plane_node, coeffs, information);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("floor_edge_robust_kernel", "NONE"), private_nh.param<double>("floor_edge_robust_kernel_size", 1.0));
      keyframe->floor_coeffs = coeffs;
      updated = true;
    }
}

后端优化：

void optimization_timer_callback(const ros::WallTimerEvent& event) {
	// 检测回环并添加回环约束
    std::vector<Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
    for(const auto& loop : loops) {
      Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
      Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
      auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("loop_closure_edge_robust_kernel", "NONE"), private_nh.param<double>("loop_closure_edge_robust_kernel_size", 1.0));
    }
    // 优化位姿图
    int num_iterations = private_nh.param<int>("g2o_solver_num_iterations", 1024);
    graph_slam->optimize(num_iterations);
}

好了，真个hdl_graph_slam中后端优化部分的主要代码就是这样啦。由于这部分的代码较多，篇幅所限，关于更详细的细节还请参考源码，笔者感觉这个开源项目的代码质量还是很好的。

7. 实验结果

笔者利用该算法在自己学校的数据集上进行了测试，测试效果图可以放一张：

从上图中可以看出，hdl_graph_slam的运行效果还是很好的。但是在实际测试过程中，也发现了一些问题，如在一些较大的场景中，有的时候会出现回环检测失败的情况，从而导致建图结果出现不一致的情况。这种情况下，只能通过建图过程中人为的多增加一些环来进行修正。另外，该算法采用的地面检测算法假设了全局一致地面的存在，如果可以对不同的地面参数进行数据关联，可能效果会更好一点。
关于hdl_graph_slam的源码解析就到这里啦，会继续更新其他优秀开源3D激光SLAM源码的解析。