測試資料： 提取碼: n9ys
本篇文章對應的**tag為 4.0
**在後續可能會有調整，如和文章有出入，以實際**為準
前端里程計本質上其實就是乙個點雲匹配方法，通過匹配累計位姿而已，此處選用的是ndt匹配方法。本著先跑通再研究理論的原則，本篇先用它來實現功能，後面會專門介紹它的理論。
為了程式清晰，我們把前端里程計封裝成乙個類，輸入和輸出介面與原有工程保持一致，這樣可以使程式更清晰。我們在include和src資料夾下各建乙個名為front_end的資料夾，來儲存這個類的標頭檔案和原始檔。執行功能的node檔案在根目錄下，名為front_end_node.cpp。
所以本篇文章的重點就是介紹這三個檔案了。
設計乙個類，核心就在於把它的功能切分成各個子功能，每個子功能對應乙個模組去實現。
要設計里程計的類，就要了解它的工作流程，才方便知道怎樣切分更合理。
我們不妨先小小梳理一下：
1）從接收到一幀點雲開始，首先是要匹配，和地圖匹配，如果它是第一幀資料，那麼它就是地圖，供下一幀匹配使用。
2）我們不能把每一幀匹配好的點雲都加入地圖，那樣太大了，所以要提取關鍵幀，即每隔一段距離取一幀點雲，用關鍵幀拼接成地圖即可。
3）到這裡，會想到乙個問題，那就是地圖會一直累加，那麼我們一直用它匹配會導致很多不必要的計算量，所以應該還需要乙個小地圖，即把和當前幀一定距離範圍內的關鍵幀找出來拼接即可，可以叫它滑窗
4）在匹配之前需要濾波，對點雲稀疏化，不然匹配會非常慢。
5) 點雲匹配還有乙個特性，就是它對位姿的**值比較敏感，所以在載體運動時，不能以它上一幀的位姿作為這一幀的**值，可以使用imu**，也可以使用運動模型**。
明白以上幾步流程，後面模組的詳細介紹就很容易理解了，我們開始吧
我們直接使用輪子，用pcl庫來做，後面的文章會介紹怎樣自己實現ndt匹配方法。
首先定義乙個ndt匹配方法的物件，記得初始化它
pcl::normaldistributionstransform::ptr ndt_ptr_ndt_ptr_->setresolution(1.0);
ndt_ptr_->setstepsize(0.1);
ndt_ptr_->settransformationepsilon(0.01);
ndt_ptr_->setmaximumiterations(30);
ndt_ptr_->setinputtarget(local_map_ptr_);
ndt_ptr_->setinputsource(filtered_cloud_ptr);
ndt_ptr_->setinputsource(filtered_cloud_ptr);
ndt_ptr_->align(*result_cloud_ptr_, predict_pose);
current_frame_.pose = ndt_ptr_->getfinaltransformation();
class frame ;if (fabs(last_key_frame_pose(0,3) - current_frame_.pose(0,3)) +
fabs(last_key_frame_pose(1,3) - current_frame_.pose(1,3)) +
fabs(last_key_frame_pose(2,3) - current_frame_.pose(2,3)) > 2.0)
local_map_frames_.push_back(key_frame);
while (local_map_frames_.size() > 20)
由於小地圖濾波和當前幀濾波採用的格仔大小不一樣，所以類內為這兩個功能各定義了乙個濾波器。
濾波格仔大小決定了匹配的效率和精度，格仔越小，點越多，精度越高，但是速度越慢，反之速度加快，精度下降。您可以通過自己調整引數，對比效果，來體會他們之間關係的實際情況。
此處採用運動模型來做位姿**，而沒有用imu，是為了方便在沒有imu時候仍然能夠實現里程計功能。
具體實現方式也比較簡單，假如當前幀是第k幀，那麼用第k-2幀位姿和第k-1幀位姿就可以計算乙個位姿變化量，車輛的運動是相對平緩的，所以在k-1幀位姿基礎上累加這個位姿變化量，基本就是第k幀的**值了。
step_pose = last_pose.inverse() * current_frame_.pose;
predict_pose = current_frame_.pose * step_pose;
last_pose = current_frame_.pose;
此處和我們功能相關的介面有這樣幾個
由於我們要把里程計軌跡和gnss軌跡做對比，所以把初始時刻gnss和imu給出的位姿作為里程計的初始位姿。
這個地圖包括全域性大地圖和用來匹配的小地圖，不過在里程計裡，顯示效果上前者包含後者。
這個不必說了，里程計就是看這個的
編譯完成以後，啟動程式
roslaunch lidar_localization front_end.launchrosbag play kitti_2011_10_03_drive_0027_synced.bag -r 0.5點雲圖就是下面這個樣子
到這一步，只是簡單地實現了雷射里程計的基本功能，它必然還有很多問題，我們會在後續改進它，比如
正常用ndt做匹配是能夠實現在10hz下實時的，不過要在點雲稀疏上做一些特殊處理，而不是像現在這樣用統一的立方格尺寸來稀疏，後續我們會把它提到實時的。
每個關鍵幀都存了點雲，目前所有關鍵幀都放在記憶體裡，必然要爆，即使不爆，會成為影響速度的原因之一。
其實除了小地圖需要的關鍵幀放在記憶體裡，其他的關鍵幀的點雲可以先儲存在硬碟裡，等用到時候再讀出來，這樣就不會有記憶體大量積累情況，基本可以實現「硬碟有多大，地圖就有多大」
對於機械雷達，補畸變是必須的，在當前程式執行過程中，您仔細觀察會發現，在拐彎處匹配有晃動，而且匹配完之後生成的地圖有重影，這就是畸變導致的。
對里程計來講，這是不可避免的了，後續會加其他約束來消除累計誤差。
這樣才方便除錯嘛。
從零開始做自動駕駛定位 十五 第一階段總結
測試資料 提取碼 n9ys 本篇文章對應的 tag為 14.0 在後續可能會有調整，如和文章有出入，以實際 為準 第一階段到這裡終於可以告一段落了！這段時間裡，我們做了這樣幾件事 1 建立了乙個基礎的ros工程，為了使 清晰，我們把topic發布 接收和感測器資料都封裝了起來。2 在此基礎上對資料進...
關於任幹從零開始做自動駕駛的個人總結
for size t i 0 i local map frames size i map filter ptr filter map cloud ptr,map cloud ptr return true 計算相對位姿 中的做法為假設雷達做的是勻速運動，即 位姿為上一時刻的注 ndt和icp配準需乙...
從零開始自動駕駛 gazebo學習入門01
一 開啟gazebo 1 在terminal輸入gazebo開啟乙個空白世界 2 在gazebo中有自帶的世界，可以使用roslaunch命令開啟 roslaunch gazebo ros 回車 會自動顯示自帶的多種世界 下圖為自帶的mud world,其他的世界可以按照提示命令去開啟。二 檢視la...