px4+vins+ego单机鲁棒飞行五（坐标系变换篇）

一、齐次矩阵变换原理

参考一、参考二

二、无人机上利用旋转矩阵求飞机中心位置

首先写出相机相对于world的旋转T_wc，然后量出相机和飞机中心body的旋转矩阵T_cb，最后

T_wc*T_cb=T_wb

则T_wb的最后一列就是无人机位置。

1. T_wc

# 从欧拉角获取四元数
def eulerAnglesToRotationMatrix(theta) :
    
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])
                
    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ])
                
    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])

    R = np.dot(R_x, R_y, R_z)
    return R

    RotationMatrix = eulerAnglesToRotationMatrix(vins_euler)
    T_wc = np.array([[RotationMatrix[0][0],RotationMatrix[0][1],RotationMatrix[0][2], vins_odom.pose.pose.position.x],
                     [RotationMatrix[1][0],RotationMatrix[1][1],RotationMatrix[1][2], vins_odom.pose.pose.position.y],
                     [RotationMatrix[2][0],RotationMatrix[2][1],RotationMatrix[2][2], vins_odom.pose.pose.position.z],
                     [0,                    0,                   0,                    1]
                     ])

2. T_cb

T_cb = np.array([[1,    0,    0,    -0.08],
                 [0,    1,    0,    -0.02],
                 [0,    0,    1,    0],
                 [0,    0,    0,    1]
                 ])

3. T_wb

T_wb = np.dot(T_wc, T_cb)