激光雷达学习笔记（三）特征提取

激光雷达获得的信息是与周围物体的距离信息，广泛应用于移动机器人领域，特别是独立移动机器人。让我们从移动机器人的独立导航开始。

移动机器人导航是指移动机器人根据最佳时间、最短路径或最低能耗标准，在特定环境中实现从起始位置到目标位置的无碰撞运动。

传统的移动机器人导航问题包括地图创建、定位和运动控制三个基本问题：我在哪里？我要去哪里？如何去？

机器人定位的目的是回答我在哪里？这个基本问题。

激光雷达采集数据后，可以通过一定的操作准确计算当前的绝对位置或相对位置，从而知道机器人在哪里。当然，有时地图的创建和定位是同步的，例如SLAM算法(Simultaneouslocalization and mapping)，这可以看作是一个探索问题。

说了这么多，我只想说激光雷达有多有用，有多强大，完成这些功能应该首先获得激光雷达数据的特征，这回到了特征提取的主题。

特征提取主要分为区域分割和特征提取两个步骤。区域分割阶段主要完成特征模式的分类和识别，即确定直线、弧等特征模式，确定属于特征模式的区域和区域的激光数据点集。特征提取阶段主要完成各种特征模式参数的确定和特征点的提取。

一、区域分割

激光扫描点首先将每帧距离数据分成不同的块。如果连续两个扫描点的距离小于一个阈值，则两个扫描点属于同一块。如果连续两个扫描点之间的距离大于一个阈值，数据帧将从这个地方分开。最后，将一帧距离数据分成几个块。分割块表示为 Ri(i=1,…,Q，其中 Q 是分割的块数)，每个块包含 Ni个点。由于扫描点分布不均匀，通常离传感器近的扫描点密度较大，而远离传感器的扫描点密度较小。因此，在进行距离数据分割时，采用自适应变阈值分割法。例如，当扫描点距离传感器中心时 D 分割阈值为 d，当扫描点与传感器中心的距离为3时D 时，阈值选择为 3d。此外，自适应分割阈值线性或非线性函数来定义自适应分割阈值。总之，在不同的扫描点选择不同的分割阈值，使距离数据的分割块能够更好地与实际环境特征模型一致。如果激光的有效测距距离为 10 米，角度分辨率为 0.25度，相邻扫描点之间的最小距离为：2×10m×sin(0.125°)=0.0436m。适当的分隔阈值可根据该值设置。

UTM-30LX 有效距离60m 角度分辨率0.25度，但室内应用一般不超过10度m

a、计算相邻两点之间的距离Dj

b、判断Dj和阈值delta的关系

如果Dj,大于闽值delta,则认为点(x,y)这是两个区域的分割点。阈值的选择通常基于动态阈值

c、(可选)判断每个区域的数据点数量。如果一个区域包含的数据点数量小于或等于三个，则该区域被视为噪声区域，并放弃这些噪声点

//激光雷达区域分割效果，不同的区域用不同的颜色分割(同一种颜色并不代表在同一区域，只是颜色有限，几种颜色在循环使用)

二、特征提取

激光雷达扫描的数据中，几个重要的特征：撕裂点(breakPoint)、角点(Corner)、直线、圆弧等。

区域分割实际上已经在数据中找到了撕裂点。折线也可视为直线加角点的特征。

直线作为一个非常关键的特征，是许多论文中提取的关键。鉴于折线的普遍存在，角度检测也是一个不可避免的问题。然后我们首先提取角度，将所有折线打断成直线和角度。

1、角点检测

假设有一条折叠线，只有一个角，那么我们可以使用多变形拟合来确定角的位置。首先，将区域内的点拟合成一条直线，然后找离直线最远的点。如果距离大于一个阈值，则可视为折线，该点为折线的分割点，否则为直线。

当一个区域包含多个角点时，需要迭代或递归不断寻找角点-->分为两段，循环进行，直到每个区域都没有角点。

///多边形拟合确定角点是否存在，角点的位置

// 多边形拟合： Points : 轮廓上的点 n -- 轮廓点数目 Eps -- 拟合精度

// 返回值： 如果轮廓段需要分段，则返回轮廓点中分段点的索引，否则返回 0 表示不需要分段

// 这是整个算法计算最复杂的地方

// 为提高程序运行效率，对点到直线的距离计算进行改进：

// 多边形拟合中的直线由点列中的点决定

// 计算点到直线的距离，

// 用坐标系旋转，将直线旋转到x轴方向，这样点到直线的距离就是每个点

// 坐标旋转后y值的绝对值

// 同时，坐标旋转矩阵是该操作中的定值，只需一次计算，不需要多次开方或三角计算

int OpenRadar::PolyContourFit( int* X, int* Y, int n , float Eps ) // 根据轮廓点，用多边形拟合轮廓点

