系統(tǒng)全面的相機(jī)標(biāo)定課程：單目/魚眼/雙目/陣列 相機(jī)標(biāo)定：原理與實(shí)戰(zhàn)

摘要

這是今年的一篇針對(duì)高分辨率的固態(tài)激光雷達(dá)（非重復(fù)性掃描型）或者多線的激光雷達(dá)和相機(jī)在無(wú)標(biāo)定板的環(huán)境中自動(dòng)化外參標(biāo)定的一篇文章。本文的方法不需要基于巧克力板，只依賴兩個(gè)傳感器采集的環(huán)境中的線特征就可以得到像素級(jí)精度的標(biāo)定結(jié)果。在理論層面，作者分析了邊緣特征提供的約束和邊緣特征在場(chǎng)景中的分布對(duì)標(biāo)定精度的影響。同時(shí)，作者分析了激光雷達(dá)的測(cè)量原理，并提出了一種基于點(diǎn)云體素分割和平面擬合的高精度的激光雷達(dá)點(diǎn)云邊緣特征提取的方法。由于邊緣特征在自然場(chǎng)景中很豐富，所以作者在室內(nèi)和室外多個(gè)數(shù)據(jù)集上進(jìn)行了實(shí)驗(yàn)并取得了不錯(cuò)的效果。

代碼已經(jīng)開(kāi)源：https://github.com/hku-mars/livox_camera_calib

介紹

雷達(dá)和相機(jī)是自動(dòng)駕駛車輛中常用的傳感器。雷達(dá)可以直接測(cè)量環(huán)境的三維信息，所以經(jīng)常被用在障礙物檢測(cè)、跟蹤和建圖。相機(jī)可以提供豐富的顏色信息，而且能和雷達(dá)的數(shù)據(jù)互補(bǔ)。隨著雷達(dá)的分辨率快速增長(zhǎng)，對(duì)于精準(zhǔn)的外參的需求也增加了?；诩す夂鸵曈X(jué)可以進(jìn)行構(gòu)建稠密的點(diǎn)云圖，點(diǎn)云著色和精準(zhǔn)的3D測(cè)量。

當(dāng)前的標(biāo)定方法還是主要基于標(biāo)定板或者特殊的圖像標(biāo)志，通過(guò)檢測(cè)、提取和匹配在圖像和點(diǎn)云中的特征然后基于最小二乘的方法計(jì)算。傳統(tǒng)的激光雷達(dá)如velodybe這種采用重復(fù)掃描和機(jī)械結(jié)構(gòu)造成點(diǎn)云稀疏且噪聲很大，這樣可能導(dǎo)致代價(jià)函數(shù)產(chǎn)生不穩(wěn)定的結(jié)果。固態(tài)激光雷達(dá)例如livox可以很好的利用非重復(fù)性掃描的特性產(chǎn)生稠密的點(diǎn)云。但由于標(biāo)定的target常常放在離標(biāo)定的傳感器不遠(yuǎn)的地方，這就造成了在場(chǎng)景深度比較大的場(chǎng)景中，外參的標(biāo)定結(jié)果可能會(huì)很差。例如在大場(chǎng)景中的點(diǎn)云著色。此外，基于特定標(biāo)定目標(biāo)的方法也是相對(duì)來(lái)說(shuō)比較麻煩的，我們其實(shí)還是喜歡基于自然場(chǎng)景中的信息進(jìn)行標(biāo)定。

為了解決上述的問(wèn)題，本文提出了一個(gè)不基于target的像素級(jí)的自動(dòng)化標(biāo)定的方法。系統(tǒng)通過(guò)檢測(cè)圖像和點(diǎn)云中的線特征，最小化重投影誤差得到得到準(zhǔn)確的外參。系統(tǒng)可以應(yīng)用在室內(nèi)或者室外的場(chǎng)景中，并且這種不對(duì)場(chǎng)景提出需求的方法可以允許我們?cè)跀?shù)據(jù)采集的任何時(shí)刻進(jìn)行標(biāo)定。具體來(lái)說(shuō)，本文的貢獻(xiàn)主要有：

作者自信的研究了lidar的測(cè)量原理，研究結(jié)果表明常用的基于深度不連續(xù)的邊緣特征提取的方法不準(zhǔn)確也不可靠。作者提出了新穎可靠的深度連續(xù)邊緣的特征提取算法。

作者在室內(nèi)和室外對(duì)本文的方法進(jìn)行了評(píng)測(cè)，結(jié)果標(biāo)定的結(jié)果都很準(zhǔn)確，并在自然場(chǎng)景中實(shí)現(xiàn)了像素級(jí)的標(biāo)定結(jié)果。除此之外，本文的方法同時(shí)適用與固態(tài)和傳統(tǒng)的機(jī)械雷達(dá)。

