直线加权拟合改进版

直线拟合算法

假设已知一系列点P（xi, yi） (i = 1...N),进行拟合，求出参数a,b

直接拟合公式

y = a*x + b

误差公式:ε = y - a*x - b

目标： J = ∑ (y - a*x -b)^2最小

极值条件：

dJ/da = ∑-2x(y - a*x - b) = 0;

dJ/db = ∑-2(y - ax - b) = 0;

化简：

∑y - a∑x - Nb = 0;

∑xy - a∑x^2 - b*∑x = 0;

代入∑x ∑y ∑xy ∑x^2,求出a b的值即可

加权拟合

未完待续

源码实现：

fitline.h

#ifndef FITLINE_H
#define FITLINE_H
#include"type.h"
#include<cmath>
int FitLine2D( Point2D32f * points, int _count, float *weights, float *line );
double CalcDist2D( Point2D32f * points, int count, float *_line, float *dist );
int FitLine2D(Point2D32f * points, int count, float *line);
void WeightL1( float *d, int count, float *w );
void WeightL12( float *d, int count, float *w );
void WeightHuber( float *d, int count, float *w, float _c );
void WeightFair( float *d, int count, float *w, float _c );
void WeightWelsch( float *d, int count, float *w, float _c );
double max(double a, double b);
#endif

fitline.cpp

#include"fitline.h"
const double eps = 1e-6;
int FitLine2D( Point2D32f * points, int _count, float *weights, float *line )
{
    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 += points[i].x;
            y += points[i].y;
            x2 += points[i].x * points[i].x;
            y2 += points[i].y * points[i].y;
            xy += points[i].x * points[i].y;
        }
        w = (float) count;
    }
    else
    {
        for( i = 0; i < count; i += 1 )
        {
            x += weights[i] * points[i].x;
            y += weights[i] * points[i].y;
            x2 += weights[i] * points[i].x * points[i].x;
            y2 += weights[i] * points[i].y * points[i].y;
            xy += weights[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;

    t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2;
    line[0] = (float) cos( t );
    line[1] = (float) sin( t );

    line[2] = (float) x;
    line[3] = (float) y;

    return 0;
}


double CalcDist2D( Point2D32f * points, int count, float *_line, float *dist )
{
    int j;
    float px = _line[2], py = _line[3];
    float nx = _line[1], ny = -_line[0];
    double sum_dist = 0.;

    for( j = 0; j < count; j++ )
    {
        float x, y;

        x = points[j].x - px;
        y = points[j].y - py;

        dist[j] = (float) fabs( nx * x + ny * y );
        sum_dist += dist[j];
    }

    return sum_dist;
}



int FitLine2D(Point2D32f * points, int count, float *line)
{
	//进行一遍拟合，获取直线参数
	FitLine2D(points, count, NULL, line);
	//计算权值，再进行一次拟合
	float *dist  = new float[count];
	float *W = new float[count];
	
	//迭代进行加权拟合，迭代次数不小于三次
	for(int i = 0; i < 5; i++)
	{
		CalcDist2D(points, count, line, dist);
	 	WeightL1( dist, count, W);
		FitLine2D(points, count, W, line);
	}
	delete [] dist;
}

void WeightL1( float *d, int count, float *w )
{
    int i;

    for( i = 0; i < count; i++ )
    {
        double t = fabs( (double) d[i] );
        w[i] = (float)(1. / max(t, eps));
    }
}

void WeightL12( float *d, int count, float *w )
{
    int i;

    for( i = 0; i < count; i++ )
    {
        w[i] = 1.0f / (float) sqrt( 1 + (double) (d[i] * d[i] * 0.5) );
    }
}


void WeightHuber( float *d, int count, float *w, float _c )
{
    int i;
    const float c = _c <= 0 ? 1.345f : _c;

    for( i = 0; i < count; i++ )
    {
        if( d[i] < c )
            w[i] = 1.0f;
        else
            w[i] = c/d[i];
    }
}


void WeightFair( float *d, int count, float *w, float _c )
{
    int i;
    const float c = _c == 0 ? 1 / 1.3998f : 1 / _c;

    for( i = 0; i < count; i++ )
    {
        w[i] = 1 / (1 + d[i] * c);
    }
}

void WeightWelsch( float *d, int count, float *w, float _c )
{
    int i;
    const float c = _c == 0 ? 1 / 2.9846f : 1 / _c;

    for( i = 0; i < count; i++ )
    {
        w[i] = (float) exp( -d[i] * d[i] * c * c );
    }
}
double max(double a, double b){

	return a > b ? a : b;
}

main.cpp

#include"fitline.h"
#include<cstdlib>
#include<cstdio>
#include<ctime>
const int COUNT = 50;
Point2D32f points[COUNT];
float line[4] = {0.0};//存储直线的参数
int main()
{
	//产生随机数
    srand((unsigned int)time(NULL));
    for(int i = 0 ; i < COUNT ; i++)
    {
        points[i].x = i;
        points[i].y = i * 2.3 + 45 + rand()%1000/1024.0;
        points[COUNT/2].x = 10;
        points[COUNT/3].y = 200;
    }
 
 
 	//直接拟合
   	FitLine2D(points, COUNT, NULL, line);
   	float a = line[1]/line[0];
	float b = line[3] - a*line[2];
	printf("a: %f  b%f\n", a, b);
   	
   	
	//加权拟合
	FitLine2D(points, COUNT, line);
	a = line[1]/line[0];
	b = line[3] - a*line[2];
	printf("a: %f  b%f\n", a, b);
	return 0;
}

type.h

#include<cstdlib>

typedef struct{
	float x;
	float y;
}Point2D32f;