深蓝学院 《多传感器融合定位》 第2章作业

整体框架参考 从零开始做自动驾驶定位(九): 建图系统结构优化

我的电脑上 rosbag play kitti_2011_10_03_drive_0027_synced.bag -r 1.3 基本前端里程计能跟上gnss轨迹

先跑通建图

roslaunch mapping.launch

下图结果为后端刚优化完前950帧后建立出一部分全局地图的结果：

橙色 组合导航位姿 /synced_gnss（基本与绿色重合）
红色 前端里程计位姿 /transformed_odom
绿色 优化后的位姿 /optimized_key_frames
/optimized_odom（未用上）

back_end中，关键帧点云数据保存在 slam_data 文件夹下 使用的时候再去读取（节省内存）

        // 把关键帧点云存储到硬盘里
        std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(key_frames_deque_.size()) + ".pcd";
        pcl::io::savePCDFileBinary(file_path, *cloud_data.cloud_ptr);

回环检测中订阅了两个来自后端的数据：

    // subscriber
    key_frame_sub_ptr_ = std::make_shared<KeyFrameSubscriber>(nh, "/key_frame", 100000);
    key_gnss_sub_ptr_ = std::make_shared<KeyFrameSubscriber>(nh, "/key_gnss", 100000);

两个数据区别如下：

        KeyFrame key_frame;
        key_frame.time = laser_odom.time;
        key_frame.index = (unsigned int)key_frames_deque_.size();
        key_frame.pose = laser_odom.pose;
        key_frames_deque_.push_back(key_frame);
        current_key_frame_ = key_frame;

        current_key_gnss_.time = gnss_odom.time;
        current_key_gnss_.index = key_frame.index;
        current_key_gnss_.pose = gnss_odom.pose;

/key_frame数据中存的是激光里程计的时间和位姿，/key_gnss存的是原始组合导航的

class KeyFrame：

class KeyFrame {
   
   
  public:
    double time = 0.0;
    unsigned int index = 0;
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();

  public:
    Eigen::Quaternionf GetQuaternion();
};

bool LoopClosingFlow::Run() {
   
   
    if (!ReadData())
        return false;

    while(HasData()) {