作者開(kāi)源了標(biāo)定的代碼。

相關(guān)工作

外參標(biāo)定的方法主要分為兩種，基于目標(biāo)和自然場(chǎng)景標(biāo)定的方法。他們之間最大的區(qū)別是如何在傳感器數(shù)據(jù)中定義和檢測(cè)特征。一些幾何的物體和棋盤格是最常用的標(biāo)定目標(biāo)，因?yàn)樗麄儗?duì)平面的法向量進(jìn)行了明確的約束，而且使問(wèn)題公式化起來(lái)很簡(jiǎn)單。但是由于需要額外的準(zhǔn)備造成這種方法不實(shí)用，尤其是在需要?jiǎng)討B(tài)變換的環(huán)境中?；跇?biāo)定目標(biāo)的方法主要有：

Unified intrinsic and extrinsic camera and lidar calibration under uncertainties（2020, ICRA）

Analytic plane covariances construction for precise planarity-based extrinsic calibration of camera and lidar（2020, ICRA）

Automatic extrinsic calibration of a camera and a 3d lidar using line and plane correspondences(2018, ?IROS)

Targetless的方法不用檢測(cè)已知形狀的特殊的標(biāo)定物體，而是直接利用存在于自然場(chǎng)景中的線特征或者面特征進(jìn)行標(biāo)定。在Camvox: A low-cost and accurate lidar-assisted visual slam system中作者第一次把激光點(diǎn)投影到圖像平面上（colormap），然后基于深度和反射強(qiáng)度給點(diǎn)上色。然后從上色的圖中提取2D的線特征并和在圖像中提取的線特征進(jìn)行匹配。相似的，Automatic targetless extrinsic calibration of a 3d lidar and camera by maximizing mutual information中作者通過(guò)最大化圖像和colormap的互信息來(lái)得到準(zhǔn)確的外參。

也有一些作者直接在點(diǎn)云中基于深度的不連續(xù)性檢測(cè)3D邊緣特征，然后把3D的邊緣特征投影到2D的圖像平面上來(lái)計(jì)算殘差。由于基于連續(xù)性檢測(cè)的方法不能保證激光點(diǎn)云嚴(yán)格的落在邊緣上，所以邊緣檢測(cè)的精度比較差。也有一些基于運(yùn)動(dòng)的方法，例如：Extrinsic calibration of multiple lidars of small fov in targetless environments和Online targetless end-to-end camera-lidar self-calibration都是基于運(yùn)動(dòng)的方式來(lái)計(jì)算外參，然后再基于環(huán)境信息優(yōu)化這個(gè)外參，但是這種基于運(yùn)動(dòng)的方法需要比較強(qiáng)的激勵(lì)。

本文的方法是Targetless的方法，本文直接利用了點(diǎn)云中檢測(cè)的3D的線特征，這樣就沒(méi)有遮擋的問(wèn)題了；同時(shí)本文提出了一個(gè)更準(zhǔn)確和可靠的邊緣特征提取的方法；本文只基于單個(gè)的一對(duì)雷達(dá)的掃描就可以得到不錯(cuò)的結(jié)果。

方法

A. 概述

下圖定義了對(duì)應(yīng)的坐標(biāo)系：雷達(dá)系L，相機(jī)系C和2D的圖像平面。同時(shí)認(rèn)為激光是視覺(jué)之間的外參是一個(gè)未知量。由于環(huán)境中存在大量的邊緣特征，本文的方法基于邊緣特征對(duì)齊雷達(dá)和圖像。

上圖也表明了單個(gè)邊的約束?？梢钥闯鰜?lái)，雷達(dá)相對(duì)于相機(jī)的一些自由度的位姿是不能得到的：

沿著邊緣的平移量；

垂直于邊緣的平移量

繞邊緣點(diǎn)和焦點(diǎn)構(gòu)成的平面的法向量的旋轉(zhuǎn)

繞邊緣方向的旋轉(zhuǎn)

實(shí)際上，一個(gè)邊緣特征可以構(gòu)建兩個(gè)關(guān)于外參的有效的約束。為了獲得足夠的外參的約束，作者檢測(cè)了不同位置的不同朝向的邊緣特征。

B.邊緣特征檢測(cè)和匹配

