CloudCompare——拟合空间球

本文由CSDN点云侠原创，原文链接。爬虫网站自重。博客长期更新，本文最新更新时间为：2023年8月6日。

1.拟合球

  源码里用到了四点定球，具体计算原理见：使用克拉默法则进行四点定球。

2.软件操作

  通过菜单栏的'Tools > Fit > Sphere'找到该功能。

  选择一个或多个点云，然后启动此工具。CloudCompare将在每个点云上拟合球体基元。在控制台中，将输出以下信息：

• center（也可以在球体实体属性中找到球体边界框的中心）
• radius（也可以在sphere实体属性中找到）
• 球体拟合RMS（在默认球体实体名称中调用）注意：理论上球体拟合算法可以处理高达50％的异常值。

球形点云

拟合结果

控制台输出

3.算法源码

GeometricalAnalysisTools::ErrorCode GeometricalAnalysisTools::DetectSphereRobust(
	GenericIndexedCloudPersist* cloud,
	double outliersRatio,
	CCVector3& center,
	PointCoordinateType& radius,
	double& rms,
	GenericProgressCallback* progressCb/*=nullptr*/,
	double confidence/*=0.99*/,
	unsigned seed/*=0*/)
{
	if (!cloud)
	{
		assert(false);
		return InvalidInput;
	}

	unsigned n = cloud->size();
	if (n < 4)
		return NotEnoughPoints;

	assert(confidence < 1.0);
	confidence = std::min(confidence, 1.0 - FLT_EPSILON);

	//we'll need an array (sorted) to compute the medians
	std::vector<PointCoordinateType> values;
	try
	{
		values.resize(n);
	}
	catch (const std::bad_alloc&)
	{
		//not enough memory
		return NotEnoughMemory;
	}

	//number of samples
	unsigned m = 1;
	const unsigned p = 4;
	if (n > p)
	{
		m = static_cast<unsigned>(log(1.0 - confidence) / log(1.0 - pow(1.0 - outliersRatio, static_cast<double>(p))));
	}

	//for progress notification
	NormalizedProgress nProgress(progressCb, m);
	if (progressCb)
	{
		if (progressCb->textCanBeEdited())
		{
			char buffer[64];
			sprintf(buffer, "Least Median of Squares samples: %u", m);
			progressCb->setInfo(buffer);
			progressCb->setMethodTitle("Detect sphere");
		}
		progressCb->update(0);
		progressCb->start();
	}

	//now we are going to randomly extract a subset of 4 points and test the resulting sphere each time
	if (seed == 0)
	{
		std::random_device randomGenerator;   // non-deterministic generator
		seed = randomGenerator();
	}
	std::mt19937 gen(seed);  // to seed mersenne twister.
	std::uniform_int_distribution<unsigned> dist(0, n - 1);
	unsigned sampleCount = 0;
	unsigned attempts = 0;
	double minError = -1.0;
	std::vector<unsigned> indexes;
	indexes.resize(p);
	while (sampleCount < m && attempts < 2*m)
	{
		//get 4 random (different) indexes
		for (unsigned j = 0; j < p; ++j)
		{
			bool isOK = false;
			while (!isOK)
			{
				indexes[j] = dist(gen);
				isOK = true;
				for (unsigned k = 0; k < j && isOK; ++k)
					if (indexes[j] == indexes[k])
						isOK = false;
			}
		}

		assert(p == 4);
		const CCVector3* A = cloud->getPoint(indexes[0]);
		const CCVector3* B = cloud->getPoint(indexes[1]);
		const CCVector3* C = cloud->getPoint(indexes[2]);
		const CCVector3* D = cloud->getPoint(indexes[3]);

		++attempts;
		CCVector3 thisCenter;
		PointCoordinateType thisRadius;
		if (ComputeSphereFrom4(*A, *B, *C, *D, thisCenter, thisRadius) != NoError)
			continue;

		//compute residuals
		for (unsigned i = 0; i < n; ++i)
		{
			PointCoordinateType error = (*cloud->getPoint(i) - thisCenter).norm() - thisRadius;
			values[i] = error*error;
		}
		
		const unsigned int	medianIndex = n / 2;

		std::nth_element(values.begin(), values.begin() + medianIndex, values.end());

		//the error is the median of the squared residuals
		double error = static_cast<double>(values[medianIndex]);

		//we keep track of the solution with the least error
		if (error < minError || minError < 0.0)
		{
			minError = error;
			center = thisCenter;
			radius = thisRadius;
		}

		++sampleCount;

		if (progressCb && !nProgress.oneStep())
		{
			//progress canceled by the user
			return ProcessCancelledByUser;
		}
	}

	//too many failures?!
	if (sampleCount < m)
	{
		return ProcessFailed;
	}

	//last step: robust estimation
	ReferenceCloud candidates(cloud);
	if (n > p)
	{
		//e robust standard deviation estimate (see Zhang's report)
		double sigma = 1.4826 * (1.0 + 5.0 /(n-p)) * sqrt(minError);

		//compute the least-squares best-fitting sphere with the points
		//having residuals below 2.5 sigma
		double maxResidual = 2.5 * sigma;
		if (candidates.reserve(n))
		{
			//compute residuals and select the points
			for (unsigned i = 0; i < n; ++i)
			{
				PointCoordinateType error = (*cloud->getPoint(i) - center).norm() - radius;
				if (error < maxResidual)
					candidates.addPointIndex(i);
			}
			candidates.resize(candidates.size());

			//eventually estimate the robust sphere parameters with least squares (iterative)
			if (RefineSphereLS(&candidates, center, radius))
			{
				//replace input cloud by this subset!
				cloud = &candidates;
				n = cloud->size();
			}
		}
		else
		{
			//not enough memory!
			//we'll keep the rough estimate...
		}
	}

	//update residuals
	{
		double residuals = 0;
		for (unsigned i = 0; i < n; ++i)
		{
			const CCVector3* P = cloud->getPoint(i);
			double e = (*P - center).norm() - radius;
			residuals += e*e;
		}
		rms = sqrt(residuals/n);
	}

	return NoError;
}

4.相关代码

[1]C++实现：PCL RANSAC拟合空间3D球体
[2]python实现：Open3D——RANSAC三维点云球面拟合
[3] Open3D 最小二乘拟合球
[4] Open3D 非线性最小二乘拟合球