文章目录
原文文献PDF地址
Least Squares Fitting最小二乘法拟合(1)
5.最小二乘球
5.1 参数化
- 一个球由它的球心 ( x 0 , y 0 , z 0 ) (x_0,y_0,z_0) (x0,y0,z0)和它的半径r0决定。在球面上的每一点都满足等式 ( x − x 0 ) 2 + ( y − y 0 ) 2 + ( y − y 0 ) 2 = r 0 2 (x-x_0)^2+(y-y_0)^2+(y-y_0)^2=r_0^2 (x−x0)2+(y−y0)2+(y−y0)2=r02
5.2 对圆心和半径进行初始估计
- 定义一个最小化函数来获得圆心和半径的初始估计,构建一个函数 f 1 = r i − r , r = ( x − x 0 ) 2 + ( y − y 0 ) 2 + ( z − z 0 ) 2 f_1=r_i-r,r=\sqrt {(x-x_0)^2+(y-y_0)^2+(z-z_0)^2} f1=ri−r,r=(x−x0)2+(y−y0)2+(z−z0)2用这个函数分别对 x 0 , y 0 , z 0 , r 0 x_0,y_0,z_0,r_0 x0,y0,z0,r0进行求偏导,会得到一个难以求解的复杂等式。
- 因此考虑函数 f 2 = r i 2 − r 2 f_2=r_i^2-r^2 f2=ri2−r2。这个函数可以被写作 f 2 = ( r i − r ) ( r i + r ) ≈ 2 r ( r i − r ) f_2=(r_i-r)(r_i+r)≈2r(r_i-r) f2=(ri−r)(ri+r)≈2r(ri−r)因为 r i + r r_i+r ri+r可以被近似为2r
- 将该函数分别对 x 0 , y 0 , z 0 , r 0 x_0,y_0,z_0,r_0 x0,y0,z0,r0进行求偏导来获得初始评估的圆心和半径。
- 因此最小化函数获得球的初始估值为 f = r i 2 − r 2 f=r_i^2-r^2 f=ri2−r2
数学初始预估
- 将等式
f
=
r
i
2
−
r
2
f=r_i^2-r^2
f=ri2−r2展开我们得到
f = ( x i − x 0 ) 2 + ( y i − y 0 ) 2 + ( z i − z 0 ) 2 − r 2 = − 2 ( 2 x i x 0 + 2 y i y 0 + 2 z i z 0 ) + ρ + ( x i 2 + y i 2 + z i 2 ) f=(x_i-x_0)^2+(y_i-y_0)^2+(z_i-z_0)^2-r^2=-2(2x_ix_0+2y_iy_0+2z_iz_0)+ρ+(x_i^2+y_i^2+z_i^2) f=(xi−x0)2+(yi−y0)2+(zi−z0)2−r2=−2(2xix0+2yiy0+2ziz0)+ρ+(xi2+yi2+zi2)
其中 ρ = ( x i 2 + y i 2 + z i 2 ) − r 2 ρ=(x_i^2+y_i^2+z_i^2)-r^2 ρ=(xi2+yi2+zi2)−r2
引入变量ρ使方程呈线性
将该上述n个点集的方程用矩阵形式表示
[ − 2 x 1 − 2 y 1 − 2 z 1 − 2 x 2 − 2 y 2 − 2 z 2 ⋮ ⋮ ⋮ − 2 x n − 2 y n − 2 z n ] [ x 0 y 0 z 0 ρ ] − [ x 1 2 + y 1 2 + z 1 2 x 2 2 + y 2 2 + z 2 2 ⋮ x n 2 + y n 2 + z n 2 ] = [ f 1 f 2 ⋮ f n ] \begin{bmatrix} -2x_1&-2y_1&-2z_1\\ -2x_2&-2y_2&-2z_2\\ \vdots&\vdots&\vdots\\ -2x_n&-2y_n&-2z_n\\ \end{bmatrix}\begin{bmatrix} x_0\\ y_0\\ z_0\\ ρ\\ \end{bmatrix}-\begin{bmatrix} x_1^2+y_1^2+z_1^2\\ x_2^2+y_2^2+z_2^2\\ \vdots\\ x_n^2+y_n^2+z_n^2\\ \end{bmatrix}=\begin{bmatrix} f_1\\ f_2\\ \vdots\\ f_n\\ \end{bmatrix} −2x1−2x2⋮−2xn−2y1−2y2⋮−2yn−2z1−2z2⋮−2zn x0y0z0ρ − x12+y12+z12x22+y22+z22⋮xn2+yn2+zn2 = f1f2⋮fn
对最小二乘求解, f i = 0 f_i=0 fi=0,引入矩阵符号
A = [ − 2 x 1 − 2 y 1 − 2 z 1 − 2 x 2 − 2 y 2 − 2 z 2 ⋮ ⋮ ⋮ − 2 x n − 2 y n − 2 z n ] , p = [ x 0 y 0 z 0 ρ ] , B = [ x 1 2 + y 1 2 + z 1 2 x 2 2 + y 2 2 + z 2 2 ⋮ x n 2 + y n 2 + z n 2 ] A=\begin{bmatrix} -2x_1&-2y_1&-2z_1\\ -2x_2&-2y_2&-2z_2\\ \vdots&\vdots&\vdots\\ -2x_n&-2y_n&-2z_n\\ \end{bmatrix},p=\begin{bmatrix} x_0\\ y_0\\ z_0\\ ρ\\ \end{bmatrix},B=\begin{bmatrix} x_1^2+y_1^2+z_1^2\\ x_2^2+y_2^2+z_2^2\\ \vdots\\ x_n^2+y_n^2+z_n^2\\ \end{bmatrix} A= −2x1−2x2⋮−2xn−2y1−2y2⋮−2yn−2z1−2z2⋮−2zn ,p= x0y0z0ρ ,B= x12+y12+z12x22+y22+z22⋮xn2+yn2+zn2
因此我们得到AP-B=0,求解这个最小二乘方程得到P。
这表明P满足等式ATAP=ATB。
求解P从而得到 x 0 , y 0 , z 0 , ρ x_0,y_0,z_0,ρ x0,y0,z0,ρ的初始估值,
半径r的初始估值可以通过求解以下方程得到
ρ = ( x i 2 + y i 2 + z i 2 ) − r 2 ρ=(x_i^2+y_i^2+z_i^2)-r^2 ρ=(xi2+yi2+zi2)−r2
5.3 高斯牛顿法
在获得球中心和半径r的初始估值后,利用高斯牛顿法来得到球中心和半径r的最终数值。
-
最小化函数
d 1 = r i − r , r i = ( x − x 0 ) 2 + ( y − y 0 ) 2 + ( z − z 0 ) 2 d_1=r_i-r,r_i=\sqrt {(x-x_0)^2+(y-y_0)^2+(z-z_0)^2} d1=ri−r,ri=(x−x0)2+(y−y0)2+(z−z0)2 -
- 建立雅可比矩阵
雅可比矩阵的元素为
[ ∂ d 1 ∂ x 0 ∂ d 1 ∂ y 0 ∂ d 1 ∂ z 0 ∂ d 1 ∂ r 0 ∂ d 2 ∂ x 0 ∂ d 2 ∂ y 0 ∂ d 2 ∂ z 0 ∂ d 2 ∂ r 0 ⋮ ⋮ ⋮ ⋮ ∂ d n ∂ x 0 ∂ d n ∂ y 0 ∂ d n ∂ z 0 ∂ d n ∂ r 0 ] \begin{bmatrix} \frac {\partial d_1}{\partial x_0}&\frac {\partial d_1}{\partial y_0}&\frac {\partial d_1}{\partial z_0}&\frac {\partial d_1}{\partial r_0}\\ \frac {\partial d_2}{\partial x_0}&\frac {\partial d_2}{\partial y_0}&\frac {\partial d_2}{\partial z_0}&\frac {\partial d_2}{\partial r_0}\\ \vdots&\vdots&\vdots&\vdots\\ \frac {\partial d_n}{\partial x_0}&\frac {\partial d_n}{\partial y_0}&\frac {\partial d_n}{\partial z_0}&\frac {\partial d_n}{\partial r_0}\\ \end{bmatrix} ∂x0∂d1∂x0∂d2⋮∂x0∂dn∂y0∂d1∂y0∂d2⋮∂y0∂dn∂z0∂d1∂z0∂d2⋮∂z0∂dn∂r0∂d1∂r0∂d2⋮∂r0∂dn
计算雅可比矩阵的各种分量并且带入矩阵,得到
J = [ − ( x 1 − x 0 ) r 1 − ( y 1 − y 0 ) r 1 − ( z 1 − z 0 ) r 1 − 1 − ( x 2 − x 0 ) r 2 − ( y 2 − y 0 ) r 2 − ( z 2 − z 0 ) r 2 − 1 ⋮ ⋮ ⋮ − ( x n − x 0 ) r n − ( y n − y 0 ) r n − ( z n − z 0 ) r n − 1 ] J=\begin{bmatrix} \frac {-(x_1-x_0)}{r_1}&\frac {-(y_1-y_0)}{r_1}&\frac {-(z_1-z_0)}{r_1}-1\\ \frac {-(x_2-x_0)}{r_2}&\frac {-(y_2-y_0)}{r_2}&\frac {-(z_2-z_0)}{r_2}-1\\ \vdots&\vdots&\vdots\\ \frac {-(x_n-x_0)}{r_n}&\frac {-(y_n-y_0)}{r_n}&\frac {-(z_n-z_0)}{r_n}-1\\ \end{bmatrix} J= r1−(x1−x0)r2−(x2−x0)⋮rn−(xn−x0)r1−(y1−y0)r2−(y2−y0)⋮rn−(yn−y0)r1−(z1−z0)−1r2−(z2−z0)−1⋮rn−(zn−z0)−1
- 建立雅可比矩阵
-
- 求解线性最小二乘系统 JP=-D,其中
p = [ p x 0 p y 0 p z 0 p r ] p=\begin{bmatrix} p_{x0}\\ p_{y0}\\ p_{z0}\\ p_r\\ \end{bmatrix} p= px0py0pz0pr
- 求解线性最小二乘系统 JP=-D,其中
-
- 增量参数如下
x 0 = x 0 + p x 0 ; x_0=x_0+p_{x0}; x0=x0+px0;
y 0 = y 0 + p y 0 ; y_0=y_0+p_{y0}; y0=y0+py0;
z 0 = z 0 + p z 0 ; z_0=z_0+p_{z0}; z0=z0+pz0;
r 0 = r 0 + p r ; r_0=r_0+p_r; r0=r0+pr;
- 增量参数如下
-
- 收敛条件
重复以上步骤直到算法收敛,收敛条件由 g = J T d g=J^Td g=JTd最小时得到。
- 收敛条件
6. 圆柱体的高斯牛顿策略
- 任何直线可以由给定的一点以及方向余弦(a,b,c)确定。所以需要6个数来描述一条直线。其中一个约束条件就是
a
2
+
b
2
+
c
2
=
1
a^2+b^2+c^2=1
a2+b2+c2=1,所以给出两个分量,第三个就可以被确定。考虑直线的约束条件,当c=1时一条垂直线,它足够确定两个方向余弦a和b。Z可以由以下关系确定
a x + b y + c z = 0 ax+by+cz=0 ax+by+cz=0
因为c=1,z=-ax-by - 所以这个约束的优点就是将6个参数减小到4个参数 a , b , x 0 , y 0 a,b,x_0,y_0 a,b,x0,y0,它同样降低了求解雅可比矩阵的复杂度和评估的时耗,因为使用高斯牛顿法时就计算了距离的导数。
- 高斯牛顿法对于圆柱的情况进行了改进
-
- 每次迭代开始前转换坐标系,所以轴上的点是坐标系上的原点,这意味着x=y=0
-
- 旋转坐标系系统使坐标系围绕Z轴转动,这样a=b=0并且c=1。
-
6.1 旋转的概念
参考上图,
B
=
{
u
1
,
u
2
,
u
3
}
B=\{u_1,u_2,u_3\}
B={u1,u2,u3}经过旋转生成了新的基准
B
′
=
{
u
1
′
,
u
2
′
,
u
3
′
}
B'=\{u_1',u_2',u_3'\}
B′={u1′,u2′,u3′},其中
u
1
,
u
2
,
u
3
和
u
1
′
,
u
2
′
,
u
3
′
u_1,u_2,u_3 和 u_1',u_2',u_3'
u1,u2,u3和u1′,u2′,u3′都是单位向量,
u
1
′
,
u
2
′
,
u
3
′
u_1',u_2',u_3'
u1′,u2′,u3′可以被表示为
[
u
1
′
]
B
=
[
c
o
s
(
θ
)
s
i
n
(
θ
)
0
]
,
[
u
2
′
]
B
=
[
−
s
i
n
(
θ
)
c
o
s
(
θ
)
0
]
,
[
u
3
′
]
B
=
[
0
0
1
]
\begin{bmatrix} u_1'\\ \end{bmatrix}_B=\begin{bmatrix} cos(θ)\\ sin(θ)\\ 0\\ \end{bmatrix},\begin{bmatrix} u_2'\\ \end{bmatrix}_B=\begin{bmatrix} -sin(θ)\\ cos(θ)\\ 0\\ \end{bmatrix},\begin{bmatrix} u_3'\\ \end{bmatrix}_B=\begin{bmatrix} 0\\ 0\\ 1\\ \end{bmatrix}
[u1′]B=
cos(θ)sin(θ)0
,[u2′]B=
−sin(θ)cos(θ)0
,[u3′]B=
001
从B’到B的变换矩阵是:
p
=
[
c
o
s
(
θ
)
s
i
n
(
θ
)
0
−
s
i
n
(
θ
)
c
o
s
(
θ
)
0
0
0
1
]
p=\begin{bmatrix} cos(θ)&sin(θ)&0\\ -sin(θ)&cos(θ)&0\\ 0&0&1\\ \end{bmatrix}
p=
cos(θ)−sin(θ)0sin(θ)cos(θ)0001
所以新的坐标(X’,Y’,Z’)可以由旧的坐标(X,Y,Z)计算得到
[
x
′
y
′
z
′
]
=
[
c
o
s
(
θ
)
s
i
n
(
θ
)
0
−
s
i
n
(
θ
)
c
o
s
(
θ
)
0
0
0
1
]
[
x
y
z
]
\begin{bmatrix} x'\\ y'\\ z'\\ \end{bmatrix}=\begin{bmatrix} cos(θ)&sin(θ)&0\\ -sin(θ)&cos(θ)&0\\ 0&0&1\\ \end{bmatrix}\begin{bmatrix} x\\ y\\ z\\ \end{bmatrix}
x′y′z′
=
cos(θ)−sin(θ)0sin(θ)cos(θ)0001
xyz
7. 最小二乘法最佳拟合圆柱
- 假设圆柱体由m个点 ( x i , y i , z i ) , m ≥ 5 (x_i,y_i,z_i),m≥5 (xi,yi,zi),m≥5n拟合
7.1 参数化
- 一个圆柱可以由以下三个部分定义
-
- 坐标轴上的一点 ( x 0 , y 0 , z 0 ) (x_0,y_0,z_0) (x0,y0,z0)
-
- 指向长轴的向量(a,b,c)
-
- 它的圆心r
-
7.2 圆柱的初始预估
设置轴线的方向是a,b,c。
(
x
0
,
y
0
,
z
0
)
(x_0,y_0,z_0)
(x0,y0,z0)是轴线上的点,
(
x
i
,
y
i
,
z
i
)
(x_i,y_i,z_i)
(xi,yi,zi)是圆柱上的点,
A
2
+
B
2
+
C
2
=
R
2
A^2+B^2+C^2=R^2
A2+B2+C2=R2其中
A
=
c
(
y
i
−
y
0
)
−
b
(
z
i
−
z
0
)
A=c(y_i-y_0)-b(z_i-z_0)
A=c(yi−y0)−b(zi−z0)
B
=
a
(
z
i
−
z
0
)
−
c
(
x
i
−
x
0
)
B=a(z_i-z_0)-c(x_i-x_0)
B=a(zi−z0)−c(xi−x0)
C
=
b
(
x
i
−
x
0
)
−
a
(
y
i
−
y
0
)
C=b(x_i-x_0)-a(y_i-y_0)
C=b(xi−x0)−a(yi−y0)
R
=
半径的初始评估
R=半径的初始评估
R=半径的初始评估
代入上式
[
c
(
y
i
−
y
0
)
−
b
(
z
i
−
z
0
)
]
2
+
[
a
(
z
i
−
z
0
)
−
b
(
x
i
−
x
0
)
]
2
+
[
b
(
x
i
−
x
0
)
−
b
(
y
i
−
y
0
)
]
2
=
R
2
[c(y_i-y_0)-b(z_i-z_0)]^2+[a(z_i-z_0)-b(x_i-x_0)]^2+[b(x_i-x_0)-b(y_i-y_0)]^2=R^2
[c(yi−y0)−b(zi−z0)]2+[a(zi−z0)−b(xi−x0)]2+[b(xi−x0)−b(yi−y0)]2=R2
这个等式是简化了的,它产生下式
A
x
2
+
B
y
2
+
C
z
2
+
D
x
y
+
E
x
z
+
F
y
z
+
G
x
+
H
y
+
I
z
+
J
=
0
Ax^2+By^2+ Cz^2+ Dxy + Exz + Fyz + Gx + Hy +Iz + J=0
Ax2+By2+Cz2+Dxy+Exz+Fyz+Gx+Hy+Iz+J=0
其中
A
=
(
b
2
+
c
2
)
A=(b^2+c^2)
A=(b2+c2)
B
=
(
a
2
+
c
2
)
B=(a^2+c^2)
B=(a2+c2)
C
=
(
a
2
+
b
2
)
C=(a^2+b^2)
C=(a2+b2)
D
=
−
2
a
b
D=-2ab
D=−2ab
E
=
−
2
a
c
E=-2ac
E=−2ac
F
=
−
2
b
c
F=-2bc
F=−2bc
G
=
−
2
(
b
2
+
c
2
)
x
0
+
2
a
b
y
0
+
2
a
c
z
0
G=-2(b^2+c^2)x_0+2aby_0+2acz_0
G=−2(b2+c2)x0+2aby0+2acz0
H
=
2
a
b
x
0
−
2
(
a
2
+
c
2
)
x
0
+
2
b
c
z
0
H=2abx_0-2(a^2+c^2)x_0+2bcz_0
H=2abx0−2(a2+c2)x0+2bcz0
I
=
2
a
c
x
0
+
2
b
c
y
0
−
2
(
a
2
+
b
2
)
z
0
I=2acx_0+2bcy_0-2(a^2+b^2)z_0
I=2acx0+2bcy0−2(a2+b2)z0
J
=
(
b
2
+
c
2
)
x
0
2
+
(
a
2
+
c
2
)
y
0
2
+
(
a
2
+
b
2
)
z
0
2
−
2
b
c
y
0
z
0
−
2
a
c
z
0
x
0
−
2
a
b
x
0
y
0
−
R
2
J=(b^2+c^2)x_0^2+(a^2+c^2)y_0^2+(a^2+b^2)z_0^2-2bcy_0z_0-2acz_0x_0-2abx_0y_0-R^2
J=(b2+c2)x02+(a2+c2)y02+(a2+b2)z02−2bcy0z0−2acz0x0−2abx0y0−R2
等式两边同时除以A
x
2
+
B
A
y
2
+
C
A
z
2
+
D
A
x
y
+
E
A
x
z
+
F
A
y
z
+
G
A
x
+
H
A
y
+
I
A
z
+
J
A
=
0
x^2+\frac{B}{A}y^2+ \frac{C}{A}z^2+\frac{D}{A}xy +\frac{E}{A}xz + \frac{F}{A}yz + \frac{G}{A}x +\frac{H}{A}y +\frac{I}{A}z +\frac{J}{A}=0
x2+ABy2+ACz2+ADxy+AExz+AFyz+AGx+AHy+AIz+AJ=0
因此
B
A
y
2
+
C
A
z
2
+
D
A
x
y
+
E
A
x
z
+
F
A
y
z
+
G
A
x
+
H
A
y
+
I
A
z
+
J
A
=
−
x
2
\frac{B}{A}y^2+ \frac{C}{A}z^2+\frac{D}{A}xy +\frac{E}{A}xz + \frac{F}{A}yz + \frac{G}{A}x +\frac{H}{A}y +\frac{I}{A}z +\frac{J}{A}=-x^2
ABy2+ACz2+ADxy+AExz+AFyz+AGx+AHy+AIz+AJ=−x2
这个可以写作一个线性系统方程
(
y
1
2
z
1
2
x
1
y
1
x
1
z
1
y
1
z
1
x
1
y
1
z
1
1
y
2
2
z
2
2
x
2
y
2
x
2
z
2
y
2
z
2
x
2
y
2
z
2
1
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
y
2
2
z
n
2
x
n
y
n
x
n
z
n
y
n
z
n
x
n
y
n
z
n
1
)
(
B
/
A
C
/
A
D
/
A
E
/
A
F
/
A
G
/
A
H
/
A
I
/
A
J
/
A
)
=
(
−
x
1
2
−
x
2
2
⋮
−
x
n
2
)
\begin{pmatrix} y_1^2&z_1^2&x_1y_1&x_1z_1&y_1z_1&x_1&y_1&z_1&1\\ y_2^2&z_2^2&x_2y_2&x_2z_2&y_2z_2&x_2&y_2&z_2&1\\ \vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots\\ y_2^2&z_n^2&x_ny_n&x_nz_n&y_nz_n&x_n&y_n&z_n&1\\ \end{pmatrix}\begin{pmatrix} B/A\\ C/A\\ D/A\\ E/A\\ F/A\\ G/A\\ H/A\\ I/A\\ J/A\\ \end{pmatrix}=\begin{pmatrix} -x_1^2\\ -x_2^2\\ \vdots\\ -x_n^2\\ \end{pmatrix}
y12y22⋮y22z12z22⋮zn2x1y1x2y2⋮xnynx1z1x2z2⋮xnzny1z1y2z2⋮ynznx1x2⋮xny1y2⋮ynz1z2⋮zn11⋮1
B/AC/AD/AE/AF/AG/AH/AI/AJ/A
=
−x12−x22⋮−xn2
这是
A
T
A
P
=
A
T
B
A^TAP=A^TB
ATAP=ATB的一种形式,P可以被解为
P
=
(
A
T
A
)
−
1
A
T
B
P=(A^TA)^{-1} A^TB
P=(ATA)−1ATB
让C’=C/A,D’=D/A,E’=E/A,F’=F/A,G’=G/A,H’=H/A,I’=I/A,J’=J/A
求解上式如果|D’|,|E’|和|F’|接近于0,B’接近于1表明(a,b,c)=(0,0,1),如果C’接近于1表明(a,b,c)=(0,1,0),否则
K
=
2
1
+
B
′
+
C
′
K=\frac{2}{1+B'+C'}
K=1+B′+C′2
A=k,B=kB’,C=kC’,D=kD’,E=kE’,F=kF’,G=kG’,I=kI’,J=kJ’
如果A和B接近于1,那么
c
′
=
(
1
−
C
)
1
/
2
,
a
′
=
E
/
−
2
c
,
b
′
=
F
/
2
c
′
c'=(1-C)^{1/2},a'=E/-2c,b'=F/2c'
c′=(1−C)1/2,a′=E/−2c,b′=F/2c′
如果A接近于1,B不接近于1
b
′
=
(
1
−
B
)
1
/
2
,
a
′
=
D
/
−
2
b
′
,
c
′
=
F
/
−
2
b
′
b'=(1-B)^{1/2},a'=D/-2b',c'=F/-2b'
b′=(1−B)1/2,a′=D/−2b′,c′=F/−2b′
如果A不接近于1
a
′
=
(
1
−
A
)
1
/
2
,
a
′
=
D
/
−
2
a
′
,
c
′
=
E
/
−
2
a
′
a'=(1-A)^{1/2},a'=D/-2a',c'=E/-2a'
a′=(1−A)1/2,a′=D/−2a′,c′=E/−2a′
方向(a’,b’,c’)被归一化得到向量(a,b,c)
7.2.1 轴上点的初始评估
已知(a,b,c)是系数G,H,I的定义,而等式
a
x
0
+
b
y
0
+
c
z
0
=
0
ax_0+by_0+cz_0=0
ax0+by0+cz0=0是用来求解参数
(
x
0
,
y
0
,
z
0
)
(x_0,y_0,z_0)
(x0,y0,z0)的,这将生成以下的线性系统
(
−
2
(
b
2
+
c
2
)
2
a
b
2
a
c
2
a
b
−
2
(
a
2
+
c
2
)
2
b
c
2
a
c
−
2
b
c
−
2
(
a
2
+
b
2
)
a
b
c
)
(
x
0
y
0
z
0
)
=
(
G
H
I
0
)
\begin{pmatrix} -2(b^2+c^2)&2ab&2ac\\ 2ab&-2(a^2+c^2)&2bc\\ 2ac&-2bc&-2(a^2+b^2)\\ a&b&c\\ \end{pmatrix}\begin{pmatrix} x_0\\ y_0\\ z_0\\ \end{pmatrix}=\begin{pmatrix} G\\ H\\ I\\ 0\\ \end{pmatrix}
−2(b2+c2)2ab2aca2ab−2(a2+c2)−2bcb2ac2bc−2(a2+b2)c
x0y0z0
=
GHI0
利用正规方程
A
T
A
P
=
A
T
B
A^TAP=A^TB
ATAP=ATB求解这个等式,P包含了在轴上点的初始估值
7.2.2 半径的初始估值
这可以通过使用J的定义来实现
J
=
(
b
2
+
c
2
)
x
0
2
+
(
a
2
+
c
2
)
y
0
2
+
(
a
2
+
b
2
)
z
0
2
−
2
b
c
y
0
z
0
−
2
a
c
z
0
x
0
−
2
a
b
x
0
y
0
−
R
2
J=(b^2+c^2)x_0^2+(a^2+c^2)y_0^2+(a^2+b^2)z_0^2-2bcy_0z_0-2acz_0x_0-2abx_0y_0-R^2
J=(b2+c2)x02+(a2+c2)y02+(a2+b2)z02−2bcy0z0−2acz0x0−2abx0y0−R2
所以
R
2
=
(
b
2
+
c
2
)
x
0
2
+
(
a
2
+
c
2
)
y
0
2
+
(
a
2
+
b
2
)
z
0
2
−
2
b
c
y
0
z
0
−
2
a
c
z
0
x
0
−
2
a
b
x
0
y
0
−
J
R^2=(b^2+c^2)x_0^2+(a^2+c^2)y_0^2+(a^2+b^2)z_0^2-2bcy_0z_0-2acz_0x_0-2abx_0y_0-J
R2=(b2+c2)x02+(a2+c2)y02+(a2+b2)z02−2bcy0z0−2acz0x0−2abx0y0−J
7.3 算法描述
-
- 对原点的转换
对数据的副本进行转换,使坐标轴上的点处于点的质心
( x i , y i , z i ) = ( x i , y i , z i ) − ( x c , y c , z c ) (x_i,y_i,z_i)=(x_i,y_i,z_i)-(x_c,y_c,z_c) (xi,yi,zi)=(xi,yi,zi)−(xc,yc,zc)
- 对原点的转换
-
- 通过旋转矩阵U,将(a,b,c)旋转到z轴上的一点
( x i y i z i ) = U ( x i y i z i ) \begin{pmatrix}x_i\\ y_i\\ z_i\end{pmatrix}=U\begin{pmatrix}x_i\\ y_i\\ z_i\end{pmatrix} xiyizi =U xiyizi
其中U为
U = ( c 2 0 s 2 0 1 0 − c 2 0 c 2 ) ( 1 0 0 0 c 1 s 1 0 − s 2 c 1 ) U= \begin{pmatrix} c_2&0&s_2\\ 0&1&0\\ -c_2&0&c_2\\ \end{pmatrix}\begin{pmatrix} 1&0&0\\ 0&c_1&s_1\\ 0&-s_2&c_1\\ \end{pmatrix} U= c20−c2010s20c2 1000c1−s20s1c1
对于这种情况(a,b,c)=(1,0,0)
s 1 = 0 , c 1 = 1 , s 2 = − 1 , c 2 = 0 s_1=0,c_1=1,s_2=-1,c_2=0 s1=0,c1=1,s2=−1,c2=0
否则
c 1 = c / b 2 + c 2 , s 1 = − b / b 2 + c 2 c_1=c/\sqrt{b^2+c^2},s_1=-b/\sqrt{b^2+c^2} c1=c/b2+c2,s1=−b/b2+c2
c 2 = ( c c 1 − b s 1 ) / a 2 + ( c c 1 − b s 1 ) 2 c_2=(cc_1-bs_1)/\sqrt{a^2+(cc_1-bs_1)^2} c2=(cc1−bs1)/a2+(cc1−bs1)2
s 2 = a / a 2 + ( c c 1 − b s 1 ) 2 s_2=a/\sqrt{a^2+(cc_1-bs_1)^2} s2=a/a2+(cc1−bs1)2
旋转圆柱体的轴,圆柱体绕着x轴旋转这样它就仍然在YZ平面上。然后将圆柱绕着y轴旋转这样圆柱就是沿着Z轴的方向。
- 通过旋转矩阵U,将(a,b,c)旋转到z轴上的一点
-
- 形成右侧向量d和雅可比矩阵
点 ( x i , y i , z i ) (x_i,y_i,z_i) (xi,yi,zi)到圆柱体的距离为 d = r i − r d=r_i-r d=ri−r其中
r i = u i 2 + v i 2 + w i 2 a 2 + b 2 + c 2 r_i=\frac{\sqrt {u_i^2+v_i^2+w_i^2}}{\sqrt {a^2+b^2+c^2}} ri=a2+b2+c2ui2+vi2+wi2
u i = c ( y i − y 0 ) − b ( z i − z 0 ) u_i=c(y_i-y_0)-b(z_i-z_0) ui=c(yi−y0)−b(zi−z0)
v i = a ( z i − z 0 ) − b ( x i − x 0 ) v_i=a(z_i-z_0)-b(x_i-x_0) vi=a(zi−z0)−b(xi−x0)
w i = b ( x i − x 0 ) − b ( y i − y 0 ) w_i=b(x_i-x_0)-b(y_i-y_0) wi=b(xi−x0)−b(yi−y0)
雅可比矩阵
J = ( − x 1 r 1 − y 1 r 1 − x 1 z 1 r 1 − y 1 z 1 r 1 − 1 − x 2 r 2 − y 2 r 2 − x 2 z 2 r 2 − y 2 z 2 r 2 − 1 ⋮ ⋮ ⋮ ⋮ ⋮ − x n r n − y n r n − x n z n r n − y n z n r n − 1 ) J=\begin{pmatrix} \frac {-x_1}{r_1}&\frac {-y_1}{r_1}&\frac {-x_1 z_1}{r_1} \frac {-y_1 z_1}{r_1}&-1\\ \frac {-x_2}{r_2}&\frac {-y_2}{r_2}&\frac {-x_2 z_2}{r_2} \frac {-y_2 z_2}{r_2}&-1\\ \vdots&\vdots&\vdots&\vdots&\vdots\\ \frac {-x_n}{r_n}&\frac {-y_n}{r_n}&\frac {-x_n z_n}{r_n} \frac {-y_n z_n}{r_n}&-1\\ \end{pmatrix} J= r1−x1r2−x2⋮rn−xnr1−y1r2−y2⋮rn−ynr1−x1z1r1−y1z1r2−x2z2r2−y2z2⋮rn−xnznrn−ynzn−1−1⋮−1⋮ -
- 求解线性最小二乘系统
J ( p x 0 p y 0 p a p b p r ) = − d J\begin{pmatrix} p_{x0}\\ p_{y0}\\ p_a\\ p_b\\ p_r\\ \end{pmatrix}=-d J px0py0papbpr =−d
-
5.根据下式更新参数
( x 0 y 0 z 0 ) = ( x 0 y 0 z 0 ) + U T ( p x 0 p y 0 − p x 0 p a − p y 0 p b ) \begin{pmatrix}x_0\\ y_0\\ z_0\end{pmatrix}=\begin{pmatrix}x_0\\ y_0\\ z_0\end{pmatrix}+U^T\begin{pmatrix}p_{x0}\\p_{y0}\\ -p_{x0}p_a-p_{y0}p_b\end{pmatrix} x0y0z0 = x0y0z0 +UT px0py0−px0pa−py0pb
( a b c ) = U T ( p a p b 1 ) \begin{pmatrix}a\\ b\\ c\end{pmatrix}=U^T\begin{pmatrix}p_a\\ p_b\\ 1\end{pmatrix} abc =UT papb1
r = r + p r r=r+p_r r=r+pr
重复以上步骤直到算法收敛,在第一步中使用的是原始数据集的副本而不是经过迭代后的转换数据集 -
- 如果我们想在最接近原点的线上表示
(
x
0
,
y
0
,
z
0
)
(x_0,y_0,z_0)
(x0,y0,z0),那么
( x 0 y 0 z 0 ) = ( x 0 y 0 z 0 ) − a x 0 + b y 0 + c z 0 a 2 + b 2 + c 2 ( a b c ) \begin{pmatrix}x_0\\ y_0\\ z_0\end{pmatrix}=\begin{pmatrix}x_0\\ y_0\\ z_0\end{pmatrix}-\frac {ax_0+by_0+cz_0}{a^2+b^2+c^2}\begin{pmatrix}a\\ b\\ c\end{pmatrix} x0y0z0 = x0y0z0 −a2+b2+c2ax0+by0+cz0 abc
- 如果我们想在最接近原点的线上表示
(
x
0
,
y
0
,
z
0
)
(x_0,y_0,z_0)
(x0,y0,z0),那么
8. 总结
在这篇报告中,详细的讨论了最小二乘法拟合线、平面、圆、球、和圆柱的方法。算法的实现方式是matlab源码都已经包含在附录中。本文还详细地讨论了实现中涉及的数学步骤。我们用来介绍这个项目的文章的摘要也包含在了附录中。
9. 附录
- Forbes, Alistair B., Least Squares Best-Fit geometric elements, NPL report April 1989.
- Assessment of Position, size and departure from nominal form of geometric features,
British Standards Institution, BS7172: 1989 - Computational Metrology, Manufacturing Review, Vol 6, No 4,1993
- Press, William H. Flannery, Brian P, Teukolsy, Saul A, Vetterling, William T.,
Numerical Recipes in C, The aart of Scientific computing, Cambridge university
Press. - Ramakrishna Machireddy, Sampling techniques for plane geometry for coordiante
measuring machines, MS Thesis, UNCC 1992 - Uppliappan Babu, Sampling Methods and substitute Geometry for Measuring
cylinders in a Co-ordiante Measuring Machines(Chapter 2), MS Thesis ,UNCC 1993 - Anton Rorres, Element Linear Algebra Application Version, NY 1991.
10. pdf
pdf 提取码:6i2w