直线拟合算法
假设已知一系列点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;
化简:
扫描二维码关注公众号,回复:
3296136 查看本文章
∑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;