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

   ROS中激光雷达的数据就是一串距离值，每隔1度一个距离值（具体情况得看激光雷达的参数），通过实测激光雷达的数据提取关键特征，直线，圆弧

1. // 进行多边形拟合： Points : 轮廓上的点      n -- 轮廓点数目  Eps -- 拟合精度
   
2. // 返回值： 若该轮廓段需要分段，则返回分段点在该轮廓点列中的索引，否则，返回 0 表示不需要分段
   
3. // 这里是整个算法计算复杂性最大的一个地方
   
4. // 为了提高程序运行效率，对点到直线的距离计算进行改进：
   
5. // 多边形拟合中的直线是由点列中的点决定的
   
6. // 为了计算点到直线的距离，
   
7. // 采用坐标系旋转，将直线旋转到x轴方向，这样点到直线的距离即为各个点
   
8. // 在坐标旋转后的y值的绝对值
   
9. // 同时，坐标旋转矩阵在该次运算中为定值，只需一次计算，不需要多次的开方或三角计算
   
10. int OpenRadar::PolyContourFit( int* X, int* Y, int n , float Eps ) // 根据轮廓点，用多边形拟合该轮廓点   
    
11. {
    
12.     double dis = sqrt((double)(((X[0] - X[n - 1])*(X[0] - X[n - 1])) +  
    
13.                      ((Y[0] - Y[n - 1])* (Y[0] - Y[n - 1]))));
    
14.     double cosTheta = (X[n- 1] - X[0]) / dis;
    
15.     double sinTheta = - ( Y[n- 1] - Y[0] )/dis;
    
16.     double MaxDis = 0;
    
17.     int i ;
    
18.     int MaxDisInd = -1;
    
19.     double dbDis;
    
20.     for(i = 1 ; i < n - 1 ; i++)
    
21.     {
    
22.         // 进行坐标旋转，求旋转后的点到x轴的距离
    
23.         dbDis = abs( (Y[i] - Y[0]) * cosTheta + (X[i] - X[0])* sinTheta);
    
24.         if( dbDis > MaxDis)
    
25.         {
    
26.             MaxDis = dbDis;
    
27.             MaxDisInd = i;
    
28.         }
    
29.     }
    
30.     if(MaxDis > Eps)
    
31.     {
    
32.         return MaxDisInd;
    
33.         //        cout << "Line 1 : " << endl;
    
34.         //        cout << "Start :" << Points[0].x << "  " << Points[0].y  << " --- " << Points[MaxDisInd].x << "  " << Points[MaxDisInd].y << endl;
    
35.         //        cout << "角度： "<<180 * atan2(Points[0].y - Points[MaxDisInd].y , Points[0].x - Points[MaxDisInd].x ) / 3.1415926;
    
36.         //        cout << "Line 2 :" << endl;
    
37.         //        cout << "Start :" << Points[MaxDisInd].x << "  " << Points[MaxDisInd].y  << " --- " << Points[n - 1].x << "  " << Points[n - 1].y << endl;
    
38.         //        cout << "角度： "<< 180 * atan2(Points[n - 1].y - Points[MaxDisInd].y , Points[n - 1].x - Points[MaxDisInd].x ) / 3.1415926;
    
39.     }
    
40.     //    else{
    
41.     //        cout << "Line 1 : " << endl;
    
42.     //        cout << "Start :" << Points[0].x << "  " << Points[0].y  << " --- " << Points[n - 1].x << "  " << Points[n - 1].y << endl;
    
43.     //        cout << "角度： "<<180 * atan2(Points[n - 1].y - Points[0].y , Points[n - 1].x - Points[0].x ) / 3.1415926;
    
44. 
    
45.     //    }
    
46.     return 0;
    
47. }
    
48. 
    
49. //将折线拆成两段
    
50. int OpenRadar::BreakPolyLine(vector<int>& BreakedRadarRho,
    
51.                              vector<double>& BreakedRadarTheta,
    
52.                              vector<int>& SepRadarRho ,   
    
53.                              vector<double>&SepRadarTheta)
    
