鐳射雷達學習筆記（三）特徵提取
鐳射雷達獲取的資訊是和周圍物體之間的距離資訊，在移動機器人尤其是自主移動機器人領域具有非常廣泛的應用，那我們就從移動機器人的自主導航開始聊吧。
移動機器人導航是指移動機器人依靠感測器在特定環境中，按時間最優、路徑最短或能耗最低等準則實現從起始位置到目標位置的無碰撞運動。
傳統的移動機器人導航問題包含三大要素：地圖建立、定位和運動控制，通過三大要素，解決三個基本問題：我在哪裡？我要去哪裡？如何去？
機器人定位的目的是回答“我在什麼地方？”這個基本的問題。
鐳射雷達採集資料之後通過一定的運算可以準確的計算出當前的絕對位置或者相對位置，那麼就可以知道機器人在什麼位置，當然有時地圖的建立和定位是同步進行的，例如SLAM演算法（Simultaneous localization and mapping），此時可以看成是一個探索問題。 www.it165.net
說了這麼多，無非是想說鐳射雷達是多麼的有用，多麼的強大，而完成這些功能首先還是應該獲取鐳射雷達資料的特徵，這就回到了特徵提取這一主題上。
特徵提取主要分為兩個步驟：區域分割和特徵提取。區域分割階段主要完成特徵模式的分類及識別確定，即確定特徵屬於哪類模式，如直線，圓弧等，並確定屬於該特徵模式的區域及區域內的鐳射資料點集。特徵提取階段主要完成各類特徵模式引數的確定以及特徵點的提取。
一、區域分割
對於每一幀距離資料，首先把鐳射掃描點分割成不同的區塊。如果連續兩個掃描點的距離小於一個閾值，這兩個掃描點屬於同一個區塊。如果連續兩個掃描點的距離大於一個閾值，資料幀就從這個地方分割開。最後把一幀距離資料分割成幾個區塊。分割的區塊表示為 Ri(i=1,…,Q，其中 Q 是分割的區塊數)，每一個區塊包含 Ni個點。由於掃描點的分佈並不是均勻的，通常情況下，離感測器近的掃描點密度大一些，而遠離感測器的掃描點密度小一些。所以進行距離資料分割時，應用自適應變閾值分割方法。例如當某個掃描點離感測器中心的距離為 D 時，分割閾值選擇為 d，當掃描點離感測器中心的距離為3D 時，閾值選擇為 3d。除此之外，也可以選用其它的線性或非線性函式來定義自適應分割閾值。總之，在不同的掃描點選用不同的分割閾值，以求距離資料的分割區塊能夠更好地與實際環境特徵模型一致。如果鐳射的有效測距距離為 10 米，並且角度解析度為 0.25度，則相鄰掃描點之間的距離最小為：2×10m×sin(0.125°)=0.0436m。根據該值可以設定合適的分隔閾值。
UTM-30LX 有效距離60m 角度解析度0.25度，，但是一般對於室內應用一般不會超過10m
a、計算相鄰兩個點之間的距離Dj
b、判斷Dj和閾值delta的關係
如果Dj,大於閩值delta,則認為點(x,y)是兩個區域的分割點，閾值的選擇一般按照動態閾值的方式
c、（可選）判斷每個區域內的資料點的個數,如果某個區域包含資料點的個數小於等於三個,那麼該區域被視為噪聲區域,捨棄這些噪聲點
//鐳射雷達區域分割效果，不同的區域用不同的顏色分割（同一種顏色並不代表在同一區域，只是顏色有限，幾種顏色在迴圈使用）
二、特徵提取
鐳射雷達掃描的資料中，幾個重要的特徵：撕裂點（breakPoint）、角點（Corner）、直線、圓弧等。
區域分割實際上就已經找到了資料中的撕裂點。折線也可以當成是一個特徵，是直線加角點構成的特徵。
直線作為一個很關鍵的特徵在很多的論文中都是提取的關鍵，鑑於折線的是普遍存在的，那麼角點的檢測同樣是一個難以迴避的問題。那麼我們就先提取角點，將所有的折線都打斷成直線和角點。
1、角點檢測
假設有一條折線，只有單個角點，那麼我們可以採用多變形擬合方式確定角點的位置。首先將區域內的點擬合成一條直線，然後找出離直線最遠的點，這個距離如果大於某個閾值，則可以認為是折線，而該點就是折線的分割點，否者就是一條直線。
當某區域含有多個角點時，就需要採用迭代或者遞迴的方式，不斷的尋找角點-->拆分成兩段，迴圈進行，直到每個區域都不存在角點。
//多邊形擬合的方式確定是否存在角點，以及角點的位置
01.// 進行多邊形擬合： Points : 輪廓上的點 n -- 輪廓點數目 Eps -- 擬合精度
02.// 返回值： 若該輪廓段需要分段，則返回分段點在該輪廓點列中的索引，否則，返回 0 表示不需要分段
03.// 這裡是整個演算法計算複雜性最大的一個地方
04.// 為了提高程式執行效率，對點到直線的距離計算進行改進：
05.// 多邊形擬閤中的直線是由點列中的點決定的
06.// 為了計算點到直線的距離，
07.// 採用座標系旋轉，將直線旋轉到x軸方向，這樣點到直線的距離即為各個點
08.// 在座標旋轉後的y值的絕對值
09.// 同時，座標旋轉矩陣在該次運算中為定值，只需一次計算，不需要多次的開方或三角計算
10.int
OpenRadar::PolyContourFit( int* X,
int* Y, int
n , float
Eps ) // 根據輪廓點，用多邊形擬合該輪廓點
11.{
12.double
dis = sqrt((double)(((X[0] - X[n - 1])*(X[0] - X[n - 1])) +
13.((Y[0] - Y[n - 1])* (Y[0] - Y[n - 1]))));
14.double
cosTheta = (X[n- 1] - X[0]) / dis;
15.double
sinTheta = - ( Y[n- 1] - Y[0] )/dis;
16.double
MaxDis = 0;
17.int
i ;
18.int
MaxDisInd = -1;
19.double
dbDis;
20.for(i = 1 ; i < n - 1 ; i++)
21.{
22.// 進行座標旋轉，求旋轉後的點到x軸的距離
23.dbDis =
abs( (Y[i] - Y[0]) * cosTheta + (X[i] - X[0])* sinTheta);
24.if( dbDis > MaxDis)
25.{
26.MaxDis = dbDis;
27.MaxDisInd = i;
28.}
29.}
30.if(MaxDis > Eps)
31.{
32.return
MaxDisInd;
33.// cout << "Line 1 : " << endl;
34.// cout << "Start :" << Points[0].x << " " << Points[0].y << " --- " << Points[MaxDisInd].x << " " << Points[MaxDisInd].y
<< endl;
35.// cout << "角度： "<<180 * atan2(Points[0].y - Points[MaxDisInd].y , Points[0].x - Points[MaxDisInd].x ) / 3.1415926;
36.// cout << "Line 2 :" << endl;
37.// cout << "Start :" << Points[MaxDisInd].x << " " << Points[MaxDisInd].y << " --- " << Points[n - 1].x
<< " " << Points[n - 1].y << endl;
38.// cout << "角度： "<< 180 * atan2(Points[n - 1].y - Points[MaxDisInd].y , Points[n - 1].x - Points[MaxDisInd].x
) / 3.1415926;
39.}
40.// else{
41.// cout << "Line 1 : " << endl;
42.// cout << "Start :" << Points[0].x << " " << Points[0].y << " --- " << Points[n - 1].x << " " << Points[n
- 1].y << endl;
43.// cout << "角度： "<<180 * atan2(Points[n - 1].y - Points[0].y , Points[n - 1].x - Points[0].x ) / 3.1415926;
44.
45.// }
46.return
0;
47.}
以上只能檢測具有單個角點的折線，任意個角點的折線採用了遞迴的方式，想提速的可以自己轉化為迭代的方式實現。
01.//將折線拆多段
02.int
OpenRadar::BreakPolyLine(vector<int>& BreakedRadarRho,
03.vector<double>& BreakedRadarTheta,
04.vector<int>& SepRadarRho ,
05.vector<double>&SepRadarTheta)
06.{
07.int
rho = 0;
08.double
theta = 0.0;
09.int
X[1200] = {0};
10.int
Y[1200] = {0};
11.int
rhoCopy[1200] = {0};
12.double
thetaCopy[1200] = {0};
13.int
pointCnt = 0;
14.int
lineCnt = 0;
15.int
N = 0;
16.SepRadarRho.clear();
17.SepRadarTheta.clear();
18.Corners.clear();
19.
20.//進行多次迭代，將所有的折線都拆分成直線段
21.
22.vector<int>CornerIndex;
23.int
CornerCnt = 0;
24.int
tempIndex = 0;
25.for
(int
i = 0; i < static_cast<int>(BreakedRadarRho.size());i++)
26.{
27.rho = BreakedRadarRho.at(i);
28.theta = BreakedRadarTheta.at(i);
29.
30.if
(rho < 0)
31.{
32.if
(pointCnt > 200)//數目比較少的點直接拋棄
33.{
34.CornerIndex.clear();
35.CornerCnt = FindCorners(CornerIndex,X,Y,0,pointCnt,200);
36.
37.if
(CornerIndex.size() == 0)
38.{
39.for
(int
k = 0 ; k < pointCnt;k++)
40.{
41.SepRadarRho.push_back(rhoCopy[k]);
42.SepRadarTheta.push_back(thetaCopy[k]);
43.}
44.SepRadarRho.push_back(-1);
45.SepRadarTheta.push_back(1000.0);
46.lineCnt++;
47.}else
48.{
49.tempIndex = 0;
50.for
(int
k = 0 ; k < pointCnt;k++)
51.{
52.SepRadarRho.push_back(rhoCopy[k]);
53.SepRadarTheta.push_back(thetaCopy[k]);
54.if
(k == CornerIndex.at(tempIndex))
55.{
56.SepRadarRho.push_back(-1);
57.SepRadarTheta.push_back(1000.0);
58.lineCnt++;
59.if
(tempIndex < static_cast<int>(CornerIndex.size()) -1)
60.{
61.tempIndex++;
62.}
63.}
64.}
65.SepRadarRho.push_back(-1);
66.SepRadarTheta.push_back(1000.0);
67.lineCnt++;
68.}
69.}
70.pointCnt = 0;
71.continue;
72.}
73.X[pointCnt] =
static_cast<int>(rho*cos(theta));
74.Y[pointCnt] =
static_cast<int>(rho*sin(theta));
75.rhoCopy[pointCnt] = rho;
76.thetaCopy[pointCnt] = theta;
77.pointCnt++;
78.}
79.
80.//cout<<"lineCnt: "<<linecnt<<endl; pre="" return=""><p>2、直線擬合</p><p> </p><p>如果某區域不存在角點，並且點資料比較大，那麼一般都是直線，（不是絕對，直線的判定方法後面的部落格再寫）</p><p>直線擬合的原理比較簡單，實際就是一個最小二乘法，或者為了提高擬合的精度可以採用加權的最下二乘法，這裡採用的是加權最小二乘。</p><p>//原始資料圖
藍點是雷達的位置</p><div style="text-align: center"><img style="width: 344px; height: 475px;" alt="\" src="http://www.it165.net/uploadfile/2013/0302/20130302100534115.jpg"></div><div><p>//擬合直線和角點圖，粗點是角點，粗實線是擬合出的直線</p><div
style="text-align: center"><img style="width: 364px; height: 463px;" alt="\" src="http://www.it165.net/uploadfile/2013/0302/20130302100551621.jpg"></div><div>由於只對點數比價多的直線進行了擬合，上部分的直線併為畫出，實際上點數比較少的直線誤差也會大，並不是關鍵的特徵。</div></div>
