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

闪电彬彬

已于 2024-02-26 15:47:22 修改

阅读量1k

点赞数 19

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

于 2024-02-26 15:38:45 首次发布

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

版权

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

53 篇文章 6 订阅

订阅专栏

数学

37 篇文章 0 订阅

订阅专栏

c++

23 篇文章 0 订阅

订阅专栏

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

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

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

在这里插入图片描述

圆柱拟合输入和输出要求

输入

10到631个点，全部采样自圆柱附近。
每个点3个坐标，坐标精确到小数点后面20位。
坐标单位是mm, 范围[-500mm, 500mm]。

输出

1点X0表示圆柱中心轴上1点，用三个坐标表示。
法向A代表圆柱中心轴法向, 需要单位化。
圆柱半径r。
圆柱度F，所有点到圆柱距离最大的2倍。

精度要求

X0点圆柱中轴距离不能超过0.0001mm。
法向A与标准法向夹角不能超过0.0000001rad。
圆柱半径r与标准半径不能超过0.0001mm。
F与标准圆柱度误差不能超过0.00001mm。

圆柱优化标函数

根据论文，圆柱拟合转化成数学表示如下：

圆柱参数化表示

圆柱中心轴上1点X0 = (x0, y0, z0)。
法向A=(a,b,c)。
半径r。

$圆柱方程\begin {array}{l}\frac {u^2+v^2+w^2} {a^2+b^2+c^2}=r^2 \\ u_i =c(y-y_0)-b(z-z_0)\\ v_i=a(z-z_0)-c(x-x_0)\\ w_i=b(x-x_0)-a(y-y_0)\ \end {array}$

优化能量方程

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

距离函数

$d_i = r_i-r$

$\begin {array}{l}r_i = \sqrt{\frac {u^2+v^2+w^2} {a^2+b^2+c^2}} \\ u_i =c(y-y_0)-b(z-z_0)\\ v_i=a(z-z_0)-c(x-x_0)\\ w_i=b(x-x_0)-a(y-y_0) \end {array}$

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

X0, A,r是未知量，拟合过程也可以理解为优化X0, A,r使得方程H最小。

用4个数表示中轴直线

如果直接拿6个参数表示直线去做迭代，1是比较麻烦，会出现比较难解的方向，2是法向长度不固定，结果不唯一。

当直线与Z轴偏差比较小的时候可以使用4个参数来表示直线。

在这里插入图片描述
如上图，绿线为Z轴，橙色线为XOY平面。

由于法向与Z轴比较相近，可以设法向为(a, b, 1), a,b 是比较小的量。

规定直线上1点需要在以(a, b, 1)为法向，过0点的平面上。

则有 ax0+by0 + z0=0, 只要知道x0, y0 可知 z0 = -ax0-by0。

转化为线性规划（法向与Z轴接近）

$设a=(x_0, y_0, A_a, A_b,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 A_a, \Delta A_b,\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 A_a^+－\Delta A_a^－\\ \Delta A_b^+- \Delta A_b^- \\ \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 A_a^+－\Delta A_a^－\\ \Delta A_b^+- \Delta A_b^- \\ \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 A_a^+, \Delta A_a^-, \Delta A_b^+, \Delta A_b^-,\Delta r^+, \Delta r^-, \Delta \Gamma \ge0\end{array} (1)$

算法描述

法向与Z轴重合时
$x_0=0, y_0=0, z_0=0, a=0,b =0, c=1$

$\begin {array}{l}r_i=\sqrt{(x_i^2+y_i^2)}\end {array}$

J, D的计算。

5个未知分别对d_i求导结果如下：

回顾一下

$\begin {array}{l}u_i =c(y_i-y_0)-b(z_i-z_0)\\ v_i=a(z_i-z_0)-c(x_i-x_0)\\ w_i=b(x_i-x_0)-a(y_i-y_0)\end {array}$

$\frac {\partial d_i} {\partial x_0}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial x_0} \\ =\frac {\frac {(x_i-x_0)^2}{\partial x_0}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\\ =\frac {-x_i}{\sqrt{x_i^2+y_i^2}}$

$\frac {\partial d_i} {\partial y_0}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial y_0} \\ =\frac {\frac {(y_i-y_0)^2}{\partial y_0}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\\ =\frac {-y_i}{\sqrt{x_i^2+y_i^2}}$

$\frac {\partial d_i} {\partial a}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial a}\\ =\frac {\frac {[a(z_i-z_0)-c(x_i-x_0)]^2}{\partial a}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\\ =\frac {-x_iz_i}{\sqrt{x_i^2+y_i^2}}$

$\frac {\partial d_i} {\partial b}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial b}\\ =\frac {\frac {[c(y_i-y_0)-b(z_i-z_0)]^2}{\partial b}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\\ =\frac {-y_iz_i}{\sqrt{x_i^2+y_i^2}}$

$\frac {\partial d_i} {\partial r}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial r} = -1$

