直线拟合算法(续:加权最小二乘)
在此之前,我写过两篇文章介绍直线拟合算法:
https://blog.csdn.net/liyuanbhu/article/details/50866802
https://blog.csdn.net/liyuanbhu/article/details/51137038
这两篇文章中的算法都有一个不足,就是离群点对计算结果干扰较大。因为考察的是各个点到直线的距离的平方和最小,这个平方运算会把远处的点的作用给放大了。
更好的计算方法是加权最小二乘法,离直线远的点可以权值取得小一点。通常权值可以取为:
w ( d ) = { 1 , d < τ d τ , d ≥ τ w(d) = \begin{cases}1, & d < \tau \cr \frac{d}{\tau}, & d \geq \tau \end{cases} w(d)={1,τd,d<τd≥τ
这里 d d d 是点到直线的距离, τ \tau τ 是我们人为设定的一个距离阈值。这里有个问题,我们没有计算出直线之前,是不知道每个点到直线的距离的。解决这个问题的方法就是采用迭代法。先不考虑加权,算出一个直线方程,然后再用加权法重新计算。每次重新计算一遍直线方程,获得的直线方程就更准确。多迭代几次得到的直线就会稳定到真实的直线上了。当然,关于这个算法的收敛性其实是需要证明的,我们就默认为它能够收敛吧。
下面推导在这个条件下的算法。
直线方程还是使用经典形式:
a
x
+
b
y
+
c
=
0
ax + by + c = 0
ax+by+c=0。这里
a
,
b
a,b
a,b 有约束条件
a
2
+
b
2
=
1
a^2 + b^2 = 1
a2+b2=1。
待求极值的函数如下(这里还是加上了拉格朗日乘子):
D
2
=
∑
i
=
0
N
w
i
2
(
a
x
i
+
b
y
i
+
c
)
2
−
λ
(
a
2
+
b
2
−
1
)
D^2 = \sum_{i=0}^{N} w_i^2(a x_{i} + b y_{i} + c)^2 - \lambda (a^2 + b^2 - 1)
D2=i=0∑Nwi2(axi+byi+c)2−λ(a2+b2−1)
对 c 求导可以得到:
∂ D 2 ∂ c = 2 ∑ i = 0 N w i 2 ( a x i + b y i + c ) = 0 \frac{\partial D^2}{\partial c} = 2 \sum_{i=0}^{N} w_i^2( a x_{i} + b y_{i} + c) = 0 ∂c∂D2=2i=0∑Nwi2(axi+byi+c)=0
所以:
c = − a ∑ w i 2 x i − b ∑ w i 2 y i ∑ w i 2 = − a x ‾ − b y ‾ c = \frac{-a \sum w_i^2 x_i - b \sum w_i^2 y_i} {\sum w_i^2} = -a \overline{x} - b \overline{y} c=∑wi2−a∑wi2xi−b∑wi2yi=−ax−by
这里
x
‾
,
y
‾
\overline{x} , \overline{y}
x,y 是加权平均值:
x
‾
=
∑
w
i
2
x
i
∑
w
i
2
y
‾
=
∑
w
i
2
y
i
∑
w
i
2
\overline{x} = \frac{\sum w_i^2 x_i} {\sum w_i^2} \\ \overline{y} = \frac{\sum w_i^2 y_i} {\sum w_i^2}
x=∑wi2∑wi2xiy=∑wi2∑wi2yi
设:
x
i
′
=
x
i
−
x
‾
y
i
′
=
y
i
−
y
‾
x'_i = x_i - \overline{x} \\ y'_i = y_i - \overline{y}
xi′=xi−xyi′=yi−y
D 2 = ∑ i = 0 N w i 2 ( a x i ′ + b y i ′ ) 2 − λ ( a 2 + b 2 − 1 ) D^2 = \sum_{i=0}^{N} w_i^2(a x'_{i} + b y'_{i})^2 - \lambda (a^2 + b^2 - 1) D2=i=0∑Nwi2(axi′+byi′)2−λ(a2+b2−1)
∂ D 2 ∂ a = 2 ∑ i = 0 N w i 2 x i ′ ( a x i ′ + b y i ′ ) − 2 λ a = 0 ∂ D 2 ∂ b = 2 ∑ i = 0 N w i 2 y i ′ ( a x i ′ + b y i ′ ) − 2 λ b = 0 \frac{\partial D^2}{\partial a} = 2 \sum_{i=0}^{N} w_i^2 x'_{i}(a x'_{i} + b y'_{i}) - 2 \lambda a = 0 \\ \frac{\partial D^2}{\partial b} = 2 \sum_{i=0}^{N} w_i^2 y'_{i}(a x'_{i} + b y'_{i}) - 2 \lambda b = 0 ∂a∂D2=2i=0∑Nwi2xi′(axi′+byi′)−2λa=0∂b∂D2=2i=0∑Nwi2yi′(axi′+byi′)−2λb=0
化简可得:
a
∑
w
i
2
x
i
′
2
+
b
∑
w
i
2
x
i
′
y
i
′
=
λ
a
a
∑
w
i
2
x
i
′
y
i
′
+
b
∑
w
i
2
y
i
′
2
=
λ
b
a \sum{w_i^2 {x'_i}^2} + b \sum{ w_i^2 x'_{i} {y'_i}} = \lambda a \\ a \sum{w_i^2 {x'_i} {y'_i}} + b \sum{w_i^2 {y'_i}^2} = \lambda b
a∑wi2xi′2+b∑wi2xi′yi′=λaa∑wi2xi′yi′+b∑wi2yi′2=λb
设 D x x = ∑ w i 2 x i ′ 2 , D x y = ∑ w i 2 x i ′ y i ′ , D y y = ∑ w i 2 y i ′ 2 D_{xx} = \sum{w_i^2 {x'_{i}}^2}, D_{xy} = \sum{w_i^2 x'_{i} y'_i}, D_{yy} = \sum{w_i^2 {y'_i}^2} Dxx=∑wi2xi′2,Dxy=∑wi2xi′yi′,Dyy=∑wi2yi′2
上面方程可以写为:
D
x
x
a
+
D
x
y
b
=
λ
a
D
x
y
a
+
D
y
y
b
=
λ
b
D_{xx} a + D_{xy} b = \lambda a \\ D_{xy} a + D_{yy} b = \lambda b
Dxxa+Dxyb=λaDxya+Dyyb=λb
这个问题至此就转化成一个特征值问题了。 ( a , b ) (a, b) (a,b) 是这个方程的特征向量时这个方程才有解。
对于这个方程有两个特征值,两个特征向量。我们应该选哪个特征向量呢?
我们应该让 ∑ i = 0 N w i 2 ( a x i ′ + b y i ′ ) 2 \sum_{i=0}^{N} w_i^2(a x'_{i} + b y'_{i})^2 ∑i=0Nwi2(axi′+byi′)2 最小。
∑ i = 0 N w i 2 ( a x i ′ + b y i ′ ) 2 = ( a b ) ( D x x D x y D x y D y y ) ( a b ) = λ \sum_{i=0}^{N}w_i^2 (a x'_{i} + b y'_{i})^2 = \left(\begin{matrix} a & b \end{matrix} \right) \left(\begin{matrix} D_{xx} & D_{xy} \\ D_{xy} & D_{yy} \end{matrix} \right) \left(\begin{matrix} a \\ b \end{matrix} \right) = \lambda i=0∑Nwi2(axi′+byi′)2=(ab)(DxxDxyDxyDyy)(ab)=λ
所以我们应该选特征值比较小的那个特征向量。
下面给个C++ 的实现代码:
inline void mean_xy(const QVector<QPointF> &line, double &mean_x, double &mean_y)
{
int size = line.size();
mean_x = 0.0;
mean_y = 0.0;
for(int i = 0; i < size; i++)
{
mean_x += line[i].x();
mean_y += line[i].y();
}
mean_x /= size;
mean_y /= size; //至此,计算出了 x y 的均值
}
inline void weightedMean_xy(const QVector<QPointF> &line,
double &mean_x, double &mean_y,
double tau, double a, double b, double c)
{
int size = line.size();
mean_x = 0.0;
mean_y = 0.0;
double sum_wx = 0, sum_wy = 0, sum_w = 0;
for(int i = 0; i < size; i++)
{
double x = line[i].x(), y = line[i].y();
double d = abs(a * x + b * y + c);
double w = d < tau ? 1 : tau / d;
sum_wx += w * w * x;
sum_wy += w * w * y;
sum_w += w * w;
}
mean_x = sum_wx / sum_w;
mean_y = sum_wy / sum_w; //至此,计算出了 x y 的均值
}
inline void Dxxyyxy(const QVector<QPointF> &line,
double mx, double my,
double &Dxx, double &Dxy, double &Dyy)
{
int size = line.size();
for(int i = 0; i < size; i++)
{
double x = line[i].x();
double y = line[i].y();
Dxx += (x - mx) * (x - mx);
Dxy += (x - mx) * (y - my);
Dyy += (y - my) * (y - my);
}
}
inline void weightedDxxxyyy(const QVector<QPointF> &line,
double mx, double my,
double &Dxx, double &Dxy, double &Dyy,
double tau, double a, double b, double c)
{
int size = line.size();
for(int i = 0; i < size; i++)
{
double x = line[i].x(), y = line[i].y();
double d = abs(a * x + b * y + c);
double w = d < tau ? 1 : tau / d;
w = w * w;
Dxx += w * (x - mx) * (x - mx);
Dxy += w * (x - mx) * (y - my);
Dyy += w * (y - my) * (y - my);
}
}
/**
* @brief lineFit 最小二乘法拟合 ax + by + c = 0 型直线.
* 参数 a 和 b 满足 a^2 + b^2 = 1。拟合的判据是点到直线的垂直距离平方和最小。不是通常意义下的一元线性回归。
* @param points 点的坐标
* @param [out] a 输出拟合系数
* @param [out] b 输出拟合系数
* @param [out] c 输出拟合系数
*/
bool lineFit(const QVector<QPointF> &points, double &a, double &b, double &c)
{
int size = points.size();
if(size == 0)
{
a = 0;
b = 0;
c = 0;
return false;
}
double x_mean = 0;
double y_mean = 0;
mean_xy(points, x_mean, y_mean);
double Dxx = 0, Dxy = 0, Dyy = 0;
Dxxyyxy(points, x_mean, y_mean, Dxx, Dxy, Dyy);
double lambda = ( (Dxx + Dyy) - sqrt( (Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy) ) / 2.0;
double den = sqrt( Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx) );
if(fabs(den) < 1e-5)
{
if( fabs(Dxx / Dyy - 1) < 1e-5) //这时没有一个特殊的直线方向,无法拟合
{
return false;
}
else
{
a = 1;
b = 0;
c = - x_mean;
}
}
else
{
a = Dxy / den;
b = (lambda - Dxx) / den;
c = - a * x_mean - b * y_mean;
}
return true;
}
bool weightedLineFitOneStep(const QVector<QPointF> &line, double &a, double &b, double &c, double tau)
{
int size = line.size();
if(size == 0)
{
a = 0;
b = 0;
c = 0;
return false;
}
double x_mean = 0;
double y_mean = 0;
weightedMean_xy(line, x_mean, y_mean, tau, a, b, c);
double Dxx = 0, Dxy = 0, Dyy = 0;
weightedDxxxyyy(line, x_mean, y_mean, Dxx, Dxy, Dyy, tau, a, b, c);
double lambda = ( (Dxx + Dyy) - sqrt( (Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy) ) / 2.0;
double den = sqrt( Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx) );
if(fabs(den) < 1e-5)
{
if( fabs(Dxx / Dyy - 1) < 1e-5) //这时没有一个特殊的直线方向,无法拟合
{
return false;
}
else
{
a = 1;
b = 0;
c = - a * x_mean - b * y_mean;
}
}
else
{
a = Dxy / den;
b = (lambda - Dxx) / den;
c = - a * x_mean - b * y_mean;
}
return true;
}
bool weightedLineFit(const QVector<QPointF> &line, double &a, double &b, double &c, double tau, int N)
{
bool ret;
ret = lineFit(line, a, b, c);
if( !ret ) return false;
for(int i = 0; i < N; i++)
{
ret = weightedLineFitOneStep(line, a, b, c, tau);
if( !ret ) return false;
}
return true;
}
下面是个测试的例子:
原始图像如下:
可以看出在直线之外有一个比较大的干扰图形。
两种你拟合算法得到的结果如下:
红色的线是直接做直线拟合的结果,绿色的是加权直线拟合的结果。可以看出加权拟合结果好很多。