写在前面

1、本文内容
使用Eigen进行最小二乘拟合曲线

2、平台/环境
Eigen(open3d), cmake, pcl

3、转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/131407456

曲线拟合方法

pcl实现的b样条曲线拟合

参考：
https://blog.csdn.net/qq_36686437/article/details/114160557
从效果上看，pcl里面的曲线拟合得到的结果比较差

最小二乘曲线拟合

原理

在二维XOY平面上，曲线由多项式 $f (x)$ 表示：
$a_0+a_1x+a_2x^2+a_3x^3+...+a_nx^n=\sum_{i=0}^n{a_ix^i}$
其中 $i\in\{0,1,2...n\}$ 是多项式的阶数， $a_i$ 是多项式的各阶系数
有一真实曲线 $l$ ，由m个点组成 $(x_j,y_j),j\in\{0,1,...m\}$ ，假设 $l$ 就是由一条确定的多项式曲线 $f (x)$ 采样得到，那么点都在一定曲线上：
$y_j = f(x_j)$
但实际上，我们是想通过一条拟合出来的曲线 $f (x)$ 来逼近真实点 $y_j$ 以满足每个真实点都在曲线上或者离曲线很近，设每个点离曲线的距离为:
$e_j = y_j-f(x_j)$
那么我们希望所有真实点离曲线 $f (x)$ 越近越好，则需要最小化所有点到曲线的距离：
$\mathop{min}\limits_{x}\ \sum_{j=0}^{j=m}{y_j-f(x_j)}$
假设我们以一个二阶多项式 $f(x) = a_0+a_1x^1+a_2x^2$ 拟合数据 $(x_j,y_j),j\in\{0,1,...m\}$ ，则有方程组:
$\begin{cases} a_0 + a_1x_0+a_2x_0^2=y_0 \\ a_0 + a_1x_1+a_2x_1^2=y_1 \\ ...\\ a_0 + a_jx_1+a_2x_j^2=y_j \\ ...\\ a_0 + a_1x_m+a_2x_m^2=y_m \end{cases}$
写成矩阵形式则为
$\begin{bmatrix} 1 & x_0& x_0^2 \\ 1 & x_1& x_1^2 \\ ... \\ 1 & x_m& x_m^2 \\ \end{bmatrix} \begin{bmatrix} a_0\\a_1\\a_2\\ \end{bmatrix}= \begin{bmatrix} y_0\\y_1\\...\\y_m \end{bmatrix}$
这其实是一个非齐次线性方程组 $A x = b$ 的最小二乘解问题，为了不和上面的a和x引起歧义，我们使用 $X\theta=b$ 表示，其最小二乘问题表示为:
$\mathop{min}\limits_{\theta}||X\theta-b||^2$
$b$ 就是 $y_j$ , $X$ 就是我们的数据：
$\begin{bmatrix} 1 & x_0& x_0^2 \\ 1 & x_1& x_1^2 \\ ... \\ 1 & x_m& x_m^2 \\ \end{bmatrix}$
我们要求的就是系数 $a_0,a_1,a_2]^T$ 即 $X\theta=b$ 里面的 $\theta$
对于 $X\theta=b$ 的解，可以做如下变换：
$X^TX\theta=X^Tb$
我们假设 $X^TX$ 可逆，则有：
$\theta=(X^TX)^{-1}X^Tb$
这里有个问题
1、对于通用最小二乘来说， $X^TX$ 可能不可逆，因为 $X$ 的行向量可能线性相关(详解岭回归与L2正则化)，但是在这里，我们注意到每行都有齐次项1，所以当 $x_0 \ne x_1 \ne ...\ne x_m$ 时， $X$ 的行向量线性无关，即当数据中无重复点时，我们认为 $X^TX$ 可逆，于是通过 $\theta=(X^TX)^{-1}X^Tb$ ，我们便可以通过解析解直接得到我们想要的 $\theta$

2、我们也可以认为 $X^TX$ 不可逆那么 $\theta=(X^TX)^{-1}X^Tb$ 的推导可以参考，并且计算时使用Qr分解：
基于最小二乘法的多项式曲线拟合：从原理到c++实现
 最小二乘问题和基于HouseHolder变换的QR分解
 一文让你彻底搞懂最小二乘法（超详细推导）

代码

