分享给有需要的人,代码质量勿喷。
/* 最小二乘法拟合平面 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);
}