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

大家好，欢迎来到IT知识分享网。

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

背景

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。拟合结果就是平行线中间的那条直线。

精度要求

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 转化为线性规划问题解决

凸包+旋转卡壳

问题解析

算法过程

正确性证明

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=(x_0,y_0,a,b),d_i=F(x_i;a),引入\Gamma ＝\stackrel{n}{\underset{i=1}{\mathrm{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)$

我们可以通过小量迭代慢慢减小Γ

增量基本原理

$设a=(a_0,a_1,\dots ,a_n)、\Gamma 是待求解变量，\hat{a},\hat{\Gamma }是初始给定值，a=\hat{a}+\Delta a,\Gamma =\hat{\Gamma }-\Delta \Gamma .\Delta a、\Delta \Gamma 是我们每次迭代后移动的量$

$定义距离函数为F(x,a),d_i=F(x_i,a),进行泰勒1阶展开，F(x,a)＝F(x,\hat{a})+\frac{\partial F}{\partial \hat{a}}\Delta a=F(x,\hat{a})+J\Delta a$

$每次迭代，其实就是希望通过调整\Delta a,\Delta \Gamma \ge 0使得F(x,\hat{a})+J\Delta a\le \hat{\Gamma }-\Delta \Gamma .$

$J=\left[\begin{array}{cccc}\frac{\partial F(x_0,\hat{a})}{\partial a_0}& \frac{\partial F(x_0,\hat{a})}{\partial a_1}& \dots & \frac{\partial F(x_0,\hat{a})}{\partial a_n}\\ & & & \\ \frac{\partial F(x_1,\hat{a})}{\partial a_0}& \frac{\partial F(x_1,\hat{a})}{\partial a_1}& \dots & \frac{\partial F(x_1,\hat{a})}{\partial a_n}\\ & & & \\ \dots & \dots & \dots & \dots \\ & & & \\ \frac{\partial F(x_n,\hat{a})}{\partial a_0}& \frac{\partial F(x_n,\hat{a})}{\partial a_1}& \dots & \frac{\partial F(x_n,\hat{a})}{\partial a_n}\end{array}\right]$

$F(x,\hat{a})=\left[\begin{array}{c}{d}_1\\ d_2\\ \dots \\ d_m\end{array}\right]$

整体问题就转化为线性规划问题

$\begin{array}{c}max\Delta \Gamma \\ s.t.F(x_i,a)+J\Delta a\le \Gamma -\Delta \Gamma ,(i=1,2\dots n)\\ -(F(x_i,a)+J\Delta a)\le \Gamma -\Delta \Gamma ,(i=1,2\dots n)\\ \Delta \Gamma \ge 0\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, Γ。

用2个数表示2D直线

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

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

如上图，绿线为Y轴，橙色线为X轴。

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

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

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

直线拟合模型转化(旋转到接近Y轴后)

将线性规划模型应用于直线拟合

m a x      Δ Γ s . t .     F ( x i , { x 0 , a } ) + J ⋅ ( Δ x 0 , Δ a ) ≤ Γ − Δ Γ , ( i = 1 , 2… n ) − F ( x i , { x 0 , a } ) − J ⋅ ( Δ x 0 , Δ a ) ≤ Γ − Δ Γ , ( i = 1 , 2… n ) Δ Γ ≥ 0 \begin {array}{c}max \ \ \ \ \Delta {\Gamma}\\ s.t.\ \ \ F(x_i, \{x_0, a\}) + J \cdot (\Delta x_0, \Delta a) \le \Gamma -\Delta \Gamma, (i=1,2…n)\\ -F(x_i, \{x_0, a\}) – J \cdot (\Delta x_0, \Delta a) \le \Gamma -\Delta \Gamma, (i=1,2…n)\\ \Delta \Gamma \ge0\end{array} max    ΔΓs.t.   F(xi​,{
x0​,a})+J⋅(Δx0​,Δa)≤Γ−ΔΓ,(i=1,2…n)−F(xi​,{
x0​,a})−J⋅(Δx0​,Δa)≤Γ−ΔΓ,(i=1,2…n)ΔΓ≥0​

上述条件还不能直接用单纯形求解，要转化为线性规划点击前往问题

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

$\begin{array}{c}max\Delta \Gamma \\ s.t.J_i\cdot (\Delta x_0^{+}－\Delta x_0^{－},\Delta a^{+}－\Delta a^{－})+\Delta \Gamma \\ \le \Gamma －d_i,(i=1,2\dots n)\\ -J_i\cdot (\Delta x_0^{+}－\Delta x_0^{－},\Delta a^{+}－\Delta a^{－})+\Delta \Gamma \\ \le \Gamma +d_i,(i=1,2\dots n)\\ \Delta x_0^{+},\Delta x_0^{-},\Delta a^{+},\Delta a^{-},\Delta \Gamma \ge 0\end{array}$

算法描述

回顾一下

$d_i=b(x_i-x_0)-a(y_i-y_0)=x_i,x_0=0,y_0=0,a=0,b=1$

J, D的计算。

$J=\begin{array}{lc}\frac{\partial d_1}{\partial x_0}& \frac{\partial d_1}{\partial a}\\ \frac{\partial d_2}{\partial x_0}& \frac{\partial d_2}{\partial a}\\ \dots & \dots \\ \frac{\partial d_n}{\partial x_0}& \frac{\partial d_n}{\partial a}\end{array},D=\begin{array}{l}{d}_1\\ d_2\\ \dots \\ d_n\end{array}$

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

$\frac{\partial d_i}{\partial x_0}=-b=-1$

$\frac{\partial d_i}{\partial a}=-y_i$

一次迭代过程

1. 确定直线初值
2. 将中轴通过刚体变换U至Z轴，U的构建可以参考代码
   $\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]=U\cdot \left(\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]-\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]\right)$
   
