hdl代码注释01：hdl_graph_slam节点文件功能总览

坤虫H.M

于 2020-10-09 10:54:49 发布

阅读量1.2k

点赞数 4

分类专栏： SLAM开源项目解读

原文链接：https://github.com/shibowing/hdl_graph_slam_noted

版权

SLAM开源项目解读专栏收录该内容

10 篇文章 5 订阅

订阅专栏

1. prefiltering_nodelet.cpp：点云滤波

这部分主要是对点云实现预处理功能,主要包括

点云网格滤波
点云离群点 outlier 的去除 (这里可以选择statistic 或者 raduis 两种方式进行滤波)
距离滤波(只要点云在指定距离范围内的点)

2. floor_detection_nodelet.cpp：地面平面检测

该 cpp 主要接收 prefiltering_nodelet.cpp 滤波后的点云,并完成地面检测。

主要功能由 boost::optional floor = detect(cloud) 函数完成：

可以设置 tilt_matrix 对 Y 轴方向的激光倾斜角进行补偿
紧接着,调用 filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false); 函数：输入是滤波后的点云,输出是平面点云 , vector 4f 对应的是想要的平面约束, plane clip 后续会根据平面约束滤出相关的点云
normal_filtering(filtered) ;： filtered 表示平面点云, normal_filtering 主要通过非垂直方向对点云进行滤波
pcl::RandomSampleConsensus ransac(model_p); ：通过 ransac 函数，根据输入的准平面点云进行再一次滤波。主要滤出外点,同时保留内点,并计算平面内点所对应的平面方程系数

3. scan_matching_odometry_nodelet.cpp：前端里程计

该函数主要计算相邻两帧之间的匹配,输入为 prefiltering_nodelet.cpp 滤波后得到的 /filtered_points ,紧接着进入matching 函数：

matching const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud

第一步：首先判断是否是关键帧
第二步：这里主要调用 PCL 里面现成的库函数,主要有( ICP , GICP , NDT , GICP_OMP, NDT_OMP ) , 值得关注的是 NDT_OMP ,该算法通过采用多线程方式去完成 NDT 算法,因此效率是传统 NDT 算法的 10 倍,精度基本差不多; 这里如果选择 KDTREE ,那么和传统的 NDT 算法一致,剩下的速度比较: DIRECT1 > DIRECT7 , 但是DIRECT7 更稳定同时速度比 KDTREE 快
选择完上述方法之后就开始完成匹配,这里主要是当前帧点云与 Keyframe 进行匹配
有两个参数比较重要: keyframe_delta_trans、keyframe_delta_angle： The minimum tranlational distance and rotation angle between keyframes.If this value is zero, frames are always compared with the previous frame.

4. hdl_graph_slam_nodelet.cpp：后端概率图构建

4.1 cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg,constsensor_msgs::PointCloud2::ConstPtr& cloud_msg)

该函数主要是odom信息与cloud信息的同步，同步之后检查关键帧是否更新。

关键帧判断：这里主要看关键帧设置的这两个阈值keyframe_delta_trans、keyframe_delta_angle；作者认为关键帧不能设置的太近，同时也不能太远；太近的关键帧会被抛弃，如果keyframe_delta_trans=0 表示关键帧之间的距离为0，因此每一帧都是关键帧，scan_matching 函数就会变成相邻帧匹配。

变成关键帧的要求就是：/hdl_graph_slam/include/hdl_graph_slam/keyframe_updater.hpp

// calculate the delta transformation from the previous keyframe
    Eigen::Isometry3d delta = prev_keypose.inverse() * pose;
    double dx = delta.translation().norm();
    double da = std::acos(Eigen::Quaterniond(delta.linear()).w());

    // too close to the previous frame
    if(dx < keyframe_delta_trans && da < keyframe_delta_angle) {
      return false;
    }

4.2 optimization_timer_callback(const ros::TimerEvent& event)

函数功能：将所有的位姿放在posegraph 中开始优化

loop detection 函数：主要就是将当前帧和历史帧遍历，寻找loop。

寻找潜在闭环帧：/hdl_graph_slam/include/hdl_graph_slam/loop_detector.hpp

 std::vector<KeyFrame::Ptr> find_candidates(const std::vector<KeyFrame::Ptr>& keyframes, const KeyFrame::Ptr& new_keyframe) const {
    // too close to the last registered loop edge
    if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) {
      return std::vector<KeyFrame::Ptr>();
    }

    std::vector<KeyFrame::Ptr> candidates;
    candidates.reserve(32);

    for(const auto& k : keyframes) {
      // traveled distance between keyframes is too small
      if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) {
        continue;
      }

      const auto& pos1 = k->node->estimate().translation();
      const auto& pos2 = new_keyframe->node->estimate().translation();

      // estimated distance between keyframes is too small
      double dist = (pos1.head<2>() - pos2.head<2>()).norm();
      if(dist > distance_thresh) {
        continue;
      }

      candidates.push_back(k);
    }

    return candidates;
  }

由于每个关键帧都建立累计距离，当前帧对应的累计距离，即边的累计距距离 < distance_from_last_edge_thresh ，那么就不能建立闭环：意思就是相邻两个闭环帧不能建立的过于密集
accum_distance_thresh 这里指的是当前帧与临近帧的距离阈值，假设距离阈值为5m, 那么当前帧距离5米范围内的关键帧不做考虑
distance_thresh 小于该阈值范围内两个关键帧为潜在闭环帧，将所有满足条件的都存起来作为candidates

