基于NDT的前端里程计框架优化

本文旨在对上一讲基于NDT的前端里程计代码解析
进行框架上的优化，主要参考知乎上专栏文章《从零开始做自动驾驶定位
》，在此基础上进行更加清晰的代码框架解读。
首先上一篇文章有以下缺点：
1、没有专门的参数配置文件.yaml
2、点云滤波、匹配作为常用的操作，应该专门设置模块。
3、没有内存管理：每个关键帧都存了点云，所有关键帧在内存中随着时间推移严重影响运行速度。这里考虑除了滑动窗局部地图涉及的关键帧放在内存里，其他关键帧的点云可以先存储在硬盘里，等用的时候再读出来。
4、代码混乱，没有进行封装。
针对以上四个方面的问题，本文在原有代码框架基础上进行了优化。

front_end

主要函数及其含义如下表所示：

优化框架中的所有函数都是bool 类型，函数最后都要return true.与此同时，需要更改输入参数的都要表示为引用。

1、配置文件

有关调用Opencv的cvFileStorage类创建、读取XML/YAML文档的操作可以参考之前写过的一篇文章CvFileStorage 类的数据存取操作与示例，
本工程中把参数内容放到YAML::Node格式的变量中，具体的配置参数放在config/front_end的config.yaml文件中。
涉及到的相关函数为：

默认构造函数FrontEnd()

默认构造函数初始化主要分为两部分：1、三个指针：局部地图、全局地图以及点云结果 2、参数初始化主函数

FrontEnd::FrontEnd()
     // 三个指针：局部、全局地图、点云
    :local_map_ptr_(new CloudData::CLOUD()),
     global_map_ptr_(new CloudData::CLOUD()),
     result_cloud_ptr_(new CloudData::CLOUD()) {
   
   
    // 参数初始化主函数
    InitWithConfig();
}

参数初始化主函数InitWithConfig()

首先产生YAML::Node类型的文件路径节点，然后分为三个步骤：分别初始化路径、匹配、滤波。

bool FrontEnd::InitWithConfig() {
   
   

    // 1.产生文件路径节点config_node
    std::string config_file_path = WORK_SPACE_PATH + "/config/front_end/config.yaml";
    YAML::Node config_node = YAML::LoadFile(config_file_path);
    // 2.初始化路径
    InitDataPath(config_node);
    // 3.初始化匹配
    InitRegistration(registration_ptr_, config_node);
    // 4.三个初始化滤波
    InitFilter("local_map", local_map_filter_ptr_, config_node);
    InitFilter("frame", frame_filter_ptr_, config_node);
    InitFilter("display", display_filter_ptr_, config_node);

    return true;
}

初始化路径InitDataPath()

首先根据YAML::Node路径节点获得文件夹路径data_path_，判断是否为文件夹，接下来分别记录地图点云存放地址data_path_和关键帧点云存放地址key_frame_path=data_path_ + “/key_frames”。

bool FrontEnd::InitDataPath(const YAML::Node& config_node) {
   
   
    // 1.获得data_path_转为string类型
    data_path_ = config_node["data_path"].as<std::string>();
    if (data_path_ == "./") {
   
   
        data_path_ = WORK_SPACE_PATH;
    }
    data_path_ += "/slam_data";

    // 2.推断data_path_是否为文件夹
    if (boost::filesystem::is_directory(data_path_)) {
   
   
        boost::filesystem::remove_all(data_path_);
    }

    // 3.创建文件夹，日志记录地图点云存放地址
    boost::filesystem::create_directory(data_path_);
    if (!boost::filesystem::is_directory(data_path_)) {
   
   
        LOG(WARNING) << "文件夹 " << data_path_ << " 未创建成功!";
        return false;
    } else {
   
   // 日志记录器LOG
        LOG(INFO) << "地图点云存放地址：" << data_path_;
    }
    // 4.创建文件夹，日志记录关键帧点云存放地址
    std::string key_frame_path = data_path_ + "/key_frames";

    boost::filesystem::create_directory(data_path_ + "/key_frames");
    if (!boost::filesystem::is_directory(key_frame_path)) {
   
   
        LOG(WARNING) << "文件夹 " << key_frame_path << " 未创建成功!";
        return false;
    } else {
   
   
        LOG(INFO) << "关键帧点云存放地址：" << key_frame_path << std::endl << std::endl;
    }

    return true;
}

匹配InitRegistration(),滤波InitFilter()

在front_end.cpp中就对应有两个函数InitRegistration和InitFilter，分别匹配和滤波模块的子类选择与参数配置功能。
输入YAML::Node类型的文件路径节点config_node，输出匹配类型的指针registration_ptr以及滤波类型的指针filter_ptr。

bool FrontEnd::InitRegistration(std::shared_ptr<RegistrationInterface>& registration_ptr, const YAML::Node& config_node) {
   
   
    // 点云匹配方式NDT
    std::string registration_method = config_node["registration_method"].as<std::string>();
    LOG(INFO) << "点云匹配方式为：" << registration_method;

    if (registration_method == "NDT") {
   
   
        // 创建指针
        registration_ptr = std::make_shared<NDTRegistration>(config_node[registration_method]);
    } else {
   
   
        LOG(ERROR) << "没找到与 " << registration_method << " 相对应的点云匹配方式!";
        return false;
    }

    return true;
}

bool FrontEnd::InitFilter(std::string filter_user, std::shared_ptr<CloudFilterInterface>& filter_ptr, const YAML::Node& config_node) {
   
   
    std::string filter_mothod = config_node[filter_user + "_filter"].as<std::string>();
    LOG(INFO) << filter_user << "选择的滤波方法为：" << filter_mothod;

    if (filter_mothod == "voxel_filter") {
   
   
        filter_ptr = std::make_shared<VoxelFilter>(config_node