欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。
本期话题：切比雪夫（最小区域法）球拟合算法
相关背景和理论
点击前往
主要介绍了应用背景和如何转化成线性规划问题
球拟合输入和输出要求
输入
- 10到631个点，全部采样自球附近上。
- 每个点3个坐标，坐标精确到小数点后面20位。
- 坐标单位是mm, 范围[-500mm, 500mm]。
输出
- 1点X0表示 球心，用三个坐标表示。
- 球半径r。
- 球度F，所有点到球面距离最大的2倍。
精度要求
- C点到标准球心距离不能超过0.0001mm。
- r与标准半径的差不能超过0.0001mm。
- F与标准球度误差不能超过0.00001mm。
球优化标函数
根据论文，球拟合转化成数学表示如下：
球参数化表示
- 球心为1点X0 = (x0, y0, z0)。
- 半径r。
球方程 ( x − x 0 ) 2 + ( y − y 0 ) 2 + ( z − z 0 ) 2 = r 2 球方程 (x-x_0)^2+(y-y_0)^2+(z-z_0)^2=r^2 球方程(x−x0)2+(y−y0)2+(z−z0)2=r2
点到球距离
第i个点 pi(xi, yi, zi)。
根据点乘得到距离
d i = ∥ ( p i − X 0 ) ∥ − r d_i =\left \| (p_i-X_0) \right \|-r di=∥(pi−X0)∥−r
展开一下：
d i = r i − r d_i = r_i -r di=ri−r
r i = ( x i − x 0 ) 2 + ( y i − y 0 ) 2 + ( z i − z 0 ) 2 r_i = \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2+(z_i-z_0)^2} ri=(xi−x0)2+(yi−y0)2+(zi−z0)2
优化能量方程
能量方程 H = f ( X 0 , r ) = max 1 n ∣ d i ∣ H=f(X0, r)=\displaystyle \max_1^n {|d_i|} H=f(X0,r)=1maxn∣di∣
上式是一个4函数，X0, r是未知量，拟合球的过程也可以理解为优化X0, r使得方程H最小。
转化为线性规划
设 a = ( x 0 , y 0 , z 0 , r ) , d i = F ( x i ; a ) , 引入 Γ ＝ M A X i = 1 n ∣ d i ∣ 设a=(x_0, y_0, z_0, r), d_i=F(x_i;\ a), 引入\Gamma＝\overset n{\underset {i=1}{MAX}}\;|d_i| 设a=(x0,y0,z0,r),di=F(xi; a),引入Γ＝i=1MAXn∣di∣
根据上述定义，可以将原来的最值问题转化为下述条件
对于所有点应该满足
F ( x i ; a ) ≤ Γ , ( F ( x i ; a ) > 0 ) F(x_i;\ a)\le \Gamma, (F(x_i;\ a)>0) F(xi; a)≤Γ,(F(xi; a)>0)
− F ( x i ; a ) ≤ Γ , ( F ( x i ; a ) < 0 ) -F(x_i;\ a)\le \Gamma, (F(x_i;\ a)<0) −F(xi; a)≤Γ,(F(xi; a)<0)
我们可以通过小量迭代慢慢减小Γ
m a x Δ Γ s . t . F ( x i , a ) + J Δ a ≤ Γ − Δ Γ , ( i = 1 , 2... n ) − ( F ( x i , a ) + J Δ a ) ≤ Γ − Δ Γ , ( i = 1 , 2... n ) Δ Γ ≥ 0 \begin {array}{c}max \ \ \ \ \Delta {\Gamma}\\ s.t.\ \ \ F(x_i, a) + J\Delta a \le \Gamma -\Delta \Gamma, (i=1,2...n)\\ \ \ \ \ \ \ \ \ \ -(F(x_i, a) + J\Delta a) \le \Gamma -\Delta \Gamma, (i=1,2...n)\\ \Delta \Gamma \ge0\end{array} max ΔΓs.t. F(xi,a)+JΔa≤Γ−ΔΓ,(i=1,2...n) −(F(xi,a)+JΔa)≤Γ−ΔΓ,(i=1,2...n)ΔΓ≥0
上述条件不需要管
F
(
x
i
,
a
)
+
J
Δ
a
正负情况，若
F
(
x
i
,
a
)
+
J
Δ
a
为正
−
(
F
(
x
i
,
a
)
+
J
Δ
a
)
≤
Γ
−
Δ
Γ
必成立，反之亦然。
上述条件不需要管F(x_i, a) + J\Delta a正负情况，若F(x_i, a) + J\Delta a为正-(F(x_i, a) + J\Delta a) \le \Gamma -\Delta \Gamma必成立，反之亦然。
上述条件不需要管F(xi,a)+JΔa正负情况，若F(xi,a)+JΔa为正−(F(xi,a)+JΔa)≤Γ−ΔΓ必成立，反之亦然。
求解出以后更新a, Γ。
对线性规划模型标准化
需要对 Δ x 0 , Δ y 0 , Δ z 0 , Δ r 拆解，要求变量都要大于等于 0 需要对\Delta x_0, \Delta y_0, \Delta z_0, \Delta r 拆解，要求变量都要大于等于0 需要对Δx0,Δy0,Δz0,Δr拆解，要求变量都要大于等于0
m a x Δ Γ s . t . J i ⋅ [ Δ x 0 + － Δ x 0 － Δ y 0 + － Δ y 0 － Δ z 0 + － Δ z 0 － Δ r + − Δ r − ] + Δ Γ ≤ Γ － d i , ( i = 1 , 2... n ) − J i ⋅ [ Δ x 0 + － Δ x 0 － Δ y 0 + － Δ y 0 － Δ z 0 + － Δ z 0 － Δ r + − Δ r − ] + Δ Γ ≤ Γ + d i , ( i = 1 , 2... n ) Δ x 0 + , Δ x 0 − , Δ y 0 + , Δ y 0 − , Δ z 0 + , Δ z 0 － , Δ r , Δ Γ ≥ 0 ( 1 ) \begin {array}{c}max \ \ \ \ \Delta {\Gamma}\\ s.t.\ \ \ J_i \cdot \begin {bmatrix} \Delta x_0^+－\Delta x_0^－\\ \Delta y_0^+－\Delta y_0^－\\\Delta z_0^+－\Delta z_0^－\\ \Delta r^+- \Delta r^- \end{bmatrix} +\Delta \Gamma\le \Gamma－d_i, (i=1,2...n)\\\\ \ \ \ \ \ \ -J_i \cdot \begin {bmatrix} \Delta x_0^+－\Delta x_0^－\\ \Delta y_0^+－\Delta y_0^－\\\Delta z_0^+－\Delta z_0^－\\ \Delta r^+- \Delta r^- \end{bmatrix}+\Delta \Gamma\le \Gamma+d_i, (i=1,2...n)\\ \Delta x_0^+, \Delta x_0^-, \Delta y_0^+, \Delta y_0^-, \Delta z_0^+,\Delta z_0^－,\Delta r, \Delta \Gamma \ge0\end{array} (1) max ΔΓs.t. Ji⋅ Δx0+－Δx0－Δy0+－Δy0－Δz0+－Δz0－Δr+−Δr− +ΔΓ≤Γ－di,(i=1,2...n) −Ji⋅ Δx0+－Δx0－Δy0+－Δy0－Δz0+－Δz0－Δr+−Δr− +ΔΓ≤Γ+di,(i=1,2...n)Δx0+,Δx0−,Δy0+,Δy0−,Δz0+,Δz0－,Δr,ΔΓ≥0(1)
算法描述
Ji, di的计算。
对4个未知数求导结果如下：
∂ d i ∂ x 0 = 1 2 ( x i − x 0 ) 2 + ( y i − y 0 ) 2 + ( z i − z 0 ) 2 ⋅ ( x i − x 0 ) ⋅ − 1 = − ( x i − x 0 ) / r i \frac {\partial d_i} {\partial x_0}=\frac {1} {2 \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2+(z_i-z_0)^2}}\cdot(x_i-x_0)\cdot -1 = -(x_i-x_0)/r_i ∂x0∂di=2(xi−x0)2+(yi−y0)2+(zi−z0)21⋅(xi−x0)⋅−1=−(xi−x0)/ri
∂ d i ∂ y 0 = 1 2 ( x i − x 0 ) 2 + ( y i − y 0 ) 2 + ( z i − z 0 ) 2 ⋅ ( y i − y 0 ) ⋅ − 1 = − ( y i − y 0 ) / r i \frac {\partial d_i} {\partial y_0}=\frac {1} {2 \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2+(z_i-z_0)^2}}\cdot(y_i-y_0)\cdot -1 = -(y_i-y_0)/r_i ∂y0∂di=2(xi−x0)2+(yi−y0)2+(zi−z0)21⋅(yi−y0)⋅−1=−(yi−y0)/ri
∂ d i ∂ z 0 = 1 2 ( x i − x 0 ) 2 + ( y i − y 0 ) 2 + ( z i − z 0 ) 2 ⋅ ( z i − z 0 ) ⋅ − 1 = − ( z i − z 0 ) / r i \frac {\partial d_i} {\partial z_0}=\frac {1} {2 \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2+(z_i-z_0)^2}}\cdot(z_i-z_0)\cdot -1 = -(z_i-z_0)/r_i ∂z0∂di=2(xi−x0)2+(yi−y0)2+(zi−z0)21⋅(zi−z0)⋅−1=−(zi−z0)/ri
∂ d i ∂ r = − 1 \frac {\partial d_i} {\partial r}=-1 ∂r∂di=−1
一次迭代过程
-
确定球初值，Γ,使用高斯拟合点击前往
-
根据上述公式(1)构建线性规划方程
-
求解 Δ p \Delta p Δp
-
更新解
x 0 = x 0 + p x 0 y 0 = y 0 + p y 0 z 0 = z 0 + p z 0 r = r + p r Γ = Γ − Δ Γ \begin {array}{c} x_0 = x_0+p_{x_0} \\y_0 = y_0+p_{y_0} \\z_0 = z_0+p_{z_0} \\ r=r+p_r \\ \Gamma = \Gamma-\Delta \Gamma \end {array} x0=x0+px0y0=y0+py0z0=z0+pz0r=r+prΓ=Γ−ΔΓ -
重复2直到收敛
最后，输出时F=2*Γ
代码实现
代码链接：https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/SphereFitter.cpp
拟合代码
#include "SphereFitter.h"
#include "../gauss/SphereFitter.h"
#include <algorithm>
#include <Eigen/Dense>
namespace Chebyshev {
double SphereFitter::F(Fitting::Sphere sphere, const Point& p)
{
auto de = p - sphere.center;
return de.norm() - sphere.r;
}
double SphereFitter::GetError(Fitting::Sphere sphere, const std::vector<Eigen::Vector3d>& points)
{
double err = 0;
for (auto& p : points) {
err = std::max(err, abs(F(sphere, p)));
}
return err;
}
Fitting::Matrix SphereFitter::Jacobi(const std::vector<Eigen::Vector3d>& points)
{
Fitting::Matrix J(points.size(), 4);
for (int i = 0; i < points.size(); ++i) {
auto p = points[i] - sphere.center;
J(i, 0) = -p.x() / p.norm();
J(i, 1) = -p.y() / p.norm();
J(i, 2) = -p.z() / p.norm();
J(i, 3) = -1;
}
return J;
}
void SphereFitter::beforHook(const std::vector<Eigen::Vector3d>& points)
{}
void SphereFitter::afterHook(const Eigen::VectorXd& xp)
{
sphere.center += Eigen::Vector3d(xp(0), xp(1), xp(2));
sphere.r += xp(3);
gamma -= xp(4);
}
Eigen::VectorXd SphereFitter::getDArray(const std::vector<Eigen::Vector3d>& points)
{
Eigen::VectorXd D(points.size());
for (int i = 0; i < points.size(); ++i)D(i) = F(sphere, points[i]);
return D;
}
bool SphereFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points)
{
if (points.size() < 4)return false;
Fitting::FittingBase* fb = new Gauss::SphereFitter();
fb->Fitting(points, &sphere);
delete fb;
gamma = GetError(sphere, points);
return true;
}
double SphereFitter::F(const Eigen::Vector3d& p)
{
return Chebyshev::SphereFitter::F(sphere, p);
}
double SphereFitter::GetError(const std::vector<Eigen::Vector3d>& points)
{
return Chebyshev::SphereFitter::GetError(sphere, points);
}
void SphereFitter::Copy(void* ele)
{
memcpy(ele, &sphere, sizeof(Fitting::Sphere));
}
SphereFitter::SphereFitter()
{
ft = Fitting::FittingType::CHEBYSHEV;
}
}
测试结果
https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/chebyshev-testdata/officialtest/fitting_result/result.txt
C27 : SPHERE : pass
C28 : SPHERE : pass
C29 : SPHERE : pass
C30 : SPHERE : pass
C31 : SPHERE : pass
C32 : SPHERE : pass
C33 : SPHERE : pass
C34 : SPHERE : pass
C35 : SPHERE : pass
C36 : SPHERE : pass
本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接。
