[代碼閱讀]lego-loam 代碼細節詳解(一)-image_projection

1 image_projection

input topic：
velodyne_points	點雲的輸入話題，在utils.h裏面可以修改
output topic：
full_cloud_projected
full_cloud_info
ground_cloud	地面點雲
segmented_cloud	只去除了outlier的點雲（降採樣）
segmented_cloud_pure	去除了outlier點和地面點的所有的點雲(沒有降採樣)
segmented_cloud_info	自定義消息類型
outlier_cloud	不屬於規則聚類的點

以上加粗的三個話題是傳遞給下一個節點所需的。

這個cpp的劃分十分工整，在雷達的回調函數裏進行了以下7個功能的處理；

• 1. Convert ros message to pcl point cloud

​ copyPointCloud(laserCloudMsg);

• 2. Start and end angle of a scan

  findStartEndAngle();

• 3. Range image projection

​ projectPointCloud();

• 4. Mark ground points

​ groundRemoval();

• 5. Point cloud segmentation

​ cloudSegmentation();

• 6. Publish all clouds

​ publishCloud();

• 7. Reset parameters for next iteration

​ resetParameters();

下面，對每個函數的功能做一個詳細的解答；

1.1 copyPointCloud(laserCloudMsg)

概述：將ROS_MSG -> PCL Poindcloud

沒什麼好說的，就是將點雲的消息類型從ROS格式轉換成了PCL的格式。

1.2 findStartEndAngle()

找到雷達旋轉的開始的角度和結束的角度

atan2()範圍在$(-\pi,\pi)$之間

所以：

startOrientation $\in (-\pi,\pi)$

endOrientation $\in (\pi,3\pi)$

理想情況下，雷達掃描點的起始點在-x軸附近，結束點也在-x附近。但是不能保證結束點就是比$\pi$稍小，起始點比$-\pi$稍大。二者相減正好≈$2\pi$。再加上結束點加了$2 \pi$。

這種情況下，需要對end點加上2 $\pi$

這兩種特殊情況的處理，來保證e-s的值始終在2$\pi$範圍附近。

1.3 projectPointCloud()

這裏的verticalAngle算的是當前點的垂直角度，以velodyne16爲例，範圍在(-15°，+15°)之間。

rowIdn是將垂直的度數範圍(0,30)除以16線的分辨率(也就是2°一線),得到每一條線的ring號。

當然，velodyne是不需要這麼麻煩的，直接每個點都有一個ring值。實際代碼可以對此進行優化，跳過這一部分。

這裏以velodyne16爲例，其垂直分辨率16，水平分辨率1800.本函數將整個點雲數據投影成一個16*1800的二維平面圖

需要注意的是：

thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

每個點的強度值，整數部分代表行號，小數部分代表列號。後面會通過intensity來恢復這個點。

fullCloud里根據0-1800*16的範圍來保存每個點；

1.4 groundRemoval()

標記地面點

通過在16*1800的下半面，也就是8*1800的部分，通過計算每個點和其相同列的上一個點之間的向量，判斷該向量和地面的夾角，小於一定的閾值則認爲是平面，在groundMat中標記爲1；

同時，在labelmat中，該點被標記爲-1.

1.5 cloudSegmentation()

重點：通過bfs進行四鄰域搜素。不理解的可以看一下四鄰域的BFS搜索。

1.5.1 labelComponents()

對所有的點進行遍歷，並通過BFS進行搜索判斷是否是同一個聚類。聚類的分割以角度值爲閾值，其計算如下圖所示。

使用當前點和其四鄰域的點進行一個角度$\beta$的計算。分別獲取當前點和鄰域點的深度，深度值大的爲$d_1$，小的爲$d_2$，$\alpha$是兩個點之間的分辨率，垂直和水平方向分別爲2°和360/1800°。由此可以計算出$\beta$角。

如果$\beta$大於一定的閾值，說明兩者之間沒有突變，所以可以認爲是同一個聚類(如上圖)，加入到隊列當中。繼續下一步的四鄰域搜索。

labelMat

value	類型
-1	地面點
0	未標記
1,2,3… =>統一都是count的值	同一個count值代表同一個聚類
999999	無效點

四鄰域搜索完成之後，會統計這次四鄰域搜索的cluster的size大小

• size>30:認爲是有效分割，count值++；
• 3<size<=30:判斷當前聚類在X方向(縱座標，0-15)上有多少個點，如果大於3個，則仍然認爲是聚類。
• 其他size值，認爲是無效點，標記爲999999。

1.5.2 對3w個點再次遍歷

分別有以下幾個操作：

• 1.對於outliers，每隔5的倍數保存一次，1800/5=360。可以認爲是一種降採樣，每1度保留一個outlier
• 2.對於地面點，每隔5的倍數保存一次。
• 3.對於以上兩個標籤都不是的點，保存到segmentedCloud，並準備發送給下一個節點。

1.6 publishCloud();

將各類型的點雲發佈出去

1.7 resetParameters();

參數重置