hdl_graph_slam源码：g2o顶点和边的管理

我自己看代码习惯农村包围城市，即先看各个模块的代码，等了解得差不多了，再去看主流程。所以就先分析g2o顶点和边、关键帧、点云滤波、前端里程计、闭环检测等功能，再去看hdl_graph_slam_nodelet.cpp中主流程是怎样进行的。

我们这篇就先从g2o顶点和边的管理开始看。

附赠自动驾驶学习资料和量产经验：链接

一、整体分析

通过上一篇分析我们知道，在整个系统中，需要用来构建g2o的顶点或边的信息包括：

• 帧间匹配的位姿

• 闭环检测的位姿

• gps的经纬高

• imu的姿态

• 检测并拟合出的地面平面参数

本篇的主要内容就是分析这些信息对应的顶点和边是怎样定义的，并且在信息到来时，是怎样被添加到概率图中的。

这里面除了帧间匹配得到的位姿对应的顶点和边不需要自己定义以外，其他信息对应的边都需要自己定义，所谓自己定义就是从g2o中继承边对应的基类，产生一个子类，在该子类中写明这个边对应的信息矩阵和观测误差计算公式即可。

对于自定义的边，在该系统中，他们的使用遵循下面的流程：

**1）在include/g2o中定义边。**这个文件夹有很多个cpp文件，每个cpp文件对应一种类型的边。

2）在src/hdl_graph_slam/graph_slam.cpp处理边。所谓处理，是指边的插入，边往概率图中插入时需要完成多个步骤，在这个cpp文件里，每条边对应一个函数，把这多个步骤都封装在这个函数中，使用只需要调用一次函数即可，这样可使代码更清晰明了。

**3）在apps/hdl_graph_slam_nodelet.cpp中调用边。**所有的边都是在这个cpp文件中被加入概率图中的，当一个信息到来，需要添加边时，就调用2）中封装好的对应的函数就好了。

二、详解顶点和边

在这部分，我们根据文章开头列出的信息，按照顺序分析他们在g2o中对应的顶点和边。

每个元素（即顶点或边）对应一个函数，所有元素的管理函数都在src/hdl_graph_slam/graph_slam.cpp中，而管理函数的调用都在apps/hdl_graph_slam_nodelet.cpp中，所以后面我提函数和函数调用的代码的时候就不注明它们出自哪个文件了

1. 帧间匹配的位姿

帧间匹配的位姿包括每一帧点云采集完成时雷达的位姿，以及相邻两帧点云之间的相对位姿，需要注意的是，我们构建概率图当然是稀疏的，不可能每一帧点云都参与，所以此处的帧均指关键帧，关键帧的选取策略后面会仔细讲。

在此处雷达自身的位姿即是顶点，而相邻两关键帧之间的相对位姿即是边。我们要分别介绍

1）顶点

添加顶点的函数代码如下

g2o::VertexSE3* GraphSLAM::add_se3_node(const Eigen::Isometry3d& pose) {
  g2o::VertexSE3* vertex(new g2o::VertexSE3());//定义并初始化一个顶点
  vertex->setId(static_cast<int>(graph->vertices().size()));//顶点对应的编号
  vertex->setEstimate(pose);//顶点对应的位姿
  graph->addVertex(vertex);//添加顶点到图中

  return vertex;
}

代码里VertexSE3是g2o中默认存在的顶点类型，它包括位置信息和姿态信息。

graph就是整个概率图，后面出现这个变量都是指这个意思，不再重复。

添加顶点时执行如下流程

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

2）边

添加边的函数代码如下

g2o::EdgeSE3* GraphSLAM::add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3* edge(new g2o::EdgeSE3());//定义并初始化一条边
  edge->setMeasurement(relative_pose);//设置边对应的观测
  edge->setInformation(information_matrix);//设置信息矩阵
  edge->vertices()[0] = v1;//设置边的一端连接的顶点
  edge->vertices()[1] = v2;//设置边的另一端连接的顶点
  graph->addEdge(edge);//添加边到图中

  return edge;
}

调用流程如下

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));

2. 闭环检测的位姿

变换检测就不需要添加顶点了，只需要添加边。因为闭环是在关键帧中间进行检测的，在进行检测时，每个关键帧已经有对应的顶点加入图中了。

