1.為什么要進行點云特征提取

首先要弄明白為什么要進行特征提取。搞過2D激光SLAM的人都曉得，在進行掃描匹配的時候，經(jīng)常是直接使用原始點云數(shù)據(jù)。這是因為一般2D激光SLAM所用的單線激光雷達每一幀點云數(shù)非常的少，一般幾千個點都算多的了。因此點云中的所有點都可以參與運算，性能不會有大的問題。

而3D激光SLAM中所用的一般是多線激光雷達，每一個數(shù)據(jù)幀中的點云數(shù)量非常的多。像KITTI數(shù)據(jù)集中所用的Velodyne 16線激光雷達，一般點云數(shù)在幾萬。而Velodyne 64線激光雷達點云數(shù)量更是多達十幾萬。這個數(shù)量太過龐大，如果每一個點都參與運算，那么對計算資源的需求將是不能接受的。

這里感興趣的同學(xué)可以使用PCL庫的ICP方法測試一下，測試代碼可以見于：

_icp_test at ddf · softdream/3d_lidar_slam (github.com) github.com/softdream/3d_lidar_slam/tree/ddf/pcl_icp_test

點云特征提取就是為了盡可能降低參與運算的點的數(shù)量。在3D激光中，雖然特征提取不像視覺中那么方便，但是點云的空間結(jié)構(gòu)化信息是可以利用起來的。最常見的有利用法向量提取特征(典型的PFH)，利用曲率提取特征(loam及其一系列衍生方法所用的方法)。

2. 本項目所用的特征提取方法說明

在本項目中，將參考loam方法中使用的特征提取方法。loam長盛不衰，與其簡潔又高效的的特征提取方式脫不開關(guān)系，因此極具借鑒意義。當(dāng)然，本人也將探索新型的特征提取方法，特別是基于深度圖(Range Image)的特征提取方法非常有吸引力，這個后續(xù)會另開分支，單獨介紹這種方法。

3. 特征提取理論與代碼編寫

特征提取這塊原理比較簡單，主要麻煩點在代碼編寫中，這個后面會敘述。loam及其衍生型的特征提取方法都是對原始點云提取Corner Point和Plane Point。而提取特征點的依據(jù)就是曲率。

那么重點就在于如何計算曲率了。

根據(jù)loam論文中所述，在計算某一點的曲率時，需要找到該點前后五個點（包括該點，一共11個點）一起來計算，計算公式為：

其中 c 是當(dāng)前點 i 的曲率， S 是當(dāng)前點周圍前后11個點的集合，??是當(dāng)前點 i 的坐標，那么???就是集合中其它點的坐標。

實際在編程的過程中要注意的地方在于如何確定集合?S 。這個后面會詳細說。這里列出特征提取的主要步驟：

3.1 確定激光點所在的掃描線并建立點的索引

在loam系列方法中，會根據(jù)激光點的垂直方向的出射角確定出它在哪個掃描線上。第 i 個點所在的角度為：

?

此外我們還需要查閱手冊確定出激光雷達在垂直方向的視場角以及垂直方向的角度分辨率來確定角度和線數(shù)的關(guān)系。以KITTI所用的Velodyne為例，其垂直視場角范圍是30°(+15°~-15°)，角度分辨率為2°。

代碼如下：

?

?

???int?scan_idx?=?-1;
?typename?CloudType::ValueType?angle?=?::atan2(?pt.z,?::sqrt(?pt.x?*?pt.x?+?pt.y?*?pt.y?)?)?*?180?/?M_PI;
???????
?if?constexpr?(?Config::N_SCANS?==?16?)?{
??if?(?angle?>=?-15?&&?angle?<=?15?)?{
???scan_idx?=?static_cast(?(?angle?+?15?)?/?2?+?0.5?);
??}
?}?

?

?

這里可以根據(jù)激光點所在線數(shù)對其進行一個分類，即每個激光掃描線上的點放在一個容器里，一共16個。還需要對每一個點建立一個索引，方便后續(xù)可以直接根據(jù)該索引找到這個點，對應(yīng)代碼為：

?

?

if?(?scan_idx?>?-1?&&?scan_idx?

	?

	?

	3.2 計算每個點的曲率

	上面說了要計算曲率必須先找到集合 S ，我們在3.1步驟中確定每個點所在的線數(shù)并將其加入到對應(yīng)的容器中就是為了給這個做準備的。因為在loam系列的方法中，集合 S 是在每個點所在掃描線上去找該點前后各5個點的。這很大程度上簡化了算法過程，因為不用考慮激光點上下兩個方向上的點了。

	?

	?
for?(?size_t?i?=?Config::Row_Index_Start;?i?

	?

	?

	每個點的曲率放在容器里，供后續(xù)使用。

	3.3 根據(jù)曲率來篩選特征點

	在步驟3.2中已經(jīng)將每個點的曲率保存在了容器scans_row_curv_vec中了。這時候只需要遍歷所有的點，并對每一個點的曲率根據(jù)閾值進行判斷即可。

	?

	?
for?(?size_t?i?=?Config::Row_Index_Start;?i?=?j_start_index?)?{
???//?plane?feature
???if?(?scans_row_curv_vec[i][j]?points.push_back(?input_cloud.points[?point_index_vec[i][j]?]?);?
???}
???//?corner?feature
???else?if(?scans_row_curv_vec[i][j]?>?2.0?)?{
????output_cloud2_refer->points.push_back(?input_cloud.points[?point_index_vec[i][j]?]?);
???}

???//j_start_index?=?j?+?window_interval;
???j_start_index?=?j?+?10;
??}
?}
}


	?

	?

	注意這里為了降低特征點的數(shù)量，每隔若干點才判斷一下，即語句j_start_index = j + 10的作用。

	另外在loam系列方法中根據(jù)曲率判斷特征點的方法比這里復(fù)雜一些，它要對點的曲率進行排序，選取曲率最高的若干個點作為角點，曲率最低的若干個點作為平面點。我這里簡化了操作。有興趣的小伙伴可以隨便修改試試。

	3.4 分別將平面點和角點放在兩個點云數(shù)據(jù)結(jié)構(gòu)里

	將從點云中提出出來的平面點和角點分別放在之前定義好的兩個點云數(shù)據(jù)結(jié)構(gòu)里，供后續(xù)使用。

	注意我在程序編寫時使用了可變參數(shù)模板作為形參，同時為了支持多態(tài)使用了CRTP機制，增加了復(fù)雜性，特別是在遞歸解析可變參數(shù)的時候，使用了std::any來存儲參數(shù)類型，這樣寫不一定是最好的，有更好方法的小伙伴們可以給出新的建議。

	4. 測試

	a. 源點云

	

	圖1. 原始點云

	b. 提取出的Corner Point 點云

	

	圖2. Corner Point點云

	c. 提取出的Plane Point點云

	

	圖3. Plane Point點云

	編輯：黃飛

	?