激光雷达获得的信息是与周围物体的距离信息，广泛应用于移动机器人领域，特别是独立移动机器人。让我们从移动机器人的独立导航开始。
移动机器人导航是指移动机器人根据最佳时间、最短路径或最低能耗标准，在特定环境中实现从起始位置到目标位置的无碰撞运动。
传统的移动机器人导航问题包括地图创建、定位和运动控制三个基本问题：我在哪里？我要去哪里？如何去？
机器人定位的目的是回答我在哪里？这个基本问题。
激光雷达采集数据后，可以通过一定的操作准确计算当前的绝对位置或相对位置，从而知道机器人在哪里。当然，有时地图的创建和定位是同步的，例如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