hdl_graph_slam源码解读(二)：g2o顶点和边的管理

最新推荐文章于 2024-05-05 23:15:56 发布
最新推荐文章于 2024-05-05 23:15:56 发布
阅读量1.2k 收藏 11
点赞数
分类专栏： SLAM开源项目解读
原文链接：https://zhuanlan.zhihu.com/p/89056230
版权

转载自知乎专栏：hdl_graph_slam源码解读(二)：g2o顶点和边的管理
https://zhuanlan.zhihu.com/p/89056230

目录

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

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

1. 整体分析

通过上一篇分析我们知道，在整个系统中，需要用来构建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 中封装好的对应的函数就好了。

2. 详解顶点和边

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

每个元素（即顶点或边）对应一个函数：

  • 所有元素的管理函数都在src/hdl_graph_slam/graph_slam.cpp中
  • 而管理函数的调用都在apps/hdl_graph_slam_nodelet.cpp中

所以后面在提到函数和函数调用的代码的时候就不注明它们出自哪个文件了。

2.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.2 闭环检测的位姿

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

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

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

  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::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.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，即测量的加速度方向，所以这条边的误差就变成了加速度和重力之间的误差，和上面的分析一致。

2.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中还有其他元素管理函数，他们是有用的，但是本系统并没有调用，所以暂时不做过多解释了。

3. 信息矩阵

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

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

3.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文件里传入的，也就是使用者可以通过配置参数来更改。

3.2 闭环检测的位姿

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

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

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

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

    同上

4. 鲁棒核

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

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

参考文献

[1] hdl_graph_slam源码解读(二)：g2o顶点和边的管理

