ROS中激光雷达数据处理之特征提取源程序 -

// 进行多边形拟合： 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 - 1]))));
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<int>& BreakedRadarRho,
vector<double>& BreakedRadarTheta,
vector<int>& SepRadarRho ,
vector<double>&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();
//进行多次迭代，将所有的折线都拆分成直线段
vector<int>CornerIndex;
int CornerCnt = 0;
int tempIndex = 0;
for (int i = 0; i < static_cast<int>(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<int>(CornerIndex.size()) -1)
{
tempIndex++;
}
}
}
SepRadarRho.push_back(-1);
SepRadarTheta.push_back(1000.0);
lineCnt++;
}
}
pointCnt = 0;
continue;
}
X[pointCnt] = static_cast<int>(rho*cos(theta));
Y[pointCnt] = static_cast<int>(rho*sin(theta));
rhoCopy[pointCnt] = rho;
thetaCopy[pointCnt] = theta;
pointCnt++;
}
//cout<<"lineCnt: "<<lineCnt<<endl;
return lineCnt;
}
//进行直线拟合
void OpenRadar::FitLine(vector<LinePara>& FittedLine,vector<int>& RadarRho,vector<double>& RadarTheta){
int rho = 0;
double theta = 0.0;
int X[1200] = {0};
int Y[1200] = {0};
int pointCnt = 0;
LinePara tmpLinePara;
FittedLine.clear();
for (int i = 0 ; i < static_cast<int>(RadarRho.size());i++)
{
rho = RadarRho.at(i);
theta = RadarTheta.at(i);
if (rho < 0)
{
if (pointCnt > 100 )//点数目足够多的点列才进行直线拟合
{
WeightedFit(X ,Y ,pointCnt,&tmpLinePara);
FittedLine.push_back(tmpLinePara);
//存储拟合的数据和拟合结果
/* FILE* pF = fopen("line.txt","w");
fprintf(pF,"[a]\n");
fprintf(pF,"%f\n",tmpLinePara.a);
fprintf(pF,"[b]\n");
fprintf(pF,"%f\n",tmpLinePara.b);
fprintf(pF,"[x]\n");
for (int j = 0; j < pointCnt ;j++)
{
fprintf(pF,"%d,",X[j]);
}
fprintf(pF,"\n[y]\n");
for (int j = 0; j < pointCnt ;j++)
{
fprintf(pF,"%d,",Y[j]);
}
fclose(pF);*/
pointCnt = 0;
continue;
}else {
pointCnt = 0;
continue;
}
}
X[pointCnt] = static_cast<int>(rho*cos(theta));
Y[pointCnt] = static_cast<int>(rho*sin(theta));
pointCnt++;
}
/* for (int i = 0; i < FittedLine.size();i++)
{
cout<<"a: "<<FittedLine.at(i).a<<" b: "<<FittedLine.at(i).b<<" ";
cout<<"x1: "<<FittedLine.at(i).startPoint.x<<" "
<<"y1: "<<FittedLine.at(i).startPoint.y<<" "
<<"x1: "<<FittedLine.at(i).endPoint.x<<" "
<<"y1: "<<FittedLine.at(i).endPoint.y<<endl;
}*/
}

复制代码