ROS中激光雷达数据处理之特征提取 C++源码

ROS中激光雷达的数据就是一串距离值，每隔1度一个距离值（具体情况得看激光雷达的参数），通过实测激光雷达的数据提取关键特征，直线，圆弧
OpenRadar7.0.zip (25.96 KB, 售价: 1 E币)

2022-4-23 03:10 上传

点击文件名下载附件

附件压缩包内容：

部分源码如下

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. //将折线拆成两段

复制代码

1. CV_IMPLEMENT_QSORT( IntQSort, int, cmp_pts )  // 该宏利用声明并定义函数IntQSort用于快速排序
   
2. int W[MAX_FITPOINTS_CNT];// =(int * )malloc(sizeof(int) * Cnt);// 权值系数        
   
3. int WeightedFit(int X[] , int Y[] , int Cnt , LinePara * EstLinePara)
   
4. {
   
5.     // 加权最小二乘法
   
6.     // Cnt: 数据点计数
   
7.     // EstLinePara : 直线拟合的估计值，可以利用最小二乘法计算得到
   
8.     // 利用最小二乘进行估计
   
9.     int * Tmp;
   
10.     int FlagFlip = 0;// 是否对X,Y进行翻转过
    
11.     //FitPara(X , Y , Cnt , EstLinePara , NULL);
    
12.     //if(abs(EstLinePara->a) > 1 || EstLinePara->a == NAN || EstLinePara->a == -NAN)
    
13.     if( abs(X[0] - X[Cnt - 1]) < abs(Y[0] - Y[Cnt - 1]) )
    
14.     {
    
15.         // 该段直线为斜率大于1
    
16.         // 沿45度线进行翻转
    
17.         // 将 X 和 Y 进行翻转
    
18.         Tmp = X;
    
19.         X = Y;
    
20.         Y = Tmp;
    
21.         FlagFlip = 1;  // 翻转
    
22.     }
    
23.     int i = 0;
    
24.     if(W == NULL)
    
25.         return -1;
    
26.     // 迭代20次
    
27.     for(i = 0 ; i < 20 ; i++)
    
28.     {
    
29.         // 计算权值
    
30.         CalW(X , Y ,Cnt , EstLinePara , W );
    
31.         FitPara(X,Y,Cnt,EstLinePara,W);// 根据权值拟合参数
    
32.     }
    
33.     //free(W);
    
34.    // EstLinePara->Dis = abs(EstLinePara->b)/(sqrt(EstLinePara->a * EstLinePara->a + EstLinePara->b * EstLinePara->b));
    
35.     if(FlagFlip == 0)
    
36.     {
    
37.         // 未翻转
    
38.         EstLinePara->Rho = atan(EstLinePara->a);
    
39.     }
    
40.     else
    
41.     {
    
42.         // 翻转过
    
43.         if(abs(EstLinePara->a) < 0.00001)
    
44.         {
    
45.             EstLinePara->a = 100000;
    
46.         }            
    
47.         else
    
48.         {
    
49.             EstLinePara->a =1.0/ EstLinePara->a;
    
50.         }         
    
51.         EstLinePara->b = - EstLinePara->b * (EstLinePara->a);
    
52.         EstLinePara->Rho = atan(EstLinePara->a);
    
53.     }
    
54. 
    
55.     //X Y若翻转过，再翻转回去
    
56.     if(FlagFlip == 1)
    
57.     {
    
58.         // 该段直线为斜率大于1
    
59.         // 沿45度线进行翻转
    
60.         // 将 X 和 Y 进行翻转
    
61.         Tmp = X;
    
62.         X = Y;
    
63.         Y = Tmp;
    
64.     }

复制代码

【必读】版权免责声明

1、本主题所有言论和内容纯属会员个人意见，与本论坛立场无关。2、本站对所发内容真实性、客观性、可用性不做任何保证也不负任何责任，网友之间仅出于学习目的进行交流。3、对提供的数字内容不拥有任何权利，其版权归原著者拥有。请勿将该数字内容进行商业交易、转载等行为，该内容只为学习所提供，使用后发生的一切问题与本站无关。 4、本网站不保证本站提供的下载资源的准确性、安全性和完整性;同时本网站也不承担用户因使用这些下载资源对自己和他人造成任何形式的损失或伤害。 5、本网站所有软件和资料均为网友推荐收集整理而来，仅供学习用途使用，请务必下载后两小时内删除，禁止商用。6、如有侵犯你版权的，请及时联系我们（电子邮箱1370723259@qq.com）指出，本站将立即改正。