#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