三维视觉--更好的点云平面参数获取（C++版）实现

之前有遇到过在使用PCL进行点云的ransac平面拟合的时候，所获得的内点平面参数不准确的情况。因为受到ransac迭代次数、点云厚度等的影响，所以会存在获取平面参数不准确的情况。

这篇文章中，我想大家分享一下当时我的解决方法，就是对ransac之后的内点进行最小二乘法的平面拟合。这样可以最大限度的发挥这两个方法的优势。

具体代码如下：

    //使用RANSAC获取点云平面的内点
    pcl::ModelCoefficients::Ptr coefficients1(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers1(new pcl::PointIndices);
	pcl::SACSegmentation<pcl::PointXYZRGB> seg1;
	seg1.setOptimizeCoefficients(true);
	seg1.setModelType(pcl::SACMODEL_PLANE);
	seg1.setMethodType(pcl::SAC_RANSAC);
	seg1.setMaxIterations(1000);
	seg1.setDistanceThreshold(1);
	seg1.setInputCloud(surface_plane);
	seg1.segment(*inliers1, *coefficients1);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp_plane(new pcl::PointCloud<pcl::PointXYZRGB>);
	for (size_t i = 0; i < inliers1->indices.size(); ++i)
	{
		temp_plane->points.push_back(surface_plane->points[inliers1->indices[i]]);
	}
    //对内点进行最小二乘数平面拟合
    Eigen::Vector4d centroid;                    // 质心
	Eigen::Matrix3d covariance_matrix;           // 协方差矩阵
	// 计算归一化协方差矩阵和质心
	pcl::computeMeanAndCovarianceMatrix(*temp_plane, covariance_matrix, centroid);
	// 计算协方差矩阵的特征值与特征向量
	Eigen::Matrix3d eigenVectors;
	Eigen::Vector3d eigenValues;
	pcl::eigen33(covariance_matrix, eigenVectors, eigenValues);
	// 查找最小特征值的位置
	Eigen::Vector3d::Index minRow, minCol;
	eigenValues.minCoeff(&minRow, &minCol);
	// 获取平面方程：AX+BY+CZ+D = 0的系数
	Eigen::Vector3d normal = eigenVectors.col(minCol);
	double D = -normal.dot(centroid.head<3>());

	cout << "平面模型系数为：\n"
		<< "A=" << normal[0] << "\n"
		<< "B=" << normal[1] << "\n"
		<< "C=" << normal[2] << "\n"
		<< "D=" << D << "\n" << endl;

草稿代码可能不是那么严谨，但运行起来问题不大。