相对于 2 个不同的基更改点表示

Question

我想用python编写一个程序，它将取一个点坐标（XYZ-ABC），例如： POINT = X 100, Y 200, Z 120, A -90, B 0, C O 关于依据： B = X 0, Y 200, Z 0, A 0, B 0, C 0 并找到同一点相对于另一个基的坐标： A = X 100, Y 200, Z 0, A 0, B 0, C 0。我找到了很多关于 3D 转换的信息，但我不知道从哪里开始。我也有 transformation.py 库。我需要一些关于如何进行此操作的提示，我必须按照数学术语遵循哪些步骤。

Answer 1

给定原点向量O=(X, Y, Z) 和旋转矩阵R，您可以从Euler angles 计算（注意，有很多变体），具有相对坐标p=(x, y, z) 的点的绝对坐标由下式给出

P = R p + O.

第二帧

P = R'p'+ O',

从第一帧到第二帧的局部坐标给出方程

p' = R'*(P - O') = R'*(R p + O - O')

其中* 表示转置（这也是旋转矩阵的逆矩阵）。

Answer 2

我来到这里。 求点 Pfa 相对于 Frame A 相对于 Frame B 的变换。Pfb?? 此示例在 Kuka 工业机器人中将位置或点从一帧转换为另一帧很有用。此外，它也很有用，对于任何类型的基或框架的仿射变换，我们只需要考虑齐次变换矩阵的旋转顺序。

  A = Rz
  B = Ry
  C = Rx
 Fa_mat --> Homogeneous transformation matrix(HTM) of Frame A, relative to World CS(coordinate system).
 Fb_mat --> HTM of Frame B, relative to World CS.
 Pfa_mat --> HTM of point A in Frame A.
 Pfb_mat --> HTM of point B in Frame B.
 Pwa_mat --> HTM of point A in World CS.
 Pwb_mat --> HTM of point B in World CS.

 If Pwa == Pwb then:

 Pwa = Fa_mat · Pfa_mat
 Pwb = Fb_mat · Pfb_mat
 Fa_mat · Pfa_mat = Fb_mat · Pfb_mat
 Pfb_mat = Pwa · Fb_mat' (Fb_mat' is the inverse)

我使用 Tait-Bryan ZYX 角度作为旋转矩阵，euler angles - Wikipedia. 这是我的python代码：

    # -*- coding: utf-8 -*-
    """
    Created on Tue Jul 18 08:54:16 2017

    @author: xabier fernandez
    """
    import math
    import numpy as np


    def point_rotation(point_mat):
        decpl = 7

        sy = math.sqrt(math.pow(point_mat[0,0],2) + math.pow(point_mat[1,0],2))
        singularity = sy < 1e-6

        if not singularity :
            A = math.atan2(point_mat[1,0], point_mat[0,0])
            B = math.atan2(-point_mat[2,0], sy)
            C = math.atan2(point_mat[2,1] , point_mat[2,2])        
        else :
            A = 0
            B = math.atan2(-point_mat[2,0], sy)
            C = math.atan2(-point_mat[1,2], point_mat[1,1])       

        A = round(math.degrees(A),decpl)
        B = round(math.degrees(B),decpl)
        C = round(math.degrees(C),decpl)

        return np.array([A,B,C])

    def point_translation(point_mat): 
        decpl = 5

        X = round(point_mat[0,3],decpl)
        Y = round(point_mat[1,3],decpl)
        Z = round(point_mat[2,3],decpl)

        return np.array([X,Y,Z])

    def point_to_mat(posX,posY,posZ,degA,degB,degC):
        t=np.zeros((4,4))

        radA=math.radians(degA)
        radB=math.radians(degB)
        radC=math.radians(degC)

        cos_a=math.cos(radA)
        sin_a=math.sin(radA)

        cos_b=math.cos(radB)
        sin_b=math.sin(radB)

        cos_c=math.cos(radC)
        sin_c=math.sin(radC)

        t[0,0]  =  cos_a*cos_b
        t[0,1]  = -sin_a*cos_c + cos_a*sin_b*sin_c
        t[0,2]  =  sin_a*sin_c + cos_a*sin_b*cos_c

        t[1,0]  =  sin_a*cos_b
        t[1,1]  =  cos_a*cos_c + sin_a*sin_b*sin_c
        t[1,2]  = -cos_a*sin_c + sin_a*sin_b*cos_c

        t[2,0]  = -sin_b
        t[2,1]  =  cos_b*sin_c
        t[2,2]  =  cos_b*cos_c

        t[0,3]  = posX
        t[1,3]  = posY
        t[2,3]  = posZ

        t[3,0]  = 0
        t[3,1]  = 0
        t[3,2]  = 0
        t[3,3]  = 1

        return t

    def test1():

        """
        -----------------------------------
        Rotational matrix 'zyx'
        -----------------------------------
        Fa--> Frame A relative to world c.s
        Fb--> Frame B relative to world c.s
        -----------------------------------
        Pwa--> Point A in world c.s
        Pwb--> Point B in world c.s
        -----------------------------------
        Pfa--> Point in frame A c.s
        Pfb--> Point in frame B c.s
        -----------------------------------
        Pwa == Pwb
        Pw = Fa x Pfa
        Pw = Fb x Pfb 
        Pfb = Fb' x Pw
        -----------------------------------
        """
        frameA_mat = point_to_mat(571.162170,-1168.71704,372.404694,-179.723297,-0.206600,0.856200)
        frameB_mat = point_to_mat(1493.90100, 209.460, 735.007, 179.572, -0.0880000, 0.130000)    

        Pfa_mat = point_to_mat(-534.884033, -825.747070,1037.32373, -165.214142, -3.16937923, -178.672119)

        inverse_frameB_mat = np.linalg.inv(frameB_mat)

        #--------------------------------------------------------------------------
        #Point A in World coordinate system
        Pwa_mat = np.dot(frameA_mat,Pfa_mat)
        Pwa_Trans = point_translation(Pwa_mat)
        Pwa_Rot = point_rotation(Pwa_mat)

        print('\n')
        print('Point A in World C.S.: ')
        print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pwa_Trans[0],Pwa_Trans[1],Pwa_Trans[2]))    
        print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pwa_Rot[0],Pwa_Rot[1],Pwa_Rot[2]))
        print('\n')


        #--------------------------------------------------------------------------
        #Point A affine transformation
        #Point A in Frame B coordinate system
        Pfb_mat= np.dot(inverse_frameB_mat,Pwa_mat)
        Pfb_Trans = point_translation(Pfb_mat)
        Pfb_Rot = point_rotation(Pfb_mat)

        print('Point A in Frame B C.S.: ')
        print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pfb_Trans[0],Pfb_Trans[1],Pfb_Trans[2]))    
        print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pfb_Rot[0],Pfb_Rot[1],Pfb_Rot[2]))

        #--------------------------------------------------------------------------
        #Point B in World coordinate system
        Pwb_mat = np.dot(frameB_mat,Pfb_mat)
        Pwb_Trans = point_translation(Pwb_mat)
        Pwb_Rot = point_rotation(Pwb_mat)

        print('\n')
        print('Point B in World C.S.: ')
        print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pwb_Trans[0],Pwb_Trans[1],Pwb_Trans[2]))    
        print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pwb_Rot[0],Pwb_Rot[1],Pwb_Rot[2]))
        print('\n')

    if __name__ == "__main__":

        test1()

