直线拟合推导

目标函数为：

\[ f(x)=\sum_{i=0}^{n-1}\left(Y_i-y_i\right)^2=\sum_{i=0}^{n-1}\left(ax_i+b-y_i\right)^2 \]

我们的目标是求当f(x)取得最小值时的a、b参数。于是分别求f(x)对于a、b的偏导数：

\[ \frac{\partial f}{\partial a}=\sum_{i=0}^{n-1}2\left(\alpha x_i+b-y_i\right)x_i = 2\sum_{i=0}^{n-1}\Big(ax_{i}^{2}+bx_{i}-x_{i}y_{i}\Big) = 2\left(a\sum_{i=0}^{n-1}x_{i}^{2}+b\sum_{i=0}^{n-1}x_{i}-\sum_{i=0}^{n-1}x_{i}y_{i}\right) \]

\[ \frac{\partial f}{\partial b} = \sum_{i=0}^{n-1} 2\left(\alpha x_i+b-y_i\right) = 2\left(a\sum_{i=0}^{n-1}x_{i}+\sum_{i=0}^{n-1}b-\sum_{i=0}^{n-1}y_{i}\right) = 2\left(a\sum_{i=0}^{n-1}x_{i}+bn-\sum_{i=0}^{n-1}y_{i}\right) \]

令以上偏导数为0;得到一个二元一次方程组:

\[ a\sum_{i=0}^{n-1}x_{i}^{2}+b\sum_{i=0}^{n-1}x_{i}=\sum_{i=0}^{n-1}x_{i}y_{i} \]

\[ a\sum_{i=0}^{n-1}x_{i}+bn=\sum_{i=0}^{n-1}y_{i} \]

记：

\[ A=\sum_{i=0}^{n-1}x_{i}^{2},B=\sum_{i=0}^{n-1}x_{i},C=\sum_{i=0}^{n-1}x_{i}y_{i},D=\sum_{i=0}^{n-1}y_{i} \]

\[ Aa + Bb = C \\ Ba + bn = D \]

解以上方程组得到a、b;就是我们要求的直线拟合参数;

\[ a = \frac{nC - BD}{nA -B^2} \\ b = \frac{DA - BC}{nA - B^2} \]

{
        float sum_x2 = 0.0;
        float sum_y = 0.0;
        float sum_x = 0.0;
        float sum_xy = 0.0;
        for(int pos = 0; pos < count; pos ++){
            sum_x2 += xs[pos] * xs[pos];
            sum_y += ys[pos];
            sum_x += xs[pos];
            sum_xy += xs[pos] * ys[pos];
        }


        float k = 0., b = 0.;
        float tmp = count * sum_x2 - sum_x * sum_x;
        if (std::abs(tmp) > 0.000001f){
            k = (count * sum_xy - sum_x * sum_y) / tmp;
            b = (sum_x2 * sum_y - sum_x * sum_xy) / tmp;
        }else{
            k = 0;
            b = 0;
        }
        std::cout << "cpp: k:" << k << " b:" << b << std::endl;
    }
}

直线极坐标表达式

如图所示，直线方程\(y = k * x + b\)，其中：

\[ b = \frac{r}{\sin \theta} \\ k = \tan(\pi - (\pi/2 - \theta)) = \tan(\pi / 2 + \theta) = -\cot \theta = -\frac{\cos \theta}{\sin \theta} \]

得到：

\[ y = -\frac{\cos \theta}{\sin \theta} x + \frac{r}{\sin \theta} \]

因此得到直线方程极坐标表达式：

\[ x \cos \theta + y \sin \theta = r \]

其中\(r > 0\)为直线与圆心距离，\(\theta\)为垂线与x轴距离。

使用笛卡尔坐标系拟合直线

int main(int argc, const char **argv)
{
    // generate observation
    const int count = 1000;
    const float k = 0.5;
    const float b = 0.2;
    std::vector<float> xs, ys;
    for(int pos = 0; pos < count; pos ++){
        float x = pos / float(count);
        float y = k * x + b;
        xs.push_back(x);
        ys.push_back(y);
    }

    // 2. eigen
    // [x, 1] * ([k, b])^t = [y]
    {
        Eigen::MatrixXd A = Eigen::MatrixXd::Zero(count, 2);
        Eigen::VectorXd B = Eigen::VectorXd::Zero(count);
        for(int pos = 0; pos < count; pos ++){
            A.block<1, 2>(pos, 0)[0] = xs[pos];
            A.block<1, 2>(pos, 0)[1] = 1.;
            B[pos] = ys[pos];
        }
        Eigen::Vector2d solved = (A.transpose() * A).inverse() * (A.transpose() * B);
        //Eigen::Vector2d solved = A.bdcSvd(Eigen::ComputeThinU|Eigen::ComputeThinV).solve(B);
        //Eigen::Vector2d solved = (A.transpose() * A).ldlt().solve(A.transpose() * B);
        std::cout << "eigen: k:" << solved[0] << " b:" << solved[1] << std::endl;
    }

    return 0;
}

使用极坐标表达式拟合直线

\[ residual = \sum_0^N \{ x \cos \theta + y \sin \theta - r \} \]

\[ X = \begin{bmatrix} \theta \\ r \end{bmatrix}_{2\times1} \]

\[ J = \begin{bmatrix} x * -\sin \theta + y * \sin \theta & -1 \end{bmatrix}_{1\times2} \]

int main(int argc, const char **argv)
{
    const int count = 1000;
    for(int pos = 0; pos < count; pos ++){
        xs.push_back(1.);
        ys.push_back(pos / float(count));
    }
    {
        // polar line
        double theta = 0.;
        double r = 0.;
        for(int iter = 0; iter < 100; iter ++){
            Eigen::MatrixXd H = Eigen::MatrixXd::Zero(2, 2);
            Eigen::VectorXd B = Eigen::VectorXd::Zero(2);
            for(int pos = 0; pos < count; pos ++){
                double x = xs[pos], y = ys[pos];
                // x \cos \theta + y \sin \theta - r = 0
                double fx = x * std::cos(theta) + y * std::sin(theta) - r;
                Eigen::MatrixXd J = Eigen::MatrixXd::Zero(1, 2);
                J(0, 0) = x * -std::sin(theta) + y * std::cos(theta);
                J(0, 1) = -1.;
                H += J.transpose() * J;
                B += -J.transpose() * fx; 
            }
            // J^TJ \delta x = -J^T fx
            Eigen::Vector2d solved = H.ldlt().solve(B);
            theta += solved[0];
            r += solved[1];
        }
        std::cout << "polar:" << " theta:" << theta << " r:" << r << " k:" << -std::cos(theta)/std::sin(theta) << " b:" << r/std::sin(theta) << std::endl;
    }
    return 0;
}

---

Reference

[0]