返回

[代码实践] 使用 Scan Context 进行回环检测

简单测试 Scan Context 进行回环检测的准确性和速度。

介绍

关于 Scan Context 的算法原理可以参考我之前整理的博客:[论文阅读]Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map 以及以 SC-LeGO-LOAM 为例使用 Scan Context。这篇博客同样基于任乾大佬写的 SLAM localization_in_auto_driving 进行一定魔改,对基于距离进行回环检测和基于 Scan Context 进行回环检测进行了简单的比较。代码在:General Mapping and Localization Framework

测试框架

关于测试框架中回环修正的部分,可以参考任乾大佬的这篇博客:从零开始做自动驾驶定位(十一): 闭环修正,这里我做了以下修改:

在 SLAM 框架中,加入以下接口。

class LoopClousreDetectorInterface {
public:
    virtual ~LoopClousreDetectorInterface() = default;

    virtual void addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) = 0;
    virtual bool DetectNearestKeyFrame(int& key_frame_index) = 0;
};

回环检测的大致流程是对每一个关键帧,在检测器中加入所需的信息,然后进行一次检测,获得当前关键帧对应的历史关键帧索引。对于激光建图来说,所需的信息无外乎是当前帧的点云以及对应的位姿估计。接下来实现基于位置信息以及点云 Scan Context 的两种检测接口。如果成功获得检测器检测出的回环帧,我们还需要进行一次点云配准,以保证点云匹配关系的正确性。

基于位置信息的回环检测

实现以下类:

class DistanceDetector : public LoopClousreDetectorInterface {
public:
    DistanceDetector(const YAML::Node& node);

    void addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) override;
    bool DetectNearestKeyFrame(int& key_frame_index) override;

private:
    int loop_step_ = 5;
    int diff_num_ = 10.0;
    int detect_area_ = 10.0;
    std::deque<KeyFrame> historical_key_frames;
};

DistanceDetector::DistanceDetector(const YAML::Node& node) {
    loop_step_ = node["loop_step"].as<int>();
    diff_num_ = node["diff_num"].as<int>();
    detect_area_ = node["detect_area"].as<float>();
}

void DistanceDetector::addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) {
    historical_key_frames.push_back(key_frame);
}
bool DistanceDetector::DetectNearestKeyFrame(int& key_frame_index) {
    static int skip_cnt = 0;
    static int skip_num = loop_step_;
    if (++skip_cnt < skip_num) return false;

    if ((int)historical_key_frames.size() < diff_num_ + 1) return false;

    int key_num = (int)historical_key_frames.size();
    float min_distance = 1000000.0;
    float distance = 0.0;

    KeyFrame history_key_frame;
    KeyFrame current_key_frame = historical_key_frames.back();

    key_frame_index = -1;
    for (int i = 0; i < key_num - 1; ++i) {
        if (key_num - i < diff_num_) break;

        history_key_frame = historical_key_frames.at(i);
        distance = fabs(current_key_frame.pose(0, 3) - history_key_frame.pose(0, 3)) +
                   fabs(current_key_frame.pose(1, 3) - history_key_frame.pose(1, 3)) +
                   fabs(current_key_frame.pose(2, 3) - history_key_frame.pose(2, 3));
        if (distance < min_distance) {
            min_distance = distance;
            key_frame_index = i;
        }
    }

    skip_cnt = 0;
    skip_num = (int)min_distance;
    if (min_distance > detect_area_) {
        skip_num = std::max((int)(min_distance / 2.0), loop_step_);
        return false;
    } else {
        skip_num = loop_step_;
        return true;
    }
}

可以看到,基于距离信息搜索回环检测的逻辑比较简单,只需要每一次将关键帧对应的位置信息存放在队列中,每一次检测时对历史帧进行一次遍历,找到最近的历史帧再判断是否低于阈值即可。这里基本上是从原框架分离出来的,没有做太多修改,原作者为了降低回环检测的时间,做了以下处理:

  • 不对较近的关键帧进行回环判断
  • 在找到了一个回环之后,对接下来的几个关键帧不进行回环检测,避免过多重复信息的加入
  • 如果当前检测到的最近帧太远(比如 100m),对接下来的若干帧都不进行检测,因为显然不会出现回环

除了这三种方式降低搜索时间以外,我们还可以将位置信息作为一个点添加至 KD 树中,然后对 KD 树进行范围搜索也可以节省搜索时间。

基于 Scan Context 的回环检测

实现以下类:

class ScanContextDetector : public LoopClousreDetectorInterface {
public:
    ScanContextDetector(const YAML::Node& node);
    void addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) override;
    bool DetectNearestKeyFrame(int& key_frame_index) override;

