找回密码
 注册

QQ登录

只需一步,快速开始

搜索

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

[复制链接]
Jonsa 发表于 2022-4-23 03:12:11 | 显示全部楼层 |阅读模式
ROS中激光雷达的数据就是一串距离值,每隔1度一个距离值(具体情况得看激光雷达的参数),通过实测激光雷达的数据提取关键特征,直线,圆弧
OpenRadar7.0.zip (25.96 KB, 售价: 1 E币)
附件压缩包内容:
1.png
部分源码如下
  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.     return 0;
  46. }

  47. //将折线拆成两段
复制代码
  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.     //X Y若翻转过,再翻转回去
  55.     if(FlagFlip == 1)
  56.     {
  57.         // 该段直线为斜率大于1
  58.         // 沿45度线进行翻转
  59.         // 将 X 和 Y 进行翻转
  60.         Tmp = X;
  61.         X = Y;
  62.         Y = Tmp;
  63.     }
复制代码
您需要登录后才可以回帖 登录 | 注册

本版积分规则

QQ|手机版|小黑屋|ELEOK |网站地图

GMT+8, 2024-4-12 19:41

Powered by Discuz! X3.5

© 2001-2024 Discuz! Team.

快速回复 返回顶部 返回列表