【SLAM】最最最简单的直线拟合情形下的多种求解方法

本文我们讨论一个最最最简单情况下的拟合的情形，并尝试使用不同的方法来进行求解。

假如有一组数$x_1,x_2,x_3,...,x_n$，对应的值为$y_1,y_2,y_3,...,y_n$，并且$y$和$x$之间近似满足$y=kx+b$的关系。求解出最佳的k的值。

为了方便后续的代码编写，这边就将$x_i$和$y_i$都定义下：

std::vector<Eigen::Vector2d> data = {
    {2.5, 2.4}, {0.5, 0.7}, {2.2, 2.9}, {1.9, 2.2}, {3.1, 3.0}, 
    {2.3, 2.7}, {2, 1.6}, {1, 1.1}, {1.5, 1.6}, {1.1, 0.9}
};

最小二乘法

微分法

按照最小二乘法的定义：

假设$\hat{y}$为满足近似关系的$y$的值，即${\hat{y}}_i=k{x}_i+b$，那么也就是求解误差平方和：

$$SSE=\sum _{i=1}^n(y_i-{\hat{y}}_i{)}^2=\sum _{i=1}^n[y_i-(k{x}_i+b){]}^2$$

最小时的$k$值。

这里，我们将误差视为关于$k$和$b$的函数，那么问题就变成了求各自偏导的问题了：

$$\frac{\partial SSE}{\partial k}=\sum _{i=1}^n(2k{x}_i^2-2x_i{y}_i+2b{x}_i)$$

$$\frac{\partial SSE}{\partial b}=\sum _{i=1}^n(2b-2y_i+2k{x}_i)$$

令各自偏导数等于0，求解出：

$$k=\frac{n{\sum }_{i=1}^n(x_i{y}_i)-{\sum }_{i=1}^n(x_i){\sum }_{i=1}^n(y_i)}{n{\sum }_{i=1}^n(x_i{)}^2-({\sum }_{i=1}^n(x_i){)}^2}$$

$$b=\frac{{\sum }_{i=1}^n{y}_i-k{\sum }_{i=1}^n{x}_i}{n}$$

下面的代码就使用该公式直接求解：

#include <iostream>
#include <vector>
#include <numeric>

int main() {
    // 创建一个二维数据集
    std::vector<Eigen::Vector2d> data = {
        {2.5, 2.4}, {0.5, 0.7}, {2.2, 2.9}, {1.9, 2.2}, {3.1, 3.0}, 
        {2.3, 2.7}, {2, 1.6}, {1, 1.1}, {1.5, 1.6}, {1.1, 0.9}
    };

    double x_sum = 0.0;
    double y_sum = 0.0;
    double xy_sum = 0.0;
    double x2_sum = 0.0;

    for (const auto& point : data) {
        x_sum += point[0];
        y_sum += point[1];
        xy_sum += point[0] * point[1];
        x2_sum += point[0] * point[0];
    }

    double n = static_cast<double>(data.size());

    // 使用微分法求解
    double slope = (n * xy_sum - x_sum * y_sum) / (n * x2_sum - x_sum * x_sum);
    double intercept = (y_sum - slope * x_sum) / n;

    // 输出结果
    std::cout << "Slope: " << slope << std::endl;
    std::cout << "Intercept: " << intercept << std::endl;

    return 0;
}

计算出的结果为：

y = 0.998198 x + 0.103262

矩阵法

按照最小二乘法的定义：

假设$\hat{y}$为满足近似关系的$y$的值，即${\hat{y}}_i=k{x}_i+b$，那么也就是求解误差平方和：

$$SSE=\sum _{i=1}^n(y_i-{\hat{y}}_i{)}^2=(\mathbf{y}-\hat{\mathbf{y}}{)}^T(\mathbf{y}-\hat{\mathbf{y}})$$

其中，

$$\mathbf{y}=\left[\begin{array}{c}{y}_1\\ y_2\\ ...\\ y_n\end{array}\right]$$

由于${\hat{y}}_i=k{x}_i+b$，那么：

$$\hat{\mathbf{y}}=\mathbf{X}\theta$$

其中，

$$\mathbf{X}=\left[\begin{array}{cc}{x}_1& 1\\ x_2& 1\\ ...& ...\\ x_n& 1\end{array}\right]$$

