平行线拟合问题（附带 C++ 源代码）

#平行线拟合问题

这个问题来源于最近项目中的实际需求，在图像中有一些平行线，要测量两个平行线的间距。这个问题应该算是机器视觉测量类问题中一个比较典型的问题。影像测量仪测量的长度基本都是这种平行线间距离。

这里假设我们已经获得了足够的数据点，第一条直线上的数据点形成集合 { ( x 1 , i , y 1 , i ) } \{(x_{1,i}, y_{1,i})\} { (x1,i​,y1,i​)}，第二条直线上的数据点形成集合 { ( x 2 , i , y 2 , i ) } \{(x_{2,i}, y_{2,i})\} { (x2,i​,y2,i​)}。这些数据点当然是有测量误差的，里面也有可能混入干扰点。所以我们需要做拟合。这里我们使用最小二乘拟合。

直线方程还是使用经典形式：$ax+by+c=0$。第一条直线的方程为 ：$ax+by+c_1=0$， 第二条直线的方程为：$ax+by+c_2=0$。这里 $a,b$ 有约束条件$a^2+b^2=1$。因此，虽然我们有四个未知数，但是其实只有三个是独立的。简单的计算可知，这两条平行线的距离为 $|c_1-c_2|$。

拟合问题其实就是个极值问题，这里有一个约束条件，所以要加一个拉格朗日乘子。

$$D^2=\sum _{i=0}^{N_1}(a{x}_{1,i}+b{y}_{1,i}+c_1{)}^2+\sum _{j=0}^{N_2}(a{x}_{2,j}+b{y}_{2,j}+c_2{)}^2+\lambda (a^2+b^2-1)$$

我们知道：
$$\begin{gathered} \frac{\partial D^2}{\partial c_1}=2\sum _{i=0}^{N_1}(a{x}_{1,i}+b{y}_{1,i}+c_1)=0 \\ \frac{\partial D^2}{\partial c_2}=2\sum _{j=0}^{N_2}(a{x}_{2,j}+b{y}_{2,j}+c_2)=0 \end{gathered}$$

化简后可以得到：
$$\begin{gathered} a\overline{x_1}+b\overline{y_1}+c_1=0 \\ a\overline{x_2}+b\overline{y_2}+c_2=0 \end{gathered}$$

这里 $\overline{x_1}$ 是 $x_{1,i}$ 的平均值，$\overline{x_2}$ 是 $x_{2,j}$ 的平均值，$\overline{y_1}$ 是 $y_{1,i}$ 的平均值，$\overline{y_2}$ 是 $y_{2,j}$ 的平均值。

我们可以每个坐标都减去平均值：

$$\begin{gathered} p_{1,i}=x_{1,i}-\overline{x_1} \\ {q}_{1,i}=y_{1,i}-\overline{y_1} \\ {p}_{2,i}=x_{2,i}-\overline{x_2} \\ {q}_{2,i}=y_{2,i}-\overline{y_2} \end{gathered}$$

那么：

$$D^2=\sum _{i=0}^{N_1}(a{p}_{1,i}+b{q}_{1,i}{)}^2+\sum _{j=0}^{N_2}(a{p}_{2,j}+b{q}_{2,j}{)}^2-\lambda (a^2+b^2-1)$$

可以看到 $c_1,c_2$都被消去了。而且这两个求和可以合并到一起的。

我们把 { p 1 , i } \{p_{1,i}\} { p1,i​} 和 { p 2 , j } \{p_{2,j}\} { p2,j​} 拼到一起成为 { x i } \{x_i\} { xi​}， { q 1 , i } \{q_{1,i}\} { q1,i​} 和 { q 2 , j } \{q_{2,j}\} { q2,j​} 拼到一起成为 { y i } \{y_i\} { yi​}。

那么上面的式子可以写为：

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

求偏导数后可以得到：
$$\begin{gathered} \frac{\partial D^2}{\partial a}=2\sum _{i=0}^{N_1+N_2}{x}_i(a{x}_i+b{y}_i)-2\lambda a=0 \\ \frac{\partial D^2}{\partial b}=2\sum _{i=0}^{N_1+N_2}{y}_i(a{x}_i+b{y}_i)-2\lambda b=0 \end{gathered}$$
化简后可以得到：
$$\begin{gathered} a\sum x_i^2+b\sum x_i{y}_i=\lambda a \\ a\sum x_i{y}_i+b\sum y_i^2=\lambda b \end{gathered}$$

设 $D_{xx}=\sum x_i^2,D_{xy}=\sum x_i{y}_i,D_{yy}=\sum y_i^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(a{x}_i+b{y}_i{)}^2$ 最小。

$$\sum _{i=0}^N(a{x}_i+b{y}_i{)}^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$$

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

下面给个参考代码，经过简单的测试。

/**
* @brief plinesFit 平行线拟合算法 最小二乘法拟合 ax + by + c = 0 型直线
* @param line1 第一条直线的数据点
* @param line2 第二条直线的数据点
* @param [out] a 输出拟合系数
* @param [out] b 输出拟合系数
* @param [out] c1 输出拟合系数，第一条直线的方程为 ax + by + c1 = 0
* @param [out] c2 输出拟合系数，第二条直线的方程为 ax + by + c2 = 0
* @return true 表示拟合成功，false 表示拟合失败
*/
bool plinesFit(const QVector<QPointF> &line1, const QVector<QPointF> &line2, double &a, double &b, double &c1, double &c2)
{

    if(line1.size() < 2 ||  line2.size() < 0)
    {
        a = 0;
        b = 0;
        c1 = 0;
        c2 = 0;
        return false;
    }
    int size = line1.size();
    double x_mean_1 = 0, x_mean_2 = 0;
    double y_mean_1 = 0, y_mean_2 = 0;
    for(int i = 0; i < size; i++)
    {
        x_mean_1 += line1[i].x();
        y_mean_1 += line1[i].y();
    }
    x_mean_1 /= size;
    y_mean_1 /= size; //至此，计算出了 x y 的均值

    double Dxx = 0, Dxy = 0, Dyy = 0;
    for(int i = 0; i < size; i++)
    {
        Dxx += (line1[i].x() - x_mean_1) * (line1[i].x() - x_mean_1);
        Dxy += (line1[i].x() - x_mean_1) * (line1[i].y() - y_mean_1);
        Dyy += (line1[i].y() - y_mean_1) * (line1[i].y() - y_mean_1);
    }

    size = line2.size();
    for(int i = 0; i < size; i++)
    {
        x_mean_2 += line2[i].x();
        y_mean_2 += line2[i].y();
    }
    x_mean_2 /= size;
    y_mean_2 /= size; //至此，计算出了 x y 的均值

    for(int i = 0; i < size; i++)
    {
        Dxx += (line2[i].x() - x_mean_2) * (line2[i].x() - x_mean_2);
        Dxy += (line2[i].x() - x_mean_2) * (line2[i].y() - y_mean_2);
        Dyy += (line2[i].y() - y_mean_2) * (line2[i].y() - y_mean_2);
    }

    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;
            c1 = - x_mean_1;
            c2 = - x_mean_2;

        }
    }
    else
    {
        a = Dxy / den;
        b = (lambda - Dxx) / den;
        c1 = - a * x_mean_1 - b * y_mean_1;
        c2 = - a * x_mean_2 - b * y_mean_2;

        if(a < 0)
        {
            a = -a;
            b = -b;
            c1 = -c1;
            c2 = -c2;
        }
    }
    return true;
}