Karto的后端优化与回环检测功能对比测试与分析
點擊上方“3D視覺工坊”,選擇“星標”
干貨第一時間送達
上篇文章講解了Karto的前端是如何工作的.
這篇文章將slam_karto中的后端優化部分的代碼添加進lesson6中,我將帶著大家一起體驗后端優化的作用與功能.
1 室外數據集
由于上一篇文章使用的數據包的場景太小了,室內只有30m的范圍,來回走一圈前端產生的累計誤差不明顯,所以,我上傳了一個新的數據集.
由于之前沒做好規劃,整個教程弄了好幾個數據集,其實只需要一個室內的,一個室外的就夠了...我自我檢討.
1.1 數據集簡介
新數據集的名字為?lesson6-rslidar-outdoor-gps.bag.
這是在室外環境下,繞著園區的一排花壇走了一圈,使用了速騰的16線雷達,RTK級別的GPS,lpms的6軸的IMU,四輪差速小車作為實驗平臺.
記錄了
imu數據, topic名字為imu
里程計數據, topic名字為odom_scout
GPS的經緯度數據, topic名字為 fix
雙天線得到的航向數據, topic名字為 heading
16線雷達的點云數據, topic名字為rslidar_points
由pointcloud2Laserscan包形成的單線雷達數據, topic名字為front_scan
TF數據: odom->footprint的tf
tf_static數據: footprint->base_link->front_laser_link, imu_link的 tf_static
詳細信息如下所示
$ rosbag info lesson6-rslidar-outdoor-gps.bag path: lesson6-rslidar-outdoor-gps.bagversion: 2.0duration: 3:33s (213s)start: Dec 01 2020 15:44:09.49 (1606808649.49)end: Dec 01 2020 15:47:43.48 (1606808863.48)size: 2.1 GBmessages: 41623compression: none [2064/2064 chunks]types: geometry_msgs/QuaternionStamped [e57f1e547e0e1fd13504588ffc8334e2] nav_msgs/Odometry [cd5e73d190d741a2f92e81eda573aca7] sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2] sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369] sensor_msgs/NavSatFix [2d3a8cd499b9b4a0249fb98fd05cfa48] sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181] tf2_msgs/TFMessage [94810edda583a504dfda3829e70d7eec]topics: /fix 1070 msgs : sensor_msgs/NavSatFix /front_scan 4127 msgs : sensor_msgs/LaserScan /heading 214 msgs : geometry_msgs/QuaternionStamped /imu 21400 msgs : sensor_msgs/Imu /odom_scout 4279 msgs : nav_msgs/Odometry /rslidar_points 4126 msgs : sensor_msgs/PointCloud2 /tf 6406 msgs : tf2_msgs/TFMessage (2 connections) /tf_static 1 msg : tf2_msgs/TFMessage這是一個數據比較全的數據包了,而且行走軌跡存在一個回環.
由于存在16線點云的數據,所以數據集是2G,壓縮成zip后是700M.
1.2 數據集鏈接
數據集的鏈接為
https://pan.baidu.com/s/1UB7mjw3vhXlItsaqdNUw-g
提取密碼為 slam
我就將我所有使用的數據集都放在騰訊文檔里了, 騰訊文檔的地址如下:
https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2
2 只使用Karto的前端進行室外2D建圖
2.1 新增代碼簡述
本篇文章在上一篇文章的添加了后端優化的代碼,相應的文件為:
lesson6/include/lesson6/spa_solver.h
lesson6/src/spa_solver.cpp
同時也對karto_slam.cc進行了更改,添加了根據參數來配置是否啟用后端優化與回環檢測的功能.
更改配置文件中的相關內容,可以實現是否啟用后端優化,與是否啟用回環檢測功能.
新增了跑室外建圖的launch與config文件:
lesson6/launch/karto_slam_outdoor.launch
lesson6/config/mapper_params_outdoor.yaml
2.2 運行代碼
下載數據包之后需要將launch中的bag_filename更改成您實際存放bag的目錄名。
首先,將mapper_params_outdoor.yaml文件的如下兩行設置成false,關閉后端優化與回環檢測功能.
2.3 結果分析
運行結束后將產生如下的地圖,每次運行的效果不同,下圖是運行效果最差的一次的截圖.
這張地圖空白區域的長度大概在100m.寬度大概為27m,分辨率是0.05m.
實際運行時rviz中還會顯示出一條紅色的軌跡,這個軌跡是里程計的數據,里程計的數據存在初值和初始方向,所以里程計的數據與機器人的位姿沒有重合.
將這段圓弧型建筑構建成的地圖放大,可以看到,產生了嚴重的疊圖現象.
3 使用Karto的前端與后端進行室外2D建圖
3.1 后端優化模塊簡介
由于前端的掃描匹配會存在累計誤差, 這個誤差隨著時間的增長會逐漸增大.
而且, 掃描匹配是將當前scan與前一段時間內的scan形成的局部地圖進行匹配, 沒有使用到從開始建圖到當前時刻的所有的scan數據.
所以, 大佬們想了個方法, 將所有的scan數據一起做個優化, 降低其累計誤差. 這就是后端優化.
在圖優化SLAM中,一般將前端和后端分開處理,
前端需要在一定范圍內的地圖中進行搜索與匹配, 找到當前scan的潛在約束關系.
后端優化的目的是 是尋找一個節點間的配置關系使得節點間約束的測量概率最大.?通俗點講, 就是對所有節點, 及其約束 進行優化, 找到最接近實際情況的一組解, 這組解代表著新的節點位姿與新的節點間的約束.
后端優化后的位姿越準, 那么, 前端生成的局部地圖就越準, 進行的掃描匹配的結果也就越準確, 越快速. 同時, 也能夠將前端的累計誤差減小.
因此,后端優化問題的精度和性能,對整個建圖系統具有決定性的影響。
更加具體的介紹可以看下這篇文章
K. Konolige, G. Grisetti, R. K¨ummerle, B. Limketkai, R. Vincent, Efficient Sparse Pose Adjustment for 2D Mapping, In Proc. of Int. Conf. on Intelligent Robots and Systems (IROS), Oct. 2010.
那為什么之前不用后端優化呢?
由于將所有位姿與地圖點放到一起做優化, 形成的矩陣的維度是非常大的, 沒法求雅克比矩陣.
近些年, 計算速度與計算能力的提升緩解了這個問題, 但依然求雅克比矩陣很難.
直到2010年, 有大神發現了所有位姿與地圖點形成的矩陣是非常稀疏的, 是一種箭頭型的結構.
利用矩陣的稀疏性, 可以通過某種方式將這個稀疏的大矩陣重新設置成一個更緊湊, 維度更小的矩陣. 使得了雅克比矩陣可以求出來.
3.2 啟用后端但不啟用回環檢測
首先,將mapper_params_outdoor.yaml文件的如下兩行設置成如下所示,啟用后端優化功能,但不啟用回環檢測功能.
再次通過上邊的roslaunch命令啟動程序,運行結束后將產生如下結果.
上圖的紅色軌跡是里程計的數據,由于存在初值,所以里程計的值與機器人沒有重合.
藍色軌跡是由后端優化 優化之后的節點和邊.
節點就是某一時刻的雷達數據,用紅色小球表示.
邊就是兩個節點之間的約束,也就是兩個節點間的坐標變換,用藍色線條表示,
由于前端的掃描匹配存在累計誤差,通過后端優化可以降低前端的累計誤差.這些誤差并不是被移除了,而是被平均分數到每個節點中去了.
首先,放大一下看看約束的效果.
可以看到,即使是啟用了后端優化功能,最終建完圖之后,這塊圓弧型建筑物的地圖還是疊圖了.
可見,后端優化模塊并沒有將累計誤差完全消除掉.
3.3 啟用后端同時啟用回環檢測
3.3.1 運行結果
再看看啟用后端優化與啟用回環檢測的效果.
將mapper_params_outdoor.yaml文件的如下兩行設置成如下所示,啟用后端優化功能,但不啟用回環檢測功能.
再次通過上邊的roslaunch命令啟動程序,運行結束后將產生如下結果.這次的地圖在圓弧型建筑這沒有產生疊圖現象!!!
非常棒!
放大一下看看約束的效果,圖中,軌跡的起始點與終止點有一條很短的藍色的線,這就是回環約束.
優化之后的圖很整齊.最終的建圖效果是這樣的
那這是為什么呢?
3.3.2 回環檢測
上面說的,后端優化不能完全消除累計誤差,那么有沒有一種方法可以消除累計誤差呢?
答案就是回環檢測.回環檢測可以極大地消除累計誤差,但也不是完全消除掉.
回環檢測的意思就是,機器人行走了一段時間后,回到了一個曾經來到過的地方,這個地方對應的柵格地圖,應該是同樣的.
所以,可以通過某種方法檢測機器人是否回到了曾經來到過的場景.如果檢測到機器人回到了這個場景,那么當前的位姿與之前看到這個場景的位姿應該是接近的,并且可以根據相同的場景求得這兩個位姿間的相對坐標變換.
可以將這個相對坐標變換,作為約束,也就是邊,添加到后端優化中.這個約束就叫做回環約束.
由于這兩個節點看到的場景就是是同一個現實場景,所以這個邊的置信度非常高,權重很大,所以就可以極大地消除累計誤差.
所以,回環檢測的目的就是為了找到這個權重很高的約束,之后將這個約束放入到圖結構中,一起做優化,來達到消除累計誤差的目的.
4 總結與Next
代碼里已經將slam_karto的后端優化模塊放進來了,
本篇文章先對后端優化與回環檢測模塊進行了建圖的對比測試, 從建圖質量的角度分析了添加了后端優化與回環檢測的功能的效果.
同時, 還簡要介紹了后端優化的概念與回環檢測的概念.
下篇文章先簡要分析下slam_karto的回環檢測與后端優化代碼的實現方式.
再下篇文章將使用G2O來實現karto的后端優化的功能.
由于本人水平有限,難免會有些代碼上的錯誤,如果您發現了錯誤或者不好的地方,請聯系我,我會改正.
本文僅做學術分享,如有侵權,請聯系刪文。
下載1
在「3D視覺工坊」公眾號后臺回復:3D視覺,即可下載 3D視覺相關資料干貨,涉及相機標定、三維重建、立體視覺、SLAM、深度學習、點云后處理、多視圖幾何等方向。
下載2
在「3D視覺工坊」公眾號后臺回復:3D視覺github資源匯總,即可下載包括結構光、標定源碼、缺陷檢測源碼、深度估計與深度補全源碼、點云處理相關源碼、立體匹配源碼、單目、雙目3D檢測、基于點云的3D檢測、6D姿態估計源碼匯總等。
下載3
在「3D視覺工坊」公眾號后臺回復:相機標定,即可下載獨家相機標定學習課件與視頻網址;后臺回復:立體匹配,即可下載獨家立體匹配學習課件與視頻網址。
重磅!3DCVer-學術論文寫作投稿?交流群已成立
掃碼添加小助手微信,可申請加入3D視覺工坊-學術論文寫作與投稿?微信交流群,旨在交流頂會、頂刊、SCI、EI等寫作與投稿事宜。
同時也可申請加入我們的細分方向交流群,目前主要有3D視覺、CV&深度學習、SLAM、三維重建、點云后處理、自動駕駛、多傳感器融合、CV入門、三維測量、VR/AR、3D人臉識別、醫療影像、缺陷檢測、行人重識別、目標跟蹤、視覺產品落地、視覺競賽、車牌識別、硬件選型、學術交流、求職交流、ORB-SLAM系列源碼交流、深度估計等微信群。
一定要備注:研究方向+學校/公司+昵稱,例如:”3D視覺?+ 上海交大 + 靜靜“。請按照格式備注,可快速被通過且邀請進群。原創投稿也請聯系。
▲長按加微信群或投稿
▲長按關注公眾號
3D視覺從入門到精通知識星球:針對3D視覺領域的視頻課程(三維重建系列、三維點云系列、結構光系列、手眼標定、相機標定、orb-slam3等視頻課程)、知識點匯總、入門進階學習路線、最新paper分享、疑問解答五個方面進行深耕,更有各類大廠的算法工程人員進行技術指導。與此同時,星球將聯合知名企業發布3D視覺相關算法開發崗位以及項目對接信息,打造成集技術與就業為一體的鐵桿粉絲聚集區,近2000星球成員為創造更好的AI世界共同進步,知識星球入口:
學習3D視覺核心技術,掃描查看介紹,3天內無條件退款
?圈里有高質量教程資料、可答疑解惑、助你高效解決問題
覺得有用,麻煩給個贊和在看~??
總結
以上是生活随笔為你收集整理的Karto的后端优化与回环检测功能对比测试与分析的全部內容,希望文章能夠幫你解決所遇到的問題。
- 上一篇: 图元识别
- 下一篇: python cv2摄像头校准,坐标系转