4.3 潜在闭环完成匹配（matching 函数）

 Loop::Ptr matching(const std::vector<KeyFrame::Ptr>& candidate_keyframes, const KeyFrame::Ptr& new_keyframe, hdl_graph_slam::GraphSLAM& graph_slam) {
    if(candidate_keyframes.empty()) {
      return nullptr;
    }

    registration->setInputTarget(new_keyframe->cloud);

    double best_score = std::numeric_limits<double>::max();
    KeyFrame::Ptr best_matched;
    Eigen::Matrix4f relative_pose;

    std::cout << std::endl;
    std::cout << "--- loop detection ---" << std::endl;
    std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl;
    std::cout << "matching" << std::flush;
    auto t1 = ros::Time::now();

    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    for(const auto& candidate : candidate_keyframes) {
      registration->setInputSource(candidate->cloud);
      Eigen::Matrix4f guess = (new_keyframe->node->estimate().inverse() * candidate->node->estimate()).matrix().cast<float>();
      guess(2, 3) = 0.0;
      registration->align(*aligned, guess);
      std::cout << "." << std::flush;

      double score = registration->getFitnessScore(fitness_score_max_range);
      if(!registration->hasConverged() || score > best_score) {
        continue;
      }

      best_score = score;
      best_matched = candidate;
      relative_pose = registration->getFinalTransformation();
    }

    auto t2 = ros::Time::now();
    std::cout << " done" << std::endl;
    std::cout << "best_score: " << boost::format("%.3f") % best_score << "    time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;

    if(best_score > fitness_score_thresh) {
      std::cout << "loop not found..." << std::endl;
      return nullptr;
    }

    std::cout << "loop found!!" << std::endl;
    std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl;

    last_edge_accum_distance = new_keyframe->accum_distance;

    return std::make_shared<Loop>(new_keyframe, best_matched, relative_pose);
  }

主要是当前帧与潜在闭环帧完成点云匹配，通过统计得分寻找最优匹配，并且只将最优匹配对应的transform作为最优闭环约束。这里对应的就是fitness_score_thresh。

4.4 全场亮点:计算不同loop的信息矩阵

calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose) ：这里relpose 表示闭环帧之间的位姿约束。

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

首先判断是否使用常值信息矩阵，一般都是单位阵1/0.5=50 作为位姿， 1/0.1=10作为角度；可以看出我们更相信位姿，而非角度；这里对应变量分别是：const_stddev_x、const_stddev_q。
这里的fitness_socre 的计算非常朴素，主要是通过：
- 建立KD-TREE
- 在目标点云里面寻找source 点云每个点对应的最近点，并记录两点间的距离
- fitness_score += nn_dists[i];、 return (fitness_score / n);：上面这两个式子实际是记录所有点的距离，并将（所有点的距离/n）作为平均距离；
- 通过求所有闭环帧，KD-TREE查找对应点的平均距离来作为fitness_score

4.5 gps对应的信息矩阵（这里体现g2o的强大之处）

作者主要看这几件事：

GPS 坐标转换：主要是GPS地球坐标系->转换到大地坐标系（UTM)-> UTM坐标系再转换成局部坐标
这里只用gps的 X和Y信息，z 方向的信息不用，主要原因是不准，这就涉及设置信息矩阵，注意信息矩阵的类型：graph_slam->add_se3_prior_xy_edge((*seek)->node, xyz.head<2>(), information_matrix);

//添加GPS约束，只给xy增加约束
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());
   //添加gps对应的测量值（xy）还是UTM坐标
   edge->setMeasurement(xy);
   //添加对应的信息矩阵
   edge->setInformation(information_matrix);
   //添加该顶点
   edge->vertices()[0] = v_se3;
   graph->addEdge(edge);

  return edge;
}

4.6 添加查找对应的keyframe，并添加地面约束

检测平面，这里主要涉及floor_detection_nodelet.cpp 里面讲的平面检测；最后通过ransac 计算平面系数
通过计算得到的平面系数来添加约束，主要对检测到的平面的顶点，添加平面约束，同时引进信息矩阵。
信息矩阵是3*3的，应该只对XYZ起到约束作用

//添加平面约束：有的设置是0.1，有的设置是100
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);

//添加平面约束
graph_slam->add_se3_plane_edge(keyframe->node, graph_slam->floor_plane_node, coeffs, information);

使用的add_se3_plane_edge函数的代码如下：

//添加平面约束
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本身自带平面这种边
  g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane());
  //添加由ransssac 得到的平面系数
  edge->setMeasurement(plane_coeffs);
  //设置信息矩阵3*3
  edge->setInformation(information_matrix);
  //设置该顶点
  edge->vertices()[0] = v_se3;
  //以及该顶点对应的平面
  edge->vertices()[1] = v_plane;
  //添加平面
  graph->addEdge(edge);

  return edge;
}

参考文献

[1] hdl_graph_slam 简单中文注释版

坤虫H.M

关注

4
点赞
踩
11

收藏

觉得还不错? 一键收藏
0
评论
hdl代码注释01：hdl_graph_slam节点文件功能总览

目录1. prefiltering_nodelet.cpp：点云滤波2. floor_detection_nodelet.cpp：地面平面检测3. scan_matching_odometry_nodelet.cpp：前端里程计4. hdl_graph_slam_nodelet.cpp：后端概率图构建4.1 cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg,constsensor_msgs::PointCloud2::ConstPtr
复制链接

扫一扫

专栏目录