$$\theta =\left[\begin{array}{cc}k& b\end{array}\right]$$

那么，该问题就转化为求解$\hat{\mathbf{y}}=\mathbf{X}\theta$的最小二乘解。

接下来，可以使用特征值方法、奇异值方法来进行求解。这部分的详细求解方法，可以参考文档：【Math】线性方程组的最小二乘解。

下面的代码就使用Eigen库的方法直接求解：

#include <Eigen/Dense>
#include <iostream>
#include <vector>

int main() {
    // 创建一个二维数据集
    std::vector<Eigen::Vector2d> data = {
        {2.5, 2.4}, {0.5, 0.7}, {2.2, 2.9}, {1.9, 2.2}, {3.1, 3.0}, 
        {2.3, 2.7}, {2, 1.6}, {1, 1.1}, {1.5, 1.6}, {1.1, 0.9}
    };

    // 创建设计矩阵A和目标向量b
    Eigen::MatrixXd A(data.size(), 2);
    Eigen::VectorXd b(data.size());
    for (size_t i = 0; i < data.size(); ++i) {
        A(i, 0) = data[i](0);
        A(i, 1) = 1;
        b(i) = data[i](1);
    }

    // 使用最小二乘法求解
    Eigen::VectorXd x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

    // 输出结果
    std::cout << "Slope: " << x(0) << std::endl;
    std::cout << "Intercept: " << x(1) << std::endl;

    return 0;
}

计算出的结果为：

y = 0.998198 x + 0.103262

迭代优化法

无论是微分法的：

$$SSE=\sum _{i=1}^n(y_i-{\hat{y}}_i{)}^2=\sum _{i=1}^n[y_i-(k{x}_i+b){]}^2$$

还是，矩阵法的：

$$SSE=\sum _{i=1}^n(y_i-{\hat{y}}_i{)}^2=(\mathbf{y}-\hat{\mathbf{y}}{)}^T(\mathbf{y}-\hat{\mathbf{y}})$$

都是求解一个最小二乘问题，上面采用的就是解析法求解的。对于这种类型的问题，同样也都可以使用迭代法进行迭代求解。

对于迭代计算的优化方法，这里就不细讲了。之后会单开一章进行讲解，也可以参考文章：最小二乘问题的四种解法——牛顿法，梯度下降法，高斯牛顿法和列文伯格-马夸特法的区别和联系。

协方差法

假设有一组样本$x_1,x_2,x_3,...,x_n$，这组样本的均值为$E(X)$，每一个样本都与$E(X)$之间存在误差，那么这组样本的方差被定义为：所有误差的和的均值，也即$D(X)=E[(X-E(X){)}^2]$，方差的作用就是用来衡量样本偏离均值的程度。

而协方差：

$$\begin{array}{rl}COV(X,Y)& =E[(X-E(X))(Y-E(Y))]\\ & =E(XY)-E(X)E(Y)\end{array}$$

仔细观察上述定义式，可知：如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值时另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值；如果两个变量的变化趋势相反，即其中一个变量大于自身的期望值时另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。

也就是说，协方差是用来衡量两个变量之间协同变化趋势的总体参数，即二个变量相互影响的参数。不太清楚的可以参考文章：深入理解协方差（图文详解）。

判断线性相关的相关系数，定义为：

$$\rho =\frac{COV(X,Y)}{\sqrt{D(X)}\sqrt{D(Y)}}$$

通过协方差的性质，如果满足$Y=kX+b$，那么有：

$$\begin{array}{rl}COV(X,Y)& =COV(X,kX+b)\\ & =COV(X,kX)\\ & =kCOV(X,X)\\ & =kD(X)\end{array}$$

即：

$$k=\frac{COV(X,Y)}{D(X)}$$

其实，这个公式和微分法计算出的$k$是一致的：

$$k=\frac{n{\sum }_{i=1}^n(x_i{y}_i)-{\sum }_{i=1}^n(x_i){\sum }_{i=1}^n(y_i)}{n{\sum }_{i=1}^n(x_i{)}^2-({\sum }_{i=1}^n(x_i){)}^2}$$

PCA法

主成分分析PCA是一种数学降维方法：利用正交变换把一系列可能线性相关的变量转换为一组线性不相关的新变量，也称为主成分，从而利用新变量在更小的维度下展示数据的特征。

