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;
- }*/
- }
51hei.png (6.64 KB, 下載次數: 90)
下載附件
2022-3-12 01:54 上傳
以上代碼:
OpenRadar7.0.zip
(25.96 KB, 下載次數: 13)
2022-3-3 11:09 上傳
點擊文件名下載附件
激光雷達數據處理之特征提取 下載積分: 黑幣 -5
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
