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