多项式拟合曲线对比

        项目中有些位置使用了多项式拟合曲线，进行了几种方式的实现，效果都不是特别理想，低阶拟合还基本OK，但是高阶拟合效果就不好，下面是效果较差的2种方式实现的代码，后期继续查看为什么效果不好。

手写了一版多项式的解法，效果还不错，对比下面两种方式，对于任意阶数拟合效果都很好。

方式1：

void PolyFit(std::vector<cv::Point>& in_point, int n, cv::Mat &cvK)
{
    int size = in_point.size();

    //所求未知数个数
    int x_num = n + 1;

    //构造矩阵U和Y
    cv::Mat mat_u = cv::Mat::zeros(size, x_num, CV_64F);
    cv::Mat mat_y = cv::Mat::zeros(size, 1, CV_64F);
    
    for (int i = 0; i < mat_u.rows; ++i)
    {
        for (int j = 0; j < mat_u.cols; ++j)
        {
            for (int k = 0; k < size; k++)
            {
                // 将横坐标设定为（-0.5, 0.5]范围内，避免求指数pow时数据溢出；
                // 经过验证横坐标在（-0.5, 0.5]范围拟合效果最理想
                // 全文对横坐标的定义必须一致
                double dX = double(k + 1 - size * 1.0 / 2) / size;
                mat_u.at<double>(i, j) += pow(dX, j + i);
            }
       }
    }

    for (int i = 0; i < mat_y.rows; ++i)
    {
        mat_y.at<double>(i, 0) = in_point[i].y;
    }

    //矩阵运算，获得系数矩阵K
    cv::Mat mat_u_t       = mat_u.t();
    cv::Mat mat_u_t_u_inv = (mat_u_t * mat_u).inv();
    cvK = mat_u_t_u_inv * mat_u_t * mat_y;
}

方式2：

std::vector<double> 
CalTic::PolyFit(std::vector<double> x, std::vector<double> y, int degree)
{
    int n = x.size();
    Mat A = Mat::zeros(degree + 1, degree + 1, CV_64F);
    Mat B = Mat::zeros(degree + 1, 1, CV_64F);
    for (int i = 0; i < degree + 1; i++)
    {
        for (int j = 0; j < degree + 1; j++)
        {
            for (int k = 0; k < n; k++)
            {
                A.at<double>(i, j) += pow(x[k], i + j);
            }
        }
    }

    for (int i = 0; i < degree + 1; i++) 
    {
        for (int k = 0; k < n; k++) 
        {
            B.at<double>(i, 0) += pow(x[k], i) * y[k];
        }
    }

    float f_det = cv::determinant(A);
    Mat coeffs;
    solve(A, B, coeffs, DECOMP_SVD);

    std::vector<double> result(degree + 1);
    for (int i = 0; i < degree + 1; i++)
    {
        result[i] = coeffs.at<double>(i, 0);
    }

    return result;
}

方式3（手写解法）

#pragma once
#include "opencv2/opencv.hpp"

class Polyfit
{
public:
	Polyfit();
	~Polyfit();
public:
    /// -------------------------------------------
	/// @brief  多项式拟合函数
	/// @note   
	/// @param  参数
	/// @return 返回
	/// -------------------------------------------
    bool DoPolyfit(const float* pSourceCurve, int nCurveLength, float* pTargetCurve, cv::Mat& cv_background, int iOrder, int iStartOffsetX = 0);

private:
    /// -------------------------------------------
	/// @brief  根据数据长度选择多项式的阶数
	/// @note   
	/// @param  参数
	/// @return 返回
	/// -------------------------------------------
    int AutoSelectDegree();

    /// -------------------------------------------
	/// @brief  计算多项式系数的增广矩阵
	/// @note   
	/// @param  参数
	/// @return 返回
	/// -------------------------------------------
    void DefineCoeffCalMatrix(const float* pSourceCurve);

	/// -------------------------------------------
	/// @brief  选择列主元
	/// @note   
	/// @param  参数
	/// @return 返回
	/// -------------------------------------------
    void SelectColumnPrincipalElement(int nColumnNum);

	/// -------------------------------------------
	/// @brief  高斯法解线性方程
	/// @note   
	/// @param  参数
	/// @return 返回
	/// -------------------------------------------
    void SolveEquationsWithGauss();

	/// -------------------------------------------
	/// @brief  在高斯法的基础上，计算出方程的解，即拟合多项式的系数
	/// @note   
	/// @param  参数
	/// @return 返回
	/// -------------------------------------------
    void CalculateEquationsAnswer();

	/// -------------------------------------------
	/// @brief  计算多项式拟合后的曲线
	/// @note   
	/// @param  参数
	/// @return 返回
	/// -------------------------------------------
    void CalculatePolynomial(float* pTargetCurve);

private:
	// 最大阶数
	static const int MAX_DEGREE = 32;

