Polynomial curve fitting using least squares

write in front

1. Contents of this article:
Using Eigen for least squares curve fitting

2. Platform/environment
Eigen (open3d), cmake, pcl

3. Please indicate the source when reprinting:
https://blog.csdn.net/qq_41102371/article/details/131407456

curve fitting method

b-spline curve fitting implemented by pcl

Reference:
https://blog.csdn.net/qq_36686437/article/details/114160557
From the effect point of view, the results obtained by curve fitting in pcl are relatively poor.

Least squares curve fitting

principle

On the two-dimensional XOY plane, the curve is represented by the polynomial $f(x)$表示：
$$f(x)=a_0+a_{1}x+a_2{x}^2+a_3{x}^3+...+a_n{x}^n=\sum _{i=0}^n{a}_i{x}^i$$
in whichi ∈ { 0 , 1 , 2... n } i\in\{0,1,2...n\}i∈{ 0,1,2... n } is the order of the polynomial,$a_i$are the coefficients of each order of the polynomial.
There is a real curve $l$ , composed of m points$(x_j,y_j),j\in \{0,1,...m\}$ , assuming$l$ is a determined polynomial curve$f\left(x\right)$ is sampled, then the points are all on a certain curve:
$$y_j=f(x_j)$$
But in fact, we want to pass a fitted curve$f\left(x\right)$ to approximate the real point$y_j$In order to satisfy that each real point is on the curve or very close to the curve, let the distance of each point from the curve be:
$$e_j=y_j-f(x_j)$$
then we want all real points to be off the curvef ( x ) f(x)The closer$f$$($$x$$)$
$$\underset{x}{\min }\sum _{j=0}^{j=m}{y}_j-f\left(x_j\right)$$
Suppose we take a second-order polynomial$f(x)=a_0+a_1{x}^1+a_2{x}^2$ Fitting data(xj, yj), j ∈ {0, 1, . . . m} (x_j,y_j),j\in\{0,1,...m\}(xj​,yj​),j∈{ 0,1,...m}，则有方程组:
$$\{\begin{array}{l}{a}_0+a_1{x}_0+a_2{x}_0^2=y_0\\ a_0+a_1{x}_1+a_2{x}_1^2=y_1\\ ...\\ a_0+a_j{x}_1+a_2{x}_j^2=y_j\\ ...\\ a_0+a_1{x}_m+a_2{x}_m^2=y_m\end{array}$$
Written in matrix form, it is
$$\left[\begin{array}{ccc}1& x_0& x_0^2\\ 1& x_1& x_1^2\\ ...& & \\ 1& x_m& x_m^2\end{array}\right]\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]=\left[\begin{array}{c}{y}_0\\ y_1\\ ...\\ y_m\end{array}\right]$$
This is actually a system of non-homogeneous linear equations $Ax=$For the least squares solution of$b$$X\theta =b$ represents, and its least squares problem is expressed as:
$$\underset{i}{\min }||X\theta -b|{|}^2$$
$b$ is$y_j$, $X$ is our data:
$$\left[\begin{array}{ccc}1& x_0& x_0^2\\ 1& x_1& x_1^2\\ ...& & \\ 1& x_m& x_m^2\end{array}\right]$$
What we require is the coefficient $[a_0,a_1,a_2{]}^T$ is$X\theta =$θ \thetain$b$$\theta$
exponentX$X\theta =The\; solution\; of\; b$ can be transformed as follows:
$$X^{T}X\theta =X^{T}b$$
we assume$X^{T}X$is reversible, then there is:
$$i=(X^{T}X{)}^{-1}{X}^{T}b$$
Here is a question
1. For universal least squares,$X^{T}X$may be irreversible becauseXXThe$row$ vectorsof$x_0\ne x_1\ne ...\ne x_m$When, $The\; row\; vector\; of\; X$ is linearly independent, that is, when there are no repeating points in the data, we$X^{T}X$is reversible, so through$i=(X^{T}X{)}^{-1}{X}^{T}b$the θ \thetawe want through the analytical solution$i$

2. We can also think of $X^{T}X$is irreversible, then$i=(X^{T}X{)}^{-1}{X}^{You\; can\; refer\; to\; the\; derivation\; of\; T}b$, and use Qr decomposition when calculating:
Polynomial Curve Fitting Based on Least Squares Method: From Principles to C++ Implementing
Least Squares Problems and QR Decomposition Based on HouseHolder Transform
This article will let you thoroughly understand the least squares method ( Super detailed derivation)

code

Two solution codes are provided, one is directly through $i=(X^{T}X{)}^{-1}{X}^{T}b$is implemented using Eigen's matrix operations, and the other is qr decomposition. The code directory structure is as follows. Please put least_square_fit_curve.cpp and CMakeLists.txt into src, and then use compile.bat and run.bat to compile and run. Please modify the path corresponding to the 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

Compile:

cd least_square_fit_curv
./compile.bat

run

run.bat

Note:

Pcl's curve drawing is used, and pcl comes with the Eigen library. You can use Open3d and Eigen provided in pcl. If you don't have pcl and open3d, you can install Eigen directly, and note out the visualization part of pcl, and install Eigen separately for reference:
cmake+Eigen library

Reference implementation of qr:
least squares problem and QR decomposition based on HouseHolder transformation.
Least squares polynomial fitting using C++ Eigen package.
Code reference for direct matrix inversion:
Fitting curves in 3D space
C++/PCL: least squares fitting Combined plane straight line, plane polynomial curve, spatial polynomial curve
C++ least squares straight line fitting, curve fitting, plane fitting, Gaussian fitting

result

The quadratic curve f ( x ) = 1 + 2 x + 4 x 2 f(x) = 1+2x+4x^2 was manually generated using code$f(x)=1+2x\_+4x{\_}^2$ sampling points, without adding noise, the fitting result is consistent with GT. Coefficient
:

Original point and curve:
Because the coefficients are consistent, the fitting result curve is consistent with the original curve and is not displayed.

In the case of adding Gaussian noise:
coefficients:

original points and curves:

fitting result curve:

it can be seen that the results obtained by the direct method and qr decomposition are consistent

reference

3D Curve 1: Polynomial Curve https://zhuanlan.zhihu.com/p/267985141
Solving definite equations, least squares solution, solution of Ax=0, Ax=b, solving homogeneous equations, solving non-homogeneous equations Group (the derivation is very detailed)
to generate Gaussian noise https://zhuanlan.zhihu.com/p/458994530
The rest of the article has been listed

over

Mainly engaged in laser/image three-dimensional reconstruction, registration/segmentation and other commonly used point cloud algorithms. For technical exchanges and consultations, please send a private message