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

闪电彬彬

已于 2024-02-27 11:57:28 修改

阅读量969

点赞数 19

分类专栏：高阶算法 c++ 数学文章标签：线性规划单纯形法最小区域法切比雪夫球拟合

于 2024-02-27 11:52:19 首次发布

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

版权

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

50 篇文章 2 订阅

订阅专栏

数学

36 篇文章 0 订阅

订阅专栏

c++

21 篇文章 0 订阅

订阅专栏

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

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

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

在这里插入图片描述

球拟合输入和输出要求

输入

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$

点到球距离

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

根据点乘得到距离

$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+(z_i-z_0)^2}$

优化能量方程

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

上式是一个4函数，X0, r是未知量，拟合球的过程也可以理解为优化X0, r使得方程H最小。

转化为线性规划

$设a=(x_0, y_0, z_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 z_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 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)$

算法描述

Ji, di的计算。

对4个未知数求导结果如下：

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

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

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

$\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} \\z_0 = z_0+p_{z_0} \\ r=r+p_r \\ \Gamma = \Gamma-\Delta \Gamma \end {array}$
重复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