{

double dis = sqrt((double)(((X[0] - X[n - 1])*(X[0] - X[n - 1]))

((Y[0] - Y[n - 1])* (Y[0] - Y[n - ))；

double cosTheta = (X[n- 1] - X[0]) / dis;

double sinTheta = - ( Y[n- 1] - Y[0] )/dis;

double MaxDis = 0;

int i ;

int MaxDisInd = -1;

double dbDis;

for(i = 1 ; i < n - 1 ; i )

{

// 旋转坐标，旋转后点击x轴的距离

dbDis = abs( (Y[i] - Y[0]) * cosTheta (X[i] - X[0])* sinTheta);

if( dbDis > MaxDis)

{

MaxDis = dbDis;

MaxDisInd = i;

}

}

if(MaxDis > Eps)

{

return MaxDisInd;

// cout << "Line 1 : " << endl;

// cout << "Start :" << Points[0].x << " " << Points[0].y << " --- " << Points[MaxDisInd].x << " " << Points[MaxDisInd].y << endl;

// cout << "角度： "<<180 * atan2(Points[0].y - Points[MaxDisInd].y , Points[0].x - Points[MaxDisInd].x ) / 3.1415926;

// cout << "Line 2 :" << endl;

// cout << "Start :" << Points[MaxDisInd].x << " " << Points[MaxDisInd].y << " --- " << Points[n - 1].x << " " << Points[n - 1].y << endl;

// cout << "角度： "<< 180 * atan2(Points[n - 1].y - Points[MaxDisInd].y , Points[n - 1].x - Points[MaxDisInd].x ) / 3.1415926;

}

// else{

// cout << "Line 1 : " << endl;

// cout << "Start :" << Points[0].x << " " << Points[0].y << " --- " << Points[n - 1].x << " " << Points[n - 1].y << endl;

// cout << "角度： "<<180 * atan2(Points[n - 1].y - Points[0].y , Points[n - 1].x - Points[0].x ) / 3.1415926;

// }

return 0;

}

以上只能检测具有单个角点的折线，任何角点的折线都是递归的。想加速的可以自己加速。转化为迭代的方式实现。

//将折线拆多段

int OpenRadar::BreakPolyLine(vector& BreakedRadarRho,

vector& BreakedRadarTheta,

vector& SepRadarRho ,

vector&SepRadarTheta)

{

int rho = 0;

double theta = 0.0;

int X[1200] = {0};

int Y[1200] = {0};

int rhoCopy[1200] = {0};

double thetaCopy[1200] = {0};

int pointCnt = 0;

int lineCnt = 0;

int N = 0;

SepRadarRho.clear();

SepRadarTheta.clear();

Corners.clear();

//进行多次迭代，将所有的折线都拆分成直线段

vectorCornerIndex;

int CornerCnt = 0;

int tempIndex = 0;

for (int i = 0; i < static_cast(BreakedRadarRho.size());i++)

{

rho = BreakedRadarRho.at(i);

theta = BreakedRadarTheta.at(i);

if (rho < 0)

{

if (pointCnt > 200)//数目比较少的点直接抛弃

{

CornerIndex.clear();

CornerCnt = FindCorners(CornerIndex,X,Y,0,pointCnt,200);

if (CornerIndex.size() == 0)

{

for (int k = 0 ; k < pointCnt;k++)

{

SepRadarRho.push_back(rhoCopy[k]);

SepRadarTheta.push_back(thetaCopy[k]);

}

SepRadarRho.push_back(-1);

SepRadarTheta.push_back(1000.0);

lineCnt++;

}else

{

tempIndex = 0;

for (int k = 0 ; k < pointCnt;k++)

{

SepRadarRho.push_back(rhoCopy[k]);

SepRadarTheta.push_back(thetaCopy[k]);

if (k == CornerIndex.at(tempIndex))

{

SepRadarRho.push_back(-1);

SepRadarTheta.push_back(1000.0);

lineCnt++;

if (tempIndex < static_cast(CornerIndex.size()) -1)

{

tempIndex++;

}

}

}

SepRadarRho.push_back(-1);

SepRadarTheta.push_back(1000.0);

lineCnt++;

}

}

pointCnt = 0;

continue;

}

X[pointCnt] = static_cast(rho*cos(theta));

Y[pointCnt] = static_cast(rho*sin(theta));

rhoCopy[pointCnt] = rho;

thetaCopy[pointCnt] = theta;

pointCnt++;

}

//cout<

return lineCnt;

}

2、直线拟合

如果某区域不存在角点，并且点数据比较大，那么一般都是直线，(不是绝对，直线的判定方法后面的博客再写)

直线拟合的原理比较简单，实际就是一个最小二乘法，或者为了提高拟合的精度可以采用加权的最下二乘法，这里采用的是加权最小二乘。

//原始数据图 蓝点是雷达的位置

//拟合直线和角点图，粗点是角点，粗实线是拟合出的直线

由于只对点数比价多的直线进行了拟合，上部分的直线并为画出，实际上点数比较少的直线误差也会大，并不是关键的特征。

源码下载：http://download.csdn.net/detail/renshengrumenglibing/5076599