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

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

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

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

在这里插入图片描述

平拟合输入和输出要求

输入

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

输出

平面上一点X0，用三个坐标表示。
法向A。
平面度F，所有点到平面距离最大的2倍。

精度要求

X0点平面距离不能超过0.0001mm。
法向A与标准法向夹角不能超过0.0000001rad。
F与标准平面度误差不能超过0.00001mm。

平面优化标函数

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

平面参数化表示

平面上1点X0 = (x0, y0, z0)。
方向单位向量A=(a,b,c)。

点到平面距离

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

根据点乘得到距离

$d_i = \frac { \left \| (p_i-X_0)\cdot A \right \|}{\left \| A \right \|}$

展开一下：

$d_i = \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)} {\sqrt{a^2+b^2+c^2}}$

优化能量方程

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

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

如何3个参数表示平面

如果直接拿6个参数去做迭代，1是比较麻烦，会出现比较难解的方向，2是平面上的点有很多个，结果不唯一。

当平面法向与Z轴偏差比较小的时候可以使用3个参数来表示。

在这里插入图片描述

如上图，绿线为Z轴，橙色线为XOY平面。

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

现在表示平面还需要一个点，规定点必须在以(a,b,1)为法向过0点的直线上。

就有直线公式 x0/a=y0/b=z0/c

那么只要知道z0就可知 x0=az0, y0=bz0.

综上，可以使用a, b, z0 表示一个法向与Z轴相近的平面。

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

$设a=(z_0, A_a, A_b), 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 z_0, \Delta A_a, \Delta A_b 拆解，要求变量都要大于等于0$

$\begin {array}{c}max \ \ \ \ \Delta {\Gamma}\\ s.t.\ \ \ J_i \cdot \begin {bmatrix} \Delta z_0^+－\Delta z_0^－\\ \Delta A_a^+－\Delta A_a^－\\ \Delta A_b^+- \Delta A_b^- \end{bmatrix} +\Delta \Gamma\le \Gamma－d_i, (i=1,2...n)\\\\ \ \ \ \ \ \ -J_i \cdot \begin {bmatrix} \Delta z_0^+－\Delta z_0^－\\ \Delta A_a^+－\Delta A_a^－\\ \Delta A_b^+- \Delta A_b^- \end{bmatrix}+\Delta \Gamma\le \Gamma+d_i, (i=1,2...n)\\ \Delta z_0^+, \Delta z_0^-, \Delta A_a^+, \Delta A_a^-, \Delta A_b^+, \Delta A_b^-, \Delta \Gamma \ge0\end{array} (1)$

算法描述

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

$d_i = \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)} {\sqrt{a^2+b^2+c^2}}=z_i$

J, D的计算。

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

求导后a=b=z0=x0=y0=0，c=1要代入

$\frac {\partial d_i} {\partial z0}=\frac {\partial \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)}{\sqrt{a^2+b^2+c^2}}} {\partial z0}_{|z0=0} = -1$

$\frac {\partial d_i} {\partial a}=\frac {\partial \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)}{\sqrt{a^2+b^2+c^2}}} {\partial a}_{|a=0} = x_i$

$\frac {\partial d_i} {\partial b}=\frac {\partial \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)}{\sqrt{a^2+b^2+c^2}}} {\partial b}_{|b=0} = y_i$

一次迭代过程

确定平面初值，Γ, 任意选择三点拟合平面
根据上述公式(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_a*p_{z_0} \\ p_b*p_{z_0} \\ p_{z_0} \end {bmatrix}\\ \\ \begin {bmatrix}a \\ b \\ c \end {bmatrix} = U^T \cdot \begin{bmatrix}p_a \\ p_b \\ 1 \end {bmatrix}.normalize() \\ \\ \Gamma = \Gamma-\Delta \Gamma \end {array}$
重复2直到收敛

最后，输出时F=2*Γ

代码实现

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

拟合代码

#include "PlaneFitter.h"
#include "../gauss/FittingPlane.h"
#include <algorithm>
#include <Eigen/Dense>namespace Chebyshev {double PlaneFitter::F(Fitting::Plane plane, const Point& p){auto de = p - plane.BasePoint;return plane.Orient.dot(de);}double PlaneFitter::GetError(Fitting::Plane plane, const std::vector<Eigen::Vector3d>& points){double err = 0;for (auto& p : points) { err = std::max(err, abs(F(plane, p)));}return err;}Fitting::Matrix PlaneFitter::Jacobi(const std::vector<Eigen::Vector3d>& points){Fitting::Matrix J(points.size(), 3);for (int i = 0; i < points.size(); ++i) {auto& p = points[i];J(i, 0) = -1;J(i, 1) = p.x();J(i, 2) = p.y();}return J;}void PlaneFitter::beforHook(const std::vector<Eigen::Vector3d>& points){U = Fitting::getRotationByOrient(plane.Orient);for (int i = 0; i < points.size(); ++i) {transPoints[i] = U * (points[i] - plane.BasePoint);}}void PlaneFitter::afterHook(const Eigen::VectorXd& xp){plane.BasePoint += U.transpose() * Eigen::Vector3d(xp(0) * xp(1), xp(0) * xp(2), xp(0));plane.Orient = U.transpose() * Eigen::Vector3d(xp(1), xp(2), 1).normalized();gamma -= xp(3);}Eigen::VectorXd PlaneFitter::getDArray(const std::vector<Eigen::Vector3d>& points){Eigen::VectorXd D(points.size());for (int i = 0; i < points.size(); ++i)D(i) = points[i].z();return D;}bool PlaneFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points){if (points.size() < 3)return false;Fitting::FittingBase* fb = new Gauss::FittingPlane();fb->Fitting(points, &plane);delete fb;gamma = GetError(plane, points);return true;}double PlaneFitter::F(const Eigen::Vector3d& p){return Chebyshev::PlaneFitter::F(plane, p);}double PlaneFitter::GetError(const std::vector<Eigen::Vector3d>& points){return Chebyshev::PlaneFitter::GetError(plane, points);}void PlaneFitter::Copy(void* ele){memcpy(ele, &plane, sizeof(Fitting::Plane));}PlaneFitter::PlaneFitter(){ft = Fitting::FittingType::CHEBYSHEV;}
}

测试结果

https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/chebyshev-testdata/officialtest/fitting_result/result.txt
C17 : PLANE : pass
C18 : PLANE : pass
C19 : PLANE : pass
C20 : PLANE : pass
C21 : PLANE : pass
C22 : PLANE : pass
C23 : PLANE : pass
C24 : PLANE : pass
C25 : PLANE : pass
C26 : PLANE : pass