加权拟合算法

我不是女神ヾ 2022-03-20 17:06 227阅读 0赞

#include<cmath>
    #include<cstdio>
    #include"QSort.h"
    #include<ctime>
    #include<cstdlib>
    #define K (4.685 / 0.6745)
    #define NO_ERR 1
    
    #define cmp_pts( x, y )   ( x < y ) 
    CV_IMPLEMENT_QSORT( floatQSort, float, cmp_pts )
    
    const int Count = 200;
    float X[Count], Y[Count];
    float Weights[Count];
    
    typedef struct 
    {
    	float theta;
    	float x;
    	float y;
    }LinePara;
    
    LinePara linePara;
    
    const double eps = 1e-6;
    
    int Med(float R[] , int Cnt)// cal median
    {
        //qsort(R , Cnt , sizeof(R[0]) , Cmp);
        floatQSort(R , Cnt , 0);
        return R[Cnt/2];
    }
    
    int CalW(float X[] , float Y[] , int Cnt , LinePara& linePara , float W[] )
    {
        int i = 0;
        float a = tan(linePara.theta);
        float b = linePara.y - tan(linePara.theta)*linePara.x;
        
        float Median = 0;
        float u;
        for(i = 0; i < Cnt ; i++)
        {
            W[i] = fabs(Y[i] - a * X[i] - b );
        }
        Median = Med(W , Cnt);
        
        Median = Median > 2 ? Median : 2;
    
        //cout<< "Median : " << Median <<endl;
        for(i = 0 ; i < Cnt ; i++)
        {
            u =  W[i]/(K * Median);
            //W[i] =(1 - u * u) * (1 - u * u);
            if(u < 1)
            {
                W[i] =(1 - u * u) * (1 - u * u);
            }
            else{
                W[i] = 0;
            }
        }
    
    //    for(int j = 0 ; j < Cnt/4; j++)
    //    {
    //        cout<< j <<" : " << W[4*j] << "  " << W[4*j + 1] << "  " << W[4*j + 2] << W[4*j + 3] << endl;
    //    }
    	return 0;
    }
    
    int FitLine2D( float* X, float* Y, int _count, float *weights, LinePara& linePara )
    {
        double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0;
        double dx2, dy2, dxy;
        int i;
        int count = _count;
        float t;
        /* Calculating the average of x and y... */
        if(weights == 0)
        {
            for( i = 0; i < count; i += 1 )
            {
                x += X[i];       //points[i].x;
                y += Y[i];       //points[i].y;
                x2 += X[i]*X[i]; //points[i].x * points[i].x;
                y2 += Y[i]*Y[i]; //points[i].y * points[i].y;
                xy += X[i]*Y[i]; //points[i].x * points[i].y;
            }
            w = (float) count;
        }
        else
        {
            for( i = 0; i < count; i += 1 )
            {
                x += weights[i] * X[i]; ///points[i].x;
                y += weights[i] * Y[i]; //points[i].y;
                x2 += weights[i] * X[i]*X[i]; //points[i].x * points[i].x;
                y2 += weights[i] * Y[i]*Y[i];//points[i].y * points[i].y;
                xy += weights[i] * X[i]*Y[i]; //points[i].x * points[i].y;
                w += weights[i];
            }
        }
        x /= w;
        y /= w;
        x2 /= w;
        y2 /= w;
        xy /= w;
    
        dx2 = x2 - x * x;
        dy2 = y2 - y * y;
        dxy = xy - x * y;
    
        linePara.theta = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2;
        linePara.x = (float) x;
        linePara.y = (float) y;
        return NO_ERR;
    }
    
    int main()
    {
    	//测试数据
        srand((unsigned int)time(NULL));
        rand();
        for(int i = 0 ; i < Count ; i++)
        {
            X[i] = i + rand()%20/32767.0;
            Y[i] = i * 2.3 + 45 + rand()%20/32767.0;
        } 
        X[20] = 2;
        Y[100] = 4;
        X[30] = 3;
    
        //加权拟合 迭代20次
        for(int i = 0; i < 5; i++)
        {
        	CalW(X, Y, Count, linePara, Weights);
        	for(int j = 0; j < Count; j++)
        	{
        		printf("%f ", Weights[j]);
        	}
        	FitLine2D(X, Y, Count, Weights, linePara);
        	printf("\na: %f  b:  %f\n", tan(linePara.theta), linePara.y- tan(linePara.theta)*linePara.x);
        }    
        
        //直接拟合
        FitLine2D(X, Y, Count, NULL, linePara);
    	printf("a: %f  b:  %f\n", tan(linePara.theta), linePara.y- tan(linePara.theta)*linePara.x);
        return 0;
    }