1）邊緣檢測(cè)：一些存在的工作把點(diǎn)云投影到圖像平面，然后從投影的點(diǎn)云中檢測(cè)特征，例如邊緣特征和互信息關(guān)聯(lián)。利用投影后的點(diǎn)檢測(cè)特征的一個(gè)問(wèn)題是由于遮擋引起的多值和零值的問(wèn)題，如下圖a所示。如果相機(jī)在激光雷達(dá)的上方，A部分就會(huì)因?yàn)檎趽鯇?dǎo)致相機(jī)看得到但是激光看不到，結(jié)果導(dǎo)致沒(méi)有點(diǎn)投影到這部分，深度值全部都是0；另外B部分激光雷達(dá)看得到但是相機(jī)看不到，這就導(dǎo)致這部分點(diǎn)干擾前景點(diǎn)的投影。這就導(dǎo)致前景點(diǎn)和背景點(diǎn)都投影到了圖像的相同區(qū)域，一個(gè)地方有多個(gè)深度值。這種現(xiàn)象對(duì)于低分辨率的LIDAR可能不明顯，但是在高分辨率的雷達(dá)中很明顯。這種問(wèn)題會(huì)導(dǎo)致在投影的點(diǎn)云中提取錯(cuò)誤的特征，或者造成和圖像中特征匹配錯(cuò)誤的匹配。

為了避免投影造成的零值或者多值問(wèn)題，作者之間在點(diǎn)云上檢測(cè)邊緣特征。這里主要有兩種邊：深度連續(xù)的和深度不連續(xù)的，如上圖4所示。深度不連續(xù)的邊指的前景后背景物體深度有跳變的地方；而深度連續(xù)的邊指平面上深度連續(xù)的連接線。很多存在的方法都是基于深度是否連續(xù)來(lái)提取線特征，因?yàn)榭梢院苋菀谆邳c(diǎn)的深度來(lái)檢測(cè)。但是在研究了lidar的測(cè)量原理后，作者認(rèn)為這種方法在高精度的標(biāo)定中是不可靠，不準(zhǔn)確的。像下圖，實(shí)際的激光脈沖不是理想的點(diǎn)，而是具有一定發(fā)散角的光束。當(dāng)從前景物體掃描到背景物體時(shí)，一部分激光脈沖被前景物體反射，而其余的則被背景反射，產(chǎn)生兩個(gè)反射脈沖到激光接收器。在前景物體反射率高的情況下，第一個(gè)脈沖的信號(hào)將占主導(dǎo)地位，使光束中心線偏離前景物體，這也會(huì)導(dǎo)致前景物體的錯(cuò)點(diǎn)超出實(shí)際邊緣。當(dāng)從前景物體掃描到背景物體時(shí)，一部分激光脈沖被前景物體反射，而其余的則被背景反射，產(chǎn)生兩個(gè)反射脈沖到激光接收器。在前景物體反射率高的情況下，第一個(gè)脈沖引起的信號(hào)將占主導(dǎo)地位，即使光束中心線偏離前景物體，這也會(huì)導(dǎo)致前景物體的假點(diǎn)超出實(shí)際邊緣。這兩種現(xiàn)象都會(huì)錯(cuò)誤地使前景物體膨脹，并在邊緣提取和校準(zhǔn)中造成重大錯(cuò)誤。

為了避免前景點(diǎn)膨脹或者錯(cuò)點(diǎn)導(dǎo)致的深度不連續(xù)的邊緣，本文提取深度連續(xù)的邊。像上圖6顯示的過(guò)程：

首先把點(diǎn)云劃分為大小一定的體素（室內(nèi)0.5m，室外1m）；

對(duì)于每個(gè)體素，反復(fù)使用 RANSAC 來(lái)擬合和提取體素中的平面；

然后，保存在一定角度范圍內(nèi)的平面對(duì)（例如，[30°, 150°]）并求解平面相交線（即深度連續(xù)邊）。

如圖 6 所示，本文的方法能夠在一個(gè)體素內(nèi)提取多條相互垂直或平行的相交線。此外，通過(guò)選擇合適的體素的尺寸，我們可以檢測(cè)出來(lái)曲線邊緣。

下圖顯示了在外參正確的情況下提取場(chǎng)景中深度連續(xù)和不連續(xù)的邊。不連續(xù)的邊的提取是基于Bundle adjustment for lidar mapping中介紹的曲率的概念做的?？梢钥闯鰜?lái)，深度連續(xù)邊的噪聲更小，更準(zhǔn)確。對(duì)于圖像中的線特征，可以直接基于Canny算法。把檢測(cè)出來(lái)的線特征保存成2Dtree的格式用于快速的索引匹配。

