C++：最小二乘法 擬合平面 Ax + By + Cz + D = 0

分享給有需要的人，代碼質量勿噴。

/* 最小二乘法擬合平面 AX + BY + CZ + D = 0 */
void RANSAC::xjPlaneLeastSquaresEstimate(std::vector<double> &xjParameters, const std::vector<xjPoint> &xjData )
{
	double A = 0, B = 0, C = 0, D = 0;

	double meanX = 0, meanY = 0, meanZ = 0;
	double meanXX = 0, meanYY = 0, meanZZ = 0;
	double meanXY = 0, meanXZ = 0, meanYZ = 0;
	int count = xjData.size();
	for (int i = 0; i < count; i++)
	{
		meanX += xjData[i].x;
		meanY += xjData[i].y;
		meanZ += xjData[i].z;

		meanXX += xjData[i].x * xjData[i].x;
		meanYY += xjData[i].y * xjData[i].y;
		meanZZ += xjData[i].z * xjData[i].z;

		meanXY += xjData[i].x * xjData[i].y;
		meanXZ += xjData[i].x * xjData[i].z;
		meanYZ += xjData[i].y * xjData[i].z;
	}
	meanX /= count;
	meanY /= count;
	meanZ /= count;
	meanXX /= count;
	meanYY /= count;
	meanZZ /= count;
	meanXY /= count;
	meanXZ /= count;
	meanYZ /= count;

	/* eigenvector */
	Matrix3d eA;
	eA(0, 0) = meanXX - meanX * meanX; eA(0, 1) = meanXY - meanX * meanY; eA(0, 2) = meanXZ - meanX * meanZ;
	eA(1, 0) = meanXY - meanX * meanY; eA(1, 1) = meanYY - meanY * meanY; eA(1, 2) = meanYZ - meanY * meanZ;
	eA(2, 0) = meanXZ - meanX * meanZ; eA(2, 1) = meanYZ - meanY * meanZ; eA(2, 2) = meanZZ - meanZ * meanZ;
	Eigen::EigenSolver<Eigen::Matrix3d> eMat(eA);
	Matrix3d eD = eMat.pseudoEigenvalueMatrix();
	Matrix3d eV = eMat.pseudoEigenvectors();
	
	/* the eigenvector corresponding to the minimum eigenvalue */
	double eD1 = eD(0, 0);
	double eD2 = eD(1, 1);
	double eD3 = eD(2, 2);
	int minNumber = 0;
	if ((abs(eD2) <= abs(eD1)) && (abs(eD2) <= abs(eD3)))
	{
		minNumber = 1;
	}
	if ((abs(eD3) <= abs(eD1)) && (abs(eD3) <= abs(eD2)))
	{
		minNumber = 2;
	}
	A = eV(0, minNumber);
	B = eV(1, minNumber);
	C = eV(2, minNumber);
	D = -(A * meanX + B * meanY + C * meanZ);

	/* result */
	if (C < 0)
	{
		A *= -1;
		B *= -1;
		C *= -1;
		D *= -1;
	}
	xjParameters.push_back(A);
	xjParameters.push_back(B);
	xjParameters.push_back(C);
	xjParameters.push_back(D);
	xjParameters.push_back(meanX);
	xjParameters.push_back(meanY);
	xjParameters.push_back(meanZ);
}