切比雪夫（最小区域法）圆拟合算法

闪电彬彬

已于 2024-02-26 17:11:39 修改

阅读量1.6k

点赞数 19

分类专栏：高阶算法 c++ 数学文章标签：切比雪夫拟合最小区域法线性规划泰勒展开

于 2024-02-23 17:28:08 首次发布

本文链接：https://blog.csdn.net/chenbb1989/article/details/136258295

版权

高阶算法同时被 3 个专栏收录

54 篇文章 7 订阅

订阅专栏

数学

38 篇文章 0 订阅

订阅专栏

c++

23 篇文章 0 订阅

订阅专栏

欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。

本期话题：切比雪夫（最小区域法）圆拟合算法

相关背景和理论
点击前往
主要介绍了应用背景和如何转化成线性规划问题

在这里插入图片描述

圆拟合输入和输出要求

输入

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$

点到圆距离

第i个点 pi(xi, yi, 0)。

根据定义得到距离

$d_i =\left \| (p_i-X_0) \right \|-r$

展开一下：

$d_i = r_i -r$

$r_i = \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2}$

优化能量方程

能量方程 $r)=\displaystyle \max_1^n {|d_i|}$

上式是一个4元二次函数中，X0, r是未知量，拟合2D圆的过程也可以理解为优化X0, r使得方程E最小。

可以对上述方程求导，使得导数等于0取得最值。但是求导后会变成一个比较复杂的方程组，不好解。可以使用高斯牛顿迭代法来求解。

转化为线性规划

$设a=(x_0, y_0, r), d_i=F(x_i;\ a), 引入\Gamma＝\overset n{\underset {i=1}{MAX}}\;|d_i|$

根据上述定义，可以将原来的最值问题转化为下述条件

对于所有点应该满足

$F(x_i;\ a)\le \Gamma, (F(x_i;\ a)>0)$

$-F(x_i;\ a)\le \Gamma, (F(x_i;\ a)<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}$

$上述条件不需要管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必成立，反之亦然。$
求解出以后更新a, Γ。

对线性规划模型标准化

$需要对\Delta x_0, \Delta y_0, \Delta r 拆解，要求变量都要大于等于0$

$\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)$

算法描述

回顾一下

$d_i = r_i -r$

$r_i = \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2}$

J, D的计算。

3个未知分别对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)/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$

$\frac {\partial d_i} {\partial r}=-1$

一次迭代过程

确定圆初值，Γ,使用高斯拟合点击前往
根据上述公式(1)构建线性规划方程
求解 $\Delta p$
更新解
$\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}$
重复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