calPoseFrom3Points

功能

已知坐标系a的原点、x轴正半轴上任一点和y轴正半轴上任一点在坐标系b下的坐标，求解坐标系a到坐标系b的旋转矩阵R和平移矩阵T。

原理

如图所示，点$$O_2$$为坐标系2的原点，$$P_X$$、$$P_Y$$分别在坐标系2的x、y轴的正半轴上。

由点$P{X}_1、P{Y}_1、O_{21}$三点构造出一个坐标系3（$P{X}_1、P{Y}_1、O_{21}$为坐标系1下各点的坐标），即$O_{21}-X_3{Y}_3{Z}_3$，坐标系3的XYZ轴指向与坐标系2相同。其中，$O_{21}$为坐标系3原点，$O_{21}$指向$P{X}_1$为X轴正方向，$O_{21}$指向$P{Y}_1$为Y轴正方向，Z轴由X轴和Y轴叉乘得到。即：
$$\overrightarrow{O21P{X}_1}=P{X}_1-O21$$
$$\overrightarrow{O21P{Y}_1}=P{Y}_1-O21$$
$$\overrightarrow{O21P{Z}_1}=\overrightarrow{O21P{X}_1}\times \overrightarrow{O21P{Y}_1}$$

单位化：
$$\overrightarrow{O{X}_1}=\overrightarrow{O21P{X}_1}/||\overrightarrow{O21P{X}_1}||$$
$$\overrightarrow{O{Y}_1}=\overrightarrow{O21P{Y}_1}/||\overrightarrow{O21P{Y}_1}||$$
$$\overrightarrow{O{Z}_1}=\overrightarrow{O21P{Z}_1}/||\overrightarrow{O21P{Z}_1}||$$

记：
$$\overrightarrow{O{X}_1}=(a_1,b_1,c_1)$$
$$\overrightarrow{O{Y}_1}=(a_2,b_2,c_2)$$
$$\overrightarrow{O{Z}_1}=(a_3,b_3,c_3)$$

$\overrightarrow{O{X}_1},\overrightarrow{O{Y}_1},\overrightarrow{O{Z}_1}$在坐标系3中的矢量表示为：
$$\overrightarrow{O{X}_3}=(1,0,0)$$
$$\overrightarrow{O{Y}_3}=(0,1,0)$$
$$\overrightarrow{O{Z}_3}=(0,0,1)$$

坐标系3到坐标系1的旋转矩阵为：
$$R_{31}=\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right]$$
由于坐标系3坐标轴指向与坐标系2坐标轴指向完全相同，因此坐标系3与坐标系2的旋转矩阵为单位矩阵，所以坐标系2到坐标系1的旋转矩阵$R_{21}=R_{31}$。

由于$O_{21}=R_{21}\cdot O_2+T$且$O_2=(0,0,0)$，因此$T_{21}=O_{21}$

使用

输入

坐标系a的原点在坐标系b下的坐标$O_{ab}(x_1,y_1,z_1)$

坐标系a的x轴正半轴上任一点在坐标系b下的坐标$P_{xb}(x_2,y_2,z_2)$

坐标系a的y轴正半轴上任一点在坐标系b下的坐标$P_{xb}(x_3,y_3,z_3)$

输出

坐标系a到坐标系b的旋转矩阵$R_{ab}$和平移矩阵$T_{ab}$

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