由于不同算法之间的坐标系不同，导致计算的结果混乱，该博客的目的是记录和统一不同算法之间的坐标系，保证坐标系的统一
import rospy
from geometry_msgs.msg import PoseStamped, Point
from nav_msgs.msg import Odometry
import math
from pyquaternion import Quaternion
import tf
import sys
vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
local_pose = PoseStamped()
local_pose.header.frame_id = 'world'
quaternion = tf.transformations.quaternion_from_euler(0, -math.pi/2, math.pi/2)
q = Quaternion([quaternion[3],quaternion[0],quaternion[1],quaternion[2]])
def vins_callback(data):
local_pose.pose.position.x = data.pose.pose.position.x
local_pose.pose.position.y = data.pose.pose.position.y
local_pose.pose.position.z = data.pose.pose.position.z
q_= Quaternion([data.pose.pose.orientation.w,data.pose.pose.orientation.x,data.pose.pose.orientation.y,data.pose.pose.orientation.z])
q_ = q_*q
local_pose.pose.orientation.w = q_[0]
local_pose.pose.orientation.x = q_[1]
local_pose.pose.orientation.y = q_[2]
local_pose.pose.orientation.z = q_[3]
rospy.init_node(vehicle_type+"_"+vehicle_id+'_vins_transfer')
rospy.Subscriber("/vins_estimator/camera_pose", Odometry, vins_callback,queue_size=1)
position_pub = rospy.Publisher(vehicle_type+"_"+vehicle_id+"/mavros/vision_pose/pose", PoseStamped, queue_size=1)
rate = rospy.Rate(30)
while not rospy.is_shutdown():
if (local_pose.pose.position == Point()):
continue
else:
print("Vins pose received")
local_pose.header.stamp = rospy.Time.now()
position_pub.publish(local_pose)
try:
rate.sleep()
except:
continue
q = Quaternion([quaternion[3],quaternion[0],quaternion[1],quaternion[2]]) # 这是因为得到的是xyzw，但是要转为wxyz
q_= Quaternion([data.pose.pose.orientation.w,data.pose.pose.orientation.x,data.pose.pose.orientation.y,data.pose.pose.orientation.z]) q_ = q_*q # 这是让原有视觉WXYZ进行一个坐标转换，得到的转换结果
mavros使用RTK定位，坐标是ENU；
正式确定，imu_data的坐标方向未FLU（同样也是ENU东北天，加速度和旋转），即前面为x，左边为y，上面为z。在初始化的时候，可能出现w为负数的情况，这里要注意了，当w为负数，所有的xyz因为都乘上了w才得到的，所有xyz的方向会被认为是反过来了。当绕z轴转360°就会发现所有的都回来了。
上述对于四元素的定义有问题，实际为如下。这样四元素的平方和才为1，并且可以表示出不同的旋转方向。当w接近1时，表示角度为0，此时不管绕z轴逆时针还是正时针，w都会减小，但xyz中的sin则表现出正负号，体现出了旋转方向。
明天看下vins的旋转坐标系，进行统一……
确定了，对于mavros来说，坐标系是FLU，同时也是东北天，对应着XYZ。但是为什么VINS启动之后，往前是Y呢，往右边是X呢，注意看四元素，它已经绕Z轴旋转了90°了，所以原本向前是X轴的，现在变成了Y轴。
当没有VINS，只使用GPS和imu的时候，角度就是朝向东方的0°。而使用VINS之后，因为VINS坐标系是右前上，与MAVROS的坐标系前左上不对应，所以就在MAVROS中给其定义了旋转了90°。
为什么xtdrone要进行vins的四元素转换。首先明确几点，1.四元素的参考坐标系是固定的，在GPS定位中就是东北天，而vins是右前上；2.mavros中的tf转换，是依照ZYX的方向来的，而tf的输入是wxyz，因此(0, -math.pi/2, math.pi/2)为Z=0,Y=90;X=90（这里的旋转会带动欧拉参考坐标系的旋转）,得到的值是（0.5，0.5，-0.5，0.5），如下图。 3.不同四元素的参考坐标系是要完全统一的，因为VINS的坐标系是右前上，而且在VINS中，默认的初始角度是朝上的（同时长轴朝前的），因此为了初始坐标统一，需要进行转换，将标准参考坐标系转换成vins的坐标系，就需要上述的转换。4. 上述的转换可以看出只是将初始角度换过去了，难道这样就可以了吗？是的，因为前面提到坐标系是一致的了，都是右前上，因此转换之后的坐标就是符合后面的角度了。
上述的分析稍微有点问题，MAVROS的初始化会根据vision_pose进行自动调整，并不是和vision_pose一模一样的。比如上述的flu转换文件中，即使这样旋转了，但是后续的旋转仍然以左前上的方向进行，在vins输出的四元素和vision_pose的四元素都可以看出来，但是在local_position中就有了变化。猜测原因是，只要定义好了初始值，mavros的local_position就会自动进行转换过来为对应的方位。比如上述flu转换之后，转换到的vision_pose就是0°了，这样在吗，mavros的local_position中就会认为前面是x轴，并且按照右手螺旋定则进行yz的分配。
综上，进行修改的时候，因综合考虑pose和四元素的转换，在思考时，应该先考虑四元素的转换，再进一步考虑pose的转换！