最小二乘圆锥拟合(梯度下降法+高斯牛顿法)

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

本期话题:最小二乘圆锥拟合

相关背景资料
点击前往

请添加图片描述

圆锥拟合输入和输出要求

输入

  1. 8到50个点,全部采样自圆锥上。
  2. 每个点3个坐标,坐标精确到小数点后面20位。
  3. 坐标单位是mm, 范围[-500mm, 500mm]。

tips
输入虽然是严格来自圆锥,但是由于浮点表示,记录的值和真实值始终是有微小的误差,点云到拟合圆锥的距离不可能完全为0。

输出

  1. 1点X0表示 点云中心在圆锥中心轴上的投影,用三个坐标表示。
  2. 法向A代表圆锥中心轴法向(指向半径减小方向)。
  3. 张开角度alpha (度,全角)。
  4. 圆半径r 。(X0, A所形成平面与圆锥相交得到的圆)。

精度要求

  1. X0点到标准圆锥中心轴距离不能超过0.0001mm。
  2. 法向A与圆锥中心轴法向夹角不能超过0.0000001rad。
  3. 张开角度alpha 与标准值不能超过0.0000001rad。
  4. r与标准半径的差不能超过0.0001mm。

圆锥优化标函数

根据论文,圆锥拟合转化成数学表示如下:

圆锥参数化表示

  1. 1点X0表示 点云中心在圆锥中心轴上的投影,用三个坐标表示。
  2. 法向A(a,b,c)代表圆锥中心轴法向(指向半径减小方向)。
  3. 张开角度alpha (度,全角)。
  4. 圆半径r。(X0, A所形成平面与圆锥相交得到的圆)。

圆锥方程 e c o s ( α 2 ) + f s i n ( α 2 ) = r c o s ( α 2 ) f = a ( x − x 0 ) + b ( y − y 0 ) + c ( z − z 0 ) e = u 2 + v 2 + w 2 u = c ( y − y 0 ) − b ( z − z 0 ) v = a ( z − z 0 ) − c ( x − x 0 ) w = b ( x − x 0 ) − a ( y − y 0 ) 圆锥方程\begin {array}{l} ecos(\frac \alpha 2) + fsin(\frac \alpha 2) = rcos(\frac \alpha 2) \\ f=a(x-x_0)+b(y-y_0)+c(z-z_0)\\ e=\sqrt{u^2+v^2+w^2}\\ u =c(y-y_0)-b(z-z_0)\\ v=a(z-z_0)-c(x-x_0)\\ w=b(x-x_0)-a(y-y_0)\ \end {array} 圆锥方程ecos(2α)+fsin(2α)=rcos(2α)f=a(xx0)+b(yy0)+c(zz0)e=u2+v2+w2 u=c(yy0)b(zz0)v=a(zz0)c(xx0)w=b(xx0)a(yy0) 

能量方程

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

距离函数

d i = e i c o s ( α 2 ) + f i s i n ( α 2 ) − r c o s ( α 2 ) d_i = e_icos(\frac \alpha 2) + f_isin(\frac \alpha 2) - rcos(\frac \alpha 2) di=eicos(2α)+fisin(2α)rcos(2α)

f i = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) e i = u 2 + v 2 + w 2 u i = c ( y − y 0 ) − b ( z − z 0 ) v i = a ( z − z 0 ) − c ( x − x 0 ) w i = b ( x − x 0 ) − a ( y − y 0 ) \begin {array}{l}f_i=a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)\\ e_i = \sqrt{u^2+v^2+w^2} \\ u_i =c(y-y_0)-b(z-z_0)\\ v_i=a(z-z_0)-c(x-x_0)\\ w_i=b(x-x_0)-a(y-y_0) \end {array} fi=a(xix0)+b(yiy0)+c(ziz0)ei=u2+v2+w2 ui=c(yy0)b(zz0)vi=a(zz0)c(xx0)wi=b(xx0)a(yy0)

距离可视化理解

在这里插入图片描述

黄色段为eicos(a/2), 绿色为fisin(a/2) 两段之和与rcos(a/2)之差为距离。

根据定义得到能量方程

E = ∑ 1 n d i 2 E=\displaystyle \sum _1^n {d_i^2} E=1ndi2

最小二乘优化能量方程

能量方程 E = f ( X 0 , A , a , r ) = ∑ 1 n d i 2 E=f(X0, A, a, r)=\displaystyle \sum _1^n {d_i^2} E=f(X0,A,a,r)=1ndi2

上式是一个8元二次函数中,X0, A, a r是未知量,拟合圆锥的过程也可以理解为优化X0, A, a, r使得方程E最小。

上述方程直接求导不好解,可以使用高斯牛顿迭代法。