private:
    SCManager sc_manager_;
};

ScanContextDetector::ScanContextDetector(const YAML::Node& node) {
    unsigned int num_exclude_recent = node["num_exclude_recent"].as<unsigned int>();
    double search_ratio = node["search_ratio"].as<double>();
    double sc_dist_thres = node["sc_dist_thres"].as<double>();

    std::cout << "Scan context 使用的参数为:"
              << "num_exclude_recent: " << num_exclude_recent << ", search_ratio: " << search_ratio
              << ", sc_dist_thres: " << sc_dist_thres << std::endl;
    sc_manager_.setParameters(num_exclude_recent, search_ratio, sc_dist_thres);
}

void ScanContextDetector::addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) {
    sc_manager_.makeAndSaveScancontextAndKeys(*cloud_data);
}

bool ScanContextDetector::DetectNearestKeyFrame(int& key_frame_index) {
    auto detect_result = sc_manager_.detectLoopClosureID();  // first: nn index, second: yaw diff
    if (detect_result.first == -1) return false;
    key_frame_index = detect_result.first;
    return true;
}

Scan Context 已经封装好了接口,用户使用时只需要简单的设置一下参数,每一次加入一帧关键帧和进行检测即可。

结果比较

比较方式

首先根据参数选择使用哪种检测方式,这里比较了三种方式:使用 GNSS 观测信息进行回环检测(在 KITTI 中这是 RTK 信息,已经准确度很高);使用激光里程计的位置信息进行回环检测,以及使用 Scan Context 进行回环检测。

bool LoopClosing::InitParam(const YAML::Node& config_node) {
    extend_frame_num_ = config_node["extend_frame_num"].as<int>();
    fitness_score_limit_ = config_node["fitness_score_limit"].as<float>();

    std::string search_criteria = config_node["search_criteria"].as<std::string>();
    if (search_criteria == "scan_context") {
        search_criteria_ = SearchCriteria::ScanContext;
        loop_clousre_detector_ptr_ = std::make_shared<ScanContextDetector>(config_node[search_criteria]);
    } else if (search_criteria == "distance_gnss") {
        search_criteria_ = SearchCriteria::Distance_GNSS;
        loop_clousre_detector_ptr_ = std::make_shared<DistanceDetector>(config_node[search_criteria]);
    } else if (search_criteria == "distance_odom") {
        search_criteria_ = SearchCriteria::Distance_Odom;
        loop_clousre_detector_ptr_ = std::make_shared<DistanceDetector>(config_node[search_criteria]);
    } else {
        std::cout << "没有和 " << search_criteria << " 对应的回环检测检测。 默认使用 GNSS 位置作为回环检测方式,\n";
        search_criteria_ = SearchCriteria::Distance_GNSS;
        loop_clousre_detector_ptr_ = std::make_shared<ScanContextDetector>(config_node["scan_context"]);
    }

    return true;
}

对每一个关键帧,获取对应的点云、里程计位姿估计和它时间同步的 GNSS 信息,按选择的方式进行回环检测,并记录时间。为了比较不同检测方式获得的回环信息的数量和准确性,所以在回环信息中加入置信度,从回环检测器中获得回环时先置为 false,只有通过点云配准检查才置为 true,方便后续 rviz 可视化。

bool LoopClosing::Update(const KeyFrame key_frame, const KeyFrame key_gnss) {
    has_new_loop_pose_ = false;

    all_key_frames_.push_back(key_frame);
    all_key_gnss_.push_back(key_gnss);

    // 从硬盘中读取关键帧点云
    CloudData::Cloud_Ptr scan_cloud_ptr(new CloudData::Cloud());
    std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(all_key_frames_.back().index) + ".pcd";
    pcl::io::loadPCDFile(file_path, *scan_cloud_ptr);
    scan_filter_ptr_->Filter(scan_cloud_ptr, scan_cloud_ptr);

    static Timer loop_closure_timer("Loop Closure");
    loop_closure_timer.tic();
    int key_frame_index = 0;
    if (search_criteria_ == SearchCriteria::Distance_GNSS) {
        // 基于距离搜索距离较近的关键帧
        loop_clousre_detector_ptr_->addLatestKeyFrame(key_gnss, scan_cloud_ptr);
    } else if (search_criteria_ == SearchCriteria::Distance_Odom) {
        // 基于里程计位置估计搜索较近的关键帧
        loop_clousre_detector_ptr_->addLatestKeyFrame(key_frame, scan_cloud_ptr);
    } else if (search_criteria_ == SearchCriteria::ScanContext) {
        // 将下采样后的点云传入 sc manager 构建并存储 scan context
        loop_clousre_detector_ptr_->addLatestKeyFrame(key_frame, scan_cloud_ptr);
    }

    has_new_loop_pose_ = loop_clousre_detector_ptr_->DetectNearestKeyFrame(key_frame_index);
    loop_closure_timer.toc();

    if (!has_new_loop_pose_) return false;

    // 初始化回环信息(此时置信度设为 false)
    current_loop_pose_.index0 = all_key_frames_.at(key_frame_index).index;
    current_loop_pose_.index1 = all_key_frames_.back().index;
    current_loop_pose_.time = all_key_frames_.back().time;
    current_loop_pose_.confidence = false;

    std::cout << "检测到闭环 "
              << ": 帧" << current_loop_pose_.index0 << "------>"
              << "帧" << current_loop_pose_.index1 << std::endl;

    if (key_frame_index < extend_frame_num_) {
        std::cout << "匹配失败!\n";
        return false;
    }

    if (CloudRegistration(key_frame_index)) {
        current_loop_pose_.confidence = true;
    }

    LOG(INFO) << loop_closure_timer;

    return true;
}

