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

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

front_end

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

函数名称	含义
InitWithConfig()	参数初始化主函数
Update()	位姿里程计更新
SetInitPose()	设置初始化位姿
SaveMap()	保存地图
GetNewLocalMap()	局部地图
GetNewGlobalMap()	全局地图
GetCurrentScan()	当前扫描点云
InitParam()	初始化参数
InitDataPath()	初始化数据路径
InitRegistration()	匹配的参数配置
InitFilter()	滤波的参数配置
UpdateWithNewFrame()	更新新的关键帧

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

1、配置文件

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

函数名称	含义
InitWithConfig()	参数初始化主函数
InitDataPath()	初始化数据路径
InitRegistration()	匹配的参数配置
InitFilter()	滤波的参数配置
InitParam()	初始化参数

默认构造函数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[filter_mothod][filter_user]);
    } else {
        LOG(ERROR) << "没有为 " << filter_user << " 找到与 " << filter_mothod << " 相对应的滤波方法!";
        return false;
    }

    return true;
}

InitParam()

// 初始化参数：局部地图
bool FrontEnd::InitParam(const YAML::Node& config_node) {
    key_frame_distance_ = config_node["key_frame_distance"].as<float>();
    local_frame_num_ = config_node["local_frame_num"].as<int>();

    return true;
}

2、匹配、滤波模块

由于匹配和滤波模块会在前端里程计、闭环修正、基于地图定位等环节都要用到，所以干脆对这两部分进行独立建模块。首先是接口，匹配，输入点云输出位姿。滤波，输入输出都是点云。

voxel_filter.h
主要包含两个构造函数，最主要的类成员函数Filter()，另外的类成员函数设置滤波参数SetFilterParam()，最主要的类成员变量 voxel_filter_。

class VoxelFilter: public CloudFilterInterface {
  public:
    // 构造函数
    VoxelFilter(const YAML::Node& node);
    VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z);
    // 后面的override表明当前派生类函数重写了基类的虚函数，防止代码中出现意外的继承行为
    bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override;

  private:
    // 设置滤波参数 
    bool SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z);

  private:
    pcl::VoxelGrid<CloudData::POINT> voxel_filter_;// 滤波器对象
};

voxel_filter.cpp
核心的滤波函数定义为：

bool VoxelFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) {
    voxel_filter_.setInputCloud(input_cloud_ptr);// 输入点云
    voxel_filter_.filter(*filtered_cloud_ptr);// 滤波
    return true;
}

两个构造函数初始化形式为：

// 构造函数初始化
VoxelFilter::VoxelFilter(const YAML::Node& node) {
    float leaf_size_x = node["leaf_size"][0].as<float>();
    float leaf_size_y = node["leaf_size"][1].as<float>();
    float leaf_size_z = node["leaf_size"][2].as<float>();
    
    SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z);
}
// 设置滤波器参数
bool VoxelFilter::SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z) {
    voxel_filter_.setLeafSize( leaf_size_x, leaf_size_y, leaf_size_z);

    LOG(INFO) << "Voxel Filter 的参数为：" << std::endl
              << leaf_size_x << ", "
              << leaf_size_y << ", "
              << leaf_size_z 
              << std::endl << std::endl;

    return true;
}
VoxelFilter::VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z) {
    SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z);
}

ndt_registration.h
与上面类似，主要包含两个构造函数，最主要的类成员函数SetInputTarget()和ScanMatch()，另外的类成员函数设置滤波参数SetRegistrationParam()，最主要的类成员变量 ndt_ptr_。

class NDTRegistration: public RegistrationInterface {
  public:
    NDTRegistration(const YAML::Node& node);
    NDTRegistration(float res, float step_size, float trans_eps, int max_iter);
    // 输入目标点云
    bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override;
    // 匹配
    bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                   const Eigen::Matrix4f& predict_pose, 
                   CloudData::CLOUD_PTR& result_cloud_ptr,
                   Eigen::Matrix4f& result_pose) override;
  
  private:
    bool SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter);

  private: 
    pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>::Ptr ndt_ptr_;
};
}

ndt_registration.cpp
主要对主要的类成员函数：输入目标点云SetInputTarget()和匹配ScanMatch()两个函数。

// 输入目标点云
bool NDTRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
    ndt_ptr_->setInputTarget(input_target);
    return true;
}
// 匹配
bool NDTRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                                const Eigen::Matrix4f& predict_pose, 
                                CloudData::CLOUD_PTR& result_cloud_ptr,
                                Eigen::Matrix4f& result_pose) {
    ndt_ptr_->setInputSource(input_source);
    ndt_ptr_->align(*result_cloud_ptr, predict_pose);
    result_pose = ndt_ptr_->getFinalTransformation();
    return true;
}

考虑到以后变换匹配、滤波方式，需要对ndt模块的所有代码都更改是不划算的，采用多态思想，生成两个基类：基滤波CloudFilterInterface类和基匹配RegistrationInterface类，滤波中主要有滤波虚函数Filter()，匹配中主要有输入目标虚函数SetInputTarget()和匹配虚函数ScanMatch()。最后，两个基类中都要加上析构虚函数。基类，只有函数声明。
CloudFilterInterface.h

class RegistrationInterface {
  public:
    // 析构虚函数
    virtual ~RegistrationInterface() = default;
    // 输入目标虚函数
    virtual bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) = 0;
    // 匹配虚函数
    virtual bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                          const Eigen::Matrix4f& predict_pose, 
                          CloudData::CLOUD_PTR& result_cloud_ptr,
                          Eigen::Matrix4f& result_pose) = 0;
};
} 

RegistrationInterface.h