高斯牛顿迭代法

用于解非线性最小二乘问题。同时,高斯牛顿法需要比较可靠的初值,所以寻找目标函数的初值也是一个比较关键的步骤。

基本原理

设 a = ( a 0 , a 1 , . . . , a n ) 是待求解向量, a ^ 是初始给定值, a = a ^ + Δ a , Δ a 是我们每次迭代后移动的量 设 a=(a_0, a_1,...,a_n)是待求解向量,\widehat {a} 是初始给定值,a = \widehat {a} +\Delta a, \Delta a 是我们每次迭代后移动的量 a=(a0,a1,...,an)是待求解向量,a 是初始给定值,a=a +Δa,Δa是我们每次迭代后移动的量

定义距离函数为 F ( x , a ) , d i = F ( x i , a ) , 进行泰勒 1 阶展开, F ( x , a ) = F ( x , a ^ ) + ∂ F ∂ a ^ Δ a = F ( x , a ^ ) + J Δ a 定义距离函数为 F(x, a), d_i = F(x_i, a), 进行泰勒1阶展开, F(x, a) = F(x, \widehat a) + \frac {\partial F}{\partial \widehat a}\Delta a = F(x, \widehat a) + J\Delta a 定义距离函数为F(x,a),di=F(xi,a),进行泰勒1阶展开,F(x,a)F(x,a )+a FΔa=F(x,a )+JΔa

每次迭代,其实就是希望通过调整 Δ a 使得 J Δ a = − F ( x , a ^ ) 每次迭代,其实就是希望通过调整\Delta a 使得 J\Delta a = -F(x, \widehat a) 每次迭代,其实就是希望通过调整Δa使得JΔaF(x,a )

J = [ ∂ F ( x 0 , a ) ∂ a 0 ∂ F ( x 0 , a ) ∂ a 1 . . . ∂ F ( x 0 , a ) ∂ a n ∂ F ( x 1 , a ) ∂ a 0 ∂ F ( x 1 , a ) ∂ a 1 . . . ∂ F ( x 1 , a ) ∂ a n . . . . . . . . . . . . ∂ F ( x n , a ) ∂ a 0 ∂ F ( x n , a ) ∂ a 1 . . . ∂ F ( x n , a ) ∂ a n ] J = \begin {bmatrix} \frac {\partial F(x_0, a)} {\partial a_0} & \frac {\partial F(x_0, a)} {\partial a_1} & ...& \frac {\partial F(x_0, a)} {\partial a_n} \\ \\ \frac {\partial F(x_1, a)} {\partial a_0} & \frac {\partial F(x_1, a)} {\partial a_1} & ...& \frac {\partial F(x_1, a)} {\partial a_n} \\\\ ... & ... & ...& ... \\ \\ \frac {\partial F(x_n, a)} {\partial a_0} & \frac {\partial F(x_n, a)} {\partial a_1} & ...& \frac {\partial F(x_n, a)} {\partial a_n} \end {bmatrix} J= a0F(x0,a)a0F(x1,a)...a0F(xn,a)a1F(x0,a)a1F(x1,a)...a1F(xn,a)............anF(x0,a)anF(x1,a)...anF(xn,a)

F ( x , a ^ ) = [ d 1 d 2 . . . d m ] F(x, \widehat a) = \begin {bmatrix} d_1 \\ d_2 \\... \\ d_m \end {bmatrix} F(x,a )= d1d2...dm

J Δ a = − F ( x , a ^ ) , 解出 Δ a , 更新 a = a ^ + Δ a , 持续迭代直到 Δ a 足够小 J\Delta a = -F(x,\widehat a), 解出\Delta a ,更新 a = \widehat {a} +\Delta a, 持续迭代直到\Delta a足够小 JΔa=F(x,a ),解出Δa,更新a=a +Δa,持续迭代直到Δa足够小

用4个数表示直线法向

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

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

在这里插入图片描述
如上图,绿线为Z轴,橙色线为XOY平面。

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

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

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

模型简化

当角度比较大时,参数中的r会比较大,求解会带来数值误差。
可以令 t=rcos(a/2)

d i = e i c o s ( α 2 ) + f i s i n ( α 2 ) − t d_i= e_icos(\frac \alpha 2) + f_isin(\frac \alpha 2) - t di=eicos(2α)+fisin(2α)t

为了使用上述方法,当得到一个拟合初值后,需要先将中心线旋转致Z轴,把X0移致0点。

此时,
x0=y0=z0=0.
a=b=0, c=1.

e i = r i r i = ( x i 2 + y i 2 ) f i = z i \begin {array}{l}e_i=r_i\\ r_i=\sqrt{(x_i^2+y_i^2)}\\ f_i=z_i \end {array} ei=riri=(xi2+yi2) fi=zi