一次迭代过程

确定圆柱初值，Γ, 高斯拟合圆柱确定初值点击前往
根据上述公式(1)构建线性规划方程
求解 $\Delta p$
更新解
$\begin {array}{l} \\ \begin {bmatrix}x_0 \\ y_0 \\ z_0 \end {bmatrix} = \begin {bmatrix}x_0 \\ y_0 \\ z_0 \end {bmatrix} + U^T \cdot \begin{bmatrix}p_{x_0} \\ p_{y_0}\\ -p_ap_{x_0}-p_bp_{y_0}\end {bmatrix} \\ \\\begin {bmatrix}a \\ b \\ c \end {bmatrix} = U^T \cdot \begin{bmatrix}p_a \\ p_b \\ 1 \end {bmatrix}.normalize() \\\\ r=r+p_r \\\\ \Gamma = \Gamma-\Delta \Gamma \end {array}$
重复2直到收敛

最后，输出时F=2*Γ

代码实现

代码链接：https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/CylinderFitter.cpp

拟合代码

#include "CylinderFitter.h"
#include "../gauss/CylinderFitter.h"
#include <corecrt_math_defines.h>
#include <Eigen/Dense>
#include<iostream>


namespace Chebyshev {
	double CylinderFitter::F(Fitting::Cylinder cylinder, const Point& p)
	{
		double di = (p - cylinder.center).cross(cylinder.orient).norm() - cylinder.r;
		return di;
	}
	double CylinderFitter::GetError(Fitting::Cylinder cylinder, const std::vector<Eigen::Vector3d>& points)
	{
		double err = 0;
		for (auto& p : points) {
			err = std::max(err, abs(F(cylinder, p)));
		}

		return err;
	}
	Fitting::Matrix CylinderFitter::Jacobi(const std::vector<Eigen::Vector3d>& points)
	{
		int n = points.size();
		Fitting::Matrix J(n, 5);
		for (int i = 0; i < n; ++i) {
			auto& p = points[i];
			double ri = Eigen::Vector2d(p.x(), p.y()).norm();

			//di求导
			J(i, 0) = -p.x() / ri;
			J(i, 1) = -p.y() / ri;
			J(i, 2) = -p.x() * p.z() / ri;
			J(i, 3) = -p.y() * p.z() / ri;
			J(i, 4) = -1;
		}
		return J;
	}

	void CylinderFitter::beforHook(const std::vector<Eigen::Vector3d>& points)
	{
		U = Fitting::getRotationByOrient(cylinder.orient);
		for (int i = 0; i < points.size(); ++i) {
			transPoints[i] = U * (points[i] - cylinder.center);
		}
	}
	void CylinderFitter::afterHook(const Eigen::VectorXd& xp)
	{
		cylinder.center += U.transpose() * Eigen::Vector3d(xp(0), xp(1), -xp(2)*xp(0)-xp(3)*xp(1));
		cylinder.orient = U.transpose() * Eigen::Vector3d(xp(2), xp(3), 1).normalized();
		cylinder.r += xp(4);
		gamma -= xp(5);
	}
	Eigen::VectorXd CylinderFitter::getDArray(const std::vector<Eigen::Vector3d>& points)
	{
		int n = points.size();
		Eigen::VectorXd D(points.size());
		for (int i = 0; i < points.size(); ++i) D(i) = Eigen::Vector2d(points[i].x(), points[i].y()).norm() - cylinder.r;
		return D;
	}
	bool CylinderFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points)
	{
		if (points.size() < 5)return false;
		Fitting::FittingBase* fb = new Gauss::CylinderFitter();
		fb->Fitting(points, &cylinder);
		delete fb;
		gamma = GetError(points);
		return true;
	}
	double CylinderFitter::F(const Eigen::Vector3d& p)
	{
		return Chebyshev::CylinderFitter::F(cylinder, p);
	}
	double CylinderFitter::GetError(const std::vector<Eigen::Vector3d>& points)
	{
		return Chebyshev::CylinderFitter::GetError(cylinder, points);
	}
	void CylinderFitter::Copy(void* ele)
	{
		memcpy(ele, &cylinder, sizeof(Fitting::Cylinder));
	}

	CylinderFitter::CylinderFitter()
	{
		ft = Fitting::FittingType::CHEBYSHEV;
	}
}

测试结果

https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/chebyshev-testdata/officialtest/fitting_result/result.txt
C37 : CYLINDER : pass
C38 : CYLINDER : pass
C39 : CYLINDER : pass
C40 : CYLINDER : pass
C41 : CYLINDER : pass
C42 : CYLINDER : pass
C43 : CYLINDER : pass
C44 : CYLINDER : pass
C45 : CYLINDER : pass
C46 : CYLINDER : pass
C47 : CYLINDER : pass
C48 : CYLINDER : pass
C49 : CYLINDER : pass
C50 : CYLINDER : pass