ROS中激光雷達數據處理之特征提取源程序

ID:393178 · 發表于 2022-3-3 11:10

ROS中激光雷達的數據就是一串距離值，每隔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 - 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;
}*/
}

復制代碼

以上代碼:

OpenRadar7.0.zip (25.96 KB, 下載次數: 13)

帳號		自動登錄	找回密碼
密碼			立即注冊

wwwxxx国产_337p日本欧洲亚洲大胆张筱雨_免费在线看成人av_日本黄色不卡视频_国产精品成熟老女人_99视频一区_亚洲精品97久久中文字幕_免费精品视频在线_亚洲色图欧美视频_欧美一区二三区

ROS中激光雷達數據處理之特征提取 源程序

ROS中激光雷達數據處理之特征提取源程序