秋招面试题记录(拟合直线，平面)

给定一组2D点，拟合直线

假设要求的直线方程为\(y=ax+b\)，采样点为\((x_i, y_i), i = 0, 1, ... , n-1\).

用fitLine函数实现，输入为一组2D点，输出参数a,b

#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <cstdlib>
#include <time.h> 

using namespace std;

int main()
{
    vector<Eigen::Vector2d> points;
    srand((unsigned)time(NULL)); 
    for (int i = 0; i < 20; i++)
    {
        Eigen::Vector2d point;
        point << i, 2 * i + 3 + rand() % 10 * 0.1;
        points.push_back(point);
    }
    fitLine(points);
    return 0;
}

求线性方程组的最小二乘解

假设要求的直线方程为\(y=ax+b\)，而采样点为\((x_i, y_i), i = 0, 1, ... , n-1\)

那么建立方程

\[\left[\begin{matrix} x_1 & 1 \\ x_2 & 1 \\ ... \\ x_n & 1 \end{matrix}\right] \left[\begin{matrix} a \\ b \end{matrix}\right] = \left[\begin{matrix} y_1 \\ y_2 \\ ... \\ y_n \end{matrix}\right]\]

我们得到了AX=B的矩阵形式。可以用SVD分解,QR分解，正规方程（cholesky分解）来求解。（具体公式参考 矩阵分解 ）

在实现中直接调用Eigen函数来完成：

void fitLine(vector<Eigen::Vector2d> points)
{
    int n = points.size();
    Eigen::MatrixXd A(n, 2);
    Eigen::VectorXd b(n);
    for (int i = 0; i < n; i++)
    {
        A(i, 0) = points[i][0];
        A(i, 1) = 1;
        b(i) = points[i][1];
    }
    cout << "SVD solution is:\n"
        << A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b) << std::endl;
    cout << "The solution using the QR decomposition is:\n"
        << A.colPivHouseholderQr().solve(b) << endl;
    cout << "The solution using normal equations is:\n"
        << (A.transpose() * A).ldlt().solve(A.transpose() * b) << endl;
}

最小二乘法的问题：噪声点的影响很大。可以使用加权最小二乘，使得距离大权重小。设置距离阈值去除离群值。

用RANSAC求解

随机选两个点，计算直线方程，统计内点比例。选内点比例最高的直线参数作为估计。可以对最终的内点重新求最小二乘解。

void fitLine(vector<Eigen::Vector2d> points)
{
    int n = points.size();
    double bestScore = -1.;
    int K = 100;
    for(int k = 0; k < K; k++)
    {
        int i1=0, i2=0;
        while(i1==i2)
        {
            i1 = randn() % n;
            i2 = randn() % n;
        }
        Eigen::Vector2d point1 = points[i1];
        Eigen::Vector2d point2 = points[i2];
        double score = 0;
        for(int i = 0; i < n; i++)
        {
            Eigen::Vector3d point3 = points[i3];
            if(abs((y3−y1)*(x2−x1)−(y2−y1)*(x3−x1))<= 0.0001 )
            {
                score+=1;
            }
        }
        if(score > bestScore)
        {
            if(point1[0] != point2[0]){
                double k = (point1[1]-point2[1])/(point1[0]-point2[0]);
                b = point1[1] - k * point1[0];
            }
            else{
                b = (point1[0] + point2[0]) / 2;
            }
            bestScore = score;
        }
    }
}

RANSAC能够有效去除噪声点，但是需要迭代多次，耗时较长。

用ceres求解

用梯度下降来求解，是最小二乘法一种通用解决方法。只要有损失函数，损失函数相对于优化变量的雅可比矩阵，就可以求解。

此处代码参考《视觉SLAM十四讲》里的cere拟合曲线

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
    // 残差的计算
    template <typename T>
    bool operator() (
    const T* const ab, // 模型参数，有 2 维
    T* residual ) const // 残差
    {
    // y-(ax+b)
    residual[0] = T ( _y ) - ( ab[0]*T ( _x )  + ab[1] );
    return true;
    }
    const double _x, _y; // x,y 数据
};

int main ( int argc, char** argv )
{
    double a=1.0, b=2.0; // 真实参数值
    int N=100; // 数据点
    double w_sigma=1.0; // 噪声 Sigma 值
    cv::RNG rng; // OpenCV 随机数产生器
    double ab[2] = {0,0}; // abc 参数的估计值

    vector<double> x_data, y_data; // 数据
    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            a*x + b + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建最小二乘问题
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
        problem.AddResidualBlock ( // 向问题中添加误差项
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 2> (
                new CURVE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr, // 核函数，这里不使用，为空
            ab // 待估计参数
        );
    }

    // 配置求解器
    ceres::Solver::Options options; // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
    options.minimizer_progress_to_stdout = true; // 输出到cout

    ceres::Solver::Summary summary; // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve ( options, &problem, &summary ); // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出结果
    cout<<summary.BriefReport() <<endl;
    cout<<"estimated a,b = ";
    for ( auto a:ab ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

ceres使用的优化方法有信任域和线搜索两类。Line Search 先固定搜索方向，然后在该方向寻找步长，以最速下降法和 Gauss-Newton 法为代表。而 Trust Region 则先固定搜索区域，再考虑找该区域内的最优点。此类方法以 L-M 为代表。

给定一组3D点，拟合平面

用矩阵分解，RANSAC方法，梯度下降拟合平面与拟合直线类似，此处不做赘述。

用PCA降维

一组三维点\(P_i=(x_i,y_i,z_i)\)

1. 先将均值中心化，即把坐标原点放到所有点的中心

\[\vec{m} = (\overline{x},\overline{y},\overline{z})\]

\[P_i'=P_i - \vec{m}\]

2. 再计算协方差矩阵

\[A = \left( \begin{matrix} x'_1 & y'_1 & z'_1 \\ x'_2 & y'_2 & z'_2 \\ ... & ... & ... \end{matrix} \right)\]

\[C = \frac{1}{N}A^TA\]

3. 计算协方差矩阵的特征向量\(e_1, e_2, e_3\)。其中对应两个较大的特征值的特征向量构成平面，而特征值最小的向量即为平面的法向量。