坤虫H.M
关注
  • 0
    点赞
  • 踩
  • 11
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
专栏目录
g2o使用总结关于边和顶点
weixin_43489667的博客
04-16 1863
图优化是非线性最小二乘问题的另一种表达而已，使用g2o求解和Ceres很多地方都相似。 最小二乘问题的关键在于：已知观测数据（输入值，观测值），待估计参数及其初始值，误差项，雅克比矩阵，求解增量更新待估计参数，在不同的方法中去寻找对应，对于方法的理解即可迎刃而解。 定义顶点和边的类 1、构建顶点Vertex类： 顶点就是待优化的参数，重要的成员函数有： setToOriginImpl() 将节点...
hdl_graph_slam:基于3D LIDAR的Graph SLAM
04-29
hdl_graph_slam hdl_graph_slam是使用3D LIDAR的实时6DOF SLAM的开源ROS软件包。 它基于3D Graph SLAM，并具有基于NDT扫描匹配的测距法估计和环路检测。 它还支持多种图形约束，例如GPS，IMU加速度（重力矢量），IMU...
hdl_graph_slam源码解析（二）
RedNation的博客
12-10 4281
hdl_graph_slam源码解析（二）3. scan_matching_odometry 3. scan_matching_odometry
hdl_graph_slam源码：g2o顶点和边的管理
最新发布
adas_l5的博客
05-05 596
我自己看代码习惯农村包围城市，即先看各个模块的代码，等了解得差不多了，再去看主流程。所以就先分析g2o顶点和边、关键帧、点云滤波、前端里程计、闭环检测等功能，再去看hdl_graph_slam_nodelet.cpp中主流程是怎样进行的。我们这篇就先从g2o顶点和边的管理开始看。
hdl_graph_slam源码解读(八)：后端优化
穿林需独行的博客
09-30 551
目录1. 整体介绍2. 代码详解2.1 前端里程计消息处理2.2 gps消息2.3 imu消息2.4 地面拟合参数2.5 执行图优化2.6 生成地图并定时发送3. 整体思考参考文献 1. 整体介绍 这部分是整个系统的核心，因为这个系统就是建图的呀，建图的核心就是综合所有信息进行优化呀。我们前面几节介绍的所有的信息都会发送到这个节点中来供它使用。这部分的代码在文件apps/hdl_graph_slam_nodelet.cpp中。 这部分的代码看着挺多，如果我们能够对它所有的函数分类，就很容易梳理它的内容了。
hdl_graph_slam源码解析（一）
热门推荐
RedNation的博客
12-07 1万+
hdl_graph_slam源码解析（一）hdl_graph_slam简介 hdl_graph_slam简介 hdl_graph_slam是由日本风桥科技大学的Kenji Koide在github上开源的六自由度三维激光SLAM算法。主要由激光里程计、回环检测以及后端图优化构成，同时融合了IMU、GPS以及地面检测的信息作为图的额外约束。其数据流如下所示： ...
hdl_graph_slam的源码分析(1)——点云滤波预处理
a850565178的博客
04-16 677
文章目录写在前面1.点云滤波预处理1.1 virtual onInit（）1.2 cloud_callback 写在前面 已经完成了在Robosense上的离线和在线SLAM建图测试，建图的实时性和准确性都还行。不过主要是在室内测试。如果在城市中，道路不平整情况下，可能会存在问题，那时候应该得引入GPS与IMU了。 为了后续能够基于此框架进行优化，作为第一个实际接触的SLAM，此篇自己按部分进行源...
hdl_graph_slam-master.zip
07-23
《hdl_graph_slam：基于图优化的SLAM与NDT法解决闭环检测》 在机器人定位与导航领域，Simultaneous Localization And Mapping（SLAM）是核心问题之一，即同时定位与建图。而`hdl_graph_slam-master.zip`中的项目，...
slamtb-graph.zip_SLAM 仿真_graph slam_slam_slamtb_slam仿真
07-14
本压缩包"slamtb-graph.zip"提供了一个SLAM的MATLAB工具箱，专注于EKF-SLAM（Extended Kalman Filter SLAM）和graph-SLAM（图优化SLAM）两种方法的仿真。 EKF-SLAM是一种应用广泛的SLAM算法，它使用扩展卡尔曼...
interactive_slam:3D Graph SLAM的交互式地图校正
05-05
Interactive_slam Interactive_slam是一个基于开源3D LIDAR的映射框架。 与现有的自动SLAM软件包相比，我们旨在开发一种半自动框架，该框架使用户能够以最少的人工来交互式直观地纠正制图失败（例如，里程表损坏，...
g2opy:SLAM图优化框架g2o的Python绑定
04-29
g2o是用于优化基于图的非线性误差函数的开源C ++框架。 g2o被设计为易于扩展到各种问题，通常可以在几行代码中指定一个新问题。 当前的实现为SLAM和BA的多个变体提供了解决方案。 机器人技术和计算机视觉中的许多...
G2O使用步骤1——添加定点和边
半不闲居士的博客
03-02 2015
前面介绍了G2O的整体框架与初始化求解器基本使用步骤，接下来介绍如何向优化器中添加定点和边 1. 顶点 我们从顶点的数据类型开始介绍，先来看看上图中和vertex有关的第1个类: HyperGraph::Vertex，然后我们看g2o 类结构图中第2个类,我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的；这个OptimizableGraph::Vertex 也非常底层,具体使用时一般都会进行扩展,因此g2o中提供了一个比较通用的适合大部分情况的模板。就
hdl_graph_slam
Seinlvan的博客
07-26 2649
目录(一)：整体介绍一、优缺点分析二、功能分解三、代码结构(二)：g2o顶点和边的管理一、整体分析二、详解顶点和边三、信息矩阵四、鲁棒核(三)：点云滤波1. 降采样滤波2. 滤除离群点(四)：关键帧一、整体介绍二、详细介绍(五)：前端里程计(六)：地面检测1. 分割地面2. 去掉法向量异常的点3. 拟合平面参数(七)：闭环检测1. 整体流程2. 检测满足条件的关键帧3. 闭环匹配4. 一点思考(八)：后端优化一、整体介绍二、代码详解 (一)：整体介绍 一、优缺点分析 通过实测和阅读代码，它有如下优缺点：
hdl_graph_slam三维激光雷达+GPS+IMU+ODOM
David_BY的博客
03-26 5210
参考：https://gitee.com/qq771304328_admin/hdl_graph_slam 需要安装的包： sudo apt-get install ros-kinetic-geodesy ros-kinetic-pcl_ros ros-kinetic-nmea-msgs ros-kinetic-libg2o 运行无法定位： E: 无法定位软件包 ros-kinet...
M2DGR数据集在一些SLAM框架上的配置与运行：ORB-SLAM系列、VINS-Mono、LOAM系列、FAST-LIO系列、hdl_graph_slam
zardforever123的博客
03-04 6856
为了说明以上这些SLAM框架的测试与数据集适配，以M2DGR数据集为例进行各自的配置与运行，方便我以后适配自己的数据集，运行的SLAM框架包括：ORB-SLAM2、ORB-SLAM3、VINS-Mono、A-LOAM、LeGO-LOAM、LIO-SAM、LVI-SAM、FAST-LIO2、FAST-LIVO、Faster-LIO、hdl_graph_slam。实验基于Ubuntu20.04，其中ORB、VINS-Mono、和LOAM系列的安装参考我以前的博客，有详细的安装方法，其他的框架在后面会简单介绍一下
g2o库简单入门
qq_34761779的博客
10-06 1864
g2o库的简单入门
G2O (General Graph Optimization)入门及简单使用
晓晨的博客
10-30 7017
g2o全称是General Graph Optimization，也就是图优化，我们在做SLAM后端或者更加常见的任何优化问题（曲线拟合）都可以使用G2O进行处理。
图优化g2o
weixin_44775404的博客
09-10 229
1.鲁棒核 作用：抑制离群点的影响。 用法：向边中添加鲁棒核，需根据实际场景调整参数，有多种类型。 robustInformation是用来计算鲁棒核函数rhi的值。 2.信息矩阵 作用：当对不同边所代表的观测有不同信任度时，对更信任的边的信息矩阵乘以更大的数。若觉得帧间匹配精度在0.1m，那么把信息矩阵设成100的对角阵即可。 g2o_param: odom_edge_noise: [0.5, 0.5, 0.5, 0.001, 0.001, 0.001] # 噪声：x y z yaw roll pitch
视觉SLAM漫谈（二）:图优化理论与g2o的使用
BBZZ2的专栏
03-01 7154
1    前言以及回顾   　　各位朋友，自从上一篇《视觉SLAM漫谈》写成以来已经有一段时间了。我收到几位热心读者的邮件。有的希望我介绍一下当前视觉SLAM程序的实用程度，更多的人希望了解一下前文提到的g2o优化库。因此我另写一篇小文章来专门介绍这个新玩意。   　　在开始本篇文章正文以前，我们先来回顾一下图优化SLAM问题的提法。至于SLAM更基础的内容，例如S
hdl_graph_slam如何在因子图中添加的地平面约束
06-08
hdl_graph_slam是一个基于激光雷达数据进行SLAM的开源软件包。在hdl_graph_slam中，可以通过添加地平面约束来提高建图的精度和鲁棒性。 具体来说，hdl_graph_slam中的地平面约束是通过添加一个平面因子来实现的。在建图过程中，hdl_graph_slam会将地图划分为多个cell，并使用RANSAC算法从每个cell中提取平面。然后，对于每个平面，hdl_graph_slam会添加一个平面因子到因子图中，该因子将该平面与周围的激光点进行匹配，从而约束平面的位置和方向。 要在hdl_graph_slam中添加地平面约束，需要在启动hdl_graph_slam时设置相应的参数。具体来说，需要将"enable_ground_removal"参数设置为true，以启用地面移除功能，并将"enable_ground_constraint"参数设置为true，以启用地平面约束。这样，hdl_graph_slam就会自动提取地面并添加相应的平面因子，从而实现地平面约束。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值