Answer 3

这些点是机器人位置的笛卡尔坐标。 XYZ（平移）和 ABC（Rz，Ry，Rx 旋转）欧拉角，相对于基础或框架。我需要（我认为）找到这个位置的单位向量矩阵。这是我到目前为止所做的：

C(b)C(a)    S(c)S(b)C(a)-C(c)S(a)    C(c)S(b)C(a)+S(c)S(a)   x
C(b)S(a)    C(c)C(a)+S(c)S(b)S(a)    C(c)S(b)S(a)-S(c)C(a)   y
 -S(b)      S(c)C(b)                   C(c)C(b)              z
   0           0                          0                  1
//For example point P= [X -534.884033,Y -825.747070, Z 1037.32373, 
A -165.214142,B -3.16937923,C -178.672119]

我也读过这个问题 [3D Camera coordinates to world coordinates (change of basis?)，但我不明白我必须做什么。目前我正在 Excel 表格中进行一些计算，试图弄清楚该怎么做。 另外我不得不说这个位置是相对于 Frame 而言的，而 Frame 又具有相对于 World 坐标系的坐标。在这种情况下，这个 Frame 的值是：

Fa= [X 571.16217, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]

现在，如果我有第二个 Frame Fb：

Fb= [X 0, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]

我知道我对 Fb 的观点 P 应该是：

Pfb =[X -1106.036,Y -822.9583, Z 1039.342,A -165.2141, B -3.169379,C -178.6721]

我知道这个结果是因为我使用了一个自动执行此计算的程序，但我不知道它是如何做到的。