闭环的边和帧间匹配的边类型一样，不再重复了。

3. gps的经纬高

gps能在建图时使用的是位置信息，也就是经度、纬度、高度，相当于给顶点（包含位置和姿态）中的位置信息一个直接的先验观测，也即先验边(prior)名称的由来。当然在添加到图中之前，会先把经度、纬度、高度转换到以地图原点为起始点的坐标系中，变成以米为单位的距离表示。

有些时候，如果认为高度不准，还可以只使用经度、纬度，而不使用高度，所以就有了下面这两种类型的边。

1）经度、纬度、高度同时观测

这是一个自定义的边，自定义对应的代码在include/g2o/edge_se3_priorxyz.hpp中，大家自己去看吧，应该都能看懂，不需要解释。

元素管理对应的函数代码如下

g2o::EdgeSE3PriorXYZ* GraphSLAM::add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorXYZ* edge(new g2o::EdgeSE3PriorXYZ());//定义并初始化
  edge->setMeasurement(xyz);//设置观测
  edge->setInformation(information_matrix);//添加信息矩阵
  edge->vertices()[0] = v_se3;//设置对应的顶点
  graph->addEdge(edge);//添加边到图中

  return edge;
}

调用流程如下

Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / gps_edge_stddev_xy;
edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, xyz.head<2>(), information_matrix);

2）经度、纬度观测

自定义边对应的代码在include/g2o/edge_se3_priorxy.hpp中

元素管理中，无非是向量少了一个维度嘛，和1）中的比变化不大

g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY());//定义并初始化
  edge->setMeasurement(xy);//设置观测（这里少了一个维度）
  edge->setInformation(information_matrix);//添加信息矩阵
  edge->vertices()[0] = v_se3;//设置对应的顶点
  graph->addEdge(edge);//添加边到图中

  return edge;
}

调用流程如下

Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity();
information_matrix.block<2, 2>(0, 0) /= gps_edge_stddev_xy;
information_matrix(2, 2) /= gps_edge_stddev_z;
edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, xyz, information_matrix);

4. imu的角度和加速度

在这个系统中，使用了imu的角度和加速度信息，角度自然就是给位姿做角度观测（四元数形式），而加速度，则是以重力作为基准方向，计算imu检测的加速度矢量和重力的不重合度，并作为误差项进行优化。

因此imu信息也对应两类边

1）角度信息

自定义边的代码在include/g2o/edge_se3_priorquat.hpp中

元素管理的代码如下

g2o::EdgeSE3PriorQuat* GraphSLAM::add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorQuat* edge(new g2o::EdgeSE3PriorQuat());//定义并初始化
  edge->setMeasurement(quat);//添加观测
  edge->setInformation(information_matrix);//添加信息矩阵
  edge->vertices()[0] = v_se3;//指定顶点
  graph->addEdge(edge);//添加边到图中

  return edge;
}

调用的代码如下

Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_orientation_edge_stddev;
auto edge = graph_slam->add_se3_prior_quat_edge(keyframe->node, *keyframe->orientation, info);
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("imu_orientation_edge_robust_kernel", "NONE"), private_nh.param<double>("imu_orientation_edge_robust_kernel_size", 1.0));

2）加速度信息

自定义边的代码在include/g2o/edge_se3_priorvec.hpp中

元素管理的代码如下

g2o::EdgeSE3PriorVec* GraphSLAM::add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix) {
  Eigen::Matrix<double, 6, 1> m;
  m.head<3>() = direction;
  m.tail<3>() = measurement;

  g2o::EdgeSE3PriorVec* edge(new g2o::EdgeSE3PriorVec());
  edge->setMeasurement(m);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

上面direction就是参考方向，而measurement就是测量的方向。这个边的误差就是这两个向量的差。

调用代码如下

Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_acceleration_edge_stddev;
g2o::OptimizableGraph::Edge* edge = graph_slam->add_se3_prior_vec_edge(keyframe->node, -Eigen::Vector3d::UnitZ(), *keyframe->acceleration, info);
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("imu_acceleration_edge_robust_kernel", "NONE"), private_nh.param<double>("imu_acceleration_edge_robust_kernel_size", 1.0));