    double  m_dCoeffCalMatrix[MAX_DEGREE + 1][MAX_DEGREE + 2];  // 计算多项式系数的增广矩阵
    double  m_dPolynomialCoeff[MAX_DEGREE + 1];                 // 多项式系数，阶数与数组的下标一致
    int     m_iSrcCurveLen;                                     // 原始曲线长度
    int     m_iCoeffAmount;                                     // 拟合多项式的系数的个数，包括常数项，即为多项式阶数加1
};

#include <iostream>
#include <cmath>
#include <fstream>
#include "string.h"
#include "ContrastPolyfit.h"

Polyfit::Polyfit()
    : m_iSrcCurveLen(0)
    , m_iCoeffAmount(0)
{
    memset(m_dCoeffCalMatrix, 0, sizeof(double) * (MAX_DEGREE + 1) * (MAX_DEGREE + 2));
    memset(m_dPolynomialCoeff, 0, sizeof(double) * (MAX_DEGREE + 1));
}

Polyfit::~Polyfit()
{
}

bool Polyfit::DoPolyfit(const float* pfSourceCurve, int iCurveLen, float* pfDstCurve, cv::Mat& cvBackGround, int iOrder, int iStartOffsetX)
{
    if (nullptr == pfSourceCurve || 0 >= iCurveLen || nullptr == pfDstCurve)
    {
        assert(false);
        return false;
    }

    m_iSrcCurveLen = iCurveLen;

   
    m_iCoeffAmount = 1 + iOrder;// AutoSelectDegree();  // 根据数据长度选择多项式的阶数
    
    DefineCoeffCalMatrix(pfSourceCurve);

    SolveEquationsWithGauss();
    
    CalculateEquationsAnswer();
    
    CalculatePolynomial(pfDstCurve);

    return true;
}

int Polyfit::AutoSelectDegree()
{
    // 多项式阶数
    int nPolynomialDegree = 0;

    // 根据数据长度选择多项式的阶数
    if (100 > m_iSrcCurveLen)
    {
        nPolynomialDegree = 4;
    }
    else if (300 > m_iSrcCurveLen)
    {
        nPolynomialDegree = 8;
    }
    else if (700 > m_iSrcCurveLen)
    {
        nPolynomialDegree = 16;
    }
    else if (1200 > m_iSrcCurveLen)
    {
        nPolynomialDegree = 24;//TWENTY_DEGREE;
    }
    else
    {
        nPolynomialDegree = 32;
    }

    return nPolynomialDegree;
}

void Polyfit::DefineCoeffCalMatrix(const float* pfSourceCurve)
{
    memset(m_dCoeffCalMatrix, 0, (MAX_DEGREE + 1) * (MAX_DEGREE + 2) * sizeof(double));
    for (int i = 0; i < m_iCoeffAmount; i++)
    {
        for (int j = 0; j < m_iCoeffAmount; j++)
        {
            for (int k = 0; k < m_iSrcCurveLen; k++)
            {
                // 将横坐标设定为（-0.5, 0.5]范围内，避免求指数pow时数据溢出；
                // 经过验证横坐标在（-0.5, 0.5]范围拟合效果最理想
                // 全文对横坐标的定义必须一致
                double d_x = double(k + 1 - m_iSrcCurveLen / 2) / m_iSrcCurveLen;

                m_dCoeffCalMatrix[i][j] += float(pow(d_x, j + i));
            }
        }
    }

    // 定义增广矩阵的常数向量
    for (int i = 0; i < m_iCoeffAmount; i++)
    {
        for (int k = 0; k < m_iSrcCurveLen; k++)
        {
            // 将横坐标设定为（-0.5, 0.5]范围内，避免求指数pow时数据溢出；
            // 经过验证横坐标在（-0.5, 0.5]范围拟合效果最理想
            // 全文对横坐标的定义必须一致
            double d_x = double(k + 1 - m_iSrcCurveLen / 2) / m_iSrcCurveLen;

            m_dCoeffCalMatrix[i][m_iCoeffAmount] += float(pow(d_x, i) * pfSourceCurve[k]);
        }
    }
}


