算法
29 分钟 读完 (大约 4320 个字)

Hdl_graph_slam论文及代码解析_回环检测

回环检测

​ 闭环的作用就不多说了，koide采用的闭环检测方法其实挺简单的，就是一般闭环检测的基础准则。

候选准则

  • 候选帧不能离当前帧太近，场景变化不大的话，会引入过多的冗余匹配，对实时应用不合适；
  • 候选帧和当前帧的位姿之间的距离要足够小，这个是前提，如果都不小，那说明大概率不是闭环了，但是也不排除前端飘得确实很厉害的情况；小范围内，这个条件还是可以满足的，室外更大的场景，这种就不行了，可能飘得很厉害，只能上特征了。

候选帧确认

​ 前面只是拿到了一堆可能是候选帧的集合（这里回环候选帧是从历史帧中拿到的），还需要进一步判断到底能不能作为闭环帧，这里判断的依据就是对候选帧\(lp_{candidate}\)和当前新来的关键帧\(kf_{current}\)做一次点云匹配，采用的就是他们组之前的工作Fast GICP、NDT_OMP做两帧点云的匹配。计算一个sum of squared error 得分score，如果这个score小于阈值，则说明可以接受其作为回环帧。具体点，利用从前端得到的两帧点云的位姿g2o::VertexSE3* node计算一个初始的相对位姿\(T_{candidate}^{current} = T_{candidate}^{-1}·T_{current}\)，作为配准初始位姿开始配准，这么做是为了提升配准方法收敛的可能性。匹配完之后返回一个度量，依据这个度量做最终的判断。

回环检测流程

​ 前端开始工作，得到序列化的关键帧\(kfs\)，整个SLAM系统开启的同时回环检测线程就开始工作了，一直在不断的积累关键帧，一定程度（这里可能是延时开启，或者给定关键帧数量阈值）之后，开始做候选回环帧的检测。做回环检测是在新来一帧关键帧\(kf_{current}\)之后就开始的（因为要同时进行局部位姿优化得到更加准确的当前帧的位姿），通过上述候选准则，与历史帧中的关键帧进行比较，从中删选出满足条件的关键帧作为当前关键帧\(kf_{current}\)的回环候选帧\(lp_{candidate}\)，因为候选回环帧可能不是一个，而是一系列组成集合\(LP_{candidate}\)（场景变化不大时，都很像），从\(LP_{candidate}\)中找到得分最高的候选帧作为最终的回环帧\(kf_{loop}\)。

注：

  • 回环检测结果一定要鲁棒且正确，因为如果一旦引入一个错误的闭环，整个后端就崩掉了。当然现在大部分工作都是引入特征描述子了，不仅几何空间，在特征空间也同样进行检测，极大提升回环的召回率。这里就先不介绍了。

  • 如果候选帧的筛查是在特征空间完成的（相对位姿可直接根据匹配对SVD分解得到），后面通常会跟一个几何验证模块，即又是一个点云匹配过程，类似于ICP或者NDT得到从几何角度拿到的相对位姿，与特征匹配得到的相对位姿进行对比，完成进一步确认。

闭环优化

误差来源

​ 每一个激光帧的位姿都是通过匹配（无论是scan2scan or scan2map）得到的，每一次匹配求位姿都存在偏差，因此检测到回环的时候肯定就互相对不上了，就存在观测值（匹配的T）与估计值（一步步叠加得到的T）之间的偏差。 \[ e=(\check{T}_i^j)^{-1}T_{jw}^{-1}T_{iw} \] ​ 这里的误差表示和视觉SLAM14讲中的PGO优化时采用的表达方式是一样的。\(T_{iw},T_{jw}\)分别表示世界坐标系下两个激光关键帧的位姿，\(\check{T}_i^j\)表示的是从i到j的相对位姿（也就是闭环检测估计出来的相对位姿，可以视为观测值）。\(T_{jw}^{-1}T_{iw}\)表示的就是用这两个激光关键帧通过前端不断积累之后的位姿所表示的两帧之间的相对位姿。

  • e对两个节点位姿的jacobian公式推导1 TODO...

回环边的构建

​ 得到当前激光关键帧\(kf_{current}\)的回环帧\(kf_{loop}\)之后，就可以构建回环边了，引入回环约束，开始后端优化。

