从零开始学自动驾驶（3）——前端里程计之代码优化

此节对应任佬代码tag5.0
在上一节的front_end_node节点中，代码依旧很长，回顾一下，大致是这个样子

emmmmm其实也还好，下面是，简化后的节点代码：

把各个部分进行封装，使整个代码看起来更加清晰和简洁，也更有利于我们对各个模块进行扩展和替换对比。。

保存全局地图模块

//接受存储地图信息
ros::ServiceServer service = nh.advertiseService("save_map", save_map_callback);

回调函数

bool save_map_callback(saveMap::Request &request, saveMap::Response &response) {
    response.succeed = _front_end_flow_ptr->SaveMap();
    _front_end_flow_ptr->PublishGlobalMap();

    return response.succeed;
}

SaveMap()函数用于保存全局地图具体实现

bool FrontEnd::SaveMap() {
    //重置全局地图
    global_map_ptr_.reset(new CloudData::CLOUD());

    std::string key_frame_path = "";
    //定义保存关键帧点云和转化位置后点云的类指针
    CloudData::CLOUD_PTR key_frame_cloud_ptr(new CloudData::CLOUD());
    CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());
    //
    for (size_t i = 0; i < global_map_frames_.size(); ++i) {
        //建立保存位置和pcd文件
        key_frame_path = data_path_ + "/key_frames/key_frame_" + std::to_string(i) + ".pcd";
        //把关键帧保存置pcd文件中
        pcl::io::loadPCDFile(key_frame_path, *key_frame_cloud_ptr);
        //把关键帧转化到到地图坐标系上，完成地图拼接，并保存
        pcl::transformPointCloud(*key_frame_cloud_ptr, 
                                *transformed_cloud_ptr, 
                                global_map_frames_.at(i).pose);
        *global_map_ptr_ += *transformed_cloud_ptr;
    }
    //建立地图map.pcd文件用于保存全局地图
    std::string map_file_path = data_path_ + "/map.pcd";
    //全局地图导入到map.pcd文件中
    pcl::io::savePCDFileBinary(map_file_path, *global_map_ptr_);
    has_new_global_map_ = true;

    return true;
}

保存局部地图和关键帧模块

bool FrontEnd::UpdateWithNewFrame(const Frame& new_key_frame) {
    // 把关键帧点云存储到硬盘里，节省内存 全局地图由关键帧组成，所以全局地图的大小就是关键帧的数量
    std::string file_path = data_path_ + "/key_frames/key_frame_" + std::to_string(global_map_frames_.size()) + ".pcd";
    //把关键帧存入建立的pcd文件中
    pcl::io::savePCDFileBinary(file_path, *new_key_frame.cloud_data.cloud_ptr);

    Frame key_frame = new_key_frame;
    // 这一步的目的是为了把关键帧的点云保存下来
    // 由于用的是共享指针，所以直接复制只是复制了一个指针而已
    // 此时无论你放多少个关键帧在容器里，这些关键帧点云指针都是指向的同一个点云
    //简单点说就是这个指针指向点云里的点云
    key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD(*new_key_frame.cloud_data.cloud_ptr));
    CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());
    
    // 更新局部地图
    local_map_frames_.push_back(key_frame);
    //局部地图大于一定数量，去除以前的数据
    while (local_map_frames_.size() > static_cast<size_t>(local_frame_num_)) {
        local_map_frames_.pop_front();
    }
    local_map_ptr_.reset(new CloudData::CLOUD());
    //更新局部点云
    for (size_t i = 0; i < local_map_frames_.size(); ++i) {
        //点云转化到局部坐标系下，进行拼接
        pcl::transformPointCloud(*local_map_frames_.at(i).cloud_data.cloud_ptr, 
                                 *transformed_cloud_ptr, 
                                 local_map_frames_.at(i).pose);
        *local_map_ptr_ += *transformed_cloud_ptr;
    }
    has_new_local_map_ = true;

    // 更新ndt匹配的目标点云
    // 关键帧数量还比较少的时候不滤波，因为点云本来就不多，太稀疏影响匹配效果
    if (local_map_frames_.size() < 10) {
        registration_ptr_->SetInputTarget(local_map_ptr_);
    } else {
        //对点云进行滤波操作，在存储到容器中作为待匹配点云
        CloudData::CLOUD_PTR filtered_local_map_ptr(new CloudData::CLOUD());
        local_map_filter_ptr_->Filter(local_map_ptr_, filtered_local_map_ptr);
        registration_ptr_->SetInputTarget(filtered_local_map_ptr);
    }

    // 保存所有关键帧信息在容器里
    // 存储之前，点云要先释放，因为已经存到了硬盘里，不释放也达不到节省内存的目的
    key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD());
    global_map_frames_.push_back(key_frame);

    return true;
}

