OpenCV（C++）的曲线拟合polyfit

曲线拟合表达式：

$f(x)=a_0+a_1\ast x+a_2\ast x^2+a_3\ast x^3+...+a_n\ast x^n=[a_0,a_1,a_2,...,a_n][1,x_1,x_1^2,...,x_1^n{]}^T$

其中：

$a_0、a_1、a_2,....a_n$是幂系数，也是拟合所求的未知量。

经推导，得到最小二次方，幂函数拟合公式如下：

${\Phi }^T\ast \Phi \ast a={\Phi }^T\ast y$

其中$\Phi$是样本点坐标$x$的超定矩阵将所有x带入该向量$[1,x,x^2,...,x^n]$中，就得到超定矩阵$\Phi$。${\Phi }^T$表示$\Phi$的转置
ployfit函数的c++代码如下所示

void polyfit(vector<Point2f> chain, int n, Mat& a)
{
	Mat y(chain.size(), 1, CV_32F, Scalar::all(0));
/* ********【预声明phy超定矩阵】************************/
/* 多项式拟合的函数为多项幂函数
 * f(x)=a0+a1*x+a2*x^2+a3*x^3+......+an*x^n
 *a0、a1、a2......an是幂系数，也是拟合所求的未知量。设有m个抽样点，则：
 * 超定矩阵phy=1 x1 x1^2 ... ...  x1^n
 *           1 x2 x2^2 ... ...  x2^n
 *           1 x3 x3^2 ... ...  x3^n
 *              ... ... ... ...
 *              ... ... ... ...
 *           1 xm xm^2 ... ...  xm^n
 *
 * *************************************************/
    cv::Mat phy(chain.size(), n, CV_32F, Scalar::all(0));
	for (int i = 0; i<phy.rows; i++)
	{
		float* pr = phy.ptr<float>(i);
		for (int j = 0; j<phy.cols; j++)
		{
			pr[j] = pow(chain[i].x, j);
		}
		y.at<float>(i) = chain[i].y;
	}
	Mat phy_t = phy.t();
	Mat phyMULphy_t = phy.t()*phy;
	Mat phyMphyInv = phyMULphy_t.inv();
	a = phyMphyInv*phy_t;
	a = a*y;
}

示例如下

#include <iostream>
#include<opencv2/opencv.hpp>
using namespace std;
using namespace cv;
//下面宏定义CV_MAT_ELEM2为方便快速访问图像像素
#define CV_MAT_ELEM2(src,dtype,y,x) \
        (dtype*)(src.data+src.step[0]*y+src.step[1]*x)

Mat polyfit(std::vector<cv::Point2f> &chain, int n)
{
	Mat y(chain.size(), 1, CV_32F, Scalar::all(0));
	/* ********【预声明phy超定矩阵】************************/
	/* 多项式拟合的函数为多项幂函数
	* f(x)=a0+a1*x+a2*x^2+a3*x^3+......+an*x^n
	*a0、a1、a2......an是幂系数，也是拟合所求的未知量。设有m个抽样点，则：
	* 超定矩阵phy=1 x1 x1^2 ... ...  x1^n
	*           1 x2 x2^2 ... ...  x2^n
	*           1 x3 x3^2 ... ...  x3^n
	*              ... ... ... ...
	*              ... ... ... ...
	*           1 xm xm^2 ... ...  xm^n
	*
	* *************************************************/
	cv::Mat phy(chain.size(), n, CV_32F, Scalar::all(0));
	for (int i = 0; i<phy.rows; i++)
	{
		float* pr = phy.ptr<float>(i);
		for (int j = 0; j<phy.cols; j++)
		{
			pr[j] = pow(chain[i].x, j);
		}
		y.at<float>(i) = chain[i].y;
	}
	Mat phy_t = phy.t();
	Mat phyMULphy_t = phy.t()*phy;
	Mat phyMphyInv = phyMULphy_t.inv();
	Mat a = phyMphyInv*phy_t;
	a = a*y;
	return a;
}

int main()
{
	vector<Point2f> sp;
	//设有二次曲线点 g(x)=2+1.2x+1.1x^2,则:
	float a[] = { 2,1.2,1.1 };
	Mat image(500, 500, CV_32FC1, Scalar(0));
	Mat imageO(500, 500, CV_8U, Scalar(0));
	RNG rng;//预声明一个随机变量，用于作为离散点的干扰项
	for (int i = 1; i<20; i += 2)
	{
		Point2f p;
		p.x = i;
		for (int k = 0; k<3; k++)
		{
			p.y += a[k] * pow(i, k);//
		}

		p.y += rng.uniform(-1, 1);//为理想点位置添加随机干扰
								  /*将上面的p点以圆点的形式绘制到image上，为了观察方便，
								  * 将y坐标做了颠倒，坐标原点在image的左下角*/
		Point2f pi;
		pi.x = p.x;
		pi.y = image.rows - p.y;
		circle(image, p, 3, Scalar(255), -1);
		/*-------------end--------------------*/
		sp.push_back(p);
		cout << p << endl;
	}
	image.convertTo(image, CV_8UC1);
	imshow("distributed Points", image);
	Mat am = polyfit(sp, 3);
	cout << am << endl;
	waitKey();
	return 0;
}