注: 本文借鉴了 博客1
圆柱体可以由一条空间直线(柱芯)和圆柱半径(柱径)表示。
其中,柱芯由一条空间直线表示
x
−
x
0
a
=
y
−
y
0
b
=
z
−
z
0
c
\frac {x-x_0}{a}= \frac{y-y_0}{b}= \frac{z-z_0}{c}
ax−x0=by−y0=cz−z0
柱芯上任意一点为(
x
0
,
y
0
,
z
0
x_0,y_0, z_0
x0,y0,z0),柱芯上的方向向量为(a, b, c)。
柱径为r
方法阐述
圆柱面上的任意点为(
x
i
,
y
i
,
z
i
)
x_i,y_i,z_i)
xi,yi,zi)
A
2
x
+
B
2
y
+
C
2
z
=
r
2
,
其中
{
A
=
c
(
y
i
−
y
0
)
−
b
(
z
i
−
z
0
)
B
=
a
(
z
i
−
z
0
)
−
c
(
x
i
−
x
0
)
C
=
b
(
x
i
−
x
0
)
−
a
(
y
i
−
y
0
)
A^2x+B^2y+C^2z=r^2, 其中\ \left\{ \begin{aligned} A&=c(y_i-y_0)-b(z_i-z_0) \\ B&=a(z_i-z_0)-c(x_i-x_0) \\ C&=b(x_i-x_0)-a(y_i-y_0) \end{aligned} \right.
A2x+B2y+C2z=r2,其中 ⎩
⎨
⎧ABC=c(yi−y0)−b(zi−z0)=a(zi−z0)−c(xi−x0)=b(xi−x0)−a(yi−y0)
至于为什么
A
2
x
+
B
2
y
+
C
2
z
=
r
2
A^2x+B^2y+C^2z=r^2
A2x+B2y+C2z=r2,本人还不是非常理解
以上写成矩阵为:
[
x
i
−
x
0
y
i
−
y
0
z
i
−
z
0
]
[
0
c
−
b
−
c
0
a
b
−
a
0
]
=
[
A
B
C
]
\begin{bmatrix} x_i-x_0 \\ y_i- y_0\\ z_i- z_0 \end{bmatrix} \begin{bmatrix} 0 &c & -b \\ -c& 0 & a\\ b & -a& 0 \end{bmatrix}= \begin{bmatrix} A\\B\\C \end{bmatrix}
xi−x0yi−y0zi−z0
0−cbc0−a−ba0
=
ABC
将以上等式中的
A
,
B
,
C
A,B,C
A,B,C代入,得:
[
c
(
y
i
−
y
0
)
−
b
(
z
i
−
z
0
)
]
2
+
[
a
(
z
i
−
z
0
)
−
c
(
x
i
−
x
0
)
]
2
+
[
b
(
x
i
−
x
0
)
−
a
(
y
i
−
y
0
)
]
2
=
r
2
[c(y_i-y_0)-b(z_i-z_0)]^2+[a(z_i-z_0)-c(x_i-x_0)]^2+[b(x_i-x_0)-a(y_i-y_0)]^2 = r^2
[c(yi−y0)−b(zi−z0)]2+[a(zi−z0)−c(xi−x0)]2+[b(xi−x0)−a(yi−y0)]2=r2
将等式化简:
θ
0
x
2
+
θ
1
y
2
+
θ
2
z
2
+
θ
3
x
y
+
θ
4
x
z
+
θ
5
y
z
+
θ
6
x
+
θ
7
y
+
θ
8
z
+
θ
9
=
0
\theta_0x^2+\theta_1y^2+\theta_2z^2+\theta_3xy+\theta_4xz+\theta_5yz+\theta_6x+\theta_7y+\theta_8z+\theta_9=0
θ0x2+θ1y2+θ2z2+θ3xy+θ4xz+θ5yz+θ6x+θ7y+θ8z+θ9=0
其中:
{
θ
0
=
b
2
+
c
2
θ
1
=
a
2
+
c
2
θ
2
=
a
2
+
b
2
θ
3
=
−
2
a
b
θ
4
=
−
2
a
c
θ
5
=
−
2
b
c
θ
6
=
−
2
(
b
2
+
c
2
)
x
0
+
2
a
b
y
0
+
2
a
c
z
0
θ
7
=
2
a
b
x
0
−
2
(
a
2
+
c
2
)
x
0
+
2
b
c
z
0
θ
8
=
2
a
c
x
0
+
2
b
c
y
0
−
2
(
a
2
+
b
2
)
z
0
θ
9
=
(
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
\\\\ \left\{ \begin{aligned} \theta_0&=b^2+c^2\\ \theta_1&=a^2+c^2\\ \theta_2&=a^2+b^2\\ \theta_3&=-2ab\\ \theta_4&=-2ac\\ \theta_5&=-2bc\\ \theta_6&=−2(b^2+c^2)x_0+2aby_0+2acz_0\\ \theta_7&=2abx_0-2(a^2+c^2)x_0+2bcz_0\\ \theta_8&=2acx_0+2bcy_0-2(a^2+b^2)z_0\\ \theta_9&=(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 \end{aligned} \right.
⎩
⎨
⎧θ0θ1θ2θ3θ4θ5θ6θ7θ8θ9=b2+c2=a2+c2=a2+b2=−2ab=−2ac=−2bc=−2(b2+c2)x0+2aby0+2acz0=2abx0−2(a2+c2)x0+2bcz0=2acx0+2bcy0−2(a2+b2)z0=(b2+c2)x02+(a2+c2)y02+(a2+b2)z02−2bcy0z0−2acz0x0−2abx0y0−r2
等式两边同时除
θ
0
\theta_0
θ0,并移项得:
θ
1
θ
0
y
2
+
θ
2
θ
0
z
2
+
θ
3
θ
0
x
y
+
θ
4
θ
0
x
z
+
θ
6
θ
0
y
z
+
θ
7
θ
0
x
+
θ
7
θ
0
y
+
θ
8
θ
0
z
+
θ
9
θ
0
=
−
x
2
\frac{\theta_1}{\theta_0}y^2+\frac{\theta_2}{\theta_0}z^2+\frac{\theta_3}{\theta_0}xy+\frac{\theta_4}{\theta_0}xz+\frac{\theta_6}{\theta_0}yz+\frac{\theta_7}{\theta_0}x+\frac{\theta_7}{\theta_0}y+\frac{\theta_8}{\theta_0}z+\frac{\theta_9}{\theta_0}=-x^2
θ0θ1y2+θ0θ2z2+θ0θ3xy+θ0θ4xz+θ0θ6yz+θ0θ7x+θ0θ7y+θ0θ8z+θ0θ9=−x2
书写成矩阵为:
[
y
i
2
z
i
2
x
i
y
i
x
i
z
i
y
i
z
i
x
i
y
i
z
i
1
]
[
θ
1
/
θ
0
θ
2
/
θ
0
θ
3
/
θ
0
θ
4
/
θ
0
θ
5
/
θ
0
θ
6
/
θ
0
θ
7
/
θ
0
θ
8
/
θ
0
θ
9
/
θ
0
]
=
[
−
x
i
2
]
\begin{bmatrix} y_i^2 & z_i^2 & x_iy_i & x_iz_i &y_iz_i & x_i & y _i & z_i & 1 \end{bmatrix} \begin{bmatrix} \theta_1/\theta_0 \\ \theta_2/\theta_0 \\ \theta_3/\theta_0 \\ \theta_4/\theta_0 \\ \theta_5/\theta_0 \\ \theta_6/\theta_0 \\ \theta_7/\theta_0 \\ \theta_8/\theta_0 \\ \theta_9/\theta_0 \\ \end{bmatrix}= \begin{bmatrix} -x_i^2 \end{bmatrix}
[yi2zi2xiyixiziyizixiyizi1]
θ1/θ0θ2/θ0θ3/θ0θ4/θ0θ5/θ0θ6/θ0θ7/θ0θ8/θ0θ9/θ0
=[−xi2]
最小二乘拟合:
[
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
n
2
z
n
2
x
n
y
n
x
n
z
n
y
n
z
n
x
n
y
n
z
n
1
]
[
θ
1
/
θ
0
θ
2
/
θ
0
θ
3
/
θ
0
θ
4
/
θ
0
θ
5
/
θ
0
θ
6
/
θ
0
θ
7
/
θ
0
θ
8
/
θ
0
θ
9
/
θ
0
]
=
[
−
x
1
2
⋮
−
x
n
2
]
\begin{bmatrix} y_1^2 & z_1^2 & x_1y_1 & x_1z_1 &y_1z_1 & x_1 & y _1 & z_1 & 1\\ \vdots & \vdots & \vdots & \vdots &\vdots &\vdots &\vdots & \vdots & \vdots\\ y_n^2 & z_n^2 & x_ny_n & x_nz_n &y_nz_n & x_n & y _n & z_n & 1\\ \end{bmatrix} \begin{bmatrix} \theta_1/\theta_0 \\ \theta_2/\theta_0 \\ \theta_3/\theta_0 \\ \theta_4/\theta_0 \\ \theta_5/\theta_0 \\ \theta_6/\theta_0 \\ \theta_7/\theta_0 \\ \theta_8/\theta_0 \\ \theta_9/\theta_0 \\ \end{bmatrix}= \begin{bmatrix} -x_1^2\\ \vdots\\ -x_n^2 \end{bmatrix}
y12⋮yn2z12⋮zn2x1y1⋮xnynx1z1⋮xnzny1z1⋮ynznx1⋮xny1⋮ynz1⋮zn1⋮1
θ1/θ0θ2/θ0θ3/θ0θ4/θ0θ5/θ0θ6/θ0θ7/θ0θ8/θ0θ9/θ0
=
−x12⋮−xn2
用eigen库写了一下矩阵的通解,命名不是很规范(已知AB矩阵,求解P)
// 矩阵 P * A = B
Eigen::MatrixXd PA_B(Eigen::MatrixXd A, Eigen::MatrixXd B)
{
Eigen::MatrixXd At = A.transpose();
return B * At * (A * At).inverse();
}
// 矩阵: A * P = B
Eigen::MatrixXd AP_B(Eigen::MatrixXd A, Eigen::MatrixXd B)
{
Eigen::MatrixXd At = A.transpose();
return (At * A).inverse() * At * B;
}
对 θ \theta θ组进行求解
求解方向向量
因为 θ 3 = − 2 a b , θ 4 = − 2 a c , θ 5 = − 2 b c \theta_3=-2ab,\theta_4=-2ac,\theta_5=-2bc θ3=−2ab,θ4=−2ac,θ5=−2bc,所以只有当abc中至少有两者为0时, θ 3 , θ 4 , θ 5 \theta_3,\theta_4,\theta_5 θ3,θ4,θ5会全部为0。
- 当 θ 3 θ 0 , θ 4 θ 0 , θ 5 θ 0 \Large\frac{\theta_3}{\theta_0}\normalsize,\Large\frac{\theta_4}{\theta_0}\normalsize,\Large\frac{\theta_5}{\theta_0} θ0θ3,θ0θ4,θ0θ5都为0时
- 若 θ 1 θ 0 \Large\frac{\theta_1}{\theta_0} θ0θ1=1(接近于1),即 a 2 + c 2 b 2 + c 2 \Large\frac{a^2+c^2}{b^2+c^2} b2+c2a2+c2 =1, a 2 = b 2 a^2=b^2 a2=b2,又因为abc中一定有两者为0,所以此时的方向向量 ( a , b , c ) (a,b,c) (a,b,c)为 ( 0 , 0 , 1 ) (0,0,1) (0,0,1)
- 若 θ 2 θ 0 \Large\frac{\theta_2}{\theta_0} θ0θ2=1(接近于1),即 a 2 + b 2 b 2 + c 2 \Large\frac{a^2+b^2}{b^2+c^2} b2+c2a2+b2 =1, a 2 = c 2 a^2=c^2 a2=c2,又因为abc中一定有两者为0,所以此时的方向向量 ( a , b , c ) (a,b,c) (a,b,c)为 ( 0 , 1 , 0 ) (0,1,0) (0,1,0)
- 当
θ
3
θ
0
,
θ
4
θ
0
,
θ
5
θ
0
\Large\frac{\theta_3}{\theta_0}\normalsize,\Large\frac{\theta_4}{\theta_0}\normalsize,\Large\frac{\theta_5}{\theta_0}
θ0θ3,θ0θ4,θ0θ5不都为0时,令:
K = 2 1 + θ 1 θ 0 + θ 2 θ 0 = 2 1 + a 2 + c 2 b 2 + c 2 + a 2 + b 2 b 2 + c 2 = 2 1 + a 2 + c 2 b 2 + c 2 + a 2 + b 2 b 2 + c 2 = 2 b 2 + c 2 b 2 + c 2 + a 2 + c 2 b 2 + c 2 + a 2 + b 2 b 2 + c 2 = 2 b 2 + c 2 b 2 + c 2 + a 2 + c 2 b 2 + c 2 + a 2 + b 2 b 2 + c 2 = 2 2 ( a 2 + b 2 + c 2 ) b 2 + c 2 = b 2 + c 2 a 2 + b 2 + c 2 K= \frac{2}{1+\Large\frac{\theta_1}{\theta_0}\normalsize+\Large\frac{\theta_2}{\theta_0}}\\ =\frac{2}{1+\Large\frac{a^2+c^2}{b^2+c^2}\normalsize+\Large\frac{a^2+b^2}{b^2+c^2}}=\frac{2}{1+\Large\frac{a^2+c^2}{b^2+c^2}\normalsize+\Large\frac{a^2+b^2}{b^2+c^2}}\\ =\frac{2}{\Large\frac{b^2+c^2}{b^2+c^2}\normalsize+\Large\frac{a^2+c^2}{b^2+c^2}\normalsize+\Large\frac{a^2+b^2}{b^2+c^2}} = \frac{2}{\Large\frac{b^2+c^2}{b^2+c^2}\normalsize+\Large\frac{a^2+c^2}{b^2+c^2}\normalsize+\Large\frac{a^2+b^2}{b^2+c^2}}\\=\frac{2}{\Large\frac{2(a^2+b^2+c^2)}{b^2+c^2}}=\frac{b^2+c^2}{a^2+b^2+c^2} K=1+θ0θ1+θ0θ22=1+b2+c2a2+c2+b2+c2a2+b22=1+b2+c2a2+c2+b2+c2a2+b22=b2+c2b2+c2+b2+c2a2+c2+b2+c2a2+b22=b2+c2b2+c2+b2+c2a2+c2+b2+c2a2+b22=b2+c22(a2+b2+c2)2=a2+b2+c2b2+c2
将以上 θ \theta θ组乘K
[ θ 1 / θ 0 θ 2 / θ 0 θ 3 / θ 0 θ 4 / θ 0 θ 5 / θ 0 θ 6 / θ 0 θ 7 / θ 0 θ 8 / θ 0 θ 9 / θ 0 ] = [ θ 1 / ( b 2 + c 2 ) θ 2 / ( b 2 + c 2 ) θ 3 / ( b 2 + c 2 ) θ 4 / ( b 2 + c 2 ) θ 5 / ( b 2 + c 2 ) θ 6 / ( b 2 + c 2 ) θ 7 / ( b 2 + c 2 ) θ 8 / ( b 2 + c 2 ) θ 9 / ( b 2 + c 2 ) ] ∗ K ( b 2 + c 2 a 2 + b 2 + c 2 ) = [ θ 1 / ( a 2 + b 2 + c 2 ) θ 2 / ( a 2 + b 2 + c 2 ) θ 3 / ( a 2 + b 2 + c 2 ) θ 4 / ( a 2 + b 2 + c 2 ) θ 5 / ( a 2 + b 2 + c 2 ) θ 6 / ( a 2 + b 2 + c 2 ) θ 7 / ( a 2 + b 2 + c 2 ) θ 8 / ( a 2 + b 2 + c 2 ) θ 9 / ( a 2 + b 2 + c 2 ) ] \begin{bmatrix} \theta_1/\theta_0 \\ \theta_2/\theta_0 \\ \theta_3/\theta_0 \\ \theta_4/\theta_0 \\ \theta_5/\theta_0 \\ \theta_6/\theta_0 \\ \theta_7/\theta_0 \\ \theta_8/\theta_0 \\ \theta_9/\theta_0 \\ \end{bmatrix}= \begin{bmatrix} \theta_1/(b^2+c^2) \\ \theta_2/(b^2+c^2) \\ \theta_3/(b^2+c^2)\\ \theta_4/(b^2+c^2)\\ \theta_5/(b^2+c^2)\\ \theta_6/(b^2+c^2)\\ \theta_7/(b^2+c^2)\\ \theta_8/(b^2+c^2)\\ \theta_9/(b^2+c^2) \end{bmatrix}*K(\frac{b^2+c^2}{a^2+b^2+c^2})= \begin{bmatrix} \theta_1/(a^2+b^2+c^2) \\ \theta_2/(a^2+b^2+c^2) \\ \theta_3/(a^2+b^2+c^2)\\ \theta_4/(a^2+b^2+c^2)\\ \theta_5/(a^2+b^2+c^2)\\ \theta_6/(a^2+b^2+c^2)\\ \theta_7/(a^2+b^2+c^2)\\ \theta_8/(a^2+b^2+c^2)\\ \theta_9/(a^2+b^2+c^2) \end{bmatrix} θ1/θ0θ2/θ0θ3/θ0θ4/θ0θ5/θ0θ6/θ0θ7/θ0θ8/θ0θ9/θ0 = θ1/(b2+c2)θ2/(b2+c2)θ3/(b2+c2)θ4/(b2+c2)θ5/(b2+c2)θ6/(b2+c2)θ7/(b2+c2)θ8/(b2+c2)θ9/(b2+c2) ∗K(a2+b2+c2b2+c2)= θ1/(a2+b2+c2)θ2/(a2+b2+c2)θ3/(a2+b2+c2)θ4/(a2+b2+c2)θ5/(a2+b2+c2)θ6/(a2+b2+c2)θ7/(a2+b2+c2)θ8/(a2+b2+c2)θ9/(a2+b2+c2)
注: ( a , b , c ) (a,b,c) (a,b,c)为方向向量,对该向量进行归一化后 a 2 + b 2 + c 2 = 1 a^2+b^2+c^2=1 a2+b2+c2=1, θ \theta θ组即为:
[ θ 1 θ 2 θ 3 θ 4 θ 5 θ 6 θ 7 θ 8 θ 9 ] T \begin{bmatrix} \theta_1&\theta_2&\theta_3&\theta_4&\theta_5&\theta_6&\theta_7&\theta_8&\theta_9 \end{bmatrix}^T [θ1θ2θ3θ4θ5θ6θ7θ8θ9]T
令 A = K , B = K θ 1 , C = K θ 2 , D = K θ 3 , E = K θ 4 , F = K θ 5 A=K,B=K\theta_1, C=K\theta_2,D=K\theta_3, E=K\theta_4,F= K\theta_5 A=K,B=Kθ1,C=Kθ2,D=Kθ3,E=Kθ4,F=Kθ5
其中
A = b 2 + c 2 a 2 + b 2 + c 2 , B = a 2 + c 2 a 2 + b 2 + c 2 , C = a 2 + b 2 a 2 + b 2 + c 2 D = − 2 a b a 2 + b 2 + c 2 , E = − 2 a c a 2 + b 2 + c 2 , F = − 2 b c a 2 + b 2 + c 2 A=\frac{b^2+c^2}{a^2+b^2+c^2},B=\frac{a^2+c^2}{a^2+b^2+c^2},C=\frac{a^2+b^2}{a^2+b^2+c^2}\\ D=\frac{-2ab}{a^2+b^2+c^2},E=\frac{-2ac}{a^2+b^2+c^2},F=\frac{-2bc}{a^2+b^2+c^2} A=a2+b2+c2b2+c2,B=a2+b2+c2a2+c2,C=a2+b2+c2a2+b2D=a2+b2+c2−2ab,E=a2+b2+c2−2ac,F=a2+b2+c2−2bc
选择 A , B , C A,B,C A,B,C中数值最大的数字(其实选出的数值只需要不为0即可)
- 若
A
A
A不为0时,用
A
A
A来计算方向向量
(
a
,
b
,
c
)
(a,b,c)
(a,b,c)
a = ( 1 − A ) 1 2 , b = D − 2 a , c = E − 2 a a=(1-A)^{\large\frac{1}{2}}\normalsize,b=\frac{D}{-2a}, c=\frac{E}{-2a} a=(1−A)21,b=−2aD,c=−2aE - 若
B
B
B不为0时,用
B
B
B来计算方向向量
(
a
,
b
,
c
)
(a,b,c)
(a,b,c)
b = ( 1 − B ) 1 2 , a = D − 2 b , c = F − 2 b b=(1-B)^{\large\frac{1}{2}}\normalsize,a=\frac{D}{-2b}, c=\frac{F}{-2b} b=(1−B)21,a=−2bD,c=−2bF - 若
C
C
C不为0时,用
C
C
C来计算方向向量
(
a
,
b
,
c
)
(a,b,c)
(a,b,c)
c = ( 1 − C ) 1 2 , a = E − 2 c , b = F − 2 c c=(1-C)^{\large\frac{1}{2}}\normalsize,a=\frac{E}{-2c}, b=\frac{F}{-2c} c=(1−C)21,a=−2cE,b=−2cF
对方向向量 ( a , b , c ) (a,b,c) (a,b,c)进行归一化,即可。
求解轴上一点
太多了下次再写!