主成分是原有变量的线性组合，其数目不多于原始变量。组合之后，相当于我们获得了一批新的观测数据，这些数据的含义不同于原有数据，但包含了之前数据的大部分特征，并且有着较低的维度，便于进一步的分析。

在空间上，PCA可以理解为把原始数据投射到一个新的坐标系统，第一主成分为第一坐标轴，它的含义代表了原始数据中多个变量经过某种变换得到的新变量的变化区间；第二成分为第二坐标轴，代表了原始数据中多个变量经过某种变换得到的第二个新变量的变化区间。这样我们把利用原始数据解释样品的差异转变为利用新变量解释样品的差异。

对于数据拟合的问题，我们可以看到，无论是正相关还是负相关，只要呈现为线性关系，那么它的主成分，相较于剩下的成分，一定是呈现出一个较大的差距。

那么，我们就可以通过这个来进行判断。

PCA的推导过程可以参考文章：一文让你彻底搞懂主成成分分析PCA的原理及代码实现(超详细推导)。

这里只介绍下最终计算方式：

假设有$n$个$m$维数据，$X_{m\times n}=x_1,x_2,...,x_n$，其中的每个$x_i$是一个$m$维的列向量，

1. 去中心化，$X=X-\frac{1}{n}{\sum }_{i=1}^n(x_i)$
2. 计算协方差矩阵，$COV=\frac{1}{n}X{X}^T$
3. 对协方差矩阵进行特征值分解得到$m$个特征矩阵（按特征值从大到小以列排）

其中，这些特征值对应的特征向量就是这组数据的主成分。
​
当然，除了对协方差矩阵进行特征值分解，SVD分解也是一种方式。

下面的代码就使用Eigen库的方法直接求解：

#include <Eigen/Dense>
#include <iostream>
#include <vector>

