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​−X3​Y3​Z3​，坐标系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​=⎣⎡​a1​b1​c1​​a2​b2​c2​​a3​b3​c3​​⎦⎤​
由于坐标系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