提供两种求解的代码，一种是直接通过 $\theta=(X^TX)^{-1}X^Tb$ 并利用Eigen的矩阵运算实现，另一种是qr分解。代码目录结构如下，请将least_square_fit_curve.cpp和CMakeLists.txt放入src，然后使用compile.bat和run.bat进行编译和运行，请修改对应PCL对应的路径
在这里插入图片描述

least_square_fit_curve.cpp

#include <iostream>
#include <string>
#include <vector>
#include <random>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/pcl_plotter.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <vtkPolyLine.h>

#include <Eigen/Dense>
#include <Eigen/Core>

std::vector<Eigen::Vector2d> GenerateGaussNoise(const std::vector<Eigen::Vector2d> &points_origin, double mu, double sigma)
{
    
    
    std::vector<Eigen::Vector2d> points_noise;
    points_noise = points_origin;
    std::normal_distribution<> norm{
    
    mu, sigma};
    std::random_device rd;
    std::default_random_engine rng{
    
    rd()};

    for (size_t i = 0; i < points_noise.size(); ++i)
    {
    
    
        points_noise[i][0] = points_noise[i][0] + norm(rng);
        points_noise[i][1] = points_noise[i][1] + norm(rng);
    }
    return points_noise;
}

void DisplayLine2D(std::vector<double> vector_1, std::vector<double> vector_2, std::vector<double> coeff, double min_x, double max_x)
{
    
    
    pcl::visualization::PCLPlotter *plot_line1(new pcl::visualization::PCLPlotter);
    // std::vector<double> func1(2, 0);
    // func1[0] = c;
    // func1[1] = b;
    // func1[2] = a;
    // plot_line1->addPlotData(func1, point_min.x, point_max.x);
    std::cout << "min_x: " << min_x << " max_x: " << max_x << std::endl;
    plot_line1->addPlotData(coeff, min_x, max_x);
    plot_line1->addPlotData(vector_1, vector_2, "display", vtkChart::POINTS); // X,Y均为double型的向量
    plot_line1->setShowLegend(true);
    plot_line1->plot();
}

std::vector<Eigen::Vector2d> GeneratePolynomialCurve2D(std::vector<double> &coefficients, Eigen::Vector2d range = Eigen::Vector2d(-10, 10), double step = 0.2)
{
    
    
    std::cout << "GeneratePolynomialCurve2D" << std::endl;
    // for (auto i : coefficients)
    // {
    
    
    //     std::cout << i << std::endl;
    // }
    // std::cout << "range: " << range(0) << " " << range(1) << std::endl;
    std::vector<Eigen::Vector2d> points;
    points.resize(int((range(1) - range(0)) / step));
    double x, y;
    for (std::size_t i = 0; i < points.size(); ++i)
    {
    
    
        x = range(0) + step * i;
        y = 0;
        for (std::size_t j = 0; j < coefficients.size(); ++j)
        {
    
    
            // y = a_0*x^0 + a_1*x^1 + a_2*x^2 + a_n*x^n
            y += coefficients[j] * std::pow(x, j);
        }
        // std::cout << x << " " << y << "         ";
        points[i] = Eigen::Vector2d(x, y);
    }
    return points;
}

