從零開始做自動駕駛定位(十一): 閉環修正

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

注釋：文中藍色文本是自己加上去的

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

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

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

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

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

一、概述

閉環校正子產品是建圖功能的最後一個子產品了，建圖這部分離完工不遠了。

閉環子產品和其他子產品的通信機制我們之前已經設計好，它接收關鍵幀位姿及對應的GNSS位姿，根據政策做檢測，如果檢測到滿足要求的閉環比對，那麼就把這一對限制關系發送出去，讓後端優化子產品把這個限制關系作為一個邊加入到機率圖中就行了

--- 了解這段話的意思，腦中産生一個場景

是以這部分的核心就在于怎麼檢測了，基本思路就是在曆史幀中找離目前關鍵幀距離最近的關鍵幀，并比對對應的點雲，如果比對誤差足夠小，那麼檢測就成功了。

但注意，這裡隻是基本思路，為了提高檢測效率和準确度，需要加一些政策。而這些政策就構成了本子產品的主要内容。

二、設計思路和代碼實作

按上面所說，閉環就是檢測曆史幀，然後比對。是以我們自然就把它分成 檢測和比對 兩大塊來分别分析。

1. 閉環檢測

當我們根據目前幀位置從曆史幀中查找和它距離最近的關鍵幀時，會面臨哪些問題？不妨羅列一下：

1）如果曆史幀産生時間和目前幀時間差太小，那麼說明這是一個小閉環，意義不大

2）每産生新的關鍵幀都對曆史幀做一次周遊，太消耗時間

3）如果明知道某一段距離内不會有符合條件的曆史幀，那麼檢測就不必要

為了解決這些問題，自然就有對應的方法

1）設定最小時間差

這裡的時間差，嚴格來講是關鍵幀路程差，關鍵幀的編号是随着車的前進順序遞增産生的，我們設定一個參數diff_num，假如目前幀編号為index，那麼尋找比對的幀應該從編号index-diff_num之前的幀中去查找。diff_num這個值可以通過配置檔案設定。

2）為避免頻繁檢測，每檢測一次，就做一次等待

這個等待的幀數，也可以通過配置檔案設定，對應的參數為loop_step，即每隔loop_step個關鍵幀檢測一次。

3）根據目前最小距離計算重新計算等待時間

假如本次檢測，目前幀和滿足條件1）的曆史幀中的最短距離是100米，而我們需要的是兩米範圍内的關鍵幀，那麼我們有理由認為在接下來的至少98米路程内，是不會有滿足條件的曆史幀的，那麼這段距離内就沒必要檢測了。

通過以上三步，我們基本可以把檢測頻率控制在一個合适的範圍内了。這三步對應的代碼在函數  DetectNearestKeyFrame 中，此處不貼代碼了。

2. 點雲比對

我們明确知道一點，在點雲比對時，scan-to-map 的比對是比scan-to-scan的比對要更精确的，是以我們不應該把目前幀點雲和最近距離曆史幀點雲比對，而應該以該曆史幀為中心，按時間往前和往後各索引幾個關鍵幀，拼接成一個小地圖。這個索引的關鍵幀個數，在配置檔案中對應參數為extend_frame_num。---  工程中會有很多的小技巧，也是作者開篇說的意思

另外一點，在前端時，我們為了提高比對效率，把前端比對用的點雲做了很多的稀疏，而對于閉環比對來講，因為頻率不高，是以我們可以适當降低稀疏度，讓點雲稠密一些，這樣可以得到更精确的閉環限制。點雲濾波提供了兩種模式，一種是和前端同樣的濾波方法voxel_filter，隻不過參數調小了一些，當然也可以在配置檔案裡自己修改參數。另一種是 no_filter，也就是不濾波的意思，就是直接用原始點雲做比對，我們預設使用的是前者，如果有興趣，各位可以把其他選項都試一試。

點雲比對結束，會傳回一個 比對誤差參數 fitness_score，這個參數越小，代表比對精度越高，我們設定了一個上限fitness_score_limit，隻有小于這個值才建立閉環限制并發送給後端。

三、效果驗證

在正式使用時，應該是gnss位置和閉環限制均添加到後端優化中，也就是這兩種限制同時使用，我們提供的程式也是預設使用這種模式。

為了單純評價閉環的效果，各位可以把 back_end.yaml配置檔案中的use_gnss先設定為false，即不使用gnss，這樣和開環裡程計結果一對比，就可以單獨評價閉環的效果了。