54. {
    
55.     int rho = 0;
    
56.     double theta = 0.0;
    
57.     int X[1200] = {0};
    
58.     int Y[1200] = {0};
    
59.     int rhoCopy[1200] = {0};
    
60.     double thetaCopy[1200] = {0};
    
61.     int pointCnt = 0;
    
62.     int lineCnt = 0;
    
63.     int N = 0;
    
64.     SepRadarRho.clear();
    
65.     SepRadarTheta.clear();
    
66.     Corners.clear();
    
67. 
    
68.     //进行多次迭代，将所有的折线都拆分成直线段
    
69.    
    
70.    vector<int>CornerIndex;
    
71.    int CornerCnt = 0;
    
72.    int tempIndex = 0;
    
73.     for (int i = 0; i < static_cast<int>(BreakedRadarRho.size());i++)
    
74.     {
    
75.         rho   = BreakedRadarRho.at(i);
    
76.         theta = BreakedRadarTheta.at(i);
    
77. 
    
78.         if (rho < 0)
    
79.         {
    
80.             if (pointCnt > 200)//数目比较少的点直接抛弃
    
81.             {
    
82.                 CornerIndex.clear();
    
83.                 CornerCnt = FindCorners(CornerIndex,X,Y,0,pointCnt,200);
    
84. 
    
85.                 if (CornerIndex.size() == 0)
    
86.                 {
    
87.                     for (int k = 0 ; k < pointCnt;k++)
    
88.                     {
    
89.                         SepRadarRho.push_back(rhoCopy[k]);
    
90.                         SepRadarTheta.push_back(thetaCopy[k]);
    
91.                     }
    
92.                     SepRadarRho.push_back(-1);
    
93.                     SepRadarTheta.push_back(1000.0);
    
94.                     lineCnt++;
    
95.                 }else
    
96.                 {
    
97.                     tempIndex = 0;
    
98.                     for (int k = 0 ; k < pointCnt;k++)
    
99.                     {
    
100.                         SepRadarRho.push_back(rhoCopy[k]);
     
101.                         SepRadarTheta.push_back(thetaCopy[k]);
     
102.                         if (k == CornerIndex.at(tempIndex))
     
103.                         {
     
104.                             SepRadarRho.push_back(-1);
     
105.                             SepRadarTheta.push_back(1000.0);
     
106.                             lineCnt++;
     
107.                             if (tempIndex < static_cast<int>(CornerIndex.size()) -1)
     
108.                             {
     
109.                                 tempIndex++;
     
110.                             }  
     
111.                         }
     
112.                     }
     
113.                     SepRadarRho.push_back(-1);
     
114.                     SepRadarTheta.push_back(1000.0);
     
115.                     lineCnt++;
     
116.                 }
     
117.             }
     
118.             pointCnt = 0;
     
119.             continue;
     
120.         }
     
121.         X[pointCnt] = static_cast<int>(rho*cos(theta));
     
122.         Y[pointCnt] = static_cast<int>(rho*sin(theta));
     
123.         rhoCopy[pointCnt]   = rho;
     
124.         thetaCopy[pointCnt] = theta;
     
125.         pointCnt++;
     
126.     }
     
127.    
     
128.     //cout<<"lineCnt: "<<lineCnt<<endl;
     
129.     return lineCnt;
     
130. }
     
131. 
     
132. //进行直线拟合
     
133. void OpenRadar::FitLine(vector<LinePara>& FittedLine,vector<int>& RadarRho,vector<double>& RadarTheta){
     
134. 
     
135.     int rho = 0;
     
136.     double theta = 0.0;
     
137.     int X[1200] = {0};
     
138.     int Y[1200] = {0};
     
139.     int pointCnt = 0;
     
140.     LinePara tmpLinePara;
     
141.     FittedLine.clear();
     
142.     for (int i = 0 ; i < static_cast<int>(RadarRho.size());i++)
     
143.     {
     
144.         rho = RadarRho.at(i);
     
145.         theta = RadarTheta.at(i);
     
146. 
     
147.         if (rho < 0)
     
148.         {
     
149.             if (pointCnt > 100 )//点数目足够多的点列才进行直线拟合
     
150.             {
     
151.                 WeightedFit(X ,Y ,pointCnt,&tmpLinePara);
     
152.                 FittedLine.push_back(tmpLinePara);
     
153. 
     
154.                 //存储拟合的数据和拟合结果
     
155.        /*         FILE* pF = fopen("line.txt","w");
     
156.                 fprintf(pF,"[a]\n");
     
157.                 fprintf(pF,"%f\n",tmpLinePara.a);
     
158.                 fprintf(pF,"[b]\n");
     
159.                 fprintf(pF,"%f\n",tmpLinePara.b);
     
160.                 fprintf(pF,"[x]\n");
     
161.                 for (int j = 0; j < pointCnt ;j++)
     
162.                 {
     
163.                     fprintf(pF,"%d,",X[j]);
     
164.                 }
     
165.                 fprintf(pF,"\n[y]\n");
     
166.                 for (int j = 0; j < pointCnt ;j++)
     
167.                 {
     
168.                     fprintf(pF,"%d,",Y[j]);
     
169.                 }
     
170.                 fclose(pF);*/
     
171. 
     
172.                 pointCnt = 0;
     
173.                 continue;
     
174.             }else {
     
175.                 pointCnt = 0;
     
176.                 continue;
     
177.             }   
     
178.         }
     
179. 
     
180.         X[pointCnt] = static_cast<int>(rho*cos(theta));
     
181.         Y[pointCnt] = static_cast<int>(rho*sin(theta));
     
182.         pointCnt++;
     
183.     }
     
184.    /* for (int i = 0; i < FittedLine.size();i++)
     
185.     {
     
186.         cout<<"a: "<<FittedLine.at(i).a<<"  b: "<<FittedLine.at(i).b<<" ";
     
187.         cout<<"x1: "<<FittedLine.at(i).startPoint.x<<" "
     
188.             <<"y1: "<<FittedLine.at(i).startPoint.y<<" "
     
189.             <<"x1: "<<FittedLine.at(i).endPoint.x<<" "
     
190.             <<"y1: "<<FittedLine.at(i).endPoint.y<<endl;
     
191.     }*/
     
192. 
     
193. }
     

复制代码

以上代码: OpenRadar7.0.zip (25.96 KB, 下载次数: 13)

2022-3-3 11:09 上传

点击文件名下载附件
激光雷达数据处理之特征提取