/// @brief 最小二乘拟合，直接用公式
/// @param data_x
/// @param data_y
/// @param coeff 多项式系数 a_0, a_1, ... a_n
/// @param order 需要拟合的阶数
void PolynomailCurveFit(const std::vector<double> &data_x,
                        const std::vector<double> &data_y,
                        std::vector<double> &coeff,
                        int order

)
{
    
    
    // Create Matrix Placeholder of size n x k, n= number of datapoints, k = order of polynomial, for exame k = 3 for cubic polynomial
    Eigen::MatrixXd X(data_x.size(), order + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Map(&data_y.front(), data_y.size());
    Eigen::VectorXd A;

    // check to make sure inputs are correct
    assert(data_x.size() == data_y.size());
    assert(data_x.size() >= order + 1);
    // Populate the matrix
    for (size_t i = 0; i < data_x.size(); ++i)
    {
    
    
        for (size_t j = 0; j < order + 1; ++j)
        {
    
    
            X(i, j) = pow(data_x.at(i), j);
        }
    }
    // std::cout << T << std::endl;

    // Solve for linear least square fit
    A = (X.transpose() * X).inverse() * X.transpose() * b;
    coeff.resize(order + 1);
    for (int k = 0; k < order + 1; k++)
    {
    
    
        coeff[k] = A[k];
    }
}

/// @brief 最小二乘拟合，用Qr分解
/// @param data_x
/// @param data_y
/// @param coeff 多项式系数 a_0, a_1, ... a_n
/// @param order 需要拟合的阶数
void PolynomailCurveFitQr(const std::vector<double> &data_x,
                          const std::vector<double> &data_y,
                          std::vector<double> &coeff,
                          int order

)
{
    
    
    // Create Matrix Placeholder of size n x k, n= number of datapoints, k = order of polynomial, for exame k = 3 for cubic polynomial
    Eigen::MatrixXd X(data_x.size(), order + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Map(&data_y.front(), data_y.size());
    Eigen::VectorXd A;

    // check to make sure inputs are correct
    assert(data_x.size() == data_y.size());
    assert(data_x.size() >= order + 1);
    // Populate the matrix
    for (size_t i = 0; i < data_x.size(); ++i)
    {
    
    
        for (size_t j = 0; j < order + 1; ++j)
        {
    
    
            X(i, j) = pow(data_x.at(i), j);
        }
    }
    // std::cout << T << std::endl;
    // Solve for linear least square fit
    A = X.householderQr().solve(b);
    coeff.resize(order + 1);
    for (int k = 0; k < order + 1; k++)
    {
    
    
        coeff[k] = A[k];
    }
}
void PrintPolynomailCoeff(const std::vector<double> &coeff, std::string name = "coeff")
{
    
    
    std::cout << name << ": ";
    for (auto i : coeff)
    {
    
    
        std::cout << i << " ";
    }
    std::cout << std::endl;
}

int main(int argc, char *argv[])
{
    
    
    std::chrono::high_resolution_clock::time_point all_s = std::chrono::high_resolution_clock::now();

    Eigen::Vector2d range = Eigen::Vector2d(-10, 10);
    std::vector<double> coeff_gt({
    
    1, 2, 4});
    std::vector<double> coeff_fit;
    std::vector<double> coeff_fit_qr;
    auto points_curve = GeneratePolynomialCurve2D(coeff_gt, range);
    std::vector<double> points_x;
    std::vector<double> points_y;
    // 添加噪声
    auto points_curve_noise = GenerateGaussNoise(points_curve, 0, 0.05);
    // 不添加噪声
    // auto points_curve_noise = points_curve;
    for (auto i : points_curve_noise)
    {
    
    
        points_x.push_back(i(0));
        points_y.push_back(i(1));
    }
    PolynomailCurveFit(points_x, points_y, coeff_fit, 2);
    PolynomailCurveFitQr(points_x, points_y, coeff_fit_qr, 2);
    PrintPolynomailCoeff(coeff_gt, "coeff_gt");
    PrintPolynomailCoeff(coeff_fit, "coeff_fit");
    PrintPolynomailCoeff(coeff_fit_qr, "coeff_fit_qr");

    std::chrono::high_resolution_clock::time_point all_e = std::chrono::high_resolution_clock::now();
    auto all_cost = std::chrono::duration_cast<std::chrono::milliseconds>(all_e - all_s).count();
    std::cout << "all cost: " << all_cost << " ms" << std::endl;

    DisplayLine2D(points_x, points_y, coeff_gt, range(0), range(1));
    DisplayLine2D(points_x, points_y, coeff_fit, range(0), range(1));
    DisplayLine2D(points_x, points_y, coeff_fit_qr, range(0), range(1));

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.18)

project(LeastSquareFitCurve)

find_package(PCL 1.8 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(least_square_fit_curve ./least_square_fit_curve.cpp)
target_link_libraries(least_square_fit_curve ${PCL_LIBRARIES})

compile.bat

cmake -S ./src -B ./build
cmake --build ./build --parallel 8

run.bat

set PATH=D:\carlos\install\PCL 1.10.0\bin;D:\carlos\install\PCL 1.10.0\3rdParty\FLANN\bin;D:\carlos\install\PCL 1.10.0\3rdParty\VTK\bin;D:\carlos\install\PCL 1.10.0\3rdParty\Qhull\bin;D:\carlos\install\PCL 1.10.0\3rdParty\OpenNI2\Tools;%PATH%

.\build\Release\least_square_fit_curve.exe