订阅数据、发布数据模块

调用FrontEndFlow类中FrontEndFlow(ros::NodeHandle& nh)函数

    //一个指向FrontEndFlow的指针，完成了各种订阅和发布话题等的功能
    _front_end_flow_ptr = std::make_shared<FrontEndFlow>(nh);

此函数完成如下功能：

FrontEndFlow::FrontEndFlow(ros::NodeHandle& nh) {
    //接收点云、imu、GNSS、雷达IMU外参标定数据
    cloud_sub_ptr_ = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
    imu_sub_ptr_ = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);
    gnss_sub_ptr_ = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
    lidar_to_imu_ptr_ = std::make_shared<TFListener>(nh, "velo_link", "imu_link");
    //发布点云、局部地图、全局地图、激光里程计、GNSS里程计数据
    cloud_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "current_scan", 100, "/map");
    local_map_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "local_map", 100, "/map");
    global_map_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "global_map", 100, "/map");
    laser_odom_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "laser_odom", "map", "lidar", 100);
    gnss_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "gnss", "map", "lidar", 100);

    front_end_ptr_ = std::make_shared<FrontEnd>();
    //重置局部地图、全局地图、当前帧扫描信息
    local_map_ptr_.reset(new CloudData::CLOUD());
    global_map_ptr_.reset(new CloudData::CLOUD());
    current_scan_ptr_.reset(new CloudData::CLOUD());
}

整体运行流程

开始运行整个流程：

//开始运行整个流程
 _front_end_flow_ptr->Run();

_front_end_flow_ptr 现在是指向 FrontEndFlow 类的指针，下面看一下run()的实现流程，run()函数会逐步调用下面的函数

bool FrontEndFlow::Run() {
    //读取数据
    ReadData();
    //读取标定信息
    if (!InitCalibration()) 
        return false;
    //初始化GNSS
    if (!InitGNSS())
        return false;
    //判断数据是否有效
    while(HasData()) {
        if (!ValidData())
            continue;
        //更新GNSS里程计信息
        UpdateGNSSOdometry();
        //更新激光里程计信息并发布
        if (UpdateLaserOdometry())
            PublishData();
    }
    return true;
}

每一部分均与上一节类似。。

读取数据

//读取数据，把数据保存在cloud_data_buff_中
bool FrontEndFlow::ReadData() {
    cloud_sub_ptr_->ParseData(cloud_data_buff_);
    imu_sub_ptr_->ParseData(imu_data_buff_);
    gnss_sub_ptr_->ParseData(gnss_data_buff_);

    return true;
}

读取imu坐标系相对于雷达的坐标变换

bool FrontEndFlow::InitCalibration() {
    static bool calibration_received = false;
    if (!calibration_received) {
        if (lidar_to_imu_ptr_->LookupData(lidar_to_imu_)) {
            calibration_received = true;
        }
    }

    return calibration_received;
}

初始化GNSS，第一帧来作为初始坐标

bool FrontEndFlow::InitGNSS() {
    static bool gnss_inited = false;
    if (!gnss_inited && gnss_data_buff_.size() > 0) {
        GNSSData gnss_data = gnss_data_buff_.front();
        gnss_data.InitOriginPosition();
        gnss_inited = true;
    }

    return gnss_inited;
}

判断三个数据容器中是否存在信息

bool FrontEndFlow::HasData() {
    if (cloud_data_buff_.size() == 0)
        return false;
    if (imu_data_buff_.size() == 0)
        return false;
    if (gnss_data_buff_.size() == 0)
        return false;
    
    return true;
}

提取有效点，对雷达、imu、GNSS做时间同步

