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

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

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

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

平拟合输入和输出要求

输入

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

输出

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

精度要求

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

平面优化标函数

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

平面参数化表示

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

点到平面距离

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

根据点乘得到距离

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

展开一下：

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

优化能量方程

能量方程 $H=f(X0,A)=\underset{1}{\overset{n}{\max }}|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 ＝\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)$

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

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

对线性规划模型标准化

$需要对\Delta z_0,\Delta A_a,\Delta A_b拆解，要求变量都要大于等于0$

$\begin{array}{c}max\Delta \Gamma \\ s.t.J_i\cdot \left[\begin{array}{c}\Delta z_0^{+}－\Delta z_0^{－}\\ \Delta A_a^{+}－\Delta A_a^{－}\\ \Delta A_b^{+}-\Delta A_b^{-}\end{array}\right]+\Delta \Gamma \le \Gamma －d_i,(i=1,\mathrm{2...}n)\\ \\ -J_i\cdot \left[\begin{array}{c}\Delta z_0^{+}－\Delta z_0^{－}\\ \Delta A_a^{+}－\Delta A_a^{－}\\ \Delta A_b^{+}-\Delta A_b^{-}\end{array}\right]+\Delta \Gamma \le \Gamma +d_i,(i=1,\mathrm{2...}n)\\ \Delta z_0^{+},\Delta z_0^{-},\Delta A_a^{+},\Delta A_a^{-},\Delta A_b^{+},\Delta A_b^{-},\Delta \Gamma \ge 0\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. 确定平面初值，Γ, 任意选择三点拟合平面

2. 根据上述公式(1)构建线性规划方程

3. 求解$\Delta p$

4. 更新解
   $\begin{array}{l}\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]+U^T\cdot \left[\begin{array}{c}{p}_a\ast p_{z_0}\\ p_b\ast p_{z_0}\\ p_{z_0}\end{array}\right]\\ \\ \left[\begin{array}{c}a\\ b\\ c\end{array}\right]=U^T\cdot \left[\begin{array}{c}{p}_a\\ p_b\\ 1\end{array}\right].normalize()\\ \\ \Gamma =\Gamma -\Delta \Gamma \end{array}$

5. 重复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

---

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