最小二乘法直线拟合

方法1：感觉鲁棒性刚好些，但在两点拟合时出现 x1 无限接近 x2 或者 y1 无限接近 y2 时效果不好。

方法2：不存在方法1的问题，但是鲁棒性感觉不如方法1好。

FitLine1(std::vector<cv::Point2f> & SrcPts, MV_Line & DstLine)
{
    int nSize = SrcPts.size();
	if (nSize < 2)
		return;

    // 这里注意，为了提高精度，需要判断拟合的方向 X 方向还是 Y 方向
	double dMinX = SrcPts[0].x, dMaxX = SrcPts[0].x;
	double dMinY = SrcPts[0].y, dMaxY = SrcPts[0].y;
	for (int i = 0; i < nSize; i++)
	{
		if (SrcPts[i].x < dMinX)
			dMinX = SrcPts[i].x;
		if (SrcPts[i].x > dMaxX)
			dMaxX = SrcPts[i].x;

		if (SrcPts[i].y < dMinY)
			dMinY = SrcPts[i].y;
		if (SrcPts[i].y > dMaxY)
			dMaxY = SrcPts[i].y;
	}

	int nType = 0;//0=X方向 1=Y方向
	if (abs(dMaxY - dMinY) > abs(dMaxX - dMinX))
		nType = 1;

	// 如果 X 方向直接复制，如果 Y 方向则颠倒 X Y 坐标顺序
		
	std::vector<cv::Point2f> Pts;
	if (nType == 0)
	{
		for (int i = 0; i < nSize; i++)
		{
			Pts.push_back(cv::Point2f(SrcPts[i].x, SrcPts[i].y));
		}
	}
	else
	{
		for (int i = 0; i < nSize; i++)
		{
			Pts.push_back(cv::Point2f(SrcPts[i].y, SrcPts[i].x));
		}
	}

	// 最小二乘法拟合
	double A = 0;
	double B = 0;
	double C = 0;
	double D = 0;
	double E = 0;
	double F = 0;
	double a = 0;
	double b = 0;
	double c = 0; 
	double dYmean = 0;

	for (int i = 0; i < Pts.size(); i++)
	{
		A += Pts[i].x * Pts[i].x;
		B += Pts[i].x;
		C += Pts[i].x * Pts[i].y;
		D += Pts[i].y;
	}
	dYmean = D / nSize;
	E = Pts.size() * A - B * B;
	a = Pts.size() * C - B * D;
	b = (-1) * E;
	c = A * D - B * C;

    // 计算结果
	DstLine.A = a / b;
	DstLine.B = 1;
	DstLine.C = c / b;
	DstLine.Error = 0;

	// 调整方向
	if (nType == 1)
	{
		DstLine.A = 1;
		DstLine.B = a / b;
		DstLine.C = c / b;
	}
		
	DstLine.SSR = 0;
	DstLine.SSE = 0;
	DstLine.SST = 0;
	DstLine.R = 1;
	
	if (DstLine.A == 0 || DstLine.B == 0)
		return;

	for (int i = 0; i < nSize; i++)
	{
		double dY = DstLine.Get_k() * SrcPts[i].x + DstLine.Get_b();
		double dError = dY - SrcPts[i].y;
		DstLine.SSR += std::pow(dY - dYmean, 2);
		DstLine.SSE += std::pow(dError, 2);
		DstLine.Error += std::abs(dError);
	}
	DstLine.SST = DstLine.SSR + DstLine.SSE;
	DstLine.R = 1 - DstLine.SSE / DstLine.SST;
}

void FitLine2(std::vector<cv::Point2f> & SrcPts, MV_Line & DstLine)
{
	int size = SrcPts.size();
	if (size < 2)
		return ;

	double x_mean = 0;
	double y_mean = 0;
	for (int i = 0; i < size; i++)
	{
		x_mean += SrcPts[i].x;
		y_mean += SrcPts[i].y;
	}
	x_mean /= size;
	y_mean /= size; //至此，计算出了 x y 的均值

	double Dxx = 0, Dxy = 0, Dyy = 0;
	for (int i = 0; i < size; i++)
	{
		Dxx += (SrcPts[i].x - x_mean) * (SrcPts[i].x - x_mean);
		Dxy += (SrcPts[i].x - x_mean) * (SrcPts[i].y - y_mean);
		Dyy += (SrcPts[i].y - y_mean) * (SrcPts[i].y - y_mean);
	}

	if (Dxx == 0)
	{
		DstLine.A = 1;
		DstLine.B = 0;
		DstLine.C = x_mean * (-1);
	}
	else if (Dyy == 0)
	{
		DstLine.A = 0;
		DstLine.B = 1;
		DstLine.C = y_mean * (-1);
	}
	else
	{
		double lambda = ((Dxx + Dyy) - sqrt((Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy)) / 2.0;
		double den = sqrt(Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx));
		DstLine.A = Dxy / den;
		DstLine.B = (lambda - Dxx) / den;
		DstLine.C = (-1)*DstLine.A * x_mean - DstLine.B * y_mean;
	}
		
	DstLine.SSR = 0;
	DstLine.SSE = 0;
	DstLine.SST = 0;
	DstLine.R = 1;

	if (DstLine.A * DstLine.B == 0)
		return;

	for (int i = 0; i < size; i++)
	{
		double dY = DstLine.Get_k() * SrcPts[i].x + DstLine.Get_b();
		double dError = dY - SrcPts[i].y;
		DstLine.SSR += std::pow(dY - y_mean, 2);
		DstLine.SSE += std::pow(dError, 2);
		DstLine.Error += std::abs(dError);
	}
	DstLine.SST = DstLine.SSR + DstLine.SSE;
	DstLine.R = 1 - DstLine.SSE / DstLine.SST;

	return ;
}