项目中有些位置使用了多项式拟合曲线,进行了几种方式的实现,效果都不是特别理想,低阶拟合还基本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);
}
}