px4+vins+ego单机鲁棒飞行五（坐标系变换篇）
- 一、齐次矩阵变换原理
- 二、无人机上利用旋转矩阵求飞机中心位置
一、齐次矩阵变换原理
参考一、参考二
二、无人机上利用旋转矩阵求飞机中心位置
首先写出相机相对于world的旋转T_wc，然后量出相机和飞机中心body的旋转矩阵T_cb，最后
T_wc*T_cb=T_wb
则T_wb的最后一列就是无人机位置。
- 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]
])
- T_cb
T_cb = np.array([[1, 0, 0, -0.08],
[0, 1, 0, -0.02],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
- T_wb
T_wb = np.dot(T_wc, T_cb)
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
