Lidar-Camera 聯合標定算法

目錄

• 初始參數求解
  • 2D雷達標定
  • 3D雷達標定
• 參數優化
  • 系統可觀性
  • 充分旋轉標定板

核心參考：曠視科技的知乎專欄-實踐之Camera-Lidar標定
需要標定相機內參+相機&LiDAR外參，約束方法分為兩種

方案	內容	備注
3D-3D	利用激光測量的三維激光點+相機測量的標定板三維坐標	本文講解
3D-2D	利用激光測量的三維激光點+相機測量的圖像二維特征（點特征，線段特征）	類似於多目攝像機的PnP算法，PnL

標定過程的通常做法是先利用少量觀測求解外參數的初始值，然后利用多幀數據的約束進行最小二乘優化對初始值進行 refine

初始參數求解

假設標定板平面在相機坐標系 \(c\) 中的參數為 \(\pi ^ { c } = [ n ^ { c } , d ] \in \mathbb{R} ^ { 4 }\) ，其中 \(n^c \in \mathbb{R}^3\) 是平面的三維法向量， \(d\) 是相機坐標系原點到平面的距離。平面上的一個三維點在相機坐標系下的坐標為 \(P^c \in \mathbb{R}^3\) ，點在平面上滿足：

\[n ^ { c ^{T} } P ^ { c } + d = 0 \]

\[P ^ { c } = R _ { cl }P ^ { l } + t _ {cl} \]

假設從激光坐標系 \(l\) 到相機坐標系之間的旋轉和平移為 \(R_{cl}=R_{3\times 3}\) , \(t_{cl}=t_{3\times 1}\) 。如果知道激光坐標系中某個激光點 \(P^l\) 落在標定板上，則通過點在平面上這個約束能夠構建關於外參數的方程：

\[n ^ { c ^{T} } ( R _ { c l } P ^ { l } + t _ { cl } ) + d ^ { c } = 0 \]

上述方程能夠提供一個約束，通過多個這樣的約束就能求解外參數。求解時，一個直觀的想法是利用 g2o 或者 ceres 等優化工具構建非線性最小二乘進行優化求解。但是對於非線性最小二乘問題，需要知道外參數的一個初始值，如果初始值不准確，則有可能會優化到局部最小值。因此，一個合理的求解流程應該是閉式解提供初始值，對該初始值利用多幀數據進行優化，得到更准確的標定結果。
雖然平面約束對於 2D 激光和 3D 激光而言是一樣的，但 2D 激光和 3D 激光閉式求解外參數的方式稍有不同。因為 3D 激光的激光點更多，從而可以直接計算激光點雲的法向量，利用這個法向量簡化外參數計算流程。

2D雷達標定

激光點從LiDAR坐標系到Camera坐標系公式為

\[P ^ { c } = R ^T_ { l c } ( P ^ { l } - t _ { lc } ) \]

考慮到，激光為 2D 激光，激光束形成的平面不妨假設為 xy 平面，即 \(z=0\) ，此時有 \(P^l=[ x , y , 0 ] ^ { T }\)，上式整理為

\[P ^ { c } = R ^T_ { l c } ( \left[ \begin{array} { l } { x } \\ { y } \\ { 0 } \end{array} \right] - t _ { t c } ) = R^T _ { l c } \left[ \begin{array} { l } { 1 } & { 0 } \\ { 0 } & { 1 } & { - t _ { c } } \\ { 0 } & { 0 } \end{array} \right] \left[ \begin{array} { l } { x } \\ { y } \\ { 1 } \end{array} \right]=H \hat{P}^l \]

\(H\) 具有9個參數，2D雷達具有2個約束，因此需要5組點得到基本數據 \(R^T_{lc}\)和\(t_c\)
P.S. \(R_{cl}\)為正交矩陣，因此需要進行正交化（Frobenious Norm）

3D雷達標定

在3D的情況下，輸入不再是點，而是平面，對前方程進行改造可以得到如下公式 (式一不再存在位移t)

\[R _ { c l} n ^ { l } = n ^ { c } \]

\[n ^ { c ^{T} } ( R _ { c l } P ^ { l } + t _ { cl } ) + d ^ { c } = 0 \]

我們發現上式只與\(R _ { c l }\)有關，下式與\(R _ { c l }, t_{cl}\)有關，所以優先解決上式。由上式可得最小二乘模式

