自动驾驶与机器人中的SLAM技术--第六章--2D SLAM 回环检测与优化

主函数文件

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/highgui.hpp>

#include "ch6/lidar_2d_utils.h"
#include "ch6/mapping_2d.h"
#include "common/io_utils.h"

DEFINE_string(bag_path, "./dataset/sad/2dmapping/floor1.bag", "数据包路径");
DEFINE_bool(with_loop_closing, false, "是否使用回环检测");

/// 测试2D lidar SLAM

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);    // 初始化Google的日志库
    FLAGS_stderrthreshold = google::INFO;  // 设置日志输出级别为INFO
    FLAGS_colorlogtostderr = true;         // 启用彩色日志输出到stderr
    google::ParseCommandLineFlags(&argc, &argv, true);  // 解析命令行参数

    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path);   // 使用FLAGS_bag_path标志指定的路径创建一个RosbagIO对象
    sad::Mapping2D mapping;

    std::system("rm -rf ./data/ch6/*");  // 执行系统命令，删除./data/ch6/目录下的所有文件

    if (mapping.Init(FLAGS_with_loop_closing) == false) {
        return -1;
    }
    
    // 为来自/pavo_scan_bottom主题的2D扫描数据添加处理程序
    // 处理程序使用mapping.ProcessScan方法处理每个扫描
    // Go()开始处理
    rosbag_io.AddScan2DHandle("/pavo_scan_bottom", [&](Scan2d::Ptr scan) { return mapping.ProcessScan(scan); }).Go();
    // 将全局地图图像保存到./data/ch6/global_map.png，分辨率为2000
    cv::imwrite("./data/ch6/global_map.png", mapping.ShowGlobalMap(2000));
    return 0;
}

 流程

一、初始化

bool Mapping2D::Init(bool with_loop_closing) {
    keyframe_id_ = 0;
    current_submap_ = std::make_shared<Submap>(SE2());
    all_submaps_.emplace_back(current_submap_);

    if (with_loop_closing) {
        loop_closing_ = std::make_shared<LoopClosing>();
        loop_closing_->AddNewSubmap(current_submap_);
    }

    return true;
}

二、扫描数据处理

bool Mapping2D::ProcessScan(MultiScan2d::Ptr scan) { return ProcessScan(MultiToScan2d(scan)); }

bool Mapping2D::ProcessScan(Scan2d::Ptr scan) {
···
}

        每一次扫描对应一个帧，用当前扫描scan构造当前帧current_frame_，并使frame_id_加1。

current_frame_ = std::make_shared<Frame>(scan);
current_frame_->id_ = frame_id_++;

        用连续的三个帧1、2、3来表示转换关系。设最新的帧即当前帧为3，如果上一帧2存在，则当前帧3位姿 = 上一帧2位姿 * motion_guess_ ，用来作为初始位姿估计。
        由于在上一帧的操作中，motion_guess_ = T1w * Tw2 = T12，即上一帧2到上上一帧1的位姿变换。所以。当前帧3位姿 = 上一帧2位姿 * motion_guess_  = Tw2 * T12 = ？为什么是这样？
        第一帧以及其他关键帧，都会加入到当前子图的占据栅格地图中，更新格子和场函数图像。

if (last_frame_) {
        // set pose from last frame
        // current_frame_->pose_ = last_frame_->pose_;
        current_frame_->pose_ = last_frame_->pose_ * motion_guess_;
        current_frame_->pose_submap_ = last_frame_->pose_submap_;
    }

    // 利用scan matching来匹配地图
    if (!first_scan_) {
        // 第一帧无法匹配，直接加入到occupancy map
        current_submap_->MatchScan(current_frame_);  // 匹配优化出当前位姿
    }

    // current_submap_->AddScanInOccupancyMap(current_frame_);
    first_scan_ = false;
    bool is_kf = IsKeyFrame();

    if (is_kf) {
        AddKeyFrame();
        current_submap_->AddScanInOccupancyMap(current_frame_);
        /*void Submap::AddScanInOccupancyMap(std::shared_ptr<Frame> frame) {
            // 更新栅格地图中的格子
            occu_map_.AddLidarFrame(frame, OccupancyMap::GridMethod::MODEL_POINTS);  
            // 更新场函数图像
            field_.SetFieldImageFromOccuMap(occu_map_.GetOccupancyGrid());          
        }*/

        // 处理回环检测
        if (loop_closing_) {
            loop_closing_->AddNewFrame(current_frame_);
        }

        if (current_submap_->HasOutsidePoints() || (current_submap_->NumFrames()) > 50) {
            /// 走出了submap或者单个submap中的关键帧较多
            ExpandSubmap();
        }
    }

        进行回环检测，此时current_frame_为loop_closing类的私有成员huan

