欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。
本期话题：利用线性规划求解点云最大内接圆
参考资料：
- Tschebyscheff approximation for the calculation of maximum inscribed minimum circumscribed geometry elements and form deviations 利用切比雪夫范数优化最值
- General solution for Tsebyshev approximation of form elements in coordinate measurement 将优化最值问题转化成线性规划问题
内容简介
- 问题提出
- 模型建立
- 算法调研
- 算法实现
- 测试
问题提出
在2D平面上给定一些点，整体形状类似于圆。求一个圆心在点云内部使得所有点都在圆外面，要求半径最大的圆。
虚线为点云，实线为要求的圆。
模型建立
给定圆心(x0, y0), 所有点到圆心的距离最小值就是圆的半径。
能量方程 R = f ( x 0 , y 0 ) = M I N i = 1 n d i 能量方程R=f(x0,y0) = \overset n{\underset {i=1}{MIN}}\;d_i 能量方程R=f(x0,y0)=i=1MINndi
d i = ( x i − x 0 ) 2 + ( y i − y 0 ) 2 d_i = \sqrt{(x_i-x_0)^2+(y_i-y_0)^2} di=(xi−x0)2+(yi−y0)2
求解过程就是优化x0,y0使得R最大。
算法调研
上述能量方程复杂的地方在于最小值不好直接表示。
切比雪夫范数方法
在Tschebyscheff approximation for the calculation of maximum inscribed minimum circumscribed geometry elements and form deviations 提到可以使用切比雪夫范数来表示。
具体如下
M I N i = 1 n d i 近似于 [ ∑ i = 1 n d i 1 / p ] p , p 是一个比较大的值。 \overset n{\underset {i=1}{MIN}}\;d_i 近似于 \left [ \displaystyle \sum_{i=1}^nd_i^{1/p} \right]^p, p是一个比较大的值。 i=1MINndi近似于[i=1∑ndi1/p]p,p是一个比较大的值。
该方法可以使用梯度下降法等方法求解最值，过程可能比较复杂个人不喜欢。
线性规划
沿着上述文章的引用找了很多文章，发现了一个比较优美的方法。
General solution for Tsebyshev approximation of form elements in coordinate measurement 将优化最值问题转化成线性规划问题
文章中介绍了通过一阶泰勒展开，引入极小量的方式，将问题转化为线性规划问题，从而使用单纯形法进行迭代求解。
化整为零
设 a = ( x 0 , y 0 ) , d i = F ( x i ; a ) , 引入 Γ ＝ M I N i = 1 n d i 设a=(x_0, y_0), d_i=F(x_i;\ a), 引入\Gamma＝\overset n{\underset {i=1}{MIN}}\;d_i 设a=(x0,y0),di=F(xi; a),引入Γ＝i=1MINndi
根据上述定义，可以将原来的最值问题转化为下述条件
对于所有点应该满足
F ( x i ; a ) ≥ Γ F(x_i;\ a)\ge \Gamma F(xi; a)≥Γ
我们可以通过小量迭代慢慢增大Γ
增量基本原理
设 a = ( a 0 , a 1 , . . . , a n ) 、 Γ 是待求解变量， a ^ , Γ ^ 是初始给定值， a = a ^ + Δ a , Γ = Γ ^ + Δ Γ . Δ a 、 Δ Γ 是我们每次迭代后移动的量 设 a=(a_0, a_1,...,a_n)、\Gamma 是待求解变量，\widehat {a}, \widehat {\Gamma} 是初始给定值，a = \widehat {a} +\Delta a, \Gamma = \widehat {\Gamma} +\Delta \Gamma. \ \Delta a、 \Delta \Gamma 是我们每次迭代后移动的量 设a=(a0,a1,...,an)、Γ是待求解变量，a ,Γ 是初始给定值，a=a +Δa,Γ=Γ +ΔΓ. Δa、ΔΓ是我们每次迭代后移动的量
定义距离函数为 F ( x , a ) , d i = F ( x i , a ) , 进行泰勒 1 阶展开， F ( x , a ) ＝ F ( x , a ^ ) + ∂ F ∂ a ^ Δ a = F ( x , a ^ ) + J Δ a 定义距离函数为 F(x, a), d_i = F(x_i, a), 进行泰勒1阶展开， F(x, a) ＝ F(x, \widehat a) + \frac {\partial F}{\partial \widehat a}\Delta a = F(x, \widehat a) + J\Delta a 定义距离函数为F(x,a),di=F(xi,a),进行泰勒1阶展开，F(x,a)＝F(x,a )+∂a ∂FΔa=F(x,a )+JΔa
每次迭代，其实就是希望通过调整 Δ a , Δ Γ ≥ 0 使得 F ( x , a ^ ) + J Δ a ≥ Γ ^ + Δ Γ . 每次迭代，其实就是希望通过调整\Delta a, \Delta \Gamma\ge0 使得 F(x, \widehat a) + J\Delta a \ge \widehat {\Gamma} +\Delta \Gamma. 每次迭代，其实就是希望通过调整Δa,ΔΓ≥0使得F(x,a )+JΔa≥Γ +ΔΓ.
J = [ ∂ F ( x 0 , a ^ ) ∂ a 0 ∂ F ( x 0 , a ^ ) ∂ a 1 . . . ∂ F ( x 0 , a ^ ) ∂ a n ∂ F ( x 1 , a ^ ) ∂ a 0 ∂ F ( x 1 , a ^ ) ∂ a 1 . . . ∂ F ( x 1 , a ^ ) ∂ a n . . . . . . . . . . . . ∂ F ( x n , a ^ ) ∂ a 0 ∂ F ( x n , a ^ ) ∂ a 1 . . . ∂ F ( x n , a ^ ) ∂ a n ] J = \begin {bmatrix} \frac {\partial F(x_0, \widehat {a})} {\partial a_0} & \frac {\partial F(x_0, \widehat {a})} {\partial a_1} & ...& \frac {\partial F(x_0, \widehat {a})} {\partial a_n} \\ \\ \frac {\partial F(x_1, \widehat {a})} {\partial a_0} & \frac {\partial F(x_1, \widehat {a})} {\partial a_1} & ...& \frac {\partial F(x_1, \widehat {a})} {\partial a_n} \\\\ ... & ... & ...& ... \\ \\ \frac {\partial F(x_n, \widehat {a})} {\partial a_0} & \frac {\partial F(x_n, \widehat {a})} {\partial a_1} & ...& \frac {\partial F(x_n, \widehat {a})} {\partial a_n} \end {bmatrix} J= ∂a0∂F(x0,a )∂a0∂F(x1,a )...∂a0∂F(xn,a )∂a1∂F(x0,a )∂a1∂F(x1,a )...∂a1∂F(xn,a )............∂an∂F(x0,a )∂an∂F(x1,a )...∂an∂F(xn,a )
F ( x , a ^ ) = [ d 1 d 2 . . . d m ] F(x, \widehat a) = \begin {bmatrix} d_1 \\ d_2 \\... \\ d_m \end {bmatrix} F(x,a )= d1d2...dm
整体问题就转化为线性规划问题
m a x Δ Γ s . t . 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 \ge \Gamma +\Delta \Gamma, (i=1,2...n)\\ \Delta \Gamma \ge0\end{array} max ΔΓs.t. F(xi,a)+JΔa≥Γ+ΔΓ,(i=1,2...n)ΔΓ≥0
求解出以后更新a, Γ。
算法描述
将线性规划模型应用于圆拟合
m a x Δ Γ s . t . F ( x i , { x 0 , y 0 } ) + J ⋅ ( Δ x 0 , Δ y 0 ) ≥ Γ + Δ Γ , ( i = 1 , 2... n ) Δ Γ ≥ 0 \begin {array}{c}max \ \ \ \ \Delta {\Gamma}\\ s.t.\ \ \ F(x_i, \{x_0, y_0\}) + J \cdot (\Delta x_0, \Delta y_0) \ge \Gamma +\Delta \Gamma, (i=1,2...n)\\ \Delta \Gamma \ge0\end{array} max ΔΓs.t. F(xi,{x0,y0})+J⋅(Δx0,Δy0)≥Γ+ΔΓ,(i=1,2...n)ΔΓ≥0
上述条件还不能直接用单纯形求解，要转化为线性规划点击前往问题
需要对 Δ x 0 , Δ y 0 拆解，要求变量都要大于等于 0 需要对\Delta x_0, \Delta y_0 拆解，要求变量都要大于等于0 需要对Δx0,Δy0拆解，要求变量都要大于等于0
m a x Δ Γ s . t . J i ⋅ ( Δ x 0 + － Δ x 0 － , Δ y 0 + － Δ y 0 － ) − Δ Γ ≥ Γ － d i , ( i = 1 , 2... n ) Δ x 0 + , Δ x 0 − , Δ y 0 + , Δ y 0 − , Δ Γ ≥ 0 \begin {array}{c}max \ \ \ \ \Delta {\Gamma}\\ s.t.\ \ \ J_i \cdot (\Delta x_0^+－\Delta x_0^－, \Delta y_0^+－\Delta y_0^－) -\Delta \Gamma\\\ge \Gamma－d_i, (i=1,2...n)\\ \Delta x_0^+, \Delta x_0^-, \Delta y_0^+, \Delta y_0^-,\Delta \Gamma \ge0\end{array} max ΔΓs.t. Ji⋅(Δx0+－Δx0－,Δy0+－Δy0－)−ΔΓ≥Γ－di,(i=1,2...n)Δx0+,Δx0−,Δy0+,Δy0−,ΔΓ≥0
Ji, di的计算。
d
i
=
(
x
i
−
x
0
)
2
+
(
y
i
−
y
0
)
2
d_i = \sqrt{(x_i-x_0)^2+(y_i-y_0)^2}
di=(xi−x0)2+(yi−y0)2
对2个未知数求导结果如下：
∂ 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 ) / d 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)/d_i ∂x0∂di=2(xi−x0)2+(yi−y0)21⋅(xi−x0)⋅−1=−(xi−x0)/di
∂ 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 ) / d 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)/d_i ∂y0∂di=2(xi−x0)2+(yi−y0)21⋅(yi−y0)⋅−1=−(yi−y0)/di
求解上述方程后，更新解
x 0 = x 0 + Δ x 0 + － Δ x 0 － y 0 = y 0 + Δ y 0 + － Δ y 0 － Γ = Γ + Δ Γ \begin {array}{l}x_0 = x_0 +\Delta x_0^+－\Delta x_0^－\\ y_0 = y_0 +\Delta y_0^+－\Delta y_0^－\\ \Gamma=\Gamma+\Delta \Gamma \end{array} x0=x0+Δx0+－Δx0－y0=y0+Δy0+－Δy0－Γ=Γ+ΔΓ
初始值确定可以使用高斯拟合2D圆点击前往
算法实现
代码链接：https://gitcode.com/chenbb1989/3DAlgorithm/tree/master/CBB3DAlgorithm/Fitting
算法步骤
- 高斯拟合初始值
- 求解d, J
- 构建线性规划方程
- 求解后更新解
- 迭代致收敛
代码框架基类设计
参考高斯拟合框架点击前往
一次迭代实现
添加了线性规划求解过程
Eigen::VectorXd FittingBase::findNext(const std::vector<Eigen::Vector3d>& points)
{
using namespace Eigen;
Eigen::VectorXd xp;
Eigen::VectorXd D = getDArray(points);
Matrix J = Jacobi(points);
if (ft == FittingType::CHEBYSHEV) {
BasicTools::Simplex::LPS lps;
int n = J.rows(), m = J.cols() * 2 + 1;
std::vector<double> c(m,0);
c[m-1] = 1;// 令最后一位为gamma
lps.InitProb(m, c, BasicTools::Simplex::MAX);
// 添加条件
std::vector<double> x(m, -1);
for (int i = 0; i < n; ++i) {
// 系数 J(i, 0), -J(i, 0),J(i, 1), -J(i, 1), ...,-1
for (int j = 0; j < J.cols(); ++j) {
x[j * 2] = J(i, j);
x[j * 2 + 1] = -x[j * 2];
}
lps.AddCondition(x, gamma - D(i), BasicTools::Simplex::GE);
}
auto res = lps.solve();
xp.resize(J.cols()+1);
xp.setZero();
/*std::cout << res.Z << std::endl;
std::cout << res.rt << std::endl;*/
if (res.rt == BasicTools::Simplex::NoSolution || res.rt == BasicTools::Simplex::NoUpBound) return xp;
xp(J.cols()) = res.x.back();
for (int i = 0; i < J.cols(); ++i) xp(i) = res.x[i * 2] - res.x[i * 2 + 1];
//}
}
else {
// 求解 Jp = -D https://blog.csdn.net/ABC_ORANGE/article/details/104489257/
xp = J.colPivHouseholderQr().solve(-D);
// xp = J.lu().solve(-D);
}
return xp;
}
代码实现
核心代码
#include "MaxInCircleFitter.h"
#include "../gauss/FittingCircle2D.h"
#include <Eigen/Dense>
namespace Chebyshev {
double F(Fitting::Circle2D circle, const Point& p)
{
return Eigen::Vector2d(p.x() - circle.center.x(), p.y() - circle.center.y()).norm();
}
double GetError(Fitting::Circle2D circle, const std::vector<Eigen::Vector3d>& points)
{
double err = -1;
for (auto& p : points) {
double d = F(circle, p);
if (err < 0 || d < err) err = d;
}
return err;
}
Fitting::Matrix MaxInCircleFitter::Jacobi(const std::vector<Eigen::Vector3d>& points)
{
Fitting::Matrix J(points.size(), 2);
for (int i = 0; i < points.size(); ++i) {
auto& p = points[i];
double ri = F(p);
J(i, 0) = -(p.x() - circle.center.x()) / ri;
J(i, 1) = -(p.y() - circle.center.y()) / ri;
}
return J;
}
void MaxInCircleFitter::afterHook(const Eigen::VectorXd& xp)
{
circle.center += Eigen::Vector2d(xp(0), xp(1));
circle.r += xp(2);
gamma = circle.r;
}
Eigen::VectorXd MaxInCircleFitter::getDArray(const std::vector<Eigen::Vector3d>& points)
{
Eigen::VectorXd D(points.size());
for (int i = 0; i < points.size(); ++i)D(i) = F(points[i]);
return D;
}
bool MaxInCircleFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points)
{
if (points.size() < 3)return false;
Fitting::FittingBase* fb = new Gauss::FittingCircle2D();
auto err = fb->Fitting(points, &circle);
delete fb;
if (err < 0)return false;
// 计算gamma
gamma = GetError(points);
circle.r = gamma;
return true;
}
double MaxInCircleFitter::F(const Eigen::Vector3d& p)
{
return Chebyshev::F(circle, p);
}
double MaxInCircleFitter::GetError(const std::vector<Eigen::Vector3d>& points)
{
return Chebyshev::GetError(circle, points);
}
void MaxInCircleFitter::Copy(void* ele)
{
memcpy(ele, &circle, sizeof(Fitting::Circle2D));
}
MaxInCircleFitter::MaxInCircleFitter()
{
ft = Fitting::FittingType::CHEBYSHEV;
}
}
测试
测试对比对象是最小区域法结果。
最小区域法是用2个同心圆去逼近点云使得δ最小。
x0,y0为圆心，r是大圆和小圆半径中间值。
小圆的半径r0=r-δ/2。
可知最大内接圆会比最小区域法的小圆稍微大一点点。
t1
最小区域法结果
x0:400.00000000000000000000
y0:390.00000000000000000000
r:20.00000000000000000000
δ:0.05000000000000000000
小圆半径 = 19.975
最大内接圆
x0:399.99855950184064568
y0:389.99798947515603231
r:19.975101964904933283
t2
最小区域法结果
x0:-200.00000000000000000000
y0:180.00000000000000000000
r:200.00000000000000000000
δ:0.02200000000000000000
小圆半径 = 199.989
最大内接圆
x0:-199.99277648219526782
y0:179.99999999999997158
r:199.98900013045519586
t3
最小区域法结果
x0:-250.00000000000000000000
y0:-250.00000000000000000000
r:125.00000000000000000000
δ:0.00500000000000000000
小圆半径 = 124.9975
最大内接圆
x0:-250.00127221264907007
y0:-250
r:124.9975000064742261
本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接。
![[极客大挑战2019]upload](https://img-blog.csdnimg.cn/direct/49f278c199284c09949439b897f06ce1.png)