算法描述

我们希望di=0。
可以对di分别求偏导

J, D的计算。

J = ∂ d 1 ∂ x 0 ∂ d 1 ∂ y 0 ∂ d 1 ∂ a ∂ d 1 ∂ b ∂ d 1 ∂ α ∂ d 1 ∂ t ∂ d 2 ∂ x 0 ∂ d 2 ∂ y 0 ∂ d 2 ∂ a ∂ d 2 ∂ b ∂ d 2 ∂ α ∂ d 2 ∂ t . . . . . . . . . . . . . . . ∂ d n ∂ x 0 ∂ d n ∂ y 0 ∂ d n ∂ a ∂ d n ∂ b ∂ d n ∂ α ∂ d n ∂ t , D = d 1 d 2 . . . d n J= \begin{array}{l} \frac {\partial d_1}{\partial x_0}& \frac {\partial d_1}{\partial y_0}& \frac {\partial d_1}{\partial a}& \frac {\partial d_1}{\partial b}& \frac {\partial d_1}{\partial \alpha}& \frac {\partial d_1}{\partial t} \\ \frac {\partial d_2}{\partial x_0}& \frac {\partial d_2}{\partial y_0}& \frac {\partial d_2}{\partial a}& \frac {\partial d_2}{\partial b}& \frac {\partial d_2}{\partial \alpha}& \frac {\partial d_2}{\partial t}\\...&...&...&...&...\\\frac {\partial d_n}{\partial x_0}& \frac {\partial d_n}{\partial y_0}& \frac {\partial d_n}{\partial a}& \frac {\partial d_n}{\partial b}& \frac {\partial d_n}{\partial \alpha}& \frac {\partial d_n}{\partial t}\\ \end {array}, \ D= \begin{array}{l} d_1\\d_2\\...\\d_n\end {array} J=x0d1x0d2...x0dny0d1y0d2...y0dnad1ad2...adnbd1bd2...bdnαd1αd2...αdntd1td2tdn, D=d1d2...dn

6个未知分别对d_i求导结果如下:

回顾一下

u i = c ( y i − y 0 ) − b ( z i − z 0 ) v i = a ( z i − z 0 ) − c ( x i − x 0 ) w i = b ( x i − x 0 ) − a ( y i − y 0 ) \begin {array}{l}u_i =c(y_i-y_0)-b(z_i-z_0)\\ v_i=a(z_i-z_0)-c(x_i-x_0)\\ w_i=b(x_i-x_0)-a(y_i-y_0)\end {array} ui=c(yiy0)b(ziz0)vi=a(ziz0)c(xix0)wi=b(xix0)a(yiy0)

