本文主要是介绍利用python通过三点法计算两个坐标系之间的位姿变换矩阵，对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！

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

这篇关于利用python通过三点法计算两个坐标系之间的位姿变换矩阵的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！