​ 检测出来的回环存储于Loop结构体中，其实光看Loop的定义，无法确定relative_pose描述的是谁到谁的变换，还需要实际看使用的时候的传入参数。hdl_graph_slam中采用的是\(T_{loop}^{current}\)，检测到的回环帧\(kf_{loop}\)到当前关键帧\(kf_{current}\)的变换，也就是key2\(\Rightarrow\)key1。

1
2
3
4
5
6
7
8
9
10
11
struct Loop {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Ptr = std::shared_ptr<Loop>;
  Loop(const KeyFrame::Ptr& key1, const KeyFrame::Ptr& key2, const Eigen::Matrix4f& relpose) 
      : key1(key1), key2(key2), relative_pose(relpose) {}
public:
  KeyFrame::Ptr key1;
  KeyFrame::Ptr key2;
  Eigen::Matrix4f relative_pose;
};

​ 回环边的构建如下：

1
2
3
4
5
6
7
8
9
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;
}

​ 回环边连接的两个顶点是用g2o::SE3表示的顶点，表示关键帧的位姿世界坐标系下，相对于第一帧的位姿。观测值是两个节点之间的相对位姿，采用Eigen::Isometry3d表示。构建的边的类型是用g2o::EdgeSE3表示的，计算的误差是一个6维的向量。g2o中的实现如下：

  • VertexSE3，g2o中采用Eigen::Isometry3来描述当前帧的位姿
1
2
3
4
5
6
7
8
9
10
11
12
13
/**
 * \brief 3D pose Vertex, represented as an Isometry3
 *
 * 3D pose vertex, represented as an Isometry3, i.e., an affine transformation
 * which is constructed by only concatenating rotation and translation
 * matrices. Hence, no scaling or projection.  To avoid that the rotational
 * part of the Isometry3 gets numerically unstable we compute the nearest
 * orthogonal matrix after a large number of calls to the oplus method.
 * 
 * The parameterization for the increments constructed is a 6d vector
 * (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
 */
class VertexSE3 : public BaseVertex<6, Isometry3> {...}
  • EdgeSE3，误差同样也是一个VertexSE3型的变量表示的，而VertexSE3实际上是Isometry3，Isometry3实际上又是（x,y,z,qx,qy,qz）不表示缩放。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
EdgeSE3 : public BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3>
{
    ...
    // 虽然代码中的注释写明，这个误差表示的是j到i节点的变换的，z^-1 * (x_i^-1 * x_j)误差应该是这样的，但是实际写的时候好像写反了
    void EdgeSE3::computeError() {
        VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
        VertexSE3 *to   = static_cast<VertexSE3*>(_vertices[1]);
        Isometry3 delta=_inverseMeasurement * from->estimate().inverse() * to->estimate();
        _error=internal::toVectorMQT(delta);
    }
    ...
}
// 其中Isometry3转换为6维的误差
Vector6 toVectorMQT(const Isometry3& t) {
    Vector6 v;
    // 表示从变换矩阵t中提取旋转矩阵，再转为归一化的四元数，返回四元数的实部，放在返回向量的后三位
    v.block<3,1>(3,0) = toCompactQuaternion(extractRotation(t));
    // 下面表示直接将平移从t中提取出来放在返回向量的前三位
    v.block<3,1>(0,0) = t.translation();
    return v;
}

​ 加入到图中：

1
2
3
4
5
6
7
8
9
10
11
// loop detection, should be false in first round of detection but it's correct
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>());
      // calc_information_matrix is a reweight procedure
      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));
}
  • 个人觉着，将回环边加入到图中时，顶点的添加顺序应该和相对位姿的作用方向严格对应，假设1&2节点互换，相对位姿也求逆就可以了。
  • 有小伙伴说VertexSE3在g2o中的实现是采用的右扰动，因此，和一般常见的左扰动不一致，就有点突然...也确实是这样的，亲测如此。

GPS点约束

​ hdl_graph_slam中GPS信号是不断接收的，因此会有一个时间戳，通过硬件同步之后，激光和GPS完成时间同步，就可以通过时间找到最近GPS点对应的激光帧的位置，从而构建关系。

  • 将一段滑窗内的gps点与攒起来的激光关键帧进行时间戳比较的时候，虽然找的是最近的，但是万一最近的gps点和某关键帧之间的时间差还是差很多，依然是无法建立关系的，需要及时剔除。
  • GPS点是WGS84坐标系下的，需要转成空间直角坐标系UTM下的北东高坐标才行。