2）匹配：檢測(cè)出來(lái)的激光的邊緣特征需要和視覺(jué)的邊緣特征匹配。對(duì)于每一個(gè)雷達(dá)邊緣，采樣邊緣上的點(diǎn)，每一個(gè)采樣點(diǎn)通過(guò)當(dāng)前估計(jì)出來(lái)的（可能不是最準(zhǔn)的）外參投影到相機(jī)平面：

其中：

利用剛體變換可以得到激光點(diǎn)在相機(jī)坐標(biāo)系下的位置，然后利用針孔模型把相機(jī)坐標(biāo)系下的點(diǎn)投影到圖像平面上，得到激光點(diǎn)在圖像上的位置：

最后根據(jù)相機(jī)的內(nèi)參標(biāo)定參數(shù)，得到相機(jī)的畸變系數(shù)，去畸變后激光點(diǎn)在圖像中的位置為：

然后利用圖像中的邊緣像素構(gòu)成的kd-tree搜出來(lái)k個(gè)最近的點(diǎn)，可以得到一個(gè)集合Q，然后計(jì)算：

可以得到均值和方差，然后利用邊緣上的一點(diǎn)q和由協(xié)方差矩陣的最小特征值對(duì)應(yīng)的特征向量得到邊緣特征方向n得到邊緣特征的表達(dá)式。同時(shí)，作者也把邊緣特征的方向投影到了圖像平面并驗(yàn)證其和計(jì)算得到的n的正交性（一個(gè)線段方向，一個(gè)法向量）。這也可以快速的去除那些距離很近但是又不平行的錯(cuò)誤匹配。下圖顯示了提取的 LiDAR 邊緣特征（紅線）、圖像邊緣特征（藍(lán)線）和匹配（綠線）。

C. 外參標(biāo)定

1）測(cè)量噪聲：提取得到的雷達(dá)邊緣點(diǎn)和對(duì)應(yīng)的圖像邊緣特征（q,n）受到測(cè)量噪聲的影響，設(shè)滿足零均值高斯分布的w是圖像中邊緣特征的噪聲，方差為1.5（由于像素的離散性）。同樣的，激光點(diǎn)云的邊緣特征也有測(cè)量噪聲。在實(shí)際的測(cè)量中，激光雷達(dá)通過(guò)掃描電機(jī)中編碼器的方向和激光點(diǎn)的飛行時(shí)間來(lái)測(cè)量深度信息。設(shè)ωi表示測(cè)量得到的電機(jī)軸向方向，而δωi ～ N (0_2*1 , Σωi )表示ωi在切平面上的測(cè)量噪聲（如上圖所示），然后利用“田”運(yùn)算通過(guò)測(cè)量得到真實(shí)的測(cè)量角：

其中N(ωi)=[N1，N2]是 ωi 處切平面的正交基，x表示叉乘對(duì)象的協(xié)方差矩陣。田s2操作表示的是在切平面ωi的δωi范圍內(nèi)的單位向量。同樣的，di是深度測(cè)量的結(jié)果，他也受一個(gè)零均值的高斯噪聲δdi的影響，真值可以表示為：

把這個(gè)結(jié)果和上邊由于電機(jī)編碼器噪聲引入的誤差結(jié)合起來(lái)可以得到：

因此：

該噪聲模型將用于產(chǎn)生一致的外部校準(zhǔn)。

2）標(biāo)定公式化和優(yōu)化：LPi是激光點(diǎn)云中檢測(cè)到的邊緣上的一個(gè)點(diǎn)，ni是和激光邊緣對(duì)應(yīng)的圖像邊緣的法向量，qi是激光邊緣對(duì)應(yīng)的圖像邊緣上的一點(diǎn)（均值）。把激光點(diǎn)的噪聲加上，如果利用真值的外參把點(diǎn)投影到圖像平面上應(yīng)該在圖像的對(duì)應(yīng)線特征上，所以：

其中w是圖像和激光點(diǎn)的噪聲，都服從零均值的高斯分布。這個(gè)公式可以看圖9，第一部分是激光點(diǎn)投影到圖像平面，第二部分是圖像上邊緣特征的一點(diǎn)，相減后投影到ni方向上就是ri。

公式9表明了一個(gè)激光線特征上的點(diǎn)可以提供一個(gè)約束，這和我們上邊介紹的是一樣的，一個(gè)線特征上有兩個(gè)獨(dú)立的點(diǎn)，可以提供兩個(gè)約束。此外，公式9就是一個(gè)關(guān)于外參的殘差方程，其中包含測(cè)量量，激光點(diǎn)，視覺(jué)線特征和一些未知的噪聲量。這個(gè)非線性的方程可以迭代的求解。

這里其實(shí)就是外參給的擾動(dòng)，我們不斷的尋找讓殘差下降的方向迭代求解。把公式9和10可以得到殘差的方程：

