天天看點
傳回

從零開始做自動駕駛定位(五): 前端裡程計之代碼優化本文純屬轉載，并認真學習一遍，感謝大佬分享！

本文純屬轉載，并認真學習一遍，感謝大佬分享！

本文章配套源代碼位址：https://github.com/Little-Potato-1990/localization_in_auto_driving

測試資料：https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg提取碼: n9ys

本篇文章對應的代碼Tag為 5.0

代碼在後續可能會有調整，如和文章有出入，以實際代碼為準

================================================================================

一、概述

看到本篇文章題目，可能就會有人問，為什麼僅僅在已有的架構上加了個前端就要再次優化，而且專門寫一篇文章來介紹？為什麼不在上一篇就直接使用優化後的代碼，而要放出一個不完善的中間版本？

其實這個問題在本系列文章的開篇中就提前解釋了一下，就是怕引起誤會，這裡再詳細說一下我的想法，可能有些啰嗦，不過我覺得咱們盡早達成這種共識是有必要的。

1. 本系列文章是面向過程，而非面向結果

所謂面向過程，是指我們要還原整個系統的開發過程，包括它所有重要的中間環節，而不隻是給出開發結果。不然，就直接在github上放出一套完整的定位融合系統的源代碼就好了，不必花這麼多精力去寫系列文章了，但那樣就違背了我們的初衷，可能也違背了各位讀者的期望。

2. 代碼優化可不可以提前進行

所謂提前進行，是指我們能不能先設計一個好的結構，然後按照這個結構編寫代碼，而不是像現在這樣分兩步走，先用最簡單的方式讓它跑起來，再做優化。

其實這倒是一個開放性的問題。

在早期的軟體開發行業，大家确實更傾向于前者，不僅要設計好，還要文檔化，代碼編寫按文檔嚴格執行，寫完之後按文檔去驗收。

不過最近很多年，這類做法正在逐漸被抛棄，取而代之，是靈活開發。先寫最簡單的架構，跑起來，再疊加功能和優化。

另外，各位根據自身經曆回顧一下，在自己的體會裡，如果這兩種方式你都嘗試過，哪種效率更高。各位可能很多人都讀過《重構》這本書，整本書其實都是在講一個核心問題：先把腦子裡的簡單想法落實在代碼上，跑通它，然後調整它。現在這類思想越來越受推崇，我覺得可能跟人的特性有關，人在有序和簡單的思維下，行動會更高效，當我們面對的是一個空白的螢幕，而腦子裡是整個軟體的複雜架構的時候，手指頭會懸在鍵盤上方遲遲不知道怎麼敲。

基于不斷重構的做法可能會導緻工作量會有所增加，但是任務進度的推進卻更快了。是以，我個人現在就是這種習慣，我按照我的習慣把開發過程複現出來，如果各位有不一樣的做法，歡迎探讨。

天也不早了，人也不少了，啰嗦這麼多，該幹點正事了。

二、優化事項

1. 功能子產品降耦合

按照我們上一篇文章的介紹，前端按照子產品可以分為比對、濾波、局部地圖滑窗、全局地圖等，其中前兩者是可以獨立成為通用子產品的。比如比對，不僅我們前端裡程計要用，以後閉環修正也要用，地圖建完以後，基于地圖做定位還要用，是以它應該具有共用性。濾波也一樣，上面提到的各個環節它也參與。

明确了要獨立的子產品以後，我們要思考的就是怎樣設計這個子產品。首先就是接口，對于比對，輸入的是點雲，輸出的是位姿。對于濾波，輸入輸出都是點雲。

除了接口以外，我們還需要考慮一件事情，就是如果我們要換不同的比對方式怎麼辦，将來從ndt變成icp的時候，那麼所有調用ndt子產品進行比對的代碼都要改動嗎？這顯然是不劃算的。解決這個問題的辦法就是多态。

多态在程式設計中是一種常用的方法，它的實作方式是先定義一個基類，然後不同的具體實作分别作為它的不同子類存在。在程式運作時執行哪個實作，取決于我們在定義類的對象時用哪個子類做的執行個體化。以我們比對子產品的具體例子來說，我們定義了一個基類RegistrationInterface，它執行比對的函數是ScanMatch()，NDTRegistration和ICPRegistration都是RegistrationInterface的子類，定義registration_ptr作為類對象的指針，那麼registration_ptr->ScanMatch()執行的到底是ndt比對還是icp比對，取決于定義指針時用哪個子類做的執行個體化，具體來講就是下面的指令。如果使用第一行初始化，則執行的是NDT比對，如果是用第二行初始化，則執行的是ICP比對。

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

