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

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

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

背景

ptb认证（切比雪夫认证）

之前有高斯认证点击前往

ptb是对几何体拟合算法的认证。

主要涉及2D直线，平面，2D圆，球，圆柱。

官方会给出点云信息，由用户将拟合结果上传到官方服务器进行对比答案，返回结果。

拟合有很多种度量标准，不同的标准出来的答案可能不完全精确。所以，要通过认证必须用官方给定的度量方法，具体可以参考论文。

认证精度要求

对于位置类型，比如圆心，直线的点等，误差不能超过0.0001mm。

对于方向，与标准值夹角不能直过0.0000001rad。

对于半径，误差不能超过0.0001mm。

对于最小区域宽度，误差不能超过0.00001mm。

学习资料

论文资料
线性规划求解最小区域
General solution for Tsebyshev approximation of form elements in coordinate measurement

线性规划应用点击前往

直线拟合输入和输出要求

输入

1. 10到631个点，全部采样自直线附近。
2. 每个点3个坐标，坐标精确到小数点后面20位，最后1个坐标为0。
3. 坐标单位是mm, 范围[-500mm, 500mm]。

输出

1. 直线上1点X0，用三个坐标表示。
2. 直线方向A，用三个坐标表示，需要单位化。
3. 直线度F，所有点到直线距离最大的2倍。

F的最小区域法理解

黑色为点云。

对于直线来讲，最小区域是指用两条平行的直线去夹住点云，使得平行线之间的距离最小。这个最小距离就是F。拟合结果就是平行线中间的那条直线。

精度要求

1. X0点到标准直线距离不能超过0.0001mm。
2. A与标准法向的夹角不能超过0.0000001rad。
3. F与标准直线度误差不能超过0.00001mm。

直线优化标函数

根据认证要求，直线拟合转化成数学表示如下：

直线参数化表示

1. 直线上1点X0 = (x0, y0,0)。
2. 方向单位向量A=(a,b, 0)。

点到直线距离

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

可以根据叉乘长度为面积，面积又等于底乘高，点到直线的距离是叉乘结果除以底。底是单位向量。

$d_i=H=\frac{\Vert (p_i-X_0)\times A\Vert }{\Vert A\Vert }$

$d_i=\Vert (p_i-X_0)\times A\Vert$

展开一下：

$d_i=\sqrt{(u_i^2+v_i^2+w_i^2)}=w_i$

$u_i=c(y_i-y_0)-b(z_i-z_0)=0$

$v_i=a(z_i-z_0)-c(x_i-x_0)=0$

$w_i=b(x_i-x_0)-a(y_i-y_0)$

优化能量方程

切比雪夫拟合要求所有距离中的最大值要最小。

能量方程 $H=f(X0,A)=\underset{1}{\overset{n}{\max }}{d}_i$

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

这里给出2种解法，1 利用凸包性质求解，2 转化为线性规划问题解决

凸包+旋转卡壳

学习资料：
凸包点击前往

问题解析

最小区域法具体要求是使用两平行线去夹点云，使得平行线之间的距离最小。
目标线为平行线的中间线。可以先求出点云的凸包，再用旋转卡壳算法确定最小区域。

算法过程

1.先找到点云的凸包。
2.以凸包一条边L为起边找到离该最远的点O, 过点O的与L平行线L’即可组成一组平行线，判断距离是否为最短，并更新。
3.继续旋转L至下一条凸包边，O可以继承上次的停止点。
4.直到遍历完所有L.

正确性证明

1.目标平行线必有一条为凸包一条边。

绿线通过凸包2个点P1，P2，蓝色平行线垂直于绿线，分别过P1，P2.
此时，是通过P1，P2间距最大的一组平行线。随着旋转与绿线夹角变小，距离就会变短。
所以，如果现条线都不为凸包的边，就可以通过旋转，使得与绿线夹角变小，从而得到更短的平行线。

2.一条边的最远点，可以作为下一条边的初始点。

可以看出对于红线来说，最远点是一个凸函数，会先上升后下降。
当找到点后，逆时针找蓝线的最高点，O对于蓝来说肯定还处长上升趋势。

代码实现

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

拟合代码

// 凸包旋转卡壳算法
namespace Chebyshev {
   
	using namespace std;
	const int M = 1e6 + 10;

	const double eps = 1e-6;
	using Point = Eigen::Vector2d;

	double operator^ (const Point & p1, const Point &p2) {
   
		return p1.x()* p2.y() - p1.y() * p2.x();
	}

	Point points[M];
	Point lowPoint;
	int st[M], top;

	bool cmp(Point p1, Point p2) {
   
		p1 = p1 - lowPoint;
		p2 = p2 - lowPoint;

		double xmult = p1^p2; // 求叉积
		if (abs(xmult)>eps) {
   
			return xmult > 0;
		}

		return p1.norm() < p2.norm();
	}

	void graham(int n) {
   
		lowPoint = points[0];
		for (int j = 0; j < n; ++j) {
   
			if (points[j].y() < lowPoint.y() || (points[j].y() == lowPoint.y() && points[j].x() < lowPoint.x())) lowPoint = points[j];
		}
		sort(points, points + n, cmp);
		top = 2;
		st[0] = 0;
		st[1] = 1;

		for (int i = 2; i < n; ++i) {
   
			while (top > 2 && ((points[st[top - 1]] - points[st[top - 2]]) ^ (points[i] - points[st[top - 1]])) <= eps)top--;
			st[top++] = i;
		}
	}

	double rotate(Fitting::Line2D & line) {
   
		double err = -1;
		st[top] = st[0]; // 将第一点连接后最后，作为最后一条边的终点
		int up = 1;

		for (int i = 0; i < top; ++i) {
   
			Point bottom = points[st[i + 1]] - points[st[i]];
			bottom.normalize();
			// 以i, i+1 线段为底
			// 查看顶部最高点, 发现下一个点比当前点高，就+1
			while (abs(bottom ^ (points[st[up]] - points[st[i]])) < abs(bottom ^ (points[st[up + 1]] - points[st[i]]))) up = (up + 1) % top;
			double d = abs((points[st[up]] - points[st[i]]) ^ bottom);
			if (err < 0 || d < err) {
   
				err = d;
				line.BasePoint = points[st[up]] + points[st[i]];
				line.BasePoint /= 2;
				line.Orient = bottom;
			}
		}

		return err;
	}

	double ConvexRotateFitting(const std::vector<Eigen::Vector3d>& point3ds, Fitting::Line2D& line)
	{
   
		for (int i = 0; i < point3ds.size(); ++i) points[i] = Point(point3ds[i].x(), point3ds[i].y());
		graham(point3ds.size());
		double err = rotate(line);
		return err;
	}
}

测试结果

https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/chebyshev-testdata/officialtest/fitting_result/result.txt

C01 : LINE_2D : pass
C02 : LINE_2D : pass
C03 : LINE_2D : pass
C04 : LINE_2D : pass
C05 : LINE_2D : pass
C06 : LINE_2D : pass
C07 : LINE_2D : pass
C08 : LINE_2D : pass

线性规划迭代法

化整为零

$设a$