∂ d i ∂ x 0 = u i 2 + v i 2 + w i 2 c o s ( α / 2 ) ∂ x 0 = ( x i − x 0 ) 2 ∂ x 0 2 u i 2 + v i 2 + w i 2 ⋅ c o s ( α / 2 ) = − x i ⋅ c o s ( α / 2 ) x i 2 + y i 2 \frac {\partial d_i} {\partial x_0}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}cos(\alpha/2)} {\partial x_0} \\ =\frac {\frac {(x_i-x_0)^2}{\partial x_0}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\cdot cos(\alpha/2)\\ =\frac {-x_i\cdot cos(\alpha/2)}{\sqrt{x_i^2+y_i^2}} x0di=x0ui2+vi2+wi2 cos(α/2)=2ui2+vi2+wi2 x0(xix0)2cos(α/2)=xi2+yi2 xicos(α/2)

∂ d i ∂ y 0 = u i 2 + v i 2 + w i 2 c o s ( α / 2 ) ∂ y 0 = ( y i − y 0 ) 2 ∂ y 0 2 u i 2 + v i 2 + w i 2 ⋅ c o s ( α / 2 ) = − y i ⋅ c o s ( α / 2 ) x i 2 + y i 2 \frac {\partial d_i} {\partial y_0}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}cos(\alpha/2)} {\partial y_0} \\ =\frac {\frac {(y_i-y_0)^2}{\partial y_0}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\cdot cos(\alpha/2)\\ =\frac {-y_i\cdot cos(\alpha/2)\\}{\sqrt{x_i^2+y_i^2}} y0di=y0ui2+vi2+wi2 cos(α/2)=2ui2+vi2+wi2 y0(yiy0)2cos(α/2)=xi2+yi2 yicos(α/2)

∂ d i ∂ a = u i 2 + v i 2 + w i 2 ⋅ c o s ( α / 2 ) + [ a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) ] ⋅ s i n ( α / 2 ) ∂ a = [ a ( z i − z 0 ) − c ( x i − x 0 ) ] 2 ∂ a 2 u i 2 + v i 2 + w i 2 ⋅ c o s ( α / 2 ) + x i s i n ( α / 2 ) = − x i z i ⋅ c o s ( α / 2 ) x i 2 + y i 2 + x i s i n ( α / 2 ) \frac {\partial d_i} {\partial a}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}\cdot cos(\alpha/2)+[a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)]\cdot sin(\alpha/2)} {\partial a}\\ =\frac {\frac {[a(z_i-z_0)-c(x_i-x_0)]^2}{\partial a}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\cdot cos(\alpha/2)+x_isin(\alpha/2)\\ =\frac {-x_iz_i\cdot cos(\alpha/2)}{\sqrt{x_i^2+y_i^2}}+x_isin(\alpha/2) adi=aui2+vi2+wi2 cos(α/2)+[a(xix0)+b(yiy0)+c(ziz0)]sin(α/2)=2ui2+vi2+wi2 a[a(ziz0)c(xix0)]2cos(α/2)+xisin(α/2)=xi2+yi2 xizicos(α/2)+xisin(α/2)

∂ d i ∂ b = u i 2 + v i 2 + w i 2 ⋅ c o s ( α / 2 ) + [ a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) ] ⋅ s i n ( α / 2 ) ∂ b = [ c ( y i − y 0 ) − b ( z i − z 0 ) ] 2 ∂ b 2 u i 2 + v i 2 + w i 2 ⋅ c o s ( α / 2 ) + y i s i n ( α / 2 ) = − y i z i ⋅ c o s ( α / 2 ) x i 2 + y i 2 + y i s i n ( α / 2 ) \frac {\partial d_i} {\partial b}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}\cdot cos(\alpha/2)+[a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)]\cdot sin(\alpha/2)} {\partial b}\\ =\frac {\frac {[c(y_i-y_0)-b(z_i-z_0)]^2}{\partial b}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\cdot cos(\alpha/2)+y_isin(\alpha/2)\\ =\frac {-y_iz_i\cdot cos(\alpha/2)}{\sqrt{x_i^2+y_i^2}}+y_isin(\alpha/2) bdi=bui2+vi2+wi2 cos(α/2)+[a(xix0)+b(yiy0)+c(ziz0)]sin(α/2)=2ui2+vi2+wi2 b[c(yiy0)b(ziz0)]2cos(α/2)+yisin(α/2)=xi2+yi2 yizicos(α/2)+yisin(α/2)

∂ d i ∂ α = e i c o s ( α 2 ) + f i s i n ( α 2 ) − t ∂ α = − e i s i n ( α / 2 ) / 2 + f i c o s ( α / 2 ) / 2 = − e i s i n ( α / 2 ) + z i c o s ( α / 2 ) 2 \frac {\partial d_i} {\partial \alpha}=\frac {e_icos(\frac \alpha 2) + f_isin(\frac \alpha 2) - t} {\partial \alpha}\\ = -e_isin(\alpha/2)/2+f_icos(\alpha/2)/2\\ =\frac {-e_isin(\alpha/2)+z_icos(\alpha/2)}{2} αdi=αeicos(2α)+fisin(2α)t=eisin(α/2)/2+ficos(α/2)/2=2eisin(α/2)+zicos(α/2)

∂ d i ∂ t = e i c o s ( α 2 ) + f i s i n ( α 2 ) − t ∂ t = − 1 \frac {\partial d_i} {\partial t}=\frac {e_icos(\frac \alpha 2) + f_isin(\frac \alpha 2) - t} {\partial t} = -1 tdi=teicos(2α)+fisin(2α)t=1

  1. 确定圆锥初值

  2. 将中轴通过刚体变换U至Z轴,U的构建可以参考代码
    [ x i y i z i ] = U ⋅ ( [ x i y i z i ] − [ x 0 y 0 z 0 ] ) \begin {bmatrix}x_i \\ y_i \\ z_i \end {bmatrix} = U \cdot \left (\begin {bmatrix}x_i \\ y_i \\ z_i \end {bmatrix}- \begin{bmatrix}x_0 \\ y_0 \\ z_0 \end {bmatrix}\right ) xiyizi =U xiyizi x0y0z0

  3. 根据上述公式构建 J ⋅ ( [ p x 0 p y 0 p a p b p α p r ] ) = − D J \cdot \left(\begin {bmatrix}p_{x_0} \\ p_{y_0}\\p_a\\p_b\\p_{\alpha}\\p_{r} \end {bmatrix} \right)=-D J px0py0papbpαpr =D

  4. 求解 Δ p \Delta p Δp

  5. 更新解
    [ x 0 y 0 z 0 ] = [ x 0 y 0 z 0 ] + U T ⋅ [ p x 0 p y 0 − p a p x 0 − p b p y 0 ] [ a b c ] = U T ⋅ [ p a p b 1 ] . n o r m a l i z e ( ) α = α + p α t = t + p r \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_{x_0} \\ p_{y_0}\\ -p_ap_{x_0}-p_bp_{y_0}\end {bmatrix} \\ \\\begin {bmatrix}a \\ b \\ c \end {bmatrix} = U^T \cdot \begin{bmatrix}p_a \\ p_b \\ 1 \end {bmatrix}.normalize() \\\\ \alpha=\alpha+p_\alpha\\\\ t=t+p_r \end {array} x0y0z0 = x0y0z0 +UT px0py0papx0pbpy0 abc =UT papb1 .normalize()α=α+pαt=t+pr

  6. 重复2直到收敛

初值确定

可以枚举法向(间隔1度),夹角大小(间隔1度)。利用最速下降法求得其他圆锥参数。取误差最小的圆锥作为初值。可以使用并行加速。

在这里插入图片描述
对于给定一个A和a, 先将A与点云一起旋转,使得A与Z轴平行。

然后去拟合下面圆锥方程。

( x − x 0 ) 2 + ( y − y 0 ) 2 = ( r − k z ) 2 , k = t a n ( α / 2 ) x 0 , y 0 , r 为待求变量 \begin {array}{l}(x-x_0)^2+(y-y_0)^2=(r-kz)^2, k=tan(\alpha/2)\\ x_0, y_0, r为待求变量 \end{array} (xx0)2+(yy0)2=(rkz)2,k=tan(α/2)x0,y0,r为待求变量

可以如下建立能量方程
距离 d i = ( x i − x 0 ) 2 + ( y i − y 0 ) 2 − ( r − k z i ) 2 最小二乘能量方程 f ( X ) = E = ∑ 1 n d i 2 , X = ( x 0 , y 0 , r ) \begin {array}{l}距离d_i = (x_i-x_0)^2+(y_i-y_0)^2-(r-kz_i)^2\\ 最小二乘能量方程\\ f(X)=E=\displaystyle \sum_1^nd_i^2, X=(x_0, y_0,r) \end{array} 距离di=(xix0)2+(yiy0)2(rkzi)2最小二乘能量方程f(X)=E=1ndi2,X=(x0,y0,r)

可以使用梯度下降法优化E最小值。

梯度下降法

学习视频:
https://www.bilibili.com/video/BV127411N7nU/?spm_id_from=333.999.0.0

迭代步骤
x k 是第 k 次迭代的初始值, ∇ f ( x k ) 是第 k 次迭代的梯度 x_k 是第k次迭代的初始值,\nabla f(x_k) 是第k次迭代的梯度 xk是第k次迭代的初始值,f(xk)是第k次迭代的梯度
搜索初次值
前两次值和梯度通过 A r m i j o W o l f e 算法求得 前两次值和梯度通过Armijo Wolfe 算法求得 前两次值和梯度通过ArmijoWolfe算法求得

梯度下降法步骤
梯度下降法步骤。

梯度求导和初值确定

∂ f ( X ) ∂ X = [ ∂ f ( X ) ∂ x 0 ∂ f ( X ) ∂ y 0 ∂ f ( X ) ∂ r ] \frac {\partial f(X)}{\partial X} =\begin {bmatrix}\frac {\partial f(X)}{\partial x_0}\\\frac {\partial f(X)}{\partial y_0}\\\frac {\partial f(X)}{\partial r}\end{bmatrix} Xf(X)= x0f(X)y0f(X)rf(X)

∂ f ( X ) ∂ x 0 = ∑ 1 n − 4 [ ( x i − x 0 ) 2 + ( y i − y 0 ) 2 − ( r − k z i ) 2 ] ( x i − x 0 ) ∂ f ( X ) ∂ y 0 = ∑ 1 n − 4 [ ( x i − x 0 ) 2 + ( y i − y 0 ) 2 − ( r − k z i ) 2 ] ( y i − y 0 ) ∂ f ( X ) ∂ r = ∑ 1 n − 4 [ ( x i − x 0 ) 2 + ( y i − y 0 ) 2 − ( r − k z i ) 2 ] ( r − k z i ) \begin {array}{l}\frac {\partial f(X)}{\partial x_0}=\displaystyle \sum_1^n{-4[ (x_i-x_0)^2+(y_i-y_0)^2-(r-kz_i)^2](x_i-x_0)}\\ \frac {\partial f(X)}{\partial y_0} =\displaystyle \sum_1^n{-4[ (x_i-x_0)^2+(y_i-y_0)^2-(r-kz_i)^2](y_i-y_0)}\\ \frac {\partial f(X)}{\partial r}=\displaystyle \sum_1^n{-4[ (x_i-x_0)^2+(y_i-y_0)^2-(r-kz_i)^2](r-kz_i)}\\\end{array} x0f(X)=1n4[(xix0)2+(yiy0)2(rkzi)2](xix0)y0f(X)=1n4[(xix0)2+(yiy0)2(rkzi)2](yiy0)rf(X)=1n4[(xix0)2+(yiy0)2(rkzi)2](rkzi)

X初始可以取离XOY平面最近的3个点投影到XOY平面上拟合1个圆。x0, y0 为圆心,r为半径。

加速优化

可以使用并行加速。
对于小角度(<160),法向可以枚举5度步长,角度1度。
对于大角度(>=160),法向可以枚举1度步长,角度1度。
先尝试小角度拟合,检查误差,比较大时再拟合大角度。

梯度下降基类设计

namespace Fitting {using VK = Eigen::VectorXd;class GradientBase {private:double search_alpha(const std::vector<Eigen::Vector3d>& points);protected:/** 设定初始值,获取初向量*/virtual VK initVK() = 0;/*transResult* 更新解*/virtual void transResult(const VK & xk)=0;/* F* 函数值*/virtual double F_X(const std::vector<Eigen::Vector3d>& points)=0;/** 梯度值*/virtual Eigen::VectorXd G_X(const std::vector<Eigen::Vector3d>& points)=0;/* 获取 结果*/virtual void Copy(void* ele) = 0;public:// 梯度下降迭代double Descending(const std::vector<Eigen::Vector3d>& points, void* ele);};
}

代码实现

代码链接:https://gitcode.com/chenbb1989/3DAlgorithm/tree/master/CBB3DAlgorithm/Fitting

#include "ConeFitter.h"
#include "GradientConeFitter.h"
#include "FittingCircle2D.h"
#include <corecrt_math_defines.h>
#include <Eigen/Dense>
#include<iostream>namespace Gauss {// 枚举+梯度下降法求解double getSquareCone(Fitting::Cone& cone, const std::vector<Eigen::Vector3d>& points) {Point center = Fitting::getCenter(points);std::vector<Point> transPoints(points.size());std::vector<Point> circlePoints(3);Fitting::Circle2D circle2d;Fitting::Cone gradientCone;Fitting::FittingBase* cfb = new FittingCircle2D();Fitting::GradientBase* gfb;Fitting::Matrix U;double coneErr = -1;//int cnt = 0;double xstep = 1, ystep = 1;double angleStart = 1, angleEnd = 90;for (double i = 0; i < 180; i += xstep) {double theta = i / 180 * M_PI;for (double j = 0; j < 360; j += ystep) {double alpha = j / 180 * M_PI;Point orient(cos(theta), sin(theta) * cos(alpha), sin(theta) * sin(alpha));U = Fitting::getRotationByOrient(orient);// 旋转for (int k = 0; k < points.size(); ++k) {transPoints[k] = U * (points[k] - center);}// 按照z绝对值排序sort(transPoints.begin(), transPoints.end(),[](const Point& a, const Point& b)->bool {return abs(a.z()) < abs(b.z()); });// 拟合圆for (int k = 0; k < 3; ++k) {circlePoints[k] = transPoints[k];circlePoints[k].z() = 0;}gradientCone.orient = orient;cfb->Fitting(circlePoints, &circle2d);for (double a = angleStart; a < angleEnd; ++a) {// 设定初值gradientCone.r = circle2d.r;gradientCone.center = Point(circle2d.center.x(), circle2d.center.y(), 0);gradientCone.alpha = a * 2 / 180 * M_PI;// 梯度下降法求解gfb = new GradientConeFitter(gradientCone);auto err = gfb->Descending(transPoints, &gradientCone);delete gfb;//err /= points.size();if (coneErr < 0 || err < coneErr) {coneErr = err;// 旋转回去cone = gradientCone;cone.center = U.transpose() * cone.center;}//cnt++;//if (cnt % 100 == 0) std::cout <<"1-" << cnt << std::endl;}}}delete cfb;if (cone.r < 0) {cone.r *= -1;cone.orient *= -1;}cone.center += center;cone.t = cone.r * cos(cone.alpha / 2);return coneErr;}
}namespace Gauss {double F(Fitting::Cone cone, const Point& p){Point dir = p - cone.center;double ei = dir.cross(cone.orient).norm();double fi = cone.orient.dot(dir);double di = ei * cos(cone.alpha / 2) + fi * sin(cone.alpha / 2) - cone.r * cos(cone.alpha / 2);return di;}double GetError(Fitting::Cone cone, const std::vector<Eigen::Vector3d>& points){double err = 0;for (auto& p : points) {double d = F(cone, p);err += d * d;}err /= points.size();return err;}Fitting::Matrix ConeFitter::Jacobi(const std::vector<Eigen::Vector3d>& points){int n = points.size();double cosA2 = cos(cone.alpha / 2);double sinA2 = sin(cone.alpha / 2);Fitting::Matrix J(n, 6);for (int i = 0; i < n; ++i) {auto& p = points[i];double ri = Eigen::Vector2d(p.x(), p.y()).norm();//di求导J(i, 0) = -p.x()*cosA2 / ri;J(i, 1) = -p.y()*cosA2 / ri;J(i, 2) = -p.x() * p.z()*cosA2 / ri + p.x()*sinA2;J(i, 3) = -p.y() * p.z() * cosA2 / ri + p.y() * sinA2;J(i, 4) = (-ri*sinA2 + p.z()*cosA2)/2;J(i, 5) = -1;}return J;}void ConeFitter::beforHook(const std::vector<Eigen::Vector3d>& points){U = Fitting::getRotationByOrient(cone.orient);for (int i = 0; i < points.size(); ++i) {transPoints[i] = U * (points[i] - cone.center);}}void ConeFitter::afterHook(const Eigen::VectorXd& xp){cone.center += U.transpose() * Eigen::Vector3d(xp(0), xp(1), -xp(2) * xp(0) - xp(3) * xp(1));cone.orient = U.transpose() * Eigen::Vector3d(xp(2), xp(3), 1).normalized();cone.alpha += xp(4);cone.t += xp(5);cone.r = cone.t / cos(cone.alpha / 2);}Eigen::VectorXd ConeFitter::getDArray(const std::vector<Eigen::Vector3d>& points){int n = points.size();Eigen::VectorXd D(points.size());double cosA2 = cos(cone.alpha / 2);double sinA2 = sin(cone.alpha / 2);double t = cone.r * cosA2;for (int i = 0; i < points.size(); ++i)D(i) = Eigen::Vector2d(points[i].x(), points[i].y()).norm() * cosA2 + points[i].z() * sinA2 - t;return D;}bool ConeFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points){if (points.size() < 6)return false;double err;Fitting::Cone initCone;err = getSquareCone(initCone, points);cone = initCone;if (err < 0)return false;return true;}double ConeFitter::F(const Eigen::Vector3d& p){return Gauss::F(cone, p);}double ConeFitter::GetError(const std::vector<Eigen::Vector3d>& points){return Gauss::GetError(cone, points);}void ConeFitter::Copy(void* ele){memcpy(ele, &cone, sizeof(Fitting::Cone));}
}

测试代码

#include "TestCone.h"
#include "ConeFitter.h"
#include <iostream>
#include <corecrt_math_defines.h>namespace Gauss {double TestCone::Fitting() {Fitting::FittingBase* fb = new Gauss::ConeFitter();double err = fb->Fitting(points, &fitResult);fitResult.alpha *= 180 / M_PI;return err;}bool TestCone::JudgeAnswer(FILE* fp) {ReadAnswer();if (!lineCmp(ans.orient, ans.center, fitResult.center))return false;if (!orientationCmp(ans.orient, fitResult.orient))return false;if (!radiusCmp(ans, fitResult))return false;if (!angleCmp(ans.alpha, fitResult.alpha))return false;return true;}void TestCone::ReadAnswer() {vector<double> nums;if (PointCloud::readNums((suffixName + "/answer/" + fileName + ".txt"), nums)) {for (int i = 0; i < 3; ++i) ans.center[i] = nums[i];for (int i = 0; i < 3; ++i) ans.orient[i] = nums[i+3];ans.r = nums[6];ans.alpha = nums[7];}else {std::cout << "read answer error" << std::endl;}}void TestCone::SaveAnswer(FILE* fp) {writePoint(fp, fitResult.center);writePoint(fp, fitResult.orient);writeNumber(fp, fitResult.r);writeNumber(fp, fitResult.alpha);}
}

测试结果

https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/gauss/fitting_result/result.txtb26 : CONE : pass
b27 : CONE : pass
b28 : CONE : pass
b29 : CONE : pass
b30 : CONE : pass
b31 : CONE : pass
b32 : CONE : pass
b33 : CONE : pass
b34 : CONE : pass
b35 : CONE : pass
b36 : CONE : pass
b37 : CONE : pass

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

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.hqwc.cn/news/455675.html

如若内容造成侵权/违法违规/事实不符,请联系编程知识网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

挑战杯 python+opencv+机器学习车牌识别

0 前言 &#x1f525; 优质竞赛项目系列&#xff0c;今天要分享的是 &#x1f6a9; 基于机器学习的车牌识别系统 &#x1f947;学长这里给一个题目综合评分(每项满分5分) 难度系数&#xff1a;4分工作量&#xff1a;4分创新点&#xff1a;3分 该项目较为新颖&#xff0c;适…

报告下载 | TopOn《2023年全球手游广告变现报告》正式发布!

2023年对于移动游戏营销行业来说&#xff0c;是一个充满变化和挑战的一年。随着经济逐步恢复&#xff0c;国内游戏市场有明显回暖&#xff0c;游戏用户规模和市场销售收入都达到了历史新高点&#xff1b;海外游戏市场规模仍不断扩大&#xff0c;但因受到国际局势动荡、隐私政策…

C++之字符串

C风格字符串 字符串处理在程序中应用广泛&#xff0c;C风格字符串是以\0&#xff08;空字符&#xff09;来结尾的字符数组。对字符串进行操作的C函数定义在头文件<string.h>或中。常用的库函数如下&#xff1a; //字符检查函数(非修改式操作) size_t strlen( const char …

Go语言每日一练链表篇(二)

传送门 牛客面试笔试必刷101题 ---------------- 链表内指定区间反转 题目以及解析 题目 解题代码及解析 package main import _"fmt" import . "nc_tools" /** type ListNode struct{* Val int* Next *ListNode* }*//*** 代码中的类名、方法名、参…

【开源】基于JAVA+Vue+SpringBoot的贫困地区人口信息管理系统

目录 一、摘要1.1 项目介绍1.2 项目录屏 二、功能模块2.1 人口信息管理模块2.2 精准扶贫管理模块2.3 特殊群体管理模块2.4 案件信息管理模块2.5 物资补助模块 三、系统设计3.1 用例设计3.2 数据库设计3.2.1 人口表3.2.2 扶贫表3.2.3 特殊群体表3.2.4 案件表3.2.5 物资补助表 四…

OJ_二分查找

题干 C实现 #define _CRT_SECURE_NO_WARNINGS #include<stdio.h> #include<vector> #include<algorithm> using namespace std;int main() {int n;scanf("%d", &n);vector<int> a(n);for (int i 0; i < n; i) {scanf("%d"…

MC34063异常发热分析

问题描述&#xff1a; 工程现场反馈若干电源转换模块损坏&#xff0c;没有输出。拿到问题模块后&#xff0c;查看有一个MC34063周围的PCB有比较明显的高温痕迹&#xff0c;配套的电感也有明显的高温过热痕迹。 问题调查&#xff1a; MC34063的电路非常经典&#xff08;虽然自…

Android 11 访问 Android/data/或者getExternalCacheDir() 非root方式

前言&#xff1a; 需求要求安装三方应用ExternalCacheDir()下载下来的apk文件。 getExternalCacheDir() : /storage/emulated/0/Android/data/com../cache/ 获取访问权限 如果手机安卓版本为Android10的时候,可以在AndroidManifest.xml中添加下列代码 android:requestLegacyExt…

centos安装inpanel

前置条件 安装python yum -y install python 安装 cd /usr/local git clone https://gitee.com/WangZhe168_admin/inpanel.git cd inpanel python install.py 安装过程需要设置账户 密码 端口号 我设置的是admin:admin 10050 使用 打开浏览器,输入 http://192.168.168.…

jquery生成多个滑块,并对每个滑块做处理

基础滑块可以参考上一篇 eval(newThree).map((item, index) > { <div id"${uniqueId}" data-value"${item.text}" class"slider2"></div>$(document).ready(function () {for (let i 0; i < sliders.length; i)…

Java设计模式大全:23种常见的设计模式详解(二)

本系列文章简介&#xff1a; 设计模式是在软件开发过程中&#xff0c;经过实践和总结得到的一套解决特定问题的可复用的模板。它是一种在特定情境中经过验证的经验和技巧的集合&#xff0c;可以帮助开发人员设计出高效、可维护、可扩展和可复用的软件系统。设计模式提供了一种在…

一个冷门的js加密逆向分析

先上加密代码供各位先看为敬 (function(){function j2f6c82(ve7deb){var i86905"VPfaI5H|Nc]$^rhn1B8dR.w/u-4!ZetJ?XFM2SY(&sbjlW6GEmAd[L0i,;yx%qozC9U_~g37OkKTpvQD:";var z1a52da8"4H_&|GNcEon:B2-?h]lx.(gkzOdA3eL,9;myV8bJwriRSt6sX75Fvu^p0Ij…