CloudCompare——拟合空间球

1.拟合球

源码里用到了四点定球，具体计算原理如下：

已知空间内不共面的四个点，设其坐标为 $A(x_1,y_1,z_1)$ 、 $B(x_2,y_2,z_2)$ 、 $C(x_3,y_3,z_3)、D(x_4,y_4,z_4)$ ，设半径为 $r$ ，球心 $O$ 坐标为 $(x, y, z)$ 。利用四点到球心距离相等的性质得到如下四个方程。
$x-x_1)^2 + (y-y_1)^2 +(z-z_1)^2 =r^2;\\ (x-x_2)^2 + (y-y_2)^2 +(z-z_2)^2 =r^2;\\ (x-x_3)^2 + (y-y_3)^2 +(z-z_3)^2 =r^2;\\ (x-x_4)^2 + (y-y_4)^2 +(z-z_4)^2 =r^2;$

展开得:
$x^2 + y^2 + z^2- 2(x_1x+y_1y+z_1z)+x_1^2+y_1^2 + z_1^2 = r^2 ①\\ x^2 + y^2 + z^2- 2(x_2x+y_2y+z_2z)+x_2^2+y_2^2 + z_2^2 = r^2②\\ x^2 + y^2 + z^2- 2(x_3x+y_3y+z_3z)+x_3^2+y_3^2 + z_3^2 = r^2③\\ x^2 + y^2 + z^2- 2(x_4x+y_4y+z_4z)+x_4^2+y_4^2 + z_4^2 = r^2④$

分别作①-②、③ - ④、② - ③得：
$x_1-x_2)x+(y_1-y_2)y+(z_1-z_2)z=1/2(x_1^2 -x_2^2 + y_1^2 -y_2^2 + z_1^2 -z_2^2 )\\ (x_3-x_4)x+(y_3-y_4)y+(z_3-z_4)z=1/2(x_3^2 -x_4^2 + y_3^2 -y_4^2 + z_3^2 -z_4^2 )\\ (x_2-x_3)x+(y_2-y_3)y+(z_2-z_3)z=1/2(x_2^2 -x_3^2 + y_2^2 -y_3^2 + z_2^2 -z_3^2 )\\$

其对应的系数行列式可设为：

$D=\left| \begin{matrix} a & b & c\\ a_1 & b_1 & c_1 \\ a_2 & b_2 & c_2 \end{matrix} \right|$

则： $a=(x_1-x_2),b=(y_1-y_2),c=(z_1-z_2),\\a_1=(x_3-x_4),b_1=(y_3-y_4)，c_1=(z_3-z_4),\\ a_2=(x_2-x_3),b_2=(y_2-y_3)，c_2=(z_2-z_3)$

常数项行列式为:

$L=\left| \begin{matrix} P\\ Q \\ R \end{matrix} \right|$

则：
$P=\frac{1}{2}(x_1^2 -x_2^2 + y_1^2 -y_2^2 + z_1^2 - z_2^2 )$
$Q=\frac{1}{2}(x_3^2 -x_4^2 + y_3^2 -y_4^2 + z_3^2 - z_4^2 )$
$R=\frac{1}{2}(x_2^2 -x_3^2 + y_2^2 -y_3^2 + z_2^2 - z_3^2 )$

现设：
$Dx=\left| \begin{matrix} P & b & c\\ Q & b_1 & c_1 \\ R & b_2 & c_2 \end{matrix} \right|$

$Dy=\left| \begin{matrix} a & P & c\\ a_1 & Q & c_1 \\ a_2 &R & c_2 \end{matrix} \right|$

$Dz=\left| \begin{matrix} a & b & P\\ a_1 & b_1 & Q \\ a_2 &b_2 & R \end{matrix} \right|$

由线性代数中的克拉默法则可知:
$x=\frac{Dx}{D}$

$y=\frac{Dy}{D}$

$z=\frac{Dz}{D}$

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 mediansstd::vector<PointCoordinateType> values;try{values.resize(n);}catch (const std::bad_alloc&){//not enough memoryreturn NotEnoughMemory;}//number of samplesunsigned 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 notificationNormalizedProgress 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 timeif (seed == 0){std::random_device randomGenerator;   // non-deterministic generatorseed = 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) indexesfor (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 residualsfor (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 residualsdouble error = static_cast<double>(values[medianIndex]);//we keep track of the solution with the least errorif (error < minError || minError < 0.0){minError = error;center = thisCenter;radius = thisRadius;}++sampleCount;if (progressCb && !nProgress.oneStep()){//progress canceled by the userreturn ProcessCancelledByUser;}}//too many failures?!if (sampleCount < m){return ProcessFailed;}//last step: robust estimationReferenceCloud 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 sigmadouble maxResidual = 2.5 * sigma;if (candidates.reserve(n)){//compute residuals and select the pointsfor (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;
}