找回密碼
 立即注冊(cè)

QQ登錄

只需一步，快速開始

搜索
查看: 2514|回復(fù): 0
打印 上一主題 下一主題
收起左側(cè)

ROS中激光雷達(dá)數(shù)據(jù)處理之特征提取 源程序

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
   ROS中激光雷達(dá)的數(shù)據(jù)就是一串距離值，每隔1度一個(gè)距離值（具體情況得看激光雷達(dá)的參數(shù)），通過實(shí)測(cè)激光雷達(dá)的數(shù)據(jù)提取關(guān)鍵特征，直線，圓弧

  1. // 進(jìn)行多邊形擬合： Points : 輪廓上的點(diǎn)      n -- 輪廓點(diǎn)數(shù)目  Eps -- 擬合精度
  2. // 返回值： 若該輪廓段需要分段，則返回分段點(diǎn)在該輪廓點(diǎn)列中的索引，否則，返回 0 表示不需要分段
  3. // 這里是整個(gè)算法計(jì)算復(fù)雜性最大的一個(gè)地方
  4. // 為了提高程序運(yùn)行效率，對(duì)點(diǎn)到直線的距離計(jì)算進(jìn)行改進(jìn)：
  5. // 多邊形擬合中的直線是由點(diǎn)列中的點(diǎn)決定的
  6. // 為了計(jì)算點(diǎn)到直線的距離，
  7. // 采用坐標(biāo)系旋轉(zhuǎn)，將直線旋轉(zhuǎn)到x軸方向，這樣點(diǎn)到直線的距離即為各個(gè)點(diǎn)
  8. // 在坐標(biāo)旋轉(zhuǎn)后的y值的絕對(duì)值
  9. // 同時(shí)，坐標(biāo)旋轉(zhuǎn)矩陣在該次運(yùn)算中為定值，只需一次計(jì)算，不需要多次的開方或三角計(jì)算
  10. int OpenRadar::PolyContourFit( int* X, int* Y, int n , float Eps ) // 根據(jù)輪廓點(diǎn)，用多邊形擬合該輪廓點(diǎn)   
  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.         // 進(jìn)行坐標(biāo)旋轉(zhuǎn)，求旋轉(zhuǎn)后的點(diǎn)到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;

  45.     //    }
  46.     return 0;
  47. }

  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();

  68.     //進(jìn)行多次迭代，將所有的折線都拆分成直線段
  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);

  78.         if (rho < 0)
  79.         {
  80.             if (pointCnt > 200)//數(shù)目比較少的點(diǎn)直接拋棄
  81.             {
  82.                 CornerIndex.clear();
  83.                 CornerCnt = FindCorners(CornerIndex,X,Y,0,pointCnt,200);

  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. }

  132. //進(jìn)行直線擬合
  133. void OpenRadar::FitLine(vector<LinePara>& FittedLine,vector<int>& RadarRho,vector<double>& RadarTheta){

  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);

  147.         if (rho < 0)
  148.         {
  149.             if (pointCnt > 100 )//點(diǎn)數(shù)目足夠多的點(diǎn)列才進(jìn)行直線擬合
  150.             {
  151.                 WeightedFit(X ,Y ,pointCnt,&tmpLinePara);
  152.                 FittedLine.push_back(tmpLinePara);

  154.                 //存儲(chǔ)擬合的數(shù)據(jù)和擬合結(jié)果
  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);*/

  172.                 pointCnt = 0;
  173.                 continue;
  174.             }else {
  175.                 pointCnt = 0;
  176.                 continue;
  177.             }   
  178.         }

  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.     }*/

  193. }
復(fù)制代碼


以上代碼: OpenRadar7.0.zip (25.96 KB, 下載次數(shù): 13)
分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏 分享淘帖 頂頂 踩踩
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表