原理:
实现:
/**
* @description: 最小二乘法拟合平面
* @param cloud 输入点云
*/
void FitPlaneByLeastSquares(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
if (cloud->points.size() < 3) return;
double meanX = 0, meanY = 0, meanZ = 0;
double meanXX = 0, meanYY = 0, meanZZ = 0;
double meanXY = 0, meanXZ = 0, meanYZ = 0;
for (int i = 0; i < cloud->points.size(); i++)
{
meanX += cloud->points[i].x;
meanY += cloud->points[i].y;
meanZ += cloud->points[i].z;
meanXX += cloud->points[i].x * cloud->points[i].x;
meanYY += cloud->points[i].y * cloud->points[i].y;
meanZZ += cloud->points[i].z * cloud->points[i].z;
meanXY += cloud->points[i].x * cloud->points[i].y;
meanXZ += cloud->points[i].x * cloud->points[i].z;
meanYZ += cloud->points[i].y * cloud->points[i].z;
}
meanX /= cloud->points.size();
meanY /= cloud->points.size();
meanZ /= cloud->points.size();
meanXX /= cloud->points.size();
meanYY /= cloud->points.size();
meanZZ /= cloud->points.size();
meanXY /= cloud->points.size();
meanXZ /= cloud->points.size();
meanYZ /= cloud->points.size();
/* eigenvector */
Eigen::Matrix3d m;
m(0, 0) = meanXX - meanX * meanX; m(0, 1) = meanXY - meanX * meanY; m(0, 2) = meanXZ - meanX * meanZ;
m(1, 0) = meanXY - meanX * meanY; m(1, 1) = meanYY - meanY * meanY; m(1, 2) = meanYZ - meanY * meanZ;
m(2, 0) = meanXZ - meanX * meanZ; m(2, 1) = meanYZ - meanY * meanZ; m(2, 2) = meanZZ - meanZ * meanZ;
Eigen::EigenSolver<Eigen::Matrix3d> PlMat(m * cloud->points.size());
Eigen::Matrix3d eigenvalue = PlMat.pseudoEigenvalueMatrix();
Eigen::Matrix3d eigenvector = PlMat.pseudoEigenvectors();
/* the eigenvector corresponding to the minimum eigenvalue */
double v1 = eigenvalue(0, 0), v2 = eigenvalue(1, 1), v3 = eigenvalue(2, 2);
int minNumber = 0;
if ((abs(v2) <= abs(v1)) && (abs(v2) <= abs(v3))) minNumber = 1;
if ((abs(v3) <= abs(v1)) && (abs(v3) <= abs(v2))) minNumber = 2;
double a = eigenvector(0, minNumber), b = eigenvector(1, minNumber), c = eigenvector(2, minNumber), d = -(a * meanX + b * meanY + c * meanZ);
if (c < 0)
{
a *= -1.0;
b *= -1.0;
c *= -1.0;
d *= -1.0;
}
std::cout << a << " " << b << " " << c << " " << d << std::endl;
}