上面参考方向设置为-Eigen::Vector3d::UnitZ()，即重力方向，测量方向设置为acceleration，即测量的加速度方向，所以这条边的误差就变成了加速度和重力之间的误差，和上面的分析一致。

5. 平面检测的信息

我们知道平面的方程是ax + by + cz + d=0，所以一个平面（plane）可以由四个参数描述。平面在这里既用来构造顶点，又参与构造边，所以元素分下面两类

1）顶点

平面对应的顶点在g2o中本来就有（g2o::VertexPlane），所以不需要自己定义。

元素管理对应的代码如下

g2o::VertexPlane* GraphSLAM::add_plane_node(const Eigen::Vector4d& plane_coeffs) {
  g2o::VertexPlane* vertex(new g2o::VertexPlane());
  vertex->setId(static_cast<int>(graph->vertices().size()));
  vertex->setEstimate(plane_coeffs);
  graph->addVertex(vertex);

  return vertex;
}

调用的代码如下

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

我们看到，添加VertexPlane类型的顶点在程序中只出现了一次，而且平面参数直接被设置成了 Eigen::Vector4d(0.0, 0.0, 1.0, 0.0)，这其实就是在初始化位姿的时候添加的，认为载体处在一个平面上，才能有这样的假设成立。

2）边

平面对应的边是自定义的，对应的代码在include/g2o/edge_se3_plane.hpp文件中

元素管理的代码如下

g2o::EdgeSE3Plane* GraphSLAM::add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane());
  edge->setMeasurement(plane_coeffs);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  edge->vertices()[1] = v_plane;
  graph->addEdge(edge);

  return edge;
}

调用的代码如下

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));

我们看到，这种操作把所有的关键帧对应的顶点都和初始化的平面（即Eigen::Vector4d(0.0, 0.0, 1.0, 0.0)）建立了联系，相当于把整个slam系统约束在平面上，认为它是在沿平面移动。

所以这种边要不要加需要根据环境决定，如果环境符合假设，那么高程几乎没有误差（我试过），如果不符合，那么相当于引入了错误的约束，会增大误差，甚至导致地图错乱。

至此所有的顶点和边就介绍完了。

其实在g2o文件夹中还有其他自定义的边，在src/hdl_graph_slam/graph_slam.cpp中还有其他元素管理函数，他们是有用的，但是本系统并没有调用，所以暂时不做过多解释了。

三、信息矩阵

细心的读者应该已经从上面的代码中发现，我们每次添加一条边的时候都要给出相应的信息矩阵，它反应的是这条边在优化中的权重，实际中，如果觉得优化结果不太好，我们可以根据实际情况去调整这些权重以取得更好的效果。

既然每种边都有权重，那我们就按照边的类型来梳理一下对应的信息矩阵。

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);

我们从上面的代码可以看出，它的信息矩阵是通过calc_information_matrix这个函数计算的，这个函数在文件src/hdl_graph_slam/information_matrix_calculator.cpp中，我们看下它的具体代码

Eigen::MatrixXd InformationMatrixCalculator::calc_information_matrix(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const {
  if(use_const_inf_matrix) {
    Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
    inf.topLeftCorner(3, 3).array() /= const_stddev_x;
    inf.bottomRightCorner(3, 3).array() /= const_stddev_q;
    return inf;
  }

  double fitness_score = calc_fitness_score(cloud1, cloud2, relpose);//计算匹配程度

  double min_var_x = std::pow(min_stddev_x, 2);
  double max_var_x = std::pow(max_stddev_x, 2);
  double min_var_q = std::pow(min_stddev_q, 2);
  double max_var_q = std::pow(max_stddev_q, 2);

  //根据匹配程度计算权重，位置权重和姿态权重分别计算
  float w_x = weight(var_gain_a, fitness_score_thresh, min_var_x, max_var_x, fitness_score);
  float w_q = weight(var_gain_a, fitness_score_thresh, min_var_q, max_var_q, fitness_score);

  Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
  inf.topLeftCorner(3, 3).array() /= w_x;
  inf.bottomRightCorner(3, 3).array() /= w_q;
  return inf;
}

可以看出，整个计算过程分为两步，第一步是计算两帧点云的匹配程度，也就是位姿的准确程度（因为位姿越不准，点云的匹配重合度就越差呀），第二步就是根据第一步的结果，计算信息矩阵里的值。

我们对这两步分别分析

1）计算匹配程度

直接上代码

double InformationMatrixCalculator::calc_fitness_score(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range) {
  pcl::search::KdTree<PointT>::Ptr tree_(new pcl::search::KdTree<PointT>());
  tree_->setInputCloud(cloud1);

  double fitness_score = 0.0;

  // Transform the input dataset using the final transformation
  pcl::PointCloud<PointT> input_transformed;
  pcl::transformPointCloud (*cloud2, input_transformed, relpose.cast<float>());

  std::vector<int> nn_indices (1);
  std::vector<float> nn_dists (1);

  // For each point in the source dataset
  int nr = 0;
  for (size_t i = 0; i < input_transformed.points.size (); ++i)
  {
    // Find its nearest neighbor in the target
    tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);

    // Deal with occlusions (incomplete targets)
    if (nn_dists[0] <= max_range)
    {
      // Add to the fitness score
      fitness_score += nn_dists[0];
      nr++;
    }
  }

  if (nr > 0)
    return (fitness_score / nr);
  else
    return (std::numeric_limits<double>::max ());
}