基于 GNSS 信息的回环检测

整体结果如下,黄色为参考真值(RTK),绿色为里程计轨迹,白色为基于回环信息优化的轨迹。轨迹之间的边为:红色为正确的回环(通过点云配准检查),黄色为不正确回环(未通过点云配准检查)。不难发现,基于 GNSS 的回环检测能够捕捉到绝大部分的回环信息,并且基本不会误判。

rviz_traj
rviz_traj

平移误差如下:

translation
translation

姿态误差如下:

attitude
attitude

轨迹绝对误差如下:

ape
ape

轨迹相对(分段)误差如下:

rpe
rpe

基于里程计位置信息的回环检测

基于里程计位置信息的回环检测结果如下,大致发现如下:由于考虑到里程计出现的漂移,所以不得不增加搜索范围,但是增加搜索范围也同样容易引入误匹配(因此黄边很多)。不过,结合点云配准来拒绝错误的匹配结果。将少量的正确结果加入图优化中,也能整体轨迹进行较好的优化校正。但由于数据集最后部分的回环没有检测出来,导致轨迹没有闭合,因此轨迹后期在高度方向上发散了。

rviz_traj
rviz_traj

平移误差如下:

translation
translation

轨迹绝对误差如下:

ape
ape

轨迹相对(分段)误差如下:

rpe
rpe

基于 Scan Context 的回环检测

使用 Scan Context 进行回环检测效果如下:由于没有对检测频率做特殊处理,所以基本上就是每个关键帧检测一次,因此可以发现检测出来的结果非常多,其中也出现了少量的误匹配,不过影响不大。从轨迹误差来看,基本上由于最后轨迹闭合了,因此三个方向的约束都不错。

rviz_traj
rviz_traj

平移误差如下:

translation
translation

姿态误差如下:

attitude
attitude

轨迹绝对误差如下:

ape
ape

轨迹相对(分段)误差如下:

rpe
rpe

建图效果

未回环时,点云拼接效果很差:

未回环-俯视
未回环-俯视

高度方向上也出现很大的偏离:

未回环-侧视
未回环-侧视

回环优化后,拼接效果很好:

回环-俯视
回环-俯视

回环-侧视
回环-侧视

其他参数

通过 EVO 获得三种方法优化后的轨迹比较参数,这里简单列举一下:

  • GNSS: RMSE = 5.57020486847518, MEAN = 4.72243917714598, Time Average = 3.22E-06 seconds
  • Odom: RMSE = 11.0530064883216, MEAN = 8.37993351549253, Time Average = 2.11E-06 seconds
  • SC: RMSE = 4.82988658987946, MEAN = 4.13946570946313, Time Average = 1.33E-03 seconds

这里,由于基于距离的回环检测有一系列筛选标准,所以不能代表每一次检测的搜索时间,但整体而言会比 SC 快,但 SC 的方法本身也已经足够快,可以应付一般需求。

结论

  • 基于距离的回环检测原理没有太大问题,通过增大范围结合点云配准可以获得少量正确的匹配,但如果错过某些闭环则会导致整体轨迹不闭合而导致某部分轨迹发散
  • 回环检测虽然能对轨迹整体进行较好的修正,但由于激光 SLAM 本身缺少 roll, pitch, yaw 以及高度方向上的可观性(这里由于在初始时刻和 RTK 进行姿态对齐了所以没体现出来),因此这些方向的优化结果会相对较差。如果要提升的话需要加入其他对这些参数可观的传感器,如磁力计。
Built with Hugo
Theme Stack designed by Jimmy