Lidar與IMU標定代碼實戰：lidar_align

作者：小L

本文轉載自「計算機視覺工坊」，該公眾號重點在於介紹深度學習、智能駕駛等領域，一個小眾的公眾號。

0. 前言

對於Lidar+IMU系統，往往需要標定Lidar與IMU的外參[4]，即從Lidar到IMU的6個位姿參數。ETH開源的lidar_align代碼[0]，用於標定Lidar和里程計Odom之間的位姿參數。本文將對代碼進行初步介紹，並總結一些使用中可能會遇到的問題。

1. 代碼整體一覽

1.1 代碼結構

代碼主要包括：頭文件、cpp、以及ROS的launch啟動文件等。其中頭文件包括：

aligner.h：Lidar和Odom對齊（外參計算）時用到的類

loader.h：從ROS的Bag或CSV格式載入數據的相關函數

sensors.h：主要包括：里程計Odom，以及雷達Lidar相關接口

transform.h：一些SO3變化的計算以及轉換，在插值、優化時使用

1.2 方法基本思想

方法本質解決的是一個優化問題，即在外參參數(6DoF)如何選擇時，使Lidar采集到的數據轉化到Odom系下后，前后兩次scan的數據點能夠盡可能的重合。

詳細一點兒來說，方法將Lidar數據根據當前假設的狀態變量（6DoF參數）變換到Odom系下，構成點雲PointCloud，之后對每一次scan時的數據，在下一次scan中通過kdtree的方式尋找最近鄰的數據點並計算距離，當總距離最小時可以認為完全匹配，即計算的外參參數正確。

1.3 主要流程

代碼主要有兩部分：載入數據與優化計算。載入數據部分從Bag數據載入雷達的數據(sensor_msgs/PointCloud2)，並從CSV或Bag數據中載入Odom獲得的6DoF位置信息。具體的位置信息如何獲得將在后面進行介紹。優化計算部分將在第2.2小節詳細展開。

2. 詳細介紹

2.1 主要數據類型

Odom數據：主要包括兩個數據：時間戳timestamp_us_與從當前時刻到初始時刻的變換T_o0_ot_。

Lidar數據：主要包括兩個參數：從Lidar到Odom的外參T_o_l_與每次掃描的數據scans_，而每次的掃描scan數據類型主要包括：掃描起始時間timestamp_us_，本次掃描的點雲raw_points_，某個點在Odom的變換（用於去畸變）T_o0_ot_，以及相關配置參數等。

Aligner數據：Aligner首先包含了需要優化的數據OptData（其中包括Lidar、Odom等數據），以及相應的配置參數（數據載入路徑、初值、優化參數、KNN相關參數等），以及優化計算的相關參數。

2.2 優化過程詳細介紹

在載入了Odom和Lidar數據之后，進行優化求解6個位姿參數。主要求解函數為：lidarOdomTransform

Aligner::lidarOdomTransform()

首先進行相關的優化配置。默認優化參數是6個，但可以考慮兩個傳感器傳輸造成的時間差，如果考慮這個因素，參數數量將變為7。

優化時，采用NLOPT優化庫[3]，默認首先全局優化這三個參數。如果提供的初值與真值相差較大，或完全沒有設置初值（默認為全0），則需要進行全局優化獲得旋轉參數。在局部優化這6個參數，局部優化開始時的初值就是3個為0的平移參數，以及全局優化計算出來的旋轉參數。全局優化、局部優化，都是調用的optimize函數。

Aligner::optimize()

在這個函數設置了NLOPT優化的相關參數，包括：是全局優化還是局部優化、優化問題的上下界、最大迭代次數、求解精度以及目標函數等。最重要的是目標函數LidarOdomMinimizer

LidarOdomMinimizer()

這個函數在優化中會不斷調用，迭代計算。首先會通過上一時刻的狀態，計算新的從Lidar到Odom的變換（這里用到了Transform.h中定義的一些變換），誤差是由lidarOdomKNNError函數獲得。

lidarOdomKNNError()

這個是一個重載函數，具有兩種重載類型。首先調用的是lidarOdomKNNError(const Lidar)，處理的是Lidar的數據，首先根據估計的Lidar到Odom的變化，對完整的scans_數據計算出每次scan時每個點在Odom下的坐標（getTimeAlignedPointcloud函數，相當於點雲去畸變），得到一個結合的點雲(CombinedPointcloud)，之后從這個點雲中尋找每個點的最近鄰，在利用另一個重載類型的lidarOdomKNNError(const Pointcloud, const Pointcloud)函數進行計算最近鄰誤差。