void LoopClosing::AddNewFrame(std::shared_ptr<Frame> frame) {
    current_frame_ = frame;
    
    // 使用当前submap的id来与其他submap进行匹配，不检查离当前submap最近的几个submap。
    // 并忽略与当前submap有关联的submap
    // 计算当前帧与其他submap中心的距离，这个距离小于阈值，则把此submap加入candidate中
    if (!DetectLoopCandidates()) {
        return;
    }

    // 取出候选submap对应的似然场，将初始位姿Ts1c = Ts1w * Twc，即当前帧在历史submap中的位姿
    // Ts1c作为似然场优化的初始位姿，结合优化后的位姿Ts1c，可以得到 Ts1s2 = Ts1c * Tcw * Tws2
    // 根据历史submap和当前submap的id以及Ts1s2，建立loopconstraint
    // has_new_loosp_置为true
    MatchInHistorySubmaps();

    if (has_new_loops_) {
        // 位姿图优化，顶点包括submap的id和pose
        // 连续约束，从i=0一直到i<last_submp_id_（即当前submap），建立第i和i+1个submap之间的边
        // e = Log(Ts1w * Tws2 * Ts2s1)
        // 回环约束，对有回环约束的两个submap进行联合优化
        // 两种约束加入到同一个optimizer中
        // 第一轮优化后（10次迭代），使用回环边的卡方值进行回环约束的验证，根据值大小对回环约束的valid_成员进行true或false的设定。无效的回环边设置为次要边
        // 第二轮优化（5次迭代），更新所有子图的位姿以及子图包含的所有关键帧的位姿
        // 移除错误的回环约束，即valid_ = false
        Optimize();
    }
}

        回环优化完毕后，判断是否需要新增子地图

if (current_submap_->HasOutsidePoints() || (current_submap_->NumFrames()) > 50) {
            /// 走出了submap或者单个submap中的关键帧较多
            ExpandSubmap();
        }

         最后是可视化输出

/// 可视化输出
    auto occu_image = current_submap_->GetOccuMap().GetOccupancyGridBlackWhite();
    Visualize2DScan(current_frame_->scan_, current_frame_->pose_, occu_image, Vec3b(0, 0, 255), 1000, 20.0,
                    current_submap_->GetPose());
    cv::putText(occu_image, "submap " + std::to_string(current_submap_->GetId()), cv::Point2f(20, 20),
                cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
    cv::putText(occu_image, "keyframes " + std::to_string(current_submap_->NumFrames()), cv::Point2f(20, 50),
                cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
    cv::imshow("occupancy map", occu_image);

    auto field_image = current_submap_->GetLikelihood().GetFieldImage();
    Visualize2DScan(current_frame_->scan_, current_frame_->pose_, field_image, Vec3b(0, 0, 255), 1000, 20.0,
                    current_submap_->GetPose());
    cv::imshow("likelihood", field_image);
    
    /// global map
    if (is_kf) {
        cv::imshow("global map", ShowGlobalMap());
    }

        变量调整，如前面所示，motion_guess_ = T1w * Tw2 = T12，即上一帧2到上上一帧1的位姿变换。

    cv::waitKey(10);

    if (last_frame_) {
        motion_guess_ = last_frame_->pose_.inverse() * current_frame_->pose_;
    }

    last_frame_ = current_frame_;

    return true;