這里的公式推導(dǎo)也比較簡(jiǎn)單，對(duì)于T直接左乘一個(gè)擾動(dòng)就可以得到JT，對(duì)于w就按正常的求導(dǎo)過(guò)程就可以。

其中ri是圖9b中介紹的那樣。公式11給出了一個(gè)邊緣特征提供的約束，把N個(gè)邊緣整合到一起可以得到：

公式13可以寫成：

公式說(shuō)明，這里是高斯分布的線性變換，可以參考狀態(tài)估計(jì)中相關(guān)的知識(shí)。

根據(jù)公式14，就可以構(gòu)建關(guān)于外參的極大似然估計(jì)了（包括最小的方差）：

這個(gè)優(yōu)化問(wèn)題的解是：

這里就是一個(gè)把公式15展開(kāi)求解優(yōu)化量的過(guò)程。

其中外參可以由下式更新：

利用公式16和更新的公式，我們就可以迭代的求解真實(shí)的外參了。迭代的終止條件是變化量小于某個(gè)閾值。

3）標(biāo)定的不確定度：除了估計(jì)外參，我們也可以得到外參的不確定度。這樣可以知道當(dāng)前的標(biāo)定結(jié)果和真值之間的誤差。為了得到不確定度，我們需要把公式13都乘上一個(gè)量，然后求解得到的對(duì)應(yīng)的變化量：

這就表明 δT，外參真值和估計(jì)結(jié)果的誤差，并在切空間中參數(shù)化后，滿足以 δT*為均值的高斯分布，當(dāng)收斂時(shí)， δT接近0，協(xié)方差為：

D. 邊的分布對(duì)標(biāo)定結(jié)果的影響

公式12中JT表示的是外參的變化對(duì)殘差的影響。當(dāng)有很少或者邊的分布很不好（同一個(gè)方向，在一個(gè)小的區(qū)域內(nèi)），JT就會(huì)很小導(dǎo)致不確定度增大（公式19中JT在分母上）。從這個(gè)角度來(lái)說(shuō)，利用協(xié)方差矩陣可以直接得到環(huán)境的質(zhì)量好壞。不考慮相機(jī)畸變，利用針孔的投影模型可以得到：

其中XYZ是雷達(dá)的邊緣點(diǎn)在相機(jī)系下的表示。從結(jié)果表示靠近圖像中心的點(diǎn)的導(dǎo)數(shù)是比較小的。因此邊緣特征在圖像中均勻分布是比較好的。另外由于激光的測(cè)量噪聲隨著深度的增加而增加，所以場(chǎng)景的深度最好適中。

E. 標(biāo)定初始化和粗對(duì)齊

本文提出的基于優(yōu)化的方法在有一個(gè)比較好的初值的時(shí)候可以得到很準(zhǔn)的標(biāo)定的結(jié)果。為了保證算法收斂到最優(yōu)值，作者把通過(guò)最大化以下定義的邊緣匹配百分比 (P.C.) 來(lái)粗略校準(zhǔn)外參的初始化步驟集成到了算法的pipeline中：

其中分子是匹配到的雷達(dá)邊，分母是雷達(dá)邊的總個(gè)數(shù)。匹配基于投影到圖像平面后的LiDAR 邊緣點(diǎn)與其最近邊緣的距離和方向（參見(jiàn)第 III-B.2 節(jié)）。粗配準(zhǔn)是通過(guò)在給定范圍內(nèi)旋轉(zhuǎn)（網(wǎng)格大小 0.5 度）和平移（網(wǎng)格大小 2cm）的替代網(wǎng)格搜索來(lái)執(zhí)行的。

實(shí)驗(yàn)和結(jié)果

作者在室內(nèi)和室外各個(gè)場(chǎng)景中都進(jìn)行了實(shí)驗(yàn)，根據(jù)圖片看標(biāo)定后數(shù)據(jù)對(duì)齊的效果都很好，標(biāo)定的速度也很快。

我也在實(shí)際的環(huán)境中做了實(shí)驗(yàn)，感覺(jué)精度還是很準(zhǔn)的，其中在安裝的時(shí)候我們是有一個(gè)大概的外參初值的，這個(gè)對(duì)優(yōu)化問(wèn)題的求解很重要，所以我們最好手動(dòng)給：

下一步就是標(biāo)固態(tài)和機(jī)械雷達(dá)了。

參考文獻(xiàn)：

[1] Yuan C , ?Liu X , ?Hong X , et al. Pixel-level Extrinsic Self Calibration of High Resolution LiDAR and Camera in Targetless Environments[J]. ?2021.