PCL 平面拟合方法 对比

目录

一、最小二乘法 (Least Squares, LS)

二、采样一致性（Sample Consensus）方法

2.1 pcl::LeastMedianSquares (LMedS)

2.2 pcl::RandomSampleConsensus (RANSAC)

2.3 pcl::MEstimatorSampleConsensus (MSAC)

2.4 pcl::RandomizedRandomSampleConsensus (RRANSAC)

2.5 pcl::RandomizedMEstimatorSampleConsensus (RMSAC)

2.6 pcl::MaximumLikelihoodSampleConsensus (MLESAC)

2.7 pcl::ProgressiveSampleConsensus (PROSAC)

三、结果与对比（XBB）

---

一、最小二乘法 (Least Squares, LS)

分享给有需要的人，代码质量勿喷

void main()
{
	//load
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	pcl::io::loadPCDFile("F:/test.pcd", *cloud);

	//mat
	int pCount = cloud->points.size();
	Eigen::MatrixXd matPoints(pCount, 3);
	for (int i = 0; i < pCount; i++)
	{
		matPoints(i, 0) = cloud->points.at(i).x;
		matPoints(i, 1) = cloud->points.at(i).y;
		matPoints(i, 2) = cloud->points.at(i).z;
	}

	//-质心
	Eigen::RowVector3d centroid = matPoints.colwise().mean();
	Eigen::MatrixXd A = matPoints;
	A.rowwise() -= centroid;

	//SVD
	Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::MatrixXd U = svd.matrixU();
	Eigen::Matrix3d S = U.inverse() * A * V.transpose().inverse();// S = U^-1 * A * VT * -1

	//ax+by+cz+d=0
	Eigen::RowVector3d normal;
	normal << V(0, 2), V(1, 2), V(2, 2);
	double nd = -normal * centroid.transpose();
	double na = V(0, 2);
	double nb = V(1, 2);
	double nc = V(2, 2);
	if (nc < 0)//同向(0,0,1)
	{
		na *= (-1);
		nb *= (-1);
		nc *= (-1);
		nd *= (-1);
	}
}

二、采样一致性（Sample Consensus）方法

pcl::SampleConsensushttps://pointclouds.org/documentation/classpcl_1_1_sample_consensus.html#details

Point Cloud Library (PCL): pcl::SampleConsensus< T > Class Template Reference (pointclouds.org)

2.1 pcl::LeastMedianSquares (LMedS)

2.2 pcl::RandomSampleConsensus (RANSAC)

2.3 pcl::MEstimatorSampleConsensus (MSAC)

2.4 pcl::RandomizedRandomSampleConsensus (RRANSAC)

2.5 pcl::RandomizedMEstimatorSampleConsensus (RMSAC)

2.6 pcl::MaximumLikelihoodSampleConsensus (MLESAC)

2.7 pcl::ProgressiveSampleConsensus (PROSAC)

分享给有需要的人，代码质量勿喷

void main()
{
	//load
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	pcl::io::loadPCDFile("F:/test.pcd", *cloud);

	//SAC Plane
	pcl::SampleConsensusModelPlane::Ptr model_plane(new pcl::SampleConsensusModelPlane(cloud));

	//fit method
	pcl::LeastMedianSquares fitMethod(model_plane);
	//pcl::RandomSampleConsensus fitMethod(model_plane);
	//pcl::MEstimatorSampleConsensus fitMethod(model_plane);
	//pcl::RandomizedRandomSampleConsensus fitMethod(model_plane);
	//pcl::RandomizedMEstimatorSampleConsensus fitMethod(model_plane);
	//pcl::MaximumLikelihoodSampleConsensus fitMethod(model_plane);
	//pcl::ProgressiveSampleConsensus fitMethod(model_plane);

	fitMethod.setDistanceThreshold(0.01);//距离阈值
	fitMethod.computeModel();

	//面参数 ax+by+cz+d=0
	Eigen::VectorXf coefficient;
	fitMethod.getModelCoefficients(coefficient);

	double na = coefficient[0];
	double nb = coefficient[1];
	double nc = coefficient[2];
	double nd = coefficient[3];
	if (nc < 0)
	{
		na = -na;
		nb = -nb;
		nc = -nc;
		nd = -nd;
	}

	//内点
	std::vector inliers;//存储内点索引的向量
	fitMethod.getInliers(inliers);//提取内点对应的索引
	pcl::PointCloud::Ptr cloud_plane(new pcl::PointCloud);
	pcl::copyPointCloud(*cloud, inliers, *cloud_plane);
}

三、结果与对比（XBB）

感觉有些结果不大合理，但是不重要，嘻嘻。

fit method	a	b	c	d
Least Squares LS	0.00938328	0.0145269	0.99985	6.47668
CloudCompare-Plane-Fit LS	0.00938328	0.014527	0.99985
LeastMedianSquares LMedS	0.0106059	0.0129623	0.99986	6.44786
RandomSampleConsensus RANSAC	0.0177447	0.00836339	0.999808	6.35407
MEstimatorSampleConsensus MSAC	0.0177447	0.00836339	0.999808	6.35407
RandomizedRandomSampleConsensus RRANSAC	-0.184171	0.266717	0.946014	10.666
RandomizedMEstimatorSampleConsensus RMSAC	-0.184171	0.266717	0.946014	10.666
MaximumLikelihoodSampleConsensus MLESAC	0.0117652	0.0209168	0.999712	6.55405
ProgressiveSampleConsensus PROSAC	0.0117652	0.0209168	0.999712	6.55405