void Polyfit::SelectColumnPrincipalElement(int iColumnNum)
{
    // 列主元
    float f_col_principal_element = m_dCoeffCalMatrix[iColumnNum][iColumnNum];

    // 列主元所在的行号
    int i_row_of_principal_element = iColumnNum;

    // 搜索列主元，记录其所在的行号
    for (int i = iColumnNum + 1; i < m_iCoeffAmount; i++)
    {
        if (fabs(m_dCoeffCalMatrix[i][iColumnNum]) > fabs(f_col_principal_element))
        {
            f_col_principal_element = m_dCoeffCalMatrix[i][iColumnNum];
            i_row_of_principal_element = i;
        }
        else
        {
            continue;
        }
    }

    // 将列主元放置在pCoeffCalMatix[iColumnNum, iColumnNum]
    if (0 == f_col_principal_element)
    {
        printf("Error, column principal element can't be zero! \n");
    }
    else
    {
        if (iColumnNum != i_row_of_principal_element)
        {
            // 若列主元所在的行号与列号不同，则交换列主元与pCoeffCalMatix[iColumnNum, iColumnNum]的位置
            for (int j = iColumnNum; j < m_iCoeffAmount + 1; j++)
            {
                float fTemp = m_dCoeffCalMatrix[i_row_of_principal_element][j];
                m_dCoeffCalMatrix[i_row_of_principal_element][j] = m_dCoeffCalMatrix[iColumnNum][j];

                m_dCoeffCalMatrix[iColumnNum][j] = fTemp;
            }
        }
    }
}

void Polyfit::SolveEquationsWithGauss()
{
    for (int i_col = 0; i_col < m_iCoeffAmount; i_col++)
    {
        
        SelectColumnPrincipalElement(i_col); // 调用列主元函数

        for (int i_row = i_col + 1; i_row < m_iCoeffAmount; i_row++)
        {
            m_dCoeffCalMatrix[i_row][i_col] = m_dCoeffCalMatrix[i_row][i_col] / m_dCoeffCalMatrix[i_col][i_col];
        }

        for (int i_row = i_col + 1; i_row < m_iCoeffAmount; i_row++)
        {
            for (int j = i_col + 1; j < m_iCoeffAmount + 1; j++)
            {
                m_dCoeffCalMatrix[i_row][j] = m_dCoeffCalMatrix[i_row][j] - m_dCoeffCalMatrix[i_row][i_col] * m_dCoeffCalMatrix[i_col][j];
            }

            m_dCoeffCalMatrix[i_row][i_col] = 0;
        }
    }
}

void Polyfit::CalculateEquationsAnswer()
{
    m_dCoeffCalMatrix[m_iCoeffAmount - 1][m_iCoeffAmount] = m_dCoeffCalMatrix[m_iCoeffAmount - 1][m_iCoeffAmount] / m_dCoeffCalMatrix[m_iCoeffAmount - 1][m_iCoeffAmount - 1];
    for (int i = m_iCoeffAmount - 2; i >= 0; i--)
    {
        float f_for_equation_answer = 0.0f;

        for (int j = i + 1; j < m_iCoeffAmount; j++)
        {
            f_for_equation_answer = f_for_equation_answer + m_dCoeffCalMatrix[i][j] * m_dCoeffCalMatrix[j][m_iCoeffAmount];
        }

        // 多项式系数
        m_dCoeffCalMatrix[i][m_iCoeffAmount] = (m_dCoeffCalMatrix[i][m_iCoeffAmount] - f_for_equation_answer) / m_dCoeffCalMatrix[i][i];
    }

    // 初始化数组
    memset(m_dPolynomialCoeff, 0, MAX_DEGREE * sizeof(double));

    // 保存多项式系数
    for (int i = 0; i < m_iCoeffAmount; i++)
    {
        // 多项式系数，阶数与数组的下标一致
        m_dPolynomialCoeff[i] = m_dCoeffCalMatrix[i][m_iCoeffAmount];
    }
}

void Polyfit::CalculatePolynomial(float* pfDstCurve)
{
    memset(pfDstCurve, 0, m_iSrcCurveLen * sizeof(float));

    // 代入多项式系数，计算拟合后的曲线
    for (int i = 0; i < m_iSrcCurveLen; i++)
    {
        // 将横坐标设定为（-0.5, 0.5]范围内，避免求指数pow时数据溢出；
        // 经过验证横坐标在（-0.5, 0.5]范围拟合效果最理想
        // 全文对横坐标的定义必须一致
        double d_x = double(i + 1 - m_iSrcCurveLen / 2) / m_iSrcCurveLen;
        
        // 横坐标的幂
        double d_power = 1;

        // 曲线纵坐标初始值为多项式的常数项
        double d_curve_y = m_dPolynomialCoeff[0];

        for (int j = 1; j < m_iCoeffAmount; j++)
        {
            d_power *= d_x;

            d_curve_y += double(d_power) * m_dPolynomialCoeff[j];
        }

        // 数值必须不小于0
        if (d_curve_y < 0)
        {
            d_curve_y = 0;
        }

        pfDstCurve[i] = float(d_curve_y);
    }
}