直线拟合算法（续：加权最小二乘）

直线拟合算法（续：加权最小二乘）

在此之前，我写过两篇文章介绍直线拟合算法：
https://blog.csdn.net/liyuanbhu/article/details/50866802
https://blog.csdn.net/liyuanbhu/article/details/51137038

这两篇文章中的算法都有一个不足，就是离群点对计算结果干扰较大。因为考察的是各个点到直线的距离的平方和最小，这个平方运算会把远处的点的作用给放大了。

更好的计算方法是加权最小二乘法，离直线远的点可以权值取得小一点。通常权值可以取为：

$$w(d)=\{\begin{array}{ll}1,& d<\tau \\ \frac{d}{\tau },& d\ge \tau \end{array}$$

这里 $d$ 是点到直线的距离，$\tau$ 是我们人为设定的一个距离阈值。这里有个问题，我们没有计算出直线之前，是不知道每个点到直线的距离的。解决这个问题的方法就是采用迭代法。先不考虑加权，算出一个直线方程，然后再用加权法重新计算。每次重新计算一遍直线方程，获得的直线方程就更准确。多迭代几次得到的直线就会稳定到真实的直线上了。当然，关于这个算法的收敛性其实是需要证明的，我们就默认为它能够收敛吧。

下面推导在这个条件下的算法。
直线方程还是使用经典形式：$ax+by+c=0$。这里 $a,b$ 有约束条件$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)$$

对 c 求导可以得到：

$$\frac{\partial D^2}{\partial c}=2\sum _{i=0}^N{w}_i^2(a{x}_i+b{y}_i+c)=0$$

所以：

$$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}$$

这里$\overline{x},\overline{y}$ 是加权平均值：
$$\begin{gathered} \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} \end{gathered}$$

设：
$$\begin{gathered} x_i^{\prime }=x_i-\overline{x} \\ {y}_i^{\prime }=y_i-\overline{y} \end{gathered}$$

$$D^2=\sum _{i=0}^N{w}_i^2(a{x}_i^{\prime }+b{y}_i^{\prime }{)}^2-\lambda (a^2+b^2-1)$$

$$\begin{gathered} \frac{\partial D^2}{\partial a}=2\sum _{i=0}^N{w}_i^2{x}_i^{\prime }(a{x}_i^{\prime }+b{y}_i^{\prime })-2\lambda a=0 \\ \frac{\partial D^2}{\partial b}=2\sum _{i=0}^N{w}_i^2{y}_i^{\prime }(a{x}_i^{\prime }+b{y}_i^{\prime })-2\lambda b=0 \end{gathered}$$

化简可得：
$$\begin{gathered} a\sum w_i^2{x_i^{\prime }}^2+b\sum w_i^2{x}_i^{\prime }{y}_i^{\prime }=\lambda a \\ a\sum w_i^2{x}_i^{\prime }{y}_i^{\prime }+b\sum w_i^2{y_i^{\prime }}^2=\lambda b \end{gathered}$$

设 $D_{xx}=\sum w_i^2{x_i^{\prime }}^2,D_{xy}=\sum w_i^2{x}_i^{\prime }{y}_i^{\prime },D_{yy}=\sum w_i^2{y_i^{\prime }}^2$

上面方程可以写为：
$$\begin{gathered} D_{xx}a+D_{xy}b=\lambda a \\ {D}_{xy}a+D_{yy}b=\lambda b \end{gathered}$$

这个问题至此就转化成一个特征值问题了。$(a,b)$ 是这个方程的特征向量时这个方程才有解。

对于这个方程有两个特征值，两个特征向量。我们应该选哪个特征向量呢？

我们应该让 ${\sum }_{i=0}^N{w}_i^2(a{x}_i^{\prime }+b{y}_i^{\prime }{)}^2$ 最小。

$$\sum _{i=0}^N{w}_i^2(a{x}_i^{\prime }+b{y}_i^{\prime }{)}^2=\left(\begin{array}{cc}a& b\end{array}\right)\left(\begin{array}{cc}{D}_{xx}& D_{xy}\\ D_{xy}& D_{yy}\end{array}\right)\left(\begin{array}{c}a\\ b\end{array}\right)=\lambda$$

所以我们应该选特征值比较小的那个特征向量。

下面给个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;
}

下面是个测试的例子：
原始图像如下：
可以看出在直线之外有一个比较大的干扰图形。

两种你拟合算法得到的结果如下：

红色的线是直接做直线拟合的结果，绿色的是加权直线拟合的结果。可以看出加权拟合结果好很多。