ROS中激光雷達數據處理之特征提取 C++源碼

ROS中激光雷達的數據就是一串距離值，每隔1度一個距離值（具體情況得看激光雷達的參數），通過實測激光雷達的數據提取關鍵特征，直線，圓弧
OpenRadar7.0.zip (25.96 KB, 售價: 1 E幣)

2022-4-23 03:10 上傳

點擊文件名下載附件
售價: 1 E幣  [記錄]  [購買]

附件壓縮包內容：

部分源碼如下

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）指出，本站將立即改正。