class CloudFilterInterface {
  public:
    // 析构虚函数
    virtual ~CloudFilterInterface() = default;
    // 滤波虚函数
    virtual bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) = 0;
};

定义好基类后，不同的具体实现分别作为它的不同子类存在，取决于在定义类的对象时用哪个子类作为实例化。如果使用第一行初始化，则执行的是NDT匹配，如果是用第二行初始化，则执行的是ICP匹配。我们如果想更换匹配方式，只需要改变初始化就可以了。

// 使用ndt匹配
std::shared_ptr<RegistrationInterface> registration_ptr = std::make_shared<NDTRegistration>();
// 使用icp匹配
std::shared_ptr<RegistrationInterface> registration_ptr = std::make_shared<ICPRegistration>();

3、关键帧点云和地图保存

每生成一个关键帧保存在硬盘里，然后把点云稀释掉，全局地图默认不生成，需要发送指令才会生成，生成之后会把地图保存成pcd文件，并在rviz上显示，最后把地图稀释掉，清理内存。
这样修改之后，运行程序时只显示局部地图，只有在主动发送地图生成命令时才生成并显示全局地图，所以数据处理结束输入一次看一下完整地图就行。
保存全局地图：

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";
        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;
    }
    // 保存为pcd格式
    std::string map_file_path = data_path_ + "/map.pcd";
    pcl::io::savePCDFileBinary(map_file_path, *global_map_ptr_);
    has_new_global_map_ = true;

    return true;
}

UpdateWithNewFrame()

这个函数进行了修改，之前是对所有关键帧放在全局地图中，这里要首先将关键帧存储到硬盘中，之后关键帧只是存储在容器中。

// 新增1.把关键帧点云存储到硬盘里，节省内存
    std::string file_path = data_path_ + "/key_frames/key_frame_" + std::to_string(global_map_frames_.size()) + ".pcd";
    pcl::io::savePCDFileBinary(file_path, *new_key_frame.cloud_data.cloud_ptr);
   创建Frame对象，保存关键帧点云
   更新局部地图
   更新ndt匹配的目标点云
   // 更新2、保存所有关键帧信息在容器里
    // 存储之前，点云要先释放，因为已经存到了硬盘里，不释放也达不到节省内存的目的
    key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD());
    global_map_frames_.push_back(key_frame);
    
    return true;

4、封装为run()

node文件的main函数中实现的功能有

读数据
判断是否有数据
初始化标定文件
初始化gnss
使用里程计模块计算数据
发送数据

把对应的流程封装在一个类里，所有通用变量放在头文件里作为类成员变量，各个步骤作为一个函数封装好，最后只留一个Run()函数作为接口给node文件去调用，这样就变得很简洁

front_end_node.cpp

这里直接调用_front_end_flow_ptr->Run()即可。

std::shared_ptr<FrontEndFlow> _front_end_flow_ptr;

// 回调函数
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;
}

int main(int argc, char *argv[]) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
    FLAGS_alsologtostderr = 1;
    // R11.OS初始化
    ros::init(argc, argv, "front_end_node");
    ros::NodeHandle nh;
    // 2.创建服务service名为save_map，调用回调函数
    ros::ServiceServer service = nh.advertiseService("save_map", save_map_callback);
    // 3.前端指针_front_end_flow_ptr ，FrontEndFlow类
    _front_end_flow_ptr = std::make_shared<FrontEndFlow>(nh);
    // 4.调用run函数即可
    ros::Rate rate(100);
    while (ros::ok()) {
        ros::spinOnce();
        _front_end_flow_ptr->Run();
        rate.sleep();
    }
    return 0;
}

front_end_flow.cpp

函数run()

bool FrontEndFlow::Run() {
    // 1.读取数据
    ReadData();
    // 2.判断是否有数据，初始化标定文件
    if (!InitCalibration()) 
        return false;
    // 3.gnss初始化位姿
    if (!InitGNSS())
        return false;
    // 4.使用里程计模块计算数据
    while(HasData()) {
        if (!ValidData())
            continue;
        // 5.利用GNSS数据更新里程计
        UpdateGNSSOdometry();
        // 6.发布数据
        if (UpdateLaserOdometry())
            PublishData();
    }
    return true;
}

构造函数初始化

FrontEndFlow::FrontEndFlow(ros::NodeHandle& nh) {
    // 三个订阅
    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");
    // 5个发布
    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());
}

1.ReadData()
读取传感器数据

// 读取传感器数据
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;
}

2.InitCalibration()
初始化标定

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;
}

3.InitGNSS()
GNSS初始化位姿

bool FrontEndFlow::InitGNSS() {
    static bool gnss_inited = false;
    if (!gnss_inited && gnss_data_buff_.size() > 0) {
        // 取得gnss数据并获得原始位姿
        GNSSData gnss_data = gnss_data_buff_.front();
        gnss_data.InitOriginPosition();
        gnss_inited = true;
    }

    return gnss_inited;
}

4.HasData()
三个传感器队列buff是否有数据

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;
}

5.ValidData()
有效数据，保证三个传感器时间同步

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;
}

6.UpdateGNSSOdometry()
利用GNSS和IMU初始化位姿里程计

bool FrontEndFlow::UpdateGNSSOdometry() {
    gnss_odometry_ = Eigen::Matrix4f::Identity();

    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_;

    return true;
}

7.UpdateLaserOdometry()
更新雷达点云里程计

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_);
        laser_odometry_ = gnss_odometry_;
        return true;
    }
    // 重要！！！后续帧Update
    laser_odometry_ = Eigen::Matrix4f::Identity();
    if (front_end_ptr_->Update(current_cloud_data_, laser_odometry_))
        return true;
    else 
        return false;
}

8.PublishData()
发布数据消息

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;
}