bool FrontEndFlow::ValidData() {
    current_cloud_data_ = cloud_data_buff_.front();
    current_imu_data_ = imu_data_buff_.front();
    current_gnss_data_ = gnss_data_buff_.front();

    double d_time = current_cloud_data_.time - current_imu_data_.time;
    if (d_time < -0.05) {
        cloud_data_buff_.pop_front();
        return false;
    }

    if (d_time > 0.05) {
        imu_data_buff_.pop_front();
        gnss_data_buff_.pop_front();
        return false;
    }

    cloud_data_buff_.pop_front();
    imu_data_buff_.pop_front();
    gnss_data_buff_.pop_front();

    return true;
}

更新GNSS里程计信息

bool FrontEndFlow::UpdateGNSSOdometry() {
    gnss_odometry_ = Eigen::Matrix4f::Identity();
    //将经度、纬度、高度信息转化为东北天坐标系下的E N U
    current_gnss_data_.UpdateXYZ();
    gnss_odometry_(0,3) = current_gnss_data_.local_E;
    gnss_odometry_(1,3) = current_gnss_data_.local_N;
    gnss_odometry_(2,3) = current_gnss_data_.local_U;
    gnss_odometry_.block<3,3>(0,0) = current_imu_data_.GetOrientationMatrix();
    gnss_odometry_ *= lidar_to_imu_;//imu相对于雷达位姿变换

    return true;
}

更新激光里程计信息

bool FrontEndFlow::UpdateLaserOdometry() {
    static bool front_end_pose_inited = false;
    //如果没有初始化，以GNSS第一帧数据进行初始化
    if (!front_end_pose_inited) {
        front_end_pose_inited = true;
        front_end_ptr_->SetInitPose(gnss_odometry_);
        //GNSS作为激光历程计的初始化
        laser_odometry_ = gnss_odometry_;
        return true;
    }

    laser_odometry_ = Eigen::Matrix4f::Identity();
    if (front_end_ptr_->Update(current_cloud_data_, laser_odometry_))
        return true;
    else 
        return false;
}

发布数据

bool FrontEndFlow::PublishData() {
    //发布GNSS、激光里程计数据
    gnss_pub_ptr_->Publish(gnss_odometry_);
    laser_odom_pub_ptr_->Publish(laser_odometry_);
    //发布当前帧扫描数据
    front_end_ptr_->GetCurrentScan(current_scan_ptr_);
    cloud_pub_ptr_->Publish(current_scan_ptr_);
    //发布局部地图数据
    if (front_end_ptr_->GetNewLocalMap(local_map_ptr_))
        local_map_pub_ptr_->Publish(local_map_ptr_);

    return true;
}

保存地图

bool FrontEndFlow::SaveMap() {
    return front_end_ptr_->SaveMap();
}

发布全局地图

bool FrontEndFlow::PublishGlobalMap() {
    if (front_end_ptr_->GetNewGlobalMap(global_map_ptr_)) { 
        global_map_pub_ptr_->Publish(global_map_ptr_);
        global_map_ptr_.reset(new CloudData::CLOUD());
    }
    return true;
}
}

似乎感觉截图的方式看起来比较舒服一些，后面的文章打算多以截图的方式给出

匹配模块

我们可以先定义一个匹配函数的基类，然后定义 匹配算法 NDT 的子类，这样后面我们更换匹配算法的时候，重新定义一个新子类即可
基类：
NDT子类

在使用过程中，我们可以直接用基类的指针指向子类来进行函数调用
下面一起看看调用匹配算法的流程是如何进行的
首先在更新激光里程计的大步骤中：

然后在Update函数下：
最后调用ndt：

中间一个小问题找了好长时间，没有发现程序在哪里调用的子类匹配算法，也就是InitRegistration函数，然后找阿找阿

到这里还是很蒙阿，那咋调用的 FrontEnd( ) 呢?? 然后复习了一下多态知识，发现类被调用的时候，默认构造函数会运行，豁然开朗：front_end_ptr = std::make_shared(); 这句当时就不知道干嘛的，原来是 指针指向类时，make_shared()调用了类的默认构造函数。

滤波模块

滤波模块与ndt模块类似

滤波基类
滤波子类
滤波调用

滤波实现

配置文件

为了方便调试，把常用的参数写在配置文件里是必须的，主要为yaml格式，先看一下文件中的主要内容：
首先找到yaml文件位置，然后配置一个节点来调用文件中内容：

调用配置文件



。。。。。。