//QSort.h

#ifndef MY_QSORT_H
    #define MY_QSORT_H
    
    #define CV_SWAP(a,b,t) ((t) = (a), (a) = (b), (b) = (t))
    
    #ifndef MIN
    #define MIN(a,b)  ((a) > (b) ? (b) : (a))
    #endif
    
    #ifndef MAX
    #define MAX(a,b)  ((a) < (b) ? (b) : (a))
    #endif
    
    
    #define CV_IMPLEMENT_QSORT_EX( func_name, T, LT, user_data_type )                   \
    void func_name( T *array, size_t total, user_data_type aux )                        \
    {                                                                                   \
        int isort_thresh = 7;                                                           \
        T t;                                                                            \
        int sp = 0;                                                                     \
                                                                                        \
        struct                                                                          \
        {                                                                               \
            T *lb;                                                                      \
            T *ub;                                                                      \
        }                                                                               \
        stack[48];                                                                      \
                                                                                        \
        aux = aux;                                                                      \
                                                                                        \
        if( total <= 1 )                                                                \
            return;                                                                     \
                                                                                        \
        stack[0].lb = array;                                                            \
        stack[0].ub = array + (total - 1);                                              \
                                                                                        \
        while( sp >= 0 )                                                                \
        {                                                                               \
            T* left = stack[sp].lb;                                                     \
            T* right = stack[sp--].ub;                                                  \
                                                                                        \
            for(;;)                                                                     \
            {                                                                           \
                int i, n = (int)(right - left) + 1, m;                                  \
                T* ptr;                                                                 \
                T* ptr2;                                                                \
                                                                                        \
                if( n <= isort_thresh )                                                 \
                {                                                                       \
                insert_sort:                                                            \
                    for( ptr = left + 1; ptr <= right; ptr++ )                          \
                    {                                                                   \
                        for( ptr2 = ptr; ptr2 > left && LT(ptr2[0],ptr2[-1]); ptr2--)   \
                            CV_SWAP( ptr2[0], ptr2[-1], t );                            \
                    }                                                                   \
                    break;                                                              \
                }                                                                       \
                else                                                                    \
                {                                                                       \
                    T* left0;                                                           \
                    T* left1;                                                           \
                    T* right0;                                                          \
                    T* right1;                                                          \
                    T* pivot;                                                           \
                    T* a;                                                               \
                    T* b;                                                               \
                    T* c;                                                               \
                    int swap_cnt = 0;                                                   \
                                                                                        \
                    left0 = left;                                                       \
                    right0 = right;                                                     \
                    pivot = left + (n/2);                                               \
                                                                                        \
                    if( n > 40 )                                                        \
                    {                                                                   \
                        int d = n / 8;                                                  \
                        a = left, b = left + d, c = left + 2*d;                         \
                        left = LT(*a, *b) ? (LT(*b, *c) ? b : (LT(*a, *c) ? c : a))     \
                                          : (LT(*c, *b) ? b : (LT(*a, *c) ? a : c));    \
                                                                                        \
                        a = pivot - d, b = pivot, c = pivot + d;                        \
                        pivot = LT(*a, *b) ? (LT(*b, *c) ? b : (LT(*a, *c) ? c : a))    \
                                          : (LT(*c, *b) ? b : (LT(*a, *c) ? a : c));    \
                                                                                        \
                        a = right - 2*d, b = right - d, c = right;                      \
                        right = LT(*a, *b) ? (LT(*b, *c) ? b : (LT(*a, *c) ? c : a))    \
                                          : (LT(*c, *b) ? b : (LT(*a, *c) ? a : c));    \
                    }                                                                   \
                                                                                        \
                    a = left, b = pivot, c = right;                                     \
                    pivot = LT(*a, *b) ? (LT(*b, *c) ? b : (LT(*a, *c) ? c : a))        \
                                       : (LT(*c, *b) ? b : (LT(*a, *c) ? a : c));       \
                    if( pivot != left0 )                                                \
                    {                                                                   \
                        CV_SWAP( *pivot, *left0, t );                                   \
                        pivot = left0;                                                  \
                    }                                                                   \
                    left = left1 = left0 + 1;                                           \
                    right = right1 = right0;                                            \
                                                                                        \
                    for(;;)                                                             \
                    {                                                                   \
                        while( left <= right && !LT(*pivot, *left) )                    \
                        {                                                               \
                            if( !LT(*left, *pivot) )                                    \
                            {                                                           \
                                if( left > left1 )                                      \
                                    CV_SWAP( *left1, *left, t );                        \
                                swap_cnt = 1;                                           \
                                left1++;                                                \
                            }                                                           \
                            left++;                                                     \
                        }                                                               \
                                                                                        \
                        while( left <= right && !LT(*right, *pivot) )                   \
                        {                                                               \
                            if( !LT(*pivot, *right) )                                   \
                            {                                                           \
                                if( right < right1 )                                    \
                                    CV_SWAP( *right1, *right, t );                      \
                                swap_cnt = 1;                                           \
                                right1--;                                               \
                            }                                                           \
                            right--;                                                    \
                        }                                                               \
                                                                                        \
                        if( left > right )                                              \
                            break;                                                      \
                        CV_SWAP( *left, *right, t );                                    \
                        swap_cnt = 1;                                                   \
                        left++;                                                         \
                        right--;                                                        \
                    }                                                                   \
                                                                                        \
                    if( swap_cnt == 0 )                                                 \
                    {                                                                   \
                        left = left0, right = right0;                                   \
                        goto insert_sort;                                               \
                    }                                                                   \
                                                                                        \
                    n = MIN( (int)(left1 - left0), (int)(left - left1) );               \
                    for( i = 0; i < n; i++ )                                            \
                        CV_SWAP( left0[i], left[i-n], t );                              \
                                                                                        \
                    n = MIN( (int)(right0 - right1), (int)(right1 - right) );           \
                    for( i = 0; i < n; i++ )                                            \
                        CV_SWAP( left[i], right0[i-n+1], t );                           \
                    n = (int)(left - left1);                                            \
                    m = (int)(right1 - right);                                          \
                    if( n > 1 )                                                         \
                    {                                                                   \
                        if( m > 1 )                                                     \
                        {                                                               \
                            if( n > m )                                                 \
                            {                                                           \
                                stack[++sp].lb = left0;                                 \
                                stack[sp].ub = left0 + n - 1;                           \
                                left = right0 - m + 1, right = right0;                  \
                            }                                                           \
                            else                                                        \
                            {                                                           \
                                stack[++sp].lb = right0 - m + 1;                        \
                                stack[sp].ub = right0;                                  \
                                left = left0, right = left0 + n - 1;                    \
                            }                                                           \
                        }                                                               \
                        else                                                            \
                            left = left0, right = left0 + n - 1;                        \
                    }                                                                   \
                    else if( m > 1 )                                                    \
                        left = right0 - m + 1, right = right0;                          \
                    else                                                                \
                        break;                                                          \
                }                                                                       \
            }                                                                           \
        }                                                                               \
    }
    
    #define CV_IMPLEMENT_QSORT( func_name, T, cmp )  \
        CV_IMPLEMENT_QSORT_EX( func_name, T, cmp, int )
    
    
    #endif

//编译指令

g++ -o fitline fitline.cpp

执行指令：

./fitline