【多傳感融合】優達學城多傳感融合學習筆記（四）——將激光雷達3D點雲映射到相機圖像（下）

將激光雷達3D點雲映射到相機圖像（下）——編程實踐

KITTI數據集的傳感器安裝配置

在上一篇筆記中，對激光雷達3D點雲映射到相機圖像的原理進行了詳細分析，下面讓我們首先了解一下我們使用的KITTI數據集中的傳感器安裝配置。在下圖中，數據採集車上安裝了兩個前向攝像頭，車頂安裝了一個Velodyne 激光雷達和一個慣性測量裝置（即IMU，不過本課程中未使用）。

對於KITTI官網下載到的所有數據集合，你都可以找到包含相機內參、外參的校正文件。下面展示了校正文件"calib_velo_to_cam.txt“的部分內容，表明了激光雷達和左攝像頭的相對位置關係（適用於我們即將使用的高速公路場景數據）：

calib_time: 15-Mar-2012 11:37:16

R: 7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02

T: -4.069766e-03 -7.631618e-02 -2.717806e-01

…

矩陣R和向量T提供給我們了相機的外參。當然，爲了完成三維點雲到相機圖像的映射，我們還需要相機的內參。它們保存在"calib_cam_to_cam.txt"文件中，相關內容如下：

calib_time: 09-Jan-2012 13:57:47

…

R_rect_00: 9.999239e-01 9.837760e-03 -7.445048e-03 -9.869795e-03 9.999421e-01 -4.278459e-03 7.402527e-03 4.351614e-03 9.999631e-01

P_rect_00: 7.215377e+02 0.000000e+00 6.095593e+02 0.000000e+00 0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00

…

矩陣R_rect_00是一個3*3的修正旋轉矩陣，用於使相機圖像共面，例如，對齊立體視覺平臺的多個攝像頭（對於這輛KITTI數據採集車，有兩部攝像頭），這可以通過將左側攝像頭的一行像素直接對齊右側攝像頭的另一行像素（而不是兩條在兩個相機平面交叉的傾斜直線）。由於我們當前主要考慮MONO（單通道）相機，因此我們將不再深究上述對齊操作的基礎理論——但是如果你對此比較感興趣，可以搜索"epipolar geometry"（對極幾何）獲取更多相關內容。矩陣P_rect_00則包含了相機的內參（我們稱之爲$K$）。下列公式展示瞭如何使用齊次座標系將KITTI數據集中的激光雷達3D點雲X映射到左側相機的2D相機圖像點Y（這裏使用了Kitti readme文件中的符號）上。

程序執行主要流程

要實現將KITTI數據集中的激光雷達3D點雲X映射到左側相機的2D相機圖像點Y上，在程序中主要需要執行以下幾步：

1. 遍歷獲取激光雷達點雲信息時，將每個3D點轉換爲齊次座標，並存入4D變量X中。
2. 應用映射公式，將X映射到相機的圖像平面，並將結果存儲到變量Y中。
3. 將變量Y從齊次座標系轉換回歐幾里得座標系，從而得到對應的圖像中的像素位置，並將結果存入變量pt中。

參考代碼如下：
加載對應的相機數據和激光雷達點雲數據，初始化前面提到的映射相關矩陣（OpenCV形式矩陣）：

初始化用於顯示映射結果的圖片數據（visImg用於顯示原始相機圖像，overlay用於顯示映射到相機圖像上的激光雷達點雲），初始化齊次座標對象X和Y：

遍歷激光雷達點雲，將其轉換到齊次座標系映射到相機圖像平面，再轉換回非齊次座標，得到對應的像素位置pt，並將其繪製在圖像overlay中（最近處爲紅色，最遠處爲綠色）。

利用OpenCV的cv::addWeighted函數將overlay圖像按照一定的不透明度和原始相機圖像visImg融合並顯示：

融合顯示的結果如下圖所示，其中映射的激光雷達點雲按照縱向距離的遠近用不同的顏色進行顯示（最近處爲紅色，最遠處爲綠色）：

從結果可以看出，對於映射到路面上的點，較近處呈現紅色，較遠處呈現綠色，與我們的預期一致。但是顯然，圖像中上半部分的紅色點是不對的。爲了解決這個問題，我們常常需要對雷達點雲信息進行一些過濾操作，請看下一節。

過濾激光雷達點雲

在前面第二節的課程中，我們已經掌握瞭如何將激光雷達點雲俯視圖映射到二維圖像上。這個映射過程實際上假設了我們只對自車前面的點雲感興趣。在代碼中，上述映射過程如下所示：

int y = (-xw * imageSize.height / worldSize.height) + imageSize.height;
int x = (-yw * imageSize.height / worldSize.height) + imageSize.width / 2;

那麼，由於Velodyne 激光雷達是安裝在車頂，且以10Hz的頻率進行360度旋轉，因此，這就意味着它也能夠測量到自車後面的3D點雲。這些點雲也包含在數據集中。但是當我們將其映射到相機圖像，它們也會形成一個有效的映射值，即便這些點雲根本無法在該前向攝像頭中被看到。這些點雲具有負的x值（縱座標），這直接導致了它們映射到相機圖像後呈現爲紅色。爲了避免這個問題，我們需要執行一些點雲過濾操作。

下面的代碼展示了進行點雲過濾的具體操作，濾除掉了一些無用的點雲，例如：

1. 位於激光雷達後方，且具有負的x值。
2. 在x軸方向上距離太遠以致超出了最大距離上限。
3. 在y軸方向上距離太遠以致超出了自車感興趣的範圍。
4. 距離道路表面太近以致於具有負的z值。
5. 反射率接近0，可能具有較大的測量誤差。

    for(auto it=lidarPoints.begin(); it!=lidarPoints.end(); ++it) {

        float maxX = 25.0, maxY = 6.0, minZ = -1.4; 
        if(it->x > maxX || it->x < 0.0 || abs(it->y) > maxY || it->z < minZ || it->r<0.01 )
        {
            continue; // skip to next point
        }

在應用了上述過濾操作後，點雲映射到圖像上的結果會被過濾掉很多的無用點雲信息，呈現的結果如下圖所示。對於車輛碰撞預警等應用，傳感器的測量質量固然是至關重要的，而過濾步驟，除了能夠增加處理速度，還可以幫助提高可靠性，也是不可或缺的步驟。