最小二乘法拟合平面实现

给算法爸爸上香

已于 2023-03-18 21:46:21 修改

阅读量1.6k

点赞数 2

CC 4.0 BY-SA版权

分类专栏： PCL 文章标签：算法计算机视觉

于 2021-06-06 20:57:26 首次发布

本文链接：https://blog.csdn.net/taifyang/article/details/117636472

PCL 专栏收录该内容

26 篇文章

订阅专栏

这段代码展示了如何在PCL（Point Cloud Library）中运用最小二乘法来拟合点云数据的平面。首先计算点云的平均值和协方差矩阵，接着通过特征值分解找到对应的最小特征值所对应的特征向量，该特征向量即为平面参数。最后，根据需要调整特征向量的方向，输出平面方程的系数。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

原理：
在这里插入图片描述
实现：

/**
 * @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;
}