定位（1）：两点对+两旋转角度计算转换矩阵

定位（1）：两点对+两旋转角度计算转换矩阵

已知：
坐标系A和坐标系B（全站仪坐标系/大地坐标系（竖直方向向上））对应的两个点的坐标。
点1在A坐标系下的坐标A1（X1,Y1,Z1）;
点1在B坐标系下的坐标B1（X01,Y01,Z01）;
点2在A坐标系下的坐标A1（X2,Y2,Z2）;
点2在B坐标系下的坐标B1（X02,Y02,Z02）;
坐标系A的俯仰角度β和横滚角度α

目标：
求解坐标系A到坐标系B的转换矩阵

分析：
对于任意一个点，在坐标系A和坐标系B之间的关系如下：

其中绕Z轴的旋转角度和平移矩阵为未知量。
将两个点对的关系方程相减，消除平移矩阵，先求解绕Z轴的旋转。
解算：
1）计算两点在坐标系A下的矢量（X,Y,Z）

X=X1-X2;
Y=Y1-Y2
Z=Z1-Z2;

2）令

3）计算绕Z轴的旋转

4）求解平移矩阵：