RANSACアルゴリズム平面円フィット_C++実装

点群のセットを入力し、RANSACアルゴリズムを使用して平面円フィットを行い、円心と半径を得る

#include 
#include 
#include 


struct PointCloud3d
{
     
	double x,y,z;
}


bool RansacCircle(QList<PointCloud3d>& cloud, double* circlePara)
{
     
	int cloudSize = cloud.size();    
	int maxIterCnt = cloudSize / 2;  //      
	double maxErrorThreshold = 0.02; //      （             ）
	int consensusCntThreshold = maxIterCnt;

	if (!cloudSize || cloudSize <= 3)
	{
     
		return false;
	}

	//                  ，             
	std::default_random_engine rng;
	std::uniform_int_distribution <int> uniform(0, cloudSize - 1);
	rng.seed(777);  //seed              
	
	QList<int> selectIndexs;            //          
	QList<PointCloud3d> selectPoints;   //         
	QList<int> consensusIndexs;         //             

	double centerX = 0, centerY = 0, R = 0; //     
	double modelMeanError = 0;              //    
	int bestConsensusCnt = 0;               //          
	int iter = 0;                           //    

	//      
	while (iter < maxIterCnt)
	{
     
		selectIndexs.clear();
		selectPoints.clear();
		//          3  ，      3           
		for (int c = 0; c < 3; ++c)
		{
     
			selectPoints.append(cloud.at(uniform(rng)));
		}

		const double& x1 = selectPoints.at(0).x;
		const double& y1 = selectPoints.at(0).y;
	
		const double& x2 = selectPoints.at(1).x;
		const double& y2 = selectPoints.at(1).y;
		
		const double& x3 = selectPoints.at(2).x;
		const double& y3 = selectPoints.at(2).y;
		
		//               
		double xa = (x1 + x2) / 2.0, ya = (y1 + y2) / 2.0;
		double xb = (x1 + x3) / 2.0, yb = (y1 + y3) / 2.0;

		double ka = (x1 - x2) / (y2 - y1);
		double kb = (x1 - x3) / (y3 - y1);

		centerX = (yb - ya + ka * xa - kb * xb) / (ka - kb);
		centerY = ka * centerX - ka * xa + ya;
		R = sqrt((centerX - xa)*(centerX - xa) + (centerY - ya)*(centerY - ya));

		double meanError = 0;
		QList<int> tmpConsensusIndexs; //             ，         ，      。
		for (int j = 0; j < cloudSize; ++j)
		{
     
			const PointCloud3d& point = cloud.at(j);
			double distance = abs(R - sqrt((point.x - centerX)*(point.x - centerX) + (point.y - centerY)*(point.y - centerY)));
			if (distance < maxErrorThreshold)
			{
     
				tmpConsensusIndexs.append(j);
			}
			meanError += distance;
		}

		if (tmpConsensusIndexs.size() >= bestConsensusCnt && tmpConsensusIndexs.size() >= consensusCntThreshold)
		{
     
			bestConsensusCnt = consensusIndexs.size();  //              
			modelMeanError = meanError / cloudSize;
			consensusIndexs.clear();
			consensusIndexs = tmpConsensusIndexs;        //          
			circlePara[0] = centerX;  //  X
			circlePara[1] = centerY;  //  Y
			circlePara[2] = R;        //  
		}
		iter++;
	}
	return true;
}