误差方程

\[ e=G(t)-p \]

这种表示方法，说明一个gps点只能控制位置，无法控制旋转，因此没有R什么事情。

gps边的构建

​ 构建一元边，GPS节点是固定的。koide这里认为gps定位结果如果认为xyz都有效，则使用add_se3_prior_xyz_edge类型的边，如果只有平面左边有效，则使用add_se3_prior_xy_edge，信息矩阵的权重和观测精度有关。这里认为信息矩阵就是协方差的逆，协方差可以看成是在三个维度上观测值的方差。

​ 构建的一元边的类型分别是：g2o::EdgeSE3PriorXY和g2o::EdgeSE3PriorXYZ,都比较好理解。koide在使用时，误差采用的是估计值-观测值，和我的理解相反了，不过不影响结果。

1
2
3
4
5
6
void computeError() override {
    const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
    // only translation is used
    Eigen::Vector3d estimate = v1->estimate().translation();
    _error = estimate - _measurement;
}

​ 加入到图：

1
2
3
4
5
6
7
8
9
if(std::isnan(xyz.z())) {
    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);
} else {
    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);
}

IMU观测

误差方程

TODO

IMU边的构建

TODO

整个后端优化流程

​ ros环境下用roslaunch随便启动一个hdl_graph_slam的launch文件，都会启动如下四个节点：prefiltering_nodelet, scan_matching_odometry_nodelet， floor_detection_nodelet， hdl_graph_slam_nodelet，看名字就知道对应的功能是什么了。其实我对ros不熟悉，认为一个launch文件通过配置多个nodelet的方式同时启动多个节点就相当于实例化出多个对象，每个对象都可以开始做事情，之间可以快速的数据共享2。这些节点都是nodelet的派生类。

​ prefiltering_nodelet节点主要负责接收从硬件传回来的点云数据，完成去畸变，简单距离过滤等操作，然后将其逐帧发布为/filtered_points主题。

​ scan_matching_odometry_nodelet负责前端匹配，发布里程计消息。它也订阅了/filtered_points话题，一旦检测到消息，就调用cloud_callback() ，将传入的点云逐帧和之前的帧进行匹配，得到以前一帧点云为基准的相对位姿，并返回以全部帧中第一帧为基准的世界坐标下的位姿，核心是matching()函数。要随着激光帧的不断发布和接收，将new frame的位姿视为下一次前端匹配中的old frame的位姿，并以当前的匹配估计值作为下一次匹配的先验，提升匹配成功的概率。 \[ T_{source}=T_{new keyframe}^{w}\\ T_{target}=T_{old frame}^{w}\\ T_{source}^{target} = T_{new keyframe}^{old keyframe} = matching()\{ICP,NDT \quad liked \quad method\} \] ​ hdl_graph_slam_nodelet在一开始初始化的时候就自动订阅如下的主题，并开始循环接受其发出的消息：看名识功能

1
2
3
4
5
6
7
/odom;
/filtered_points;
/gpsimu_driver/imu_data;
/floor_detection/floor_coeffs;
/gps/geopoint;
/gpsimu_driver/nmea_sentence;
/gps/navsat，

​ hdl_graph_slam_nodelet在初始化时通过订阅/odom和/filtered_points的消息并调用cloud_callback()，将估计的里程计位姿和点云绑定在一起，并检查当前帧是否构成成为关键帧的条件，满足才会将关键帧留下来。

​ hdl_graph_slam_nodelet还包含了成员optimization_timer和map_publish_timer，都是ros下的一个定时器3类型，且在其虚函数onInit()函数中做了初始化。这两个定时器分别表示每隔3s做一次优化和每隔10s发布一次地图点的两个不同操作。optimization_timer_callback()和map_points_publish_timer_callback()就是定时器到点了之后执行的函数。

1
2
3
4
5
6
double graph_update_interval = private_nh.param<double>("graph_update_interval", 3.0);
double map_cloud_update_interval = private_nh.param<double>("map_cloud_update_interval", 10.0);
optimization_timer = mt_nh.createWallTimer(ros::WallDuration(graph_update_interval), 
                                               &HdlGraphSlamNodelet::optimization_timer_callback, this);
