做婚庆策划的网站,天津非常好的网站建设,网站怎么做适配,买到一个域名以后如何做网站欢迎关注更多精彩 关注我#xff0c;学习常用算法与数据结构#xff0c;一题多解#xff0c;降维打击。
本期话题#xff1a;切比雪夫#xff08;最小区域法#xff09;平面拟合算法
相关背景和理论 点击前往 主要介绍了应用背景和如何转化成线性规划问题 平拟合输入和…欢迎关注更多精彩 关注我学习常用算法与数据结构一题多解降维打击。
本期话题切比雪夫最小区域法平面拟合算法
相关背景和理论 点击前往 主要介绍了应用背景和如何转化成线性规划问题 平拟合输入和输出要求
输入
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 ∥ ( p i − X 0 ) ⋅ A ∥ ∥ A ∥ d_i \frac { \left \| (p_i-X_0)\cdot A \right \|}{\left \| A \right \|} di∥A∥∥(pi−X0)⋅A∥
展开一下 d i a ( x i − x 0 ) b ( y i − y 0 ) c ( z i − z 0 ) a 2 b 2 c 2 d_i \frac {a(x_i-x_0) b(y_i-y_0) c(z_i-z_0)} {\sqrt{a^2b^2c^2}} dia2b2c2 a(xi−x0)b(yi−y0)c(zi−z0)
优化能量方程
能量方程 H f ( X 0 , A ) max 1 n ∣ d i ∣ Hf(X0, A)\displaystyle \max_1^n {|d_i|} Hf(X0,A)1maxn∣di∣
X0, A是未知量拟合平面的过程也可以理解为优化X0, A使得方程H最小。
如何3个参数表示平面
如果直接拿6个参数去做迭代1是比较麻烦会出现比较难解的方向2是平面上的点有很多个结果不唯一。
当平面法向与Z轴偏差比较小的时候可以使用3个参数来表示。 如上图绿线为Z轴橙色线为XOY平面。
由于法向与Z轴比较相近可以设法向为(a, b, 1), a,b 是比较小的量。
现在表示 平面还需要一个点规定点必须在以(a,b,1)为法向过0点的直线上。
就有直线公式 x0/ay0/bz0/c
那么只要知道z0就可知 x0az0, y0bz0.
综上可以使用a, b, z0 表示一个法向与Z轴相近的 平面。
转化为线性规划法向与Z轴接近 设 a ( z 0 , A a , A b ) , d i F ( x i ; a ) , 引入 Γ M A X i 1 n ∣ d i ∣ 设a(z_0, A_a, A_b), d_iF(x_i;\ a), 引入\Gamma\overset n{\underset {i1}{MAX}}\;|d_i| 设a(z0,Aa,Ab),diF(xi; a),引入Γi1MAXn∣di∣
根据上述定义可以将原来的最值问题转化为下述条件
对于所有点应该满足 F ( x i ; a ) ≤ Γ , ( F ( x i ; a ) 0 ) F(x_i;\ a)\le \Gamma, (F(x_i;\ a)0) F(xi; a)≤Γ,(F(xi; a)0) − F ( x i ; a ) ≤ Γ , ( F ( x i ; a ) 0 ) -F(x_i;\ a)\le \Gamma, (F(x_i;\ a)0) −F(xi; a)≤Γ,(F(xi; a)0)
我们可以通过小量迭代慢慢减小Γ m a x Δ Γ s . t . F ( x i , a ) J Δ a ≤ Γ − Δ Γ , ( i 1 , 2... n ) − ( F ( x i , a ) J Δ a ) ≤ Γ − Δ Γ , ( i 1 , 2... n ) Δ Γ ≥ 0 \begin {array}{c}max \ \ \ \ \Delta {\Gamma}\\ s.t.\ \ \ F(x_i, a) J\Delta a \le \Gamma -\Delta \Gamma, (i1,2...n)\\ \ \ \ \ \ \ \ \ \ -(F(x_i, a) J\Delta a) \le \Gamma -\Delta \Gamma, (i1,2...n)\\ \Delta \Gamma \ge0\end{array} max ΔΓs.t. F(xi,a)JΔa≤Γ−ΔΓ,(i1,2...n) −(F(xi,a)JΔa)≤Γ−ΔΓ,(i1,2...n)ΔΓ≥0 上述条件不需要管 F ( x i , a ) J Δ a 正负情况若 F ( x i , a ) J Δ a 为正 − ( F ( x i , a ) J Δ a ) ≤ Γ − Δ Γ 必成立反之亦然。 上述条件不需要管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必成立反之亦然。 上述条件不需要管F(xi,a)JΔa正负情况若F(xi,a)JΔa为正−(F(xi,a)JΔa)≤Γ−ΔΓ必成立反之亦然。 求解出以后更新a, Γ。
对线性规划模型标准化 需要对 Δ z 0 , Δ A a , Δ A b 拆解要求变量都要大于等于 0 需要对\Delta z_0, \Delta A_a, \Delta A_b 拆解要求变量都要大于等于0 需要对Δz0,ΔAa,ΔAb拆解要求变量都要大于等于0 m a x Δ Γ s . t . J i ⋅ [ Δ z 0 Δ z 0 Δ A a Δ A a Δ A b − Δ A b − ] Δ Γ ≤ Γ d i , ( i 1 , 2... n ) − J i ⋅ [ Δ z 0 Δ z 0 Δ A a Δ A a Δ A b − Δ A b − ] Δ Γ ≤ Γ d i , ( i 1 , 2... n ) Δ z 0 , Δ z 0 − , Δ A a , Δ A a − , Δ A b , Δ A b − , Δ Γ ≥ 0 ( 1 ) \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 \Gammad_i, (i1,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 \Gammad_i, (i1,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) max ΔΓs.t. Ji⋅ Δz0Δz0ΔAaΔAaΔAb−ΔAb− ΔΓ≤Γdi,(i1,2...n) −Ji⋅ Δz0Δz0ΔAaΔAaΔAb−ΔAb− ΔΓ≤Γdi,(i1,2...n)Δz0,Δz0−,ΔAa,ΔAa−,ΔAb,ΔAb−,ΔΓ≥0(1)
算法描述
法向与Z轴重合时 x 0 0 , y 0 0 , z 0 0 , a 0 , b 0 , c 1 x_00, y_00, z_00, a0,b 0, c1 x00,y00,z00,a0,b0,c1 d i a ( x i − x 0 ) b ( y i − y 0 ) c ( z i − z 0 ) a 2 b 2 c 2 z i d_i \frac {a(x_i-x_0) b(y_i-y_0) c(z_i-z_0)} {\sqrt{a^2b^2c^2}}z_i dia2b2c2 a(xi−x0)b(yi−y0)c(zi−z0)zi
J, D的计算。
3个未知分别对d_i求导结果如下
求导后abz0x0y00c1要代入 ∂ d i ∂ z 0 ∂ a ( x i − x 0 ) b ( y i − y 0 ) c ( z i − z 0 ) a 2 b 2 c 2 ∂ z 0 ∣ z 0 0 − 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^2b^2c^2}}} {\partial z0}_{|z00} -1 ∂z0∂di∂z0∂a2b2c2 a(xi−x0)b(yi−y0)c(zi−z0)∣z00−1 ∂ d i ∂ a ∂ a ( x i − x 0 ) b ( y i − y 0 ) c ( z i − z 0 ) a 2 b 2 c 2 ∂ a ∣ a 0 x i \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^2b^2c^2}}} {\partial a}_{|a0} x_i ∂a∂di∂a∂a2b2c2 a(xi−x0)b(yi−y0)c(zi−z0)∣a0xi ∂ d i ∂ b ∂ a ( x i − x 0 ) b ( y i − y 0 ) c ( z i − z 0 ) a 2 b 2 c 2 ∂ b ∣ b 0 y 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^2b^2c^2}}} {\partial b}_{|b0} y_i ∂b∂di∂b∂a2b2c2 a(xi−x0)b(yi−y0)c(zi−z0)∣b0yi
一次迭代过程 确定平面初值Γ, 任意选择三点拟合平面 根据上述公式(1)构建线性规划方程 求解 Δ p \Delta p Δp 更新解 [ x 0 y 0 z 0 ] [ x 0 y 0 z 0 ] U T ⋅ [ p a ∗ p z 0 p b ∗ p z 0 p z 0 ] [ a b c ] U T ⋅ [ p a p b 1 ] . n o r m a l i z e ( ) Γ Γ − Δ Γ \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} x0y0z0 x0y0z0 UT⋅ pa∗pz0pb∗pz0pz0 abc UT⋅ papb1 .normalize()ΓΓ−ΔΓ 重复2直到收敛
最后输出时F2*Γ
代码实现
代码链接https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/chebyshev/PlaneFitter.cpp
拟合代码
#include PlaneFitter.h
#include ../gauss/FittingPlane.h
#include algorithm
#include Eigen/Densenamespace 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::vectorEigen::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::vectorEigen::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::vectorEigen::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::vectorEigen::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::vectorEigen::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::vectorEigen::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 本人码农希望通过自己的分享让大家更容易学懂计算机知识。创作不易帮忙点击公众号的链接。
