3D激光SLAM核心：為什么要進(jìn)行點(diǎn)云特征提取

1.為什么要進(jìn)行點(diǎn)云特征提取

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

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

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

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

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

2. 本項(xiàng)目所用的特征提取方法說明

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

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

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

那么重點(diǎn)就在于如何計(jì)算曲率了。

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

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

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

3.1 確定激光點(diǎn)所在的掃描線并建立點(diǎn)的索引

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

?

此外我們還需要查閱手冊(cè)確定出激光雷達(dá)在垂直方向的視場(chǎng)角以及垂直方向的角度分辨率來確定角度和線數(shù)的關(guān)系。以KITTI所用的Velodyne為例，其垂直視場(chǎng)角范圍是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ù)激光點(diǎn)所在線數(shù)對(duì)其進(jìn)行一個(gè)分類，即每個(gè)激光掃描線上的點(diǎn)放在一個(gè)容器里，一共16個(gè)。還需要對(duì)每一個(gè)點(diǎn)建立一個(gè)索引，方便后續(xù)可以直接根據(jù)該索引找到這個(gè)點(diǎn)，對(duì)應(yīng)代碼為：

?

?

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

	?

	?

	3.2 計(jì)算每個(gè)點(diǎn)的曲率

	上面說了要計(jì)算曲率必須先找到集合 S ，我們?cè)?.1步驟中確定每個(gè)點(diǎn)所在的線數(shù)并將其加入到對(duì)應(yīng)的容器中就是為了給這個(gè)做準(zhǔn)備的。因?yàn)樵趌oam系列的方法中，集合 S 是在每個(gè)點(diǎn)所在掃描線上去找該點(diǎn)前后各5個(gè)點(diǎn)的。這很大程度上簡(jiǎn)化了算法過程，因?yàn)椴挥每紤]激光點(diǎn)上下兩個(gè)方向上的點(diǎn)了。

	?

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

	?

	?

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

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

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

	?

	?
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;
??}
?}
}


	?

	?

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

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

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

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

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

	4. 測(cè)試

	a. 源點(diǎn)云

	

	圖1. 原始點(diǎn)云

	b. 提取出的Corner Point 點(diǎn)云

	

	圖2. Corner Point點(diǎn)云

	c. 提取出的Plane Point點(diǎn)云

	

	圖3. Plane Point點(diǎn)云

	編輯：黃飛

	?