CloudCompare——拟合空间球
本文由CSDN点云侠原创，CloudCompare——拟合空间球，爬虫自重。如果你不是在点云侠的博客中看到该文章，那么此处便是不要脸的爬虫与GPT生成的文章。
1.拟合球
??源码里用到了四点定球，具体计算原理如下：
??已知空间内不共面的四个点，设其坐标为
A
(
x
1
,
y
1
,
z
1
)
A(x_1,y_1,z_1)
A(x1?,y1?,z1?)、
B
(
x
2
,
y
2
,
z
2
)
B(x_2,y_2,z_2)
B(x2?,y2?,z2?)、
C
(
x
3
,
y
3
,
z
3
)
、
D
(
x
4
,
y
4
,
z
4
)
C(x_3,y_3,z_3)、D(x_4,y_4,z_4)
C(x3?,y3?,z3?)、D(x4?,y4?,z4?)，设半径为
r
r
r，球心
O
O
O坐标为
(
x
,
y
,
z
)
(x,y,z)
(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-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?x1?)2+(y?y1?)2+(z?z1?)2=r2;(x?x2?)2+(y?y2?)2+(z?z2?)2=r2;(x?x3?)2+(y?y3?)2+(z?z3?)2=r2;(x?x4?)2+(y?y4?)2+(z?z4?)2=r2;
展开得:
x
2
+
y
2
+
z
2
?
2
(
x
1
x
+
y
1
y
+
z
1
z
)
+
x
1
2
+
y
1
2
+
z
1
2
=
r
2
①
x
2
+
y
2
+
z
2
?
2
(
x
2
x
+
y
2
y
+
z
2
z
)
+
x
2
2
+
y
2
2
+
z
2
2
=
r
2
②
x
2
+
y
2
+
z
2
?
2
(
x
3
x
+
y
3
y
+
z
3
z
)
+
x
3
2
+
y
3
2
+
z
3
2
=
r
2
③
x
2
+
y
2
+
z
2
?
2
(
x
4
x
+
y
4
y
+
z
4
z
)
+
x
4
2
+
y
4
2
+
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④
x2+y2+z2?2(x1?x+y1?y+z1?z)+x12?+y12?+z12?=r2①x2+y2+z2?2(x2?x+y2?y+z2?z)+x22?+y22?+z22?=r2②x2+y2+z2?2(x3?x+y3?y+z3?z)+x32?+y32?+z32?=r2③x2+y2+z2?2(x4?x+y4?y+z4?z)+x42?+y42?+z42?=r2④
分别作①-②、③ - ④、② - ③得：
(
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
)
(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 )\\
(x1??x2?)x+(y1??y2?)y+(z1??z2?)z=1/2(x12??x22?+y12??y22?+z12??z22?)(x3??x4?)x+(y3??y4?)y+(z3??z4?)z=1/2(x32??x42?+y32??y42?+z32??z42?)(x2??x3?)x+(y2??y3?)y+(z2??z3?)z=1/2(x22??x32?+y22??y32?+z22??z32?)
其对应的系数行列式可设为：
D = ∣ a b c a 1 b 1 c 1 a 2 b 2 c 2 ∣ D=\left| \begin{matrix} a & b & c\\ a_1 & b_1 & c_1 \\ a_2 & b_2 & c_2 \end{matrix} \right| D= ?aa1?a2??bb1?b2??cc1?c2?? ?
则： 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 ) 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) a=(x1??x2?),b=(y1??y2?),c=(z1??z2?),a1?=(x3??x4?),b1?=(y3??y4?)，c1?=(z3??z4?),a2?=(x2??x3?),b2?=(y2??y3?)，c2?=(z2??z3?)
常数项行列式为:
L = ∣ P Q R ∣ L=\left| \begin{matrix} P\\ Q \\ R \end{matrix} \right| L= ?PQR? ?
则：
P
=
1
2
(
x
1
2
?
x
2
2
+
y
1
2
?
y
2
2
+
z
1
2
?
z
2
2
)
P=\frac{1}{2}(x_1^2 -x_2^2 + y_1^2 -y_2^2 + z_1^2 - z_2^2 )
P=21?(x12??x22?+y12??y22?+z12??z22?)
Q
=
1
2
(
x
3
2
?
x
4
2
+
y
3
2
?
y
4
2
+
z
3
2
?
z
4
2
)
Q=\frac{1}{2}(x_3^2 -x_4^2 + y_3^2 -y_4^2 + z_3^2 - z_4^2 )
Q=21?(x32??x42?+y32??y42?+z32??z42?)
R
=
1
2
(
x
2
2
?
x
3
2
+
y
2
2
?
y
3
2
+
z
2
2
?
z
3
2
)
R=\frac{1}{2}(x_2^2 -x_3^2 + y_2^2 -y_3^2 + z_2^2 - z_3^2 )
R=21?(x22??x32?+y22??y32?+z22??z32?)
现设：
D
x
=
∣
P
b
c
Q
b
1
c
1
R
b
2
c
2
∣
Dx=\left| \begin{matrix} P & b & c\\ Q & b_1 & c_1 \\ R & b_2 & c_2 \end{matrix} \right|
Dx=
?PQR?bb1?b2??cc1?c2??
?
D y = ∣ a P c a 1 Q c 1 a 2 R c 2 ∣ Dy=\left| \begin{matrix} a & P & c\\ a_1 & Q & c_1 \\ a_2 &R & c_2 \end{matrix} \right| Dy= ?aa1?a2??PQR?cc1?c2?? ?
D z = ∣ a b P a 1 b 1 Q a 2 b 2 R ∣ Dz=\left| \begin{matrix} a & b & P\\ a_1 & b_1 & Q \\ a_2 &b_2 & R \end{matrix} \right| Dz= ?aa1?a2??bb1?b2??PQR? ?
由线性代数中的克拉默法则可知:
x
=
D
x
D
x=\frac{Dx}{D}
x=DDx?
y = D y D y=\frac{Dy}{D} y=DDy?
z = D z D z=\frac{Dz}{D} z=DDz?
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 非线性最小二乘拟合球
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！