計算最近鄰誤差時，構建了一個KD-Tree，並行計算kNNError函數，利用pcl庫的nearestKSearch函數搜索一定范圍（全局優化時是1m，局部優化時是0.1m）的最近鄰，計算最近2個點的誤差。

小結

優化的目標函數是每次scan的每個點在完整點雲中的最近鄰的距離，首先通過粗的全局優化估計一部分參數，再局部優化求解精細的6DoF參數。

3. 配置與運行

3.1 安裝

首先在安裝時需要安裝NLOPT：sudo apt-get install libnlopt-dev。之后把代碼拷貝到ros的工作空間，使用 catkin_make進行編譯。

3.2 編譯可能遇到的問題

這個代碼是個人編寫使用，沒有在大多數的ubuntu和ros版本進行測試，所以可能會遇到各種各樣的問題。以Ubuntu18與ROS-melodic為例，首先會遇到一個定義沖突的報錯：

1. 定義沖突問題

error: conflicting declaration ‘typedef struct LZ4_stream_t LZ4_stream_t’ typedef struct { long long table[LZ4_STREAMSIZE_U64]; } LZ4_stream_t;

這個原因是ROS版本下有兩個頭文件定義發生沖突，github的issue中給出了兩種解決辦法，之一是重命名頭文件避免沖突：

sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak

sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4.h.bak

sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h

sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h

（詳見： https://github.com/ethz-asl/lidar_align/issues/16 ）

2. 找不到"FindNLOPT.cmake"

By not providing "FindNLOPT.cmake" in CMAKE_MODULE_PATH this project has asked CMake to find a package configuration file provided by "NLOPT", but CMake did not find one. Solutions: Move "NLOPTConfig.cmake" file to src directory.

解決方法：將"\lidar_align\FindNLOPT.cmake"文件移動到工作路徑下中的"\src"文件夾下，再次編譯即可。

3.3 測試運行

在github的issue中，由於存在准備數據（尤其是Odom數據）有錯誤的問題，造成運行失敗。作者上傳了測試數據 https://drive.google.com/open?id=11fUwbVnvej4NZ_0Mntk7XJ2YrZ5Dk3Ub 可以運行測試。

3.4 數據准備

Lidar的數據直接是ros中的sensor_msgs/PointCloud2即可，但位置數據需要提供CSV格式或者ROS下的geometry_msgs/TransformStamped消息類型。后者如何獲得？如果是IMU直接進行積分就好，但這樣積分勢必會不准確，作者也在issue中提到沒有考慮noise的問題( https://github.com/ethz-asl/lidar_align/issues/5#issuecomment-432232087 )，所以目前看來對IMU進行積分，湊合使用就好。

[1]給出了一種IMU計算Odom的實現。

3.5 數據采集要求

作者在issue和readme中指出，該方法存在的局限性是，必須要求采集數據時系統進行非平面運動，對平移要求不高但要求旋轉必須充分。但對數據量、運動范圍沒有經過嚴格的測試。這個局限性也限制了不能用於給無人車這種系統標定。

參考資料

[0]. 原版代碼github： https://github.com/ethz-asl/lidar_align

[1]. IMU數據計算Odom的實現： https://www.cnblogs.com/gangyin/p/13366683.html

[2]. lidarr_align原理簡要介紹： https://blog.csdn.net/miracle629/article/details/87854450

[3]. NLOPT優化庫介紹： https://nlopt.readthedocs.io/en/latest

[4]. Lidar和IMU標定需要標什么？ https://blog.csdn.net/tfb760/article/details/108532974

本文僅做學術分享，如有侵權，請聯系刪文。

下載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人臉識別、醫療影像、缺陷檢測、行人重識別、目標跟蹤、視覺產品落地、視覺競賽、車牌識別、硬件選型、學術交流、求職交流等微信群。

一定要備注：研究方向+學校/公司+昵稱，例如：”3D視覺 + 上海交大 + 靜靜“。請按照格式備注，可快速被通過且邀請進群。原創投稿也請聯系。

▲長按加微信群或投稿

▲長按關注公眾號

3D視覺從入門到精通知識星球：針對3D視覺領域的知識點匯總、入門進階學習路線、最新paper分享、疑問解答四個方面進行深耕，更有各類大廠的算法工程人員進行技術指導。與此同時，星球將聯合知名企業發布3D視覺相關算法開發崗位以及項目對接信息，打造成集技術與就業為一體的鐵桿粉絲聚集區，近2000星球成員為創造更好的AI世界共同進步，知識星球入口：

學習3D視覺核心技術，掃描查看介紹，3天內無條件退款