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

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

附件压缩包内容：

部分源码如下

// 进行多边形拟合： Points : 轮廓上的点      n -- 轮廓点数目  Eps -- 拟合精度
// 返回值： 若该轮廓段需要分段，则返回分段点在该轮廓点列中的索引，否则，返回 0 表示不需要分段
// 这里是整个算法计算复杂性最大的一个地方
// 为了提高程序运行效率，对点到直线的距离计算进行改进：
// 多边形拟合中的直线是由点列中的点决定的
// 为了计算点到直线的距离，
// 采用坐标系旋转，将直线旋转到x轴方向，这样点到直线的距离即为各个点
// 在坐标旋转后的y值的绝对值
// 同时，坐标旋转矩阵在该次运算中为定值，只需一次计算，不需要多次的开方或三角计算
int OpenRadar::PolyContourFit( int* X, int* Y, int n , float Eps ) // 根据轮廓点，用多边形拟合该轮廓点   
{
    double dis = sqrt((double)(((X[0] - X[n - 1])*(X[0] - X[n - 1])) +  
                     ((Y[0] - Y[n - 1])* (Y[0] - Y[n - 1]))));
    double cosTheta = (X[n- 1] - X[0]) / dis;
    double sinTheta = - ( Y[n- 1] - Y[0] )/dis;
    double MaxDis = 0;
    int i ;
    int MaxDisInd = -1;
    double dbDis;
    for(i = 1 ; i < n - 1 ; i++)
    {
        // 进行坐标旋转，求旋转后的点到x轴的距离
        dbDis = abs( (Y[i] - Y[0]) * cosTheta + (X[i] - X[0])* sinTheta);
        if( dbDis > MaxDis)
        {
            MaxDis = dbDis;
            MaxDisInd = i;
        }
    }
    if(MaxDis > Eps)
    {
        return MaxDisInd;
        //        cout << "Line 1 : " << endl;
        //        cout << "Start :" << Points[0].x << "  " << Points[0].y  << " --- " << Points[MaxDisInd].x << "  " << Points[MaxDisInd].y << endl;
        //        cout << "角度： "<<180 * atan2(Points[0].y - Points[MaxDisInd].y , Points[0].x - Points[MaxDisInd].x ) / 3.1415926;
        //        cout << "Line 2 :" << endl;
        //        cout << "Start :" << Points[MaxDisInd].x << "  " << Points[MaxDisInd].y  << " --- " << Points[n - 1].x << "  " << Points[n - 1].y << endl;
        //        cout << "角度： "<< 180 * atan2(Points[n - 1].y - Points[MaxDisInd].y , Points[n - 1].x - Points[MaxDisInd].x ) / 3.1415926;
    }
    //    else{
    //        cout << "Line 1 : " << endl;
    //        cout << "Start :" << Points[0].x << "  " << Points[0].y  << " --- " << Points[n - 1].x << "  " << Points[n - 1].y << endl;
    //        cout << "角度： "<<180 * atan2(Points[n - 1].y - Points[0].y , Points[n - 1].x - Points[0].x ) / 3.1415926;

    //    }
    return 0;
}

//将折线拆成两段

CV_IMPLEMENT_QSORT( IntQSort, int, cmp_pts )  // 该宏利用声明并定义函数IntQSort用于快速排序
int W[MAX_FITPOINTS_CNT];// =(int * )malloc(sizeof(int) * Cnt);// 权值系数        
int WeightedFit(int X[] , int Y[] , int Cnt , LinePara * EstLinePara)
{
    // 加权最小二乘法
    // Cnt: 数据点计数
    // EstLinePara : 直线拟合的估计值，可以利用最小二乘法计算得到
    // 利用最小二乘进行估计
    int * Tmp;
    int FlagFlip = 0;// 是否对X,Y进行翻转过
    //FitPara(X , Y , Cnt , EstLinePara , NULL);
    //if(abs(EstLinePara->a) > 1 || EstLinePara->a == NAN || EstLinePara->a == -NAN)
    if( abs(X[0] - X[Cnt - 1]) < abs(Y[0] - Y[Cnt - 1]) )
    {
        // 该段直线为斜率大于1
        // 沿45度线进行翻转
        // 将 X 和 Y 进行翻转
        Tmp = X;
        X = Y;
        Y = Tmp;
        FlagFlip = 1;  // 翻转
    }
    int i = 0;
    if(W == NULL)
        return -1;
    // 迭代20次
    for(i = 0 ; i < 20 ; i++)
    {
        // 计算权值
        CalW(X , Y ,Cnt , EstLinePara , W );
        FitPara(X,Y,Cnt,EstLinePara,W);// 根据权值拟合参数
    }
    //free(W);
   // EstLinePara->Dis = abs(EstLinePara->b)/(sqrt(EstLinePara->a * EstLinePara->a + EstLinePara->b * EstLinePara->b));
    if(FlagFlip == 0)
    {
        // 未翻转
        EstLinePara->Rho = atan(EstLinePara->a);
    }
    else
    {
        // 翻转过
        if(abs(EstLinePara->a) < 0.00001)
        {
            EstLinePara->a = 100000;
        }             
        else
        {
            EstLinePara->a =1.0/ EstLinePara->a;
        }         
        EstLinePara->b = - EstLinePara->b * (EstLinePara->a);
        EstLinePara->Rho = atan(EstLinePara->a);
    }

    //X Y若翻转过，再翻转回去
    if(FlagFlip == 1)
    {
        // 该段直线为斜率大于1
        // 沿45度线进行翻转
        // 将 X 和 Y 进行翻转
        Tmp = X;
        X = Y;
        Y = Tmp;
    }