一般对算法这边来说最直接的就是拿到变换矩阵了，不知道为什么还要舍近求远获得欧拉角，不过楼主需要的话 ...
感谢层主，膜拜
我一定把它付诸实践
我又搜索了一些其他的方法，再次请楼主给于指导。一些方法贴入后面的回复。
方法一 本文转载自简书，作者是梁间。表示感谢
数学模型已知两个坐标系在各方向上尺度缩放比例一致，两个坐标系的转换关系可以用7个参数来表示，3个旋转参数，3个平移参数，1个比例参数。已知三点在A、B两个坐标系中的坐标，那么这7个参数可以唯一确定。
坐标转换的数学模型为：
https://math.jianshu.com/math?formula=%5Cbegin%7Bbmatrix%7D%20X%20%5C%5C%20Y%20%5C%5C%20Z%20%5Cend%7Bbmatrix%7D_A%20%3D%20%CE%BB%20%5Cleft(%20%5Cbegin%7Bbmatrix%7D%20%CE%94X%20%5C%5C%20%CE%94Y%20%5C%5C%20%CE%94Z%20%5Cend%7Bbmatrix%7D%20%2B%20R%5Cbegin%7Bbmatrix%7D%20X%20%5C%5C%20Y%20%5C%5C%20Z%20%5Cend%7Bbmatrix%7D_B%20%5Cright)%20...(1)
其中，λ是比例参数，R是旋转矩阵，Δ是平移向量，A、B分别是两个坐标系中的坐标。
比例参数λ最容易计算
https://math.jianshu.com/math?formula=%CE%BB%20%3D%20%5Cfrac%7B%7CP_1P_2%7C_A%7D%7B%7CP_1P_2%7C_B%7D%20%3D%20%5Cfrac%7B%7CP_2P_3%7C_A%7D%7B%7CP_2P_3%7C_B%7D%20%3D%20%5Cfrac%7B%7CP_3P_1%7C_A%7D%7B%7CP_3P_1%7C_B%7D
其中https://math.jianshu.com/math?formula=%7CP_1P_2%7C_A 为https://math.jianshu.com/math?formula=P_1P_2两点在A坐标系中的距离。
旋转矩阵R是一个3x3的正交矩阵，有3个自由度。可利用反对称矩阵S来构造旋转矩阵R：
https://math.jianshu.com/math?formula=S%3D%20%5Cbegin%7Bbmatrix%7D%200%26-c%26-b%20%5C%5C%20c%260%26-a%20%5C%5C%20b%26a%260%20%5Cend%7Bbmatrix%7D
那么
https://math.jianshu.com/math?formula=R%3D%5Cfrac%7BI%2BS%7D%7BI%E2%88%92S%7D%20...(2)
其中I是单位矩阵，这里R只有a、b、c三个变量，解出a、b、c即可确定旋转矩阵R。
把https://math.jianshu.com/math?formula=P_1P_2两点带入(1)式并相减消去ΔX、ΔY、ΔZ
https://math.jianshu.com/math?formula=%5Cbegin%7Bbmatrix%7D%20P_%7B1AX%7D-P_%7B2AX%7D%20%5C%5C%20P_%7B1AY%7D-P_%7B2AY%7D%20%5C%5CP_%7B1AZ%7D-P_%7B2AZ%7D%20%5Cend%7Bbmatrix%7D%20%3D%20%CE%BBR%5Cbegin%7Bbmatrix%7D%20P_%7B1BX%7D-P_%7B2BX%7D%20%5C%5C%20P_%7B1BY%7D-P_%7B2BY%7D%20%5C%5CP_%7B1BZ%7D-P_%7B2BZ%7D%20%5Cend%7Bbmatrix%7D%20...(3)
这里https://math.jianshu.com/math?formula=P_%7B1AX%7D为https://math.jianshu.com/math?formula=P_1点在A坐标系X轴向坐标值，为已知量。我们简化一下写法设定：
https://math.jianshu.com/math?formula=X_%7BA12%7D%20%3D%20P_%7B1AX%7D-P_%7B2AX%7D
这样(3)式可写为
https://math.jianshu.com/math?formula=%5Cbegin%7Bbmatrix%7D%20X_%7BA12%7D%5C%5C%20Y_%7BA12%7D%20%5C%5C%20Z_%7BA12%7D%20%5Cend%7Bbmatrix%7D%20%3D%20%CE%BBR%5Cbegin%7Bbmatrix%7D%20X_%7BB12%7D%5C%5C%20Y_%7BB12%7D%20%5C%5C%20Z_%7BB12%7D%20%5Cend%7Bbmatrix%7D%20...(4)
把(2)式带入(4)
https://math.jianshu.com/math?formula=(I-S)%5Cbegin%7Bbmatrix%7D%20X_%7BA12%7D%5C%5C%20Y_%7BA12%7D%20%5C%5C%20Z_%7BA12%7D%20%5Cend%7Bbmatrix%7D%20%3D%20%CE%BB(I%2BS)%5Cbegin%7Bbmatrix%7D%20X_%7BB12%7D%5C%5C%20Y_%7BB12%7D%20%5C%5C%20Z_%7BB12%7D%20%5Cend%7Bbmatrix%7D
带入S
https://math.jianshu.com/math?formula=%5Cbegin%7Bbmatrix%7D%201%26c%26b%20%5C%5C%20-c%261%26a%20%5C%5C%20-b%26-a%261%5Cend%7Bbmatrix%7D%5Cbegin%7Bbmatrix%7D%20X_%7BA12%7D%5C%5C%20Y_%7BA12%7D%20%5C%5C%20Z_%7BA12%7D%20%5Cend%7Bbmatrix%7D%20%3D%20%CE%BB%5Cbegin%7Bbmatrix%7D%201%26-c%26-b%20%5C%5C%20c%261%26-a%20%5C%5C%20b%26a%261%20%5Cend%7Bbmatrix%7D%5Cbegin%7Bbmatrix%7D%20X_%7BB12%7D%5C%5C%20Y_%7BB12%7D%20%5C%5C%20Z_%7BB12%7D%20%5Cend%7Bbmatrix%7D
展开
https://math.jianshu.com/math?formula=%5Cbegin%7Bbmatrix%7D%20X_%7BA12%7D%2BcY_%7BA12%7D%2BbZ_%7BA12%7D%20%5C%5C%20-cX_%7BA12%7D%2BY_%7BA12%7D%2BaZ_%7BA12%7D%20%5C%5C%20-bX_%7BA12%7D-aY_%7BA12%7D%2BZ_%7BA12%7D%5Cend%7Bbmatrix%7D%20%3D%20%CE%BB%5Cbegin%7Bbmatrix%7D%20X_%7BB12%7D-cY_%7BB12%7D-bZ_%7BB12%7D%5C%5C%20cX_%7BB12%7D%2BY_%7BB12%7D-aZ_%7BB12%7D%20%5C%5C%20bX_%7BB12%7D%2BaY_%7BB12%7D%2BZ_%7BB12%7D%20%5Cend%7Bbmatrix%7D
整理可得
https://math.jianshu.com/math?formula=%5Cbegin%7Bbmatrix%7D%20X_%7BA12%7D%20-%CE%BBX_%7BB12%7D%20%5C%5C%20Y_%7BA12%7D%20-%20%CE%BBY_%7BB12%7D%20%5C%5C%20Z_%7BA12%7D%20-%20%CE%BB%20Z_%7BB12%7D%20%5Cend%7Bbmatrix%7D%20%3D%20%5Cbegin%7Bbmatrix%7D-c(%CE%BBY_%7BB12%7D%2BY_%7BA12%7D)-b%CE%BB(Z_%7BB12%7D%2BbZ_%7BA12%7D)%5C%5C%20c(%CE%BBX_%7BB12%7D%2BX_%7BA12%7D)-a(%CE%BBZ_%7BB12%7D%20%2BZ_%7BA12%7D%20)%5C%5C%20b(%CE%BBX_%7BB12%7D%2BX_%7BA12%7D)%2Ba(%CE%BBY_%7BB12%7D%2BY_%7BA12%7D%20)%5Cend%7Bbmatrix%7D...(5)
(5)式只有两个独立方程，解不出a、b、c三个未知量。带入https://math.jianshu.com/math?formula=P_1P_3点得到和(5)式类似的方程组，两个方程组联立，取3个独立方程
https://math.jianshu.com/math?formula=%5Cbegin%7Bbmatrix%7D%20X_%7BA12%7D%20-%CE%BBX_%7BB12%7D%20%5C%5C%20Y_%7BA12%7D%20-%20%CE%BBY_%7BB12%7D%20%5C%5C%20Z_%7BA13%7D%20-%20%CE%BB%20Z_%7BB13%7D%20%5Cend%7Bbmatrix%7D%20%3D%20%5Cbegin%7Bbmatrix%7D%20-b(%20%CE%BBZ_%7BB12%7D%2BZ_%7BA12%7D)%20-c(%CE%BBY_%7BB12%7D%2BY_%7BA12%7D)%20%5C%5C%20-a(%CE%BBZ_%7BB12%7D%20%2BZ_%7BA12%7D%20)%2Bc(%CE%BBX_%7BB12%7D%2BX_%7BA12%7D)%5C%5C%20a(%CE%BBY_%7BB13%7D%2BY_%7BA13%7D%20)%2Bb(%CE%BBX_%7BB13%20%7D%2BX_%7BA13%7D)%5Cend%7Bbmatrix%7D
https://math.jianshu.com/math?formula=%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-b(%CE%BBZ_%7BB12%7D%2BZ_%7BA12%7D)%20%26-c(%CE%BBY_%7BB12%7D%2BY_%7BA12%7D)%20%5C%5C%20-a(%CE%BBZ_%7BB12%7D%20%2BZ_%7BA12%7D%20)%26%200%20%26%20c(%CE%BBX_%7BB12%7D%2BX_%7BA12%7D)%5C%5C%20a(%CE%BBY_%7BB13%7D%2BY_%7BA13%7D%20)%26b(%CE%BBX_%7BB13%20%7D%2BX_%7BA13%7D)%260%5Cend%7Bbmatrix%7D%20%5Cbegin%7Bbmatrix%7D%20a%5C%5Cb%20%5C%5Cc%20%5Cend%7Bbmatrix%7D
可结出
https://math.jianshu.com/math?formula=%5Cbegin%7Bbmatrix%7D%20a%5C%5C%20b%20%5C%5Cc%20%5Cend%7Bbmatrix%7D%20%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-(%CE%BBZ_%7BB12%7D%2BZ_%7BA12%7D)%20%26-(%CE%BBY_%7BB12%7D%2BY_%7BA12%7D)%20%5C%5C%20-(%CE%BBZ_%7BB12%7D%20%2BZ_%7BA12%7D%20)%26%200%20%26%20(%CE%BBX_%7BB12%7D%2BX_%7BA12%7D)%5C%5C%20(%CE%BBY_%7BB13%7D%2BY_%7BA13%7D%20)%26(%CE%BBX_%7BB13%20%7D%2BX_%7BA13%7D)%260%5Cend%7Bbmatrix%7D%20%5E%7B-1%20%7D%5Cbegin%7Bbmatrix%7D%20X_%7BA12%7D%20-%CE%BBX_%7BB12%7D%20%5C%5C%20Y_%7BA12%7D%20-%20%CE%BBY_%7BB12%7D%20%5C%5C%20Z_%7BA13%7D%20-%20%CE%BB%20Z_%7BB13%7D%20%5Cend%7Bbmatrix%7D
把a、b、c带入(2)式可到旋转矩阵R，把任意一点坐标带入(1)式可得Δ。
作者：梁间
链接：https://www.jianshu.com/p/58cf5655f9a9
来源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。
方法二，这里提到了SVD算法
通过实际的坐标点例子，来直观解释通过SVD求对应的坐标系关系。https://pic4.zhimg.com/v2-2df24d430fbbf3216a4bd0a372aae2c1_1440w.jpg已知四个点在坐标系A中的坐标为：（0， 0， 0）; （1，0，0）; （1，1，0）; （0，1，0）可得矩阵A为
https://pic4.zhimg.com/v2-f7fe3bc495bde4b6d5606ee245aebca7_1440w.jpg
矩阵A这四个点在坐标系B中的坐标为：（2，2， 2）; （3，2，2）; （3，3，2）; （2，3，2）可得矩阵B为
https://pic3.zhimg.com/v2-f236f5aca65c1ef0d92a85ab92ad239e_1440w.jpg
矩阵B步骤一：求两个数据集的质心https://pic4.zhimg.com/v2-45d12a13f6fb4a783566a9dbee3e4dbf_1440w.jpg根据上述公式，可得质心为https://pic1.zhimg.com/v2-84156c5f6d67090b70b7b36f8a541d84_1440w.jpghttps://pic1.zhimg.com/v2-637684f9bc47526dd6927ff5108c1b0c_1440w.jpg步骤二：将两个数据集的质心移动至同一个点，即只存在于一个旋转的转换关系。对应坐标系中的点同时减去质心，计算后的矩阵A和B分别为
https://pic2.zhimg.com/v2-538e0e3fa074310d67ba7699e04be657_1440w.jpg
计算后的矩阵A
https://pic2.zhimg.com/v2-538e0e3fa074310d67ba7699e04be657_1440w.jpg
计算后的矩阵B备注：此处的矩阵A和矩阵B一样，因为举得例子较为特殊，只存在平移关系。计算过程通用。步骤三：通过SVD算法计算旋转和平移关系。定义一个3X3的矩阵，将矩阵A的每一行数据与矩阵B进行点乘，会产生四组3X3的矩阵，将这四组数据求和，得到最终的3X3的矩阵，就是我们需要用SVD算法来进行奇异值分解的矩阵H。https://pic2.zhimg.com/v2-f6d07bd7566b5c3f4a5728b3c4aed499_1440w.jpghttps://picx.zhimg.com/v2-8a0305d8ed347a2ca23bb8aae3134b95_1440w.jpg上述公式中，颜色相同的框内数据进行点乘，构成3X3的矩阵a1,a2,a3,a4。矩阵H = a1 + a2 + a3 + a4。计算结果如下https://pic4.zhimg.com/v2-da12757858d3782e16323538c6159463_1440w.jpg通过SVD算法分解该矩阵，这里直接通过MATLAB接口调用，具体原理在前面的章节中已描述。https://pic1.zhimg.com/v2-59ef204c265760c8559f1269b1c5b584_1440w.jpg步骤四：计算旋转和平移关系根据上述求出的u1和v1，可求得旋转矩阵R为https://pic3.zhimg.com/v2-e3be81d02887a1f1ed08b3d628bd9a36_1440w.jpg将该旋转矩阵转为欧拉角则Rx = 0, Ry = 0, Rz = 0。根据公式https://pica.zhimg.com/v2-f0d3f917390553e56592969e2b1d17a0_1440w.jpg得平移矩阵为https://pica.zhimg.com/v2-916f71297a294b2e22612ace9d250bb4_1440w.jpg总结：如果在实际项目中，需要获取多台设备间的关系，如机器人相对于产品间的关系，或者机床相对于产品的关系，则该方法较为实用。注意：在实际的选择参考点时，不要在一条线上选点。如上述选的四个点，要求不能共线。要不然会减少有效数据。
本帖最后由 DaedraMech 于 2025-7-29 20:42 编辑
没想到水水大侠对此问题如此孜孜以求，钻研精神和信息获取能力令小弟佩服，我比较功利，方法能解决问题就行。
我把之前回复中的齐次变换矩阵改一下符号，楼主可以对比下和文章中的表达方式是不是一回事：
平移、旋转、缩放这类坐标变换我们统称为线性变换。平移和缩放非常简单，没什么好说的，重点就是如何求得旋转矩阵R，上述两种方法就是通过纯代数的方法求解R。
你用什么方法和选择的工具有关，上述两种方法多用在图像处理和机器学习中，算法工程师手里的工具是集成编程环境，是代码，采用代数方法自然是比较直接的。但要知道矩阵可是联系代数和几何的桥梁啊，而我们则掌握了强大的几何工具SW，利用【旋转矩阵可以表示坐标系】这一几何意义我们可以直接得到R，完全不需要上面繁琐的运算。
页:
1
[2]