map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), 
                                              &HdlGraphSlamNodelet::map_points_publish_timer_callback, this);

​ optimization_timer_callback()回调函数中，首先执行flush_keyframe_queue()判断是否有关键帧更新，并对已有的关键帧构建成图，包括添加节点，构建序列节点之间的边（观测就是相对位姿\(T_{curr}^{prev}=T_{curr}^{-1}·T_{prev}\)，估计值就是各自的位姿）。关键帧更新成功之后，开始依次调用flush_floor_queue(), flush_gps_queue()和flush_imu_queue()，功能分别是：

  • flush_floor_queue()： 首先需要知道floor_detection_nodelet在一开始系统启动的时候就存在了，同时该节点会自动订阅/filtered_points节点的消息，收到消息之后就会执行cloud_callback()回调函数（onInit函数中），立马针对接收到的点云提取平面，发布平面参数。hdl_graph_slam_nodelet通过订阅相应的主题，就可以拿到平面提取结果。所有的平面提取结果是通过时间戳的形式和每一个关键帧建立关系的。因此flush_floor_queue()就是要通过时间比对，找到对应关系，往图中添加平面节点。
  • flush_gps_queue()：在hdl_graph_slam_nodelet的onInit()中设置了如果当前环境支持gps，就会自动分别从"/gps/geopoint", "/gpsimu_driver/nmea_sentence"和"/gps/navsat"节点订阅消息，分别执行gps_callback(), nmea_callback()和navsat_callback()。gps_callback()负责将现有的gps数据存储下来到队列中，nmea_callback()和navsat_callback()一样都是用已经写好的驱动包读取（解析）GNSS数据然后调用gps_callback()4，这样就把需要的数据都接收完了。然后flush_gps_queue()主要是通过时间检查，关联gps点和关键帧，找到对应关系，往图中添加gps节点。
  • flush_imu_queue()：TODO...

​ 这些传感器的约束就通过这三个函数引入了，如果这三个全都没有，直接退出优化，没有额外的观测，无法平差。其中任意一个有的话就继续。

​ 接下来就开始回环检测，将检测到的回环帧们构建成回环约束加入到图中。用于回环检测的对象是，当前时刻之前攒下的局部滑动窗口中的关键帧和现存的关键帧集合，一开始是空的，隔一段时间之后才会有积累，因此第一次loop detection肯定返回false的。然后将临时的关键帧拷贝到集合中。最后就开始图优化的过程了。LiDAR SLAM的图优化这里没用什么技巧，就是正常的选择求解器，设定循环次数，开始优化就结束了。优化完之后每个节点的位姿通过KeyFrameSnapshot和KeyFrame的转换（内部构造函数）中直接将图的节点位姿传递给关键帧的位姿，用于保存。

​ map_points_publish_timer_callback()就是将通过绑定的位姿将optimization_timer_callback()优化好的关键帧中的点云变换到统一的世界坐标系下，并抽稀，然后发布就完了。

Reference

  1. 1.在最新版的g2o中已经变成了和ceres一样的自动求导了... ↩︎
  2. 2.https://m.jb51.cc/xml/294932.html ↩︎
  3. 3.https://www.ncnynl.com/archives/201702/1296.html ↩︎
  4. 4.实际中可能IMU中也集成了一个GNSS，因此有两个 ↩︎
------ copyright ------

本博客所有文章除特别声明外，均采用 许可协议

title:Hdl_graph_slam论文及代码解析_回环检测

author:AmazingHao

link:http://whu-lyh.github.io/blogs/2023/03/06/Hdl_graph_slam/

publish time:2023-03-06

update time:2023-03-06

# ,
Hdl_graph_slam论文及代码解析_后端优化
B-Spline探秘
| visits
关注我

链接

分类

标签云

B-Spline CloudCompare Docker FFMPEG Geometics Geometry Hexo LiDAR_SLAM Linux MarkDown Mathemetics OSG PCL PaperReview Point Cloud Pytorch ROS VS WindowsScript Zotero 服务器 鱼眼图像

最新文章

related posts

归档

Your browser is out-of-date!

Update your browser to view this website correctly. Update my browser now

×