具體的操作方法是先啟動程式和播放bag，然後在資料運作完之後輸入指令

rosservice call /optimize_map

這時在slam_data/trajectory檔案夾中會生成一個optimized.txt檔案，這裡存放的是優化後的位姿，該檔案夾中還有ground_truth.txt檔案，存放對應的gnss位姿，運作如下指令便可以生成對比結果了。

evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz

我把我自己測試的結果貼在這裡

可見閉環限制還是消除了很大一部分誤差。

另外，在輸入上面的優化指令之後，rviz上會顯示全局點雲地圖，從這張地圖中可以看到，開環運作時不閉合的地圖，在這裡閉合了，這也是閉環的重要作用之一。

四、一點思考

其實閉環的原理和實作都不是很難，實作一個基本思路再加一些政策就可以。

這裡想讨論的是閉環的作用，以及某些情況下可能發生的要不要加閉環的争論。

想明白要不要，首先得了解它能幹什麼

是以我們要明白閉環所具備的兩個特性：

1）它作為限制，能消除部分累計誤差，但不是全部

2）它能夠消除點雲地圖重影

我們就結合這兩個特點來回答要不要加閉環的問題。

這個問題要分場景來答：

1. 沒有外界先驗資訊限制的場景

所謂先驗資訊就是此處的gnss位姿等可以用來做觀測的位姿資訊。

沒有限制資訊的往往是室内場景。

對于這個場景，應該不會有太多的争議，它把開環裡程計所建的不閉合的地圖閉合了，同時消除了部分誤差，是以必然是要加的。

這裡就同時用到了上面個提到的閉環的兩個特性。

當然，在大場景有沒有先驗資訊的情況下，殘餘誤差仍然不可忽視，如果想進一步提高地圖精度，往往會加一些landmark之類的限制。

2. 有先驗資訊參考限制的場景

這種情況可能就會産生一些争議了，比如經常會被質疑，為什麼有了gnss還要加閉環？

這個問題的核心就在于gnss做限制還殘留哪些問題，以及閉環能不能解決這些問題。

gnss的位置在差分模式下(RTK)，精度确實很高，如果隻取固定解，誤差能保證在10cm以内，但是我們如果經常用隻加gnss、不加閉環方案建圖時，會發現在采集車輛重複經過的區域裡，某些部分是有重影的。

具體原因也可以合了解釋。RTK本身是存在誤差的，而且RTK坐标和雷達坐标之間要補償相對位姿，是以标定誤差也會以觀測誤差的形式存在。而圖優化本身是一個平差方法，觀測誤差必然會在優化結果上有所展現。--- 想一下

雖然這個誤差不大，做的好的話，關鍵幀位置誤差往往能控制在10cm以内，差一點也基本能保證在15cm左右。對于一般的自動駕駛任務，這并沒有超出名額。但是關鍵幀誤差會在點雲地圖上帶來等量大小的點雲重影，因為點雲地圖是按照關鍵幀位姿把每一幀點雲拼接成的。而且這還隻是考慮了位置誤差影響，姿态誤差同樣也會帶來重影。

用地圖定位時，其實就是把目前幀點雲和地圖比對，而帶有重影的地圖，對定位品質的影響是比較大的，是以需要消除重影。

由于重影是目前幀和曆史幀位姿誤差造成的，而閉環限制就是目前幀點雲和曆史幀點雲之間重新比對得到的限制關系，或者可以說它就是直接以重影大小作為觀測誤差的限制，是以它對重影的消除要有效的多 --- 了解

這時候，關鍵幀位姿精度可能沒有提高，比如目前幀和曆史幀可能都有10cm的誤差，但是它們二者之間的一緻性提高了，導緻點雲重合度提高了，這個才是關鍵。

總結一下，對于在有gnss先驗資訊的情況下還要加閉環的思路，對它的質疑往往給出的理由是，隻用gnss限制時定位精度已經很高了，就不再需要閉環去限制了，而我們需要注意到，閉環在這裡根本就不是用來提高定位精度的，而是用來消除點雲重影的。

是以這裡主要起作用的是閉環的第二個特性，而不是第一個，提出這樣的質疑往往也是由于對這第二個特性認識不夠。

上一篇：從零開始做自動駕駛定位(十): 後端優化

下一篇：從零開始做自動駕駛定位(十二): 前端裡程計擴充