目录
- 一、算法原理
- 二、代码实现
本文由CSDN点云侠原创,原文链接。如果你不是在点云侠的博客中看到该文章,那么此处便是不要脸的爬虫。
一、算法原理
由圆柱面的几何特性可得,圆柱面上的点到其轴线的距离恒等于半径 r 0 r_0 r0,如图1 所示。
其中, P P P为圆柱面上任意一点, P 0 ( x 0 , y 0 , z 0 ) P_0( x_0,y_0,z_0) P0(x0,y0,z0) 为圆柱轴线上一点, ( a , b , c ) ( a,b,c) (a,b,c)为圆柱轴线向
量, r 0 r_0 r0为圆柱底圆半径。圆柱面上任意一点到其轴线的距离为半径 r 0 r_0 r0,即:
( x − x 0 ) 2 + ( y − y 0 ) 2 + ( z − z 0 ) 2 − [ a ( x − x 0 ) + b ( y − y 0 ) + c ( z − z 0 ) ] 2 = r 0 2 (1) (x-x_0)^2+(y-y_0)^2+(z-z_0)^2-[a(x-x_0)+b(y-y_0)+c(z-z_0)]^2=r_0^2\tag{1} (x−x0)2+(y−y0)2+(z−z0)2−[a(x−x0)+b(y−y0)+c(z−z0)]2=r02(1)
式( 1) 即为圆柱面方程。根据圆柱面上点的坐标 ( x , y , z ) ( x,y,z) (x,y,z),求出圆柱轴线上一点 ( x 0 , y 0 , z 0 ) ( x_0,y_0,z_0) (x0,y0,z0) 、圆柱轴线向量 ( a , b , c ) ( a,b,c) (a,b,c) 、圆柱底圆半径 r 0 r_0 r0这七个参数,就可以唯一确定一个圆柱。
柱面拟合方法详细步骤如下:
(1) 确定柱面模型参数初始值。圆柱拟合初值的估计原理如下:
搜寻圆柱面上任意一点的若干邻近点,将这些点拟合成平面,得到的平面法向量单位化即该点的单位法向量。对圆柱面上每个点处理,得到每个点的单位法向量。将每个点的单位法向量看成点,将这些点拟合成平面,得到平面法向量,即圆柱轴线向量初始值 a 0 、 b 0 、 c 0 a_0、b_0、c_0 a0、b0、c0,该步骤使用了主成分分析法进行求解。
先对圆柱进行坐标转换,使圆柱轴线向量 ( a 0 , b 0 , c 0 ) (a_0,b_0,c_0) (a0,b0,c0)变换为平行 Z Z Z轴的向量即为 ( 0 , 0 , ( a 0 ) 2 + ( b 0 ) 2 + ( c 0 ) 2 ) ( 0,0,\sqrt{( a_0 )^ 2 + ( b_0)^2 + ( c_0)^ 2} ) (0,0,(a0)2+(b0)2+(c0)2) 。设旋转矩阵为 R 3 × 3 R_{3 ×3} R3×3,要使:
将旋转矩阵与圆柱上的点的坐标 ( x , y , z ) (x,y,z) (x,y,z)相乘,得到的新坐标 ( x ′ , y ′ , z ′ ) (x',y',z') (x′,y′,z′),将新坐标中的 x ′ , y ′ x',y' x′,y′取出得新坐标系下圆柱底面的坐标,根据 ( x ′ , y ′ ) (x',y') (x′,y′)可拟合一个圆。
圆的方程为 ( x − x 1 0 ′ ) 2 + ( y − y 1 0 ′ ) 2 = ( r 0 ) 2 (x-x_1^{0'})^2+(y-y_1^{0'})^2=(r_0)^2 (x−x10′)2+(y−y10′)2=(r0)2若知道圆心坐标 x 1 0 ′ , y 1 0 ′ x_1^{0'},y_1^{0'} x10′,y10′、半径 r 0 r_0 r0这三个参数,就可以唯一确定一个圆面。
首先,圆坐标 ( x ′ , y ′ ) (x',y') (x′,y′)准备完毕;其次,找误差方程,将圆的方程展开得到 x 2 + ( x 1 0 ′ ) 2 + y 2 + ( y 1 0 ′ ) 2 − 2 y y 1 0 ′ − ( r 0 ) 2 = 0 x^2+(x_1^{0'})^2+y^2+(y_1^{0'})^2-2yy_1^{0'}-(r_0)^2=0 x2+(x10′)2+y2+(y10′)2−2yy10′−(r0)2=0,即 2 x ( x 1 0 ′ ) + 2 y ( y 1 0 ′ ) + ( r 0 ) 2 − ( x 1 0 ′ ) 2 − ( y 1 0 ′ ) 2 − ( x 2 + y 2 ) = 0 2x(x_1^{0'})+2y(y_1^{0'})+(r_0)^2-(x_1^{0'})^2-(y_1^{0'})^2-(x^2+y^2)=0 2x(x10′)+2y(y10′)+(r0)2−(x10′)2−(y10′)2−(x2+y2)=0。可以设 x 1 0 ′ 、 y 1 0 ′ 、 ( r 0 ) 2 − ( x 1 0 ′ ) 2 − ( y 1 0 ′ ) 2 x_1^{0'}、y_1^{0'}、(r_0)^2-(x_1^{0'})^2-(y_1^{0'})^2 x10′、y10′、(r0)2−(x10′)2−(y10′)2为待求参数。于是误差方程为: V = 2 x ( x 1 0 ′ ) + 2 y ( y 1 0 ′ ) + ( r 0 ) 2 − ( x 1 0 ′ ) 2 − ( y 1 0 ′ ) 2 − ( x 2 + y 2 ) V=2x(x_1^{0'})+2y(y_1^{0'})+(r_0)^2-(x_1^{0'})^2-(y_1^{0'})^2-(x^2+y^2) V=2x(x10′)+2y(y10′)+(r0)2−(x10′)2−(y10′)2−(x2+y2),写成矩阵的形式即:
假设获取的圆的点等精度,即等权。根据线性最小二乘原理,即 V T V = m i n V^TV=min VTV=min,求得位置参数 X X X:
X = ( B T B ) − 1 B T L (5) X=(B^TB)^{-1}B^TL\tag{5} X=(BTB)−1BTL(5)
求出新坐标系下圆柱轴线上一点的初始值 x 1 0 ′ , y 1 0 ′ x_1^{0'},y_1^{0'} x10′,y10′以及底圆半径的初始值 r 0 r_0 r0,再根据公式:
将新坐标系下的 x 1 0 ′ , y 1 0 ′ x_1^{0'},y_1^{0'} x10′,y10′转换成旧坐标系下的坐标 x 1 0 , y 1 0 x_1^{0},y_1^{0} x10,y10。无论在新坐标系下还是在旧坐标系下,底圆半径的初值 r 0 r_0 r0都不变。为了尽量减小误差,将圆柱轴线上一点的初值 z 1 0 z_1^{0} z10的值设为原坐标系下圆柱点 z z z的最大值与最小值和的一半。于是求出圆柱轴线上一点、圆柱底半径这四个参数的初始值 x 1 0 , y 1 0 , z 1 0 , r 0 x_1^{0},y_1^{0},z_1^{0},r_0 x10,y10,z10,r0。
(2)建立改进误差方程式求解参数值。圆柱面方程为:
( x − x 1 ) 2 + ( y − y 1 ) 2 + ( z − z 1 ) 2 − [ a ( x − x 1 ) + b ( y − y 1 ) + c ( z − z 1 ) ] 2 = r 2 (7) (x-x_1)^2+(y-y_1)^2+(z-z_1)^2-[a(x-x_1)+b(y-y_1)+c(z-z_1)]^2=r^2\tag{7} (x−x1)2+(y−y1)2+(z−z1)2−[a(x−x1)+b(y−y1)+c(z−z1)]2=r2(7)
传统方法,令
f = ( x − x 1 ) 2 + ( y − y 1 ) 2 + ( z − z 1 ) 2 − [ a ( x − x 1 ) + b ( y − y 1 ) + c ( z − z 1 ) ] 2 − r 2 (8) f=(x-x_1)^2+(y-y_1)^2+(z-z_1)^2-[a(x-x_1)+b(y-y_1)+c(z-z_1)]^2-r^2\tag{8} f=(x−x1)2+(y−y1)2+(z−z1)2−[a(x−x1)+b(y−y1)+c(z−z1)]2−r2(8)
对 f f f线性化得
这样,误差方程可列为:
其中
V n × 1 = B n × 7 X 7 × 1 − L n × 1 (23) V_{n\times 1}=B_{n\times 7}X_{7\times 1}-L_{n\times 1}\tag{23} Vn×1=Bn×7X7×1−Ln×1(23)
假设获取的圆柱面上的点等精度,即权相等。根据最小二乘原理,求得参数
X = ( B T B ) − 1 B T L (24) X=(B^TB)^{-1}B^TL\tag{24} X=(BTB)−1BTL(24)
这是一个循环的迭代过程,每一次迭代计算过程中代入的初值都等于上一次的初值加上求出的 X X X的改正值,当 X X X的数值小到满足要求的精度时推出迭代。