以上就是多态的實作原理。這樣做的好處是，我們如果想更換比對方式，隻需要改變初始化就可以了。反之，如果不這樣做，那麼就得ndt和icp分别定義對象ndt_registration和icp_registration，更換比對方式時所有調用的地方都要更換變量名字。

同樣的，濾波子產品我們也采用這樣的方式設計，因為用固定尺寸方格濾波的方式有點太粗暴了，後續可能會嘗試更好的濾波方式。

建立一個檔案夾，名為models，存儲比對和濾波這兩個類，以及以後可能新增的通用子產品。

其實類裡面的内容倒是很簡單。

比對類NDTRegistration内部主要函數為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;
}

濾波類VoxelFilter内部主要函數就是Filter，這個函數參數同時包含輸入和輸出。

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

2. 配置檔案

為了友善調試，常用參數寫在配置檔案裡是必須的，本工程采用yaml格式作為配置檔案格式，在程式中，它可以把參數内容對應的放到YAML::Node格式的變量中，前端裡程計的配置參數放在config/front_end檔案夾下，為了把配置檔案内容傳入剛才所提到的比對和濾波兩個子產品，每個子產品均增加一個構造函數，函數參數就是YAML::Node類型，同時基類指針用哪個子類執行個體化也可以由配置參數決定。

這樣，在front_end.cpp中就對應有兩個函數InitRegistration和InitFilter，分别比對和濾波子產品的子類選擇與參數配置功能。

bool FrontEnd::InitRegistration(std::shared_ptr<RegistrationInterface>& registration_ptr, const YAML::Node& config_node) {
    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;
}

3. 關鍵幀點雲和地圖儲存功能

上一篇裡，我們的關鍵幀點雲和全局地圖都是放在記憶體裡，這樣是不利于大場景建圖的，記憶體爆掉都是有可能的。是以我們的做法是沒産生一個關鍵幀就把它存放在硬碟裡，然後把點雲釋放掉，全局地圖預設不生成，必須主動發送指令才會生成，生成之後會把地圖儲存成pcd檔案，并在rviz上顯示，最後再重新把地圖釋放掉，清理記憶體。

生成地圖的指令是用ROS的service實作的，ROS制作service的方法如果不清楚還要麻煩各位在網上查一查。本工程對應的生成地圖的指令是

rosservice call /save_map

地圖預設路徑是在工程目錄下的slam_data檔案夾下，您也可以在剛才提到的配置檔案中自己定義路徑，第一行的data_path變量就是它了。

注意，這樣修改之後，運作程式時隻顯示局部地圖，隻有在主動發送地圖生成指令時才生成并顯示全局地圖，是以資料處理結束輸入一次看一下完整地圖就行。

4. ROS流程封裝

按照上一篇的做法，node檔案的main函數中實作的功能有

  • 讀資料
  • 判斷是否有資料
  • 初始化标定檔案
  • 初始化gnss
  • 使用裡程計子產品計算資料
  • 發送資料

可見，步驟還是有一些的，按照一個合理的變成政策，每個功能應該用一個函數封裝，不然看着也亂。而如果直接在node檔案中做這一步，檔案中代碼并不會減少，還要定義很多全局變量，并和流程無關的代碼攪在一起，這樣顯然是不合理的。

解決這樣的問題，借助類同樣也是一個好的方法，我們把對應的流程封裝在一個類裡，所有通用變量放在頭檔案裡作為類成員變量，各個步驟作為一個函數封裝好，最後隻留一個Run()函數作為接口給node檔案去調用，這樣就變得很簡潔，我們看修改之後的Run函數就知道了。

bool FrontEndFlow::Run() {
    ReadData();

    if (!InitCalibration()) 
        return false;

    if (!InitGNSS())
        return false;
    
    while(HasData()) {
        if (!ValidData())
            continue;
        UpdateGNSSOdometry();
        if (UpdateLaserOdometry())
            PublishData();
    }

    return true;
}

此時node檔案main函數就剩下類對象定義和調用了

_front_end_flow_ptr = std::make_shared<FrontEndFlow>(nh);

ros::Rate rate(100);
while (ros::ok()) {
    ros::spinOnce();

    _front_end_flow_ptr->Run();

    rate.sleep();
}

封裝好的類和FrontEnd類同樣放在front_end檔案夾下。

最後，圖就不貼了，和上一篇是一樣的，本篇隻是修改代碼布局。

上一篇：從零開始做自動駕駛定位(四): 前端裡程計之初試

下一篇：從零開始做自動駕駛定位(六): 傳感器時間同步

雷射SLAM
上一篇: 從零開始做自動駕駛定位(七): 裡程計精度評價文純屬轉載，并認真學習一遍，感謝大佬分享！
下一篇: 從零開始自動駕駛-gazebo學習入門01

繼續閱讀