int main() {
    // 创建一个二维数据集
    std::vector<Eigen::Vector2d> data = {
        {2.5, 2.4}, {0.5, 0.7}, {2.2, 2.9}, {1.9, 2.2}, {3.1, 3.0}, 
        {2.3, 2.7}, {2, 1.6}, {1, 1.1}, {1.5, 1.6}, {1.1, 0.9}
    };

    // 计算数据的均值
    Eigen::Vector2d mean = Eigen::Vector2d::Zero();
    for (const auto& point : data) {
        mean += point;
    }
    mean /= static_cast<double>(data.size());

    // 将数据中心化
    Eigen::MatrixXd centeredData(data.size(), 2);
    for (size_t i = 0; i < data.size(); ++i) {
        centeredData.row(i) = data[i] - mean;
    }

    // 计算协方差矩阵
    Eigen::MatrixXd covariance = centeredData.transpose() * centeredData;

    // -----------------方法一：使用特征值
    // 计算协方差矩阵的特征值和特征向量
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(covariance);
    Eigen::VectorXd eigenvalues = solver.eigenvalues();
    Eigen::MatrixXd eigenvectors = solver.eigenvectors();
    // 找到最大特征值对应的特征向量
    int maxEigenvalueIndex;
    eigenvalues.maxCoeff(&maxEigenvalueIndex);
    Eigen::Vector2d principalComponent = eigenvectors.col(maxEigenvalueIndex);

    // -----------------方法二：使用奇异值
    // 使用SVD找到数据的主成分
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(centeredData, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Vector2d principalComponent = svd.matrixV().col(0);

    // 计算直线的斜率和截距
    double slope = principalComponent[1] / principalComponent[0];
    double intercept = mean[1] - slope * mean[0];

    std::cout << "Slope: " << slope << std::endl;
    std::cout << "Intercept: " << intercept << std::endl;

    return 0;
}

计算出的结果为：

y = 1.08454 x - 0.0530116

可以看出，这种方法计算出来的拟合值和上文的其他方法不太一样。但是也算是比较接近了。如果从SSE的角度衡量的话，自然是不如上面的方法精确的。

Ransac法

上面的所有方法，都有一个共性的缺点：不能避免异常离群点对最终拟合结果的影响。而RANSAC方法，就可以比较好的解决这个问题。

RANSAC实际上是一种框架，而不是一个具体的拟合的算法。所谓具体的拟合方法，就是比如上面提到的这些算法。

RANSAC（Random Sample Consensus，随机采样一致）算法是从一组含有外点（outliers）的数据中正确估计数学模型参数的迭代算法。外点一般指的的数据中的噪声，尤其是一些离群点。所以，RANSAC也是一种外点检测算法。

内点就是组成模型参数的数据，外点就是不适合模型的数据。同时RANSAC假设：在给定一组含有少部分内点的数据，存在一个程序可以估计出符合内点的模型。在本文中，这个程序，就是具体的拟合的算法。

RANSAC是通过反复选择数据集去估计出模型，一直迭代到估计出认为比较好的模型。 具体的实现步骤可以分为以下几步：

1. 选择出可以估计出模型的较小的数据集（对于直线拟合来说就是较少点，甚至可以是两个点）；
2. 使用这个数据集来计算出数据模型（对较少点利用具体的拟合算法，来拟合出一条直线）；
3. 将所有数据带入这个模型，计算出内点的数目（累加在一定误差范围内的适合当前迭代推出模型的数据）；
4. 比较当前模型和之前推出的最好的模型的内点的数量，记录最大内点数的模型参数和内点数；
5. 重复1-4步，直到迭代结束或者当前模型已经足够好了（内点数目大于一定数量）。

更细节的内容可以参考文章：RANSAC算法(附RANSAC直线拟合C++与Python版本)。

下面的代码就使用Eigen库的方法直接求解：

#include <Eigen/Dense>
#include <vector>
#include <random>
#include <limits>
#include <algorithm>

// RANSAC直线拟合
Eigen::Vector2d ransacLineFitting(const std::vector<Eigen::Vector2d>& data,
                                  int maxIterations,
                                  double distanceThreshold,
                                  int numPoints) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> dis(0, data.size() - 1);

    int bestInliers = 0;
    Eigen::Vector2d bestLine;

    for (int i = 0; i < maxIterations; ++i) {
        // 随机选择numPoints个点
        std::vector<Eigen::Vector2d> points;
        for (int j = 0; j < numPoints; ++j) {
            points.push_back(data[dis(gen)]);
        }

        // 计算直线参数
        // 利用上文的具体的拟合算法对points进行拟合，甚至可以numPoints取2，直接算斜率
        // 拟合结果：y = kx + b

        // 计算内点数
        int inliers = 0;
        for (const auto& point : data) {
            double distance = (point[1] - k * point[0] - b) * (point[1] - k * point[0] - b);
            if (distance < distanceThreshold) {
                ++inliers;
            }
        }

        // 更新最佳模型
        if (inliers > bestInliers) {
            bestInliers = inliers;
            Eigen::Vector2d line(k, b);
            bestLine = line;
        }
    }

    return bestLine;
}

int main() {
    // 创建一个二维数据集
    std::vector<Eigen::Vector2d> data = {
        {2.5, 2.4}, {0.5, 0.7}, {2.2, 2.9}, {1.9, 2.2}, {3.1, 3.0}, 
        {2.3, 2.7}, {2, 1.6}, {1, 1.1}, {1.5, 1.6}, {1.1, 0.9}
    };

    // 使用RANSAC进行直线拟合
    Eigen::Vector2d line = ransacLineFitting(data, 1000, 0.1, 2);

    std::cout << "Line parameters: " << line[0] << ", " << line[1] << std::endl;

    return 0;
}

需要注意的是：对于RANSAC方法，numPoints的取值，不能太小，也不能太大。太小的话，可能导致拟合结果无法代表较多点数的方向；太大的话，可能无法规避掉外点造成的影响。需要权衡。

上面的几种方法都可以进行直线拟合，至于选择哪一种，取决于具体的情形和问题。

一般来说，最小二乘法是最常见、使用最广泛的方法，也是最需要掌握的。如果你是SLAM相关的从业人员，那么矩阵法、迭代优化法、Ransac法也是必须掌握的能力。

相关阅读

• 最优化方法——最小二乘法与梯度下降法
• 线性拟合1-最小二乘
• 深入理解协方差（图文详解）
• 超全面的协方差矩阵介绍
• 一文让你彻底搞懂主成成分分析PCA的原理及代码实现(超详细推导)
• RANSAC算法(附RANSAC直线拟合C++与Python版本)