可以看出，它的思路就是对于当前点云中的每一个点，在另一帧点云中找最近的点，如果这两点之间的距离在合理范围内，则把他俩之间的距离差作为不匹配度的误差加如最终的fitness_score中。

2）计算权重

这里要问，既然有了fitness了，为什么直接把它当权重不行吗，为什么还要进行一次计算呢。因为fitness的变化范围比较大，而且fitness和不匹配度只是正相关关系，而其曲线分布并不合理，所以要重新改变曲线形状。

就是下面这个函数

double weight(double a, double max_x, double min_y, double max_y, double x) const {
    double y = (1.0 - std::exp(-a * x)) / (1.0 - std::exp(-a * max_x));
    return min_y + (max_y - min_y) * y;
}

而这一步所需要参数是从launch文件里传入的，也就是使用者可以通过配置参数来更改。

2. 闭环检测的位姿

都属于点云匹配得到的边，所以计算方式和帧间匹配的边是一样的。

3. gps的经纬高

1）经度、纬度、高度同时观测

Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity();
information_matrix.block<2, 2>(0, 0) /= gps_edge_stddev_xy;
information_matrix(2, 2) /= gps_edge_stddev_z;
edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, xyz, information_matrix);

可以看出，信息矩阵是通过gps_edge_stddev_xy和gps_edge_stddev_z两个参数直接计算的，前者代表经度、纬度的权重，后者代表高度的权重。使用者同样可以通过配置文件直接更改这两个参数。

2）经度、纬度观测

Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / gps_edge_stddev_xy;
edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, xyz.head<2>(), information_matrix);

少了高度而已，不用解释。

4. imu的角度和加速度

1）imu角度信息

Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_orientation_edge_stddev;
auto edge = graph_slam->add_se3_prior_quat_edge(keyframe->node, *keyframe->orientation, info);

同样从配置文件的参数直接计算，不解释

2）imu加速度信息

Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_acceleration_edge_stddev;
g2o::OptimizableGraph::Edge* edge = graph_slam->add_se3_prior_vec_edge(keyframe->node, -Eigen::Vector3d::UnitZ(), *keyframe->acceleration, info);

同上

四、鲁棒核

在g2o中添加边时，如果添加鲁棒核，则系统会对一些异常数据更鲁棒，这套代码中，所有的边后面都跟了添加鲁棒核的操作。

其实就是调用了这个函数

void GraphSLAM::add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size) {
  if(kernel_type == "NONE") {
    return;
  }

  g2o::RobustKernel* kernel = robust_kernel_factory->construct(kernel_type);
  if(kernel == nullptr) {
    std::cerr << "warning : invalid robust kernel type: " << kernel_type << std::endl;
    return;
  }

  kernel->setDelta(kernel_size);

  g2o::OptimizableGraph::Edge* edge_ = dynamic_cast<g2o::OptimizableGraph::Edge*>(edge);
  edge_->setRobustKernel(kernel);
}

鲁棒核的类型可以在配置文件里设置，每种边都对应了一个设置变量，即他们各自的鲁棒核可以分别设置。