欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。
本期话题：切比雪夫（最小区域法）直线拟合算法
相关背景和理论
点击前往
主要介绍了应用背景和如何转化成线性规划问题
圆拟合输入和输出要求
输入
- 10到631个点，全部采样自2D圆附近。
- 每个点3个坐标，坐标精确到小数点后面20位，最后1个坐标为0。
- 坐标单位是mm, 范围[-500mm, 500mm]。
输出
- 圆心1点C，用三个坐标表示。
- 半径r。
- 圆度F，所有点到圆距离最大的2倍。
F的最小区域法理解
黑色为点云。
对于圆来讲，最小区域是指用两个同心圆夹住点云，使得圆之间的半径之差最小。这个最小值就是F。拟合结果就是两圆中间的圆（橙色圆）。
精度要求
- C点到标准圆心距离不能超过0.0001mm。
- r与标准半径的差不能超过0.0001mm。
- F与标准圆度误差不能超过0.00001mm。
圆优化标函数
根据认证要求，圆拟合转化成数学表示如下：
圆参数化表示
- 圆心C = (x0, y0,0)。
- 半径r。
圆方程 ( x − x 0 ) 2 + ( y − y 0 ) 2 = r 2 圆方程 (x-x_0)^2+(y-y_0)^2=r^2 圆方程(x−x0)2+(y−y0)2=r2
点到圆距离
第i个点 pi(xi, yi, 0)。
根据定义得到距离
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 r_i = \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2} ri=(xi−x0)2+(yi−y0)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是未知量，拟合2D圆的过程也可以理解为优化X0, r使得方程E最小。
可以对上述方程求导，使得导数等于0取得最值。但是求导后会变成一个比较复杂的方程组，不好解。可以使用高斯牛顿迭代法来求解。
转化为线性规划
设 a = ( x 0 , y 0 , r ) , d i = F ( x i ; a ) , 引入 Γ ＝ M A X i = 1 n ∣ d i ∣ 设a=(x_0, y_0, r), d_i=F(x_i;\ a), 引入\Gamma＝\overset n{\underset {i=1}{MAX}}\;|d_i| 设a=(x0,y0,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 , Δ r 拆解，要求变量都要大于等于 0 需要对\Delta x_0, \Delta y_0, \Delta r 拆解，要求变量都要大于等于0 需要对Δx0,Δy0,Δr拆解，要求变量都要大于等于0
m a x Δ Γ s . t . J i ⋅ [ Δ x 0 + － Δ x 0 － Δ y 0 + － Δ y 0 － Δ r + − Δ r − ] + Δ Γ ≤ Γ － d i , ( i = 1 , 2... n ) − J i ⋅ [ Δ x 0 + － Δ x 0 － Δ y 0 + － Δ y 0 － Δ r + − Δ r − ] + Δ Γ ≤ Γ + d i , ( i = 1 , 2... n ) Δ x 0 + , Δ x 0 − , Δ y 0 + , Δ y 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 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 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 r, \Delta \Gamma \ge0\end{array} (1) max ΔΓs.t. Ji⋅ Δx0+－Δx0－Δy0+－Δy0－Δr+−Δr− +ΔΓ≤Γ－di,(i=1,2...n) −Ji⋅ Δx0+－Δx0－Δy0+－Δy0－Δr+−Δr− +ΔΓ≤Γ+di,(i=1,2...n)Δx0+,Δx0−,Δy0+,Δy0−,Δr,ΔΓ≥0(1)
算法描述
回顾一下
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 r_i = \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2} ri=(xi−x0)2+(yi−y0)2
J, D的计算。
3个未知分别对d_i求导结果如下：
∂ d i ∂ x 0 = 1 2 ( x i − x 0 ) 2 + ( y i − y 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}}\cdot(x_i-x_0)\cdot -1 = -(x_i-x_0)/r_i ∂x0∂di=2(xi−x0)2+(yi−y0)21⋅(xi−x0)⋅−1=−(xi−x0)/ri
∂ d i ∂ y 0 = 1 2 ( x i − x 0 ) 2 + ( y i − y 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}}\cdot(y_i-y_0)\cdot -1 = -(y_i-y_0)/r_i ∂y0∂di=2(xi−x0)2+(yi−y0)21⋅(yi−y0)⋅−1=−(yi−y0)/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 r = r + p r Γ = Γ − Δ Γ \begin {array}{c} x_0 = x_0+p_{x_0} \\y_0 = y_0+p_{y_0} \\ r=r+p_r \\ \Gamma = \Gamma-\Delta \Gamma \end {array} x0=x0+px0y0=y0+py0r=r+prΓ=Γ−ΔΓ -
重复2直到收敛
最后，输出时F=2*Γ
代码实现
代码链接：https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/CircleFitter.cpp
拟合代码
namespace Chebyshev {
double CircleFitter::F(Fitting::Circle2D circle, const Point& p)
{
auto de = Eigen::Vector2d(p.x(), p.y()) - circle.center;
return de.norm() - circle.r;
}
double CircleFitter::GetError(Fitting::Circle2D circle, const std::vector<Eigen::Vector3d>& points)
{
double err = 0;
for (auto& p : points) {
err = std::max(err, abs(F(circle, p)));
}
return err;
}
Fitting::Matrix CircleFitter::Jacobi(const std::vector<Eigen::Vector3d>& points)
{
Fitting::Matrix J(points.size(), 3);
for (int i = 0; i < points.size(); ++i) {
Eigen::Vector2d p = { points[i].x() - circle.center.x(),points[i].y() - circle.center.y() };
J(i, 0) = -p.x() / p.norm();
J(i, 1) = -p.y() / p.norm();
J(i, 2) = -1;
}
return J;
}
void CircleFitter::beforHook(const std::vector<Eigen::Vector3d>& points)
{}
void CircleFitter::afterHook(const Eigen::VectorXd& xp)
{
circle.center += Eigen::Vector2d(xp(0), xp(1));
circle.r += xp(2);
gamma -= xp(3);
}
Eigen::VectorXd CircleFitter::getDArray(const std::vector<Eigen::Vector3d>& points)
{
Eigen::VectorXd D(points.size());
for (int i = 0; i < points.size(); ++i)D(i) = F(circle, points[i]);
return D;
}
bool CircleFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points)
{
if (points.size() < 3)return false;
Fitting::FittingBase* fb = new Gauss::FittingCircle2D();
fb->Fitting(points, &circle);
delete fb;
gamma = GetError(circle, points);
return true;
}
double CircleFitter::F(const Eigen::Vector3d& p)
{
return Chebyshev::CircleFitter::F(circle, p);
}
double CircleFitter::GetError(const std::vector<Eigen::Vector3d>& points)
{
return Chebyshev::CircleFitter::GetError(circle, points);
}
void CircleFitter::Copy(void* ele)
{
memcpy(ele, &circle, sizeof(Fitting::Circle2D));
}
CircleFitter::CircleFitter()
{
ft = Fitting::FittingType::CHEBYSHEV;
}
}
测试结果
https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/chebyshev-testdata/officialtest/fitting_result/result.txt
C09 : CIRCLE_2D : pass
C10 : CIRCLE_2D : pass
C11 : CIRCLE_2D : pass
C12 : CIRCLE_2D : pass
C13 : CIRCLE_2D : pass
C14 : CIRCLE_2D : pass
C15 : CIRCLE_2D : pass
C16 : CIRCLE_2D : pass
本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接。