3. 根据上述公式构建线性规划方程
4. 求解$\Delta p$
5. 更新解
   $\begin{array}{l}\\ \left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+U^T\cdot \left[\begin{array}{c}{p}_{x_0}\\ -p_a{p}_{x_0}\end{array}\right]\\ \\ \left[\begin{array}{c}a\\ b\end{array}\right]=U^T\cdot \left[\begin{array}{c}{p}_a\\ 1\end{array}\right].normalize()\\ \\ \Gamma =\Gamma -\Delta \Gamma \end{array}$
   
6. 重复2直到收敛

最后，输出时F=2*Γ

初值确定

枚举所有两点确定直线，直线度误差最小的作为结果。

代码实现

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

拟合代码

 namespace Chebyshev { 
    double LineFitter::F(Fitting::Line2D line, const Point& p) { 
    auto de = Eigen::Vector2d(p.x(), p.y()) - line.BasePoint; return abs(de.x() * line.Orient.y() - de.y() * line.Orient.x()); } double LineFitter::GetError(Fitting::Line2D line, const std::vector<Eigen::Vector3d>& points) { 
    double err = 0; for (auto& p : points) { 
    err = std::max(err, F(line, p)); } return err; } Fitting::Matrix LineFitter::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]; J(i, 0) = -1; J(i, 1) = -p.y(); } return J; } void LineFitter::beforHook(const std::vector<Eigen::Vector3d>& points) { 
    U.setIdentity(); U(0, 0) = line.Orient.y(); U(0, 1) = -line.Orient.x(); U(1, 0) = line.Orient.x(); U(1, 1) = line.Orient.y(); Point kBasePt(line.BasePoint.x(), line.BasePoint.y(), 0); for (int i = 0; i < points.size(); ++i)transPoints[i] = U * (points[i] - kBasePt); } void LineFitter::afterHook(const Eigen::VectorXd& xp) { 
    Point bp = U.transpose() * Point(xp(0), -xp(0) * xp(1), 0); line.BasePoint += Eigen::Vector2d(bp.x(), bp.y()); bp = U.transpose() * Point(xp(1), 1, 0).normalized(); line.Orient = Eigen::Vector2d(bp.x(), bp.y()); gamma -= xp(2); } Eigen::VectorXd LineFitter::getDArray(const std::vector<Eigen::Vector3d>& points) { 
    Eigen::VectorXd D(points.size()); for (int i = 0; i < points.size(); ++i)D(i) = points[i].x(); return D; } bool LineFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points) { 
    if (points.size() < 2)return false; gamma = -1; // 枚举任意两点，选取误差最小的直线 for (int i = 0; i < points.size(); ++i) { 
    for (int j = i + 1; j < points.size(); ++j) { 
    Fitting::Line2D tline; tline.BasePoint = { 
    points[i].x(), points[i].y() }; tline.Orient = { 
    points[i].x() - points[j].x(), points[i].y() - points[j].y() }; tline.Orient.normalize(); double err = GetError(tline, points); if (gamma < 0 || err < gamma) { 
    gamma = err; line = tline; } } } return true; } double LineFitter::F(const Eigen::Vector3d& p) { 
    return Chebyshev::LineFitter::F(line, p); } double LineFitter::GetError(const std::vector<Eigen::Vector3d>& points) { 
    return Chebyshev::LineFitter::GetError(line, points); } void LineFitter::Copy(void* ele) { 
    memcpy(ele, &line, sizeof(Fitting::Line2D)); } LineFitter::LineFitter() { 
    ft = Fitting::FittingType::CHEBYSHEV; } } 

测试代码

 void TestAllCase() { 
    string caseDir = "D:/selfad/alg_and_graph/3DAlgorithm/CBB3DAlgorithm/Fitting/chebyshev/chebyshev-testdata/officialtest/"; FILE* caseList = fopen((caseDir+"data/kind.txt").c_str(), "r"); char baseID[100], kind[100]; FILE* testResult = fopen((caseDir + "fitting_result/result.txt").c_str(), "w"); while (fscanf(caseList, "%s", baseID) != EOF) { 
    strcpy(kind, baseID + 4); baseID[3] = 0; /*puts(baseID); puts(kind);*/ Fitting::TestBase* testLogic = getTestObj(kind); if (testLogic == NULL)continue; fprintf(testResult, "%s : %s : ", baseID, kind); printf("%s : %s : ", baseID, kind); testLogic->SetFile(caseDir, baseID); testLogic->readPoints(); testLogic->Fitting(); FILE* ansfp = fopen((caseDir + "fitting_result/" + baseID + ".txt").c_str(), "w"); testLogic->SaveAnswer(ansfp); fprintf(testResult, "%s\n", testLogic->JudgeAnswer(NULL)?"pass":"failed"); printf("%s\n", testLogic->JudgeAnswer(NULL)?"pass":"failed"); fclose(ansfp); delete testLogic; } fclose(caseList); fclose(testResult); puts("TEST COMPLETE"); } 

测试结果

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 

本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接。