\[C=\sum_{i=1}^{N}||n_{i}^{c}-R_{cl}n_{i}^{l}||^{2}=\sum_{i=1}^{N}(n_{i}^{c}-R_{cl}n_{i}^{l})^{T}(n_{i}^{c}-R_{cl}n_{i}^{l})=\sum_{i=1}^{N}(n_{i}^{c^{T}}n_{i}^{c}+n_{i}^{l^{T}}n_{i}^{l}-2n_{i}^{c^{T}}R_{cl}n_{i}^{l}) \]

因為其他的因式都是確定的，為了讓整體最小，就要關注其中的這個因式，希望它能夠取到最大值

\[F=\sum_{i=1}^{N}n_{i}^{c^{T}}R_{cl}n_{i}^{l} \]

因為\(n_{1\times3}·R_{3\times3}·n_{3\times1}\)得到的是\(1\times1\)矩陣，一定等於自身的Trace，而Trace具有回轉平移的性質，即\(\operatorname{Trace}(ABC)=\operatorname{Trace}(BCA)=\operatorname{Trace}(CAB)\)，因此有

\[F=\operatorname{Trace}(\sum_{i=1}^{N}n_{i}^{c^{T}}R_{cl}n_{i}^{l})=\operatorname{Trace}(\sum_{i=1}^{N}R_{cl}n_{i}^{l}n_{i}^{c^{T}})=\operatorname{Trace}(R_{cl}·\sum_{i=1}^{N}n_{i}^{l}n_{i}^{c^{T}})=\operatorname{Trace}(R_{cl}H) \]

對H進行SVD分解，可以得到\(H=UAV^{T}\)，其中\(U\)和\(V\)為\(3\times 3\)正定矩陣，\(V\)為\(3\times 3\)非負對角陣。而由某定理可知，\(R_{cl}\)應為\(R_{cl}=VU^{T}\)。得到\(R_{cl}\)后，帶入下式可以用最小二乘法計算\(t_{cl}\)

\[P^{c}=R_{cl}P^{l}+t_{cl} \]

參數優化

使用LM算法進行優化（我的博客LM算法）
參數化使用\(SO_3\)或者四元數，可以查看博客旋轉矩陣&四元數

\[\hat{R_{cl}},\hat{t_{cl}}=\arg\min_{R_{cl},t_{cl}}\sum_{i=1}^{N}\frac{1}{N_{i}}\sum_{m=1}^{Ni}||n_{i}^{c^{T}}(R_{cl}P_{im}^{l}+t_{cl})+d_{i}^{c}||^{2} \]

利用標定板邊緣直線約束來標定3D激光^[1]；

對2D激光的多種標定板的有效約束的討論^[2]

同時，把激光的點想象成一個采樣方式，2D激光就是確定一條線，對線進行采樣，從這一群點能夠還原得到一條最置信的線，而這個線對平面的約束只有2。同理，3D激光就是一個面，對面進行采樣，從這群點能夠得到一個最置信的平面，而這個面的約束有3。

系統可觀性

參考文獻^[3]的第二章

TODO 沒看懂

充分旋轉標定板

既然采集數據時只平移標定板不行，那是否意味着簡單旋轉一下標定板就可以了呢？比如標定板只繞着 y 軸旋轉。這里可通過設置不同的旋轉矩陣去驗證 H 矩陣是否有零空間。通過簡單修改代碼（注釋或反注釋設置旋轉矩陣的那幾行代碼），運行后會發現：標定時需要充分旋轉 x 和 y 兩個軸，標定過程中標定板只旋轉一個軸一樣會存在零空間基底。

這意味着拿着標定板旋轉時，需要充分旋轉 roll 和 pitch。更直白一點，假設在長方形的標定板中心畫了個十字線，那請繞着十字線的兩個軸充分旋轉，比如繞着豎軸旋轉，然后還要繞着橫軸旋轉。

---

1. Automatic Extrinsic Calibration of a Camera and a 3D LiDAR using Line and Plane Correspondences,arXiv ↩︎

2. Wenbo Dong, A Novel Method for the Extrinsic Calibration of a 2D Laser Rangefinder and a Camera,arXiv ↩︎

3. 8 ↩︎