calPoseFrom3Points
功能
已知坐标系a的原点、x轴正半轴上任一点和y轴正半轴上任一点在坐标系b下的坐标，求解坐标系a到坐标系b的旋转矩阵R和平移矩阵T。
原理
如图所示，点 O 2 O_2 O2为坐标系2的原点， P X P_X PX、 P Y P_Y PY分别在坐标系2的x、y轴的正半轴上。
由点
P
X
1
、
P
Y
1
、
O
21
PX_1、PY_1、O_{21}
PX1、PY1、O21三点构造出一个坐标系3（
P
X
1
、
P
Y
1
、
O
21
PX_1、PY_1、O_{21}
PX1、PY1、O21为坐标系1下各点的坐标），即
O
21
−
X
3
Y
3
Z
3
O_{21}-X_3Y_3Z_3
O21−X3Y3Z3，坐标系3的XYZ轴指向与坐标系2相同。其中，
O
21
O_{21}
O21为坐标系3原点，
O
21
O_{21}
O21指向
P
X
1
PX_1
PX1为X轴正方向，
O
21
O_{21}
O21指向
P
Y
1
PY_1
PY1为Y轴正方向，Z轴由X轴和Y轴叉乘得到。即：
O
21
P
X
1
⃗
=
P
X
1
−
O
21
\vec{O{21}PX_1}=PX_1 - O{21}
O21PX1
=PX1−O21
O
21
P
Y
1
⃗
=
P
Y
1
−
O
21
\vec{O{21}PY_1}=PY_1 - O{21}
O21PY1
=PY1−O21
O
21
P
Z
1
⃗
=
O
21
P
X
1
⃗
×
O
21
P
Y
1
⃗
\vec{O{21}PZ_1}=\vec{O{21}PX_1}\times\vec{O{21}PY_1}
O21PZ1
=O21PX1
×O21PY1
单位化：
O
X
1
⃗
=
O
21
P
X
1
⃗
/
∣
∣
O
21
P
X
1
⃗
∣
∣
\vec{OX_1}=\vec{O{21}PX_1}/||\vec{O{21}PX_1}||
OX1
=O21PX1
/∣∣O21PX1
∣∣
O
Y
1
⃗
=
O
21
P
Y
1
⃗
/
∣
∣
O
21
P
Y
1
⃗
∣
∣
\vec{OY_1}=\vec{O{21}PY_1}/||\vec{O{21}PY_1}||
OY1
=O21PY1
/∣∣O21PY1
∣∣
O
Z
1
⃗
=
O
21
P
Z
1
⃗
/
∣
∣
O
21
P
Z
1
⃗
∣
∣
\vec{OZ_1}=\vec{O{21}PZ_1}/||\vec{O{21}PZ_1}||
OZ1
=O21PZ1
/∣∣O21PZ1
∣∣
记：
O
X
1
⃗
=
(
a
1
,
b
1
,
c
1
)
\vec{OX_1}=(a_1,b_1,c_1)
OX1
=(a1,b1,c1)
O
Y
1
⃗
=
(
a
2
,
b
2
,
c
2
)
\vec{OY_1}=(a_2,b_2,c_2)
OY1
=(a2,b2,c2)
O
Z
1
⃗
=
(
a
3
,
b
3
,
c
3
)
\vec{OZ_1}=(a_3,b_3,c_3)
OZ1
=(a3,b3,c3)
O
X
1
⃗
,
O
Y
1
⃗
,
O
Z
1
⃗
\vec{OX_1},\vec{OY_1},\vec{OZ_1}
OX1
,OY1
,OZ1
在坐标系3中的矢量表示为：
O
X
3
⃗
=
(
1
,
0
,
0
)
\vec{OX_3}=(1,0,0)
OX3
=(1,0,0)
O
Y
3
⃗
=
(
0
,
1
,
0
)
\vec{OY_3}=(0,1,0)
OY3
=(0,1,0)
O
Z
3
⃗
=
(
0
,
0
,
1
)
\vec{OZ_3}=(0,0,1)
OZ3
=(0,0,1)
坐标系3到坐标系1的旋转矩阵为：
R
31
=
[
a
1
a
2
a
3
b
1
b
2
b
3
c
1
c
2
c
3
]
R_{31}=\left[\begin{matrix} a_1&a_2&a_3\\ b_1&b_2&b_3\\ c_1&c_2&c_3 \end{matrix}\right]
R31=⎣⎡a1b1c1a2b2c2a3b3c3⎦⎤
由于坐标系3坐标轴指向与坐标系2坐标轴指向完全相同，因此坐标系3与坐标系2的旋转矩阵为单位矩阵，所以坐标系2到坐标系1的旋转矩阵
R
21
=
R
31
R_{21}=R_{31}
R21=R31。
由于 O 21 = R 21 ⋅ O 2 + T O_{21}=R_{21} \cdot O_2 +T O21=R21⋅O2+T且 O 2 = ( 0 , 0 , 0 ) O_2 = (0,0,0) O2=(0,0,0)，因此 T 21 = O 21 T_{21}=O_{21} T21=O21
使用
输入
坐标系a的原点在坐标系b下的坐标 O a b ( x 1 , y 1 , z 1 ) O_{ab}(x_1,y_1,z_1) Oab(x1,y1,z1)
坐标系a的x轴正半轴上任一点在坐标系b下的坐标 P x b ( x 2 , y 2 , z 2 ) P_{xb}(x_2,y_2,z_2) Pxb(x2,y2,z2)
坐标系a的y轴正半轴上任一点在坐标系b下的坐标 P x b ( x 3 , y 3 , z 3 ) P_{xb}(x_3,y_3,z_3) Pxb(x3,y3,z3)
输出
坐标系a到坐标系b的旋转矩阵 R a b R_{ab} Rab和平移矩阵 T a b T_{ab} Tab
DEMO
import geomeas as gm
import numpy as np
Oab = np.array([-37.84381632, 152.36389864, 41.68600167])
Pxb = np.array([-19.59820338, 139.58818292, 45.55380309])
Pyb = np.array([-38.23270656, 157.3130709, 59.86810327])
print(gm.Pose().calPoseFrom3Points(Oab, Pxb, Pyb))
链接
https://gitee.com/huangzhexiaohao/geo-meas/blob/master/src/geomeas.py