由于不同算法之间的坐标系不同，导致计算的结果混乱，该博客的目的是记录和统一不同算法之间的坐标系，保证坐标系的统一

一、VINS算法

1. vins算法，使用D435I相机。
2. 该坐标方向为：右前上分别为xyz。
3. 角度：由于是四元素，暂时不考虑
4. 使用xtdrone的代码，其中xyz方向还是右前上，

import rospy
from geometry_msgs.msg import PoseStamped, Point
from nav_msgs.msg import Odometry
import math
from pyquaternion import Quaternion
import tf
import sysvehicle_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.xlocal_pose.pose.position.y = data.pose.pose.position.ylocal_pose.pose.position.z = data.pose.pose.position.zq_= Quaternion([data.pose.pose.orientation.w,data.pose.pose.orientation.x,data.pose.pose.orientation.y,data.pose.pose.orientation.z])q_ = q_*qlocal_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()):continueelse:print("Vins pose received")local_pose.header.stamp = rospy.Time.now()position_pub.publish(local_pose) try:rate.sleep()except:continue

二、MAVROS坐标

1. 无人机坐标方向没有统一的概念，他是根据外部输入进行输出的。在vins输入中，由于初始化是在右前上，所以得到的local_position就为右前上。对于GPS定位，由于初始化是东北天？，所以就导致出使的xyz也是东北天，这和无人机的方位没有关系。
2. 所以问题就在于，融合imu的时候，需要考虑imu的方向。
3. imu加速度方向：向上加速为z增加；向前加速x增加；向左加速y增加。
4. imu角速度方向：绕x轴（加速度轴）逆时针为x（角速度变量）增加、绕y轴逆时针为y增加，绕z轴逆时针为z增加
5. imu四元素合成：imu会根据加速度和角速度进行一个四元素融合，得到xyzw值。

三、四元素和欧拉角

1. 感觉四元素和欧拉角还有点不明确，通过这两个链接进一步了解：四元素与欧拉角和欧拉角的旋转。了解到，欧拉角的旋转有旋转顺序，我喜欢用ZYX，但是mavros是什么就不知道了的；欧拉角是根据自身的坐标系进行下一次的旋转的，这是根据IMU特性决定的，因此不是根据实验参考坐标系来，欧拉角的定义存在死锁问题，比如绕y旋转90°时，会导致z与x的旋转融合。四元素的定义是，绕旋转轴（xyz）旋转一个角度w，就是这么简单；其中w就是旋转角一半的余弦；xyz也要也是对应角度余弦(我还以为是矢量坐标，好像就是诶)；最后，为了平方和为1，所以将xyz是之前的角度余弦呈上w得到的。
2. 仿真和转换网站：四元素转换网站，可以用来仿真欧拉角的死锁，转换理解。

四、理解ros imu中的的四元素

1. imu的四元素的坐标又是以什么的坐标系来的呢，这值得思考。初步发现，w值居然不是为1，而是0.7，这是为什么呢？默认猜测是如下，imu已经将绕z轴旋转了90°了。
   
2. tf_conversions.transformations.quaternion_from_euler(roll, pitch, yaw) # 说明欧拉顺序为XYZ
   quaternion = tf.transformations.quaternion_from_euler(0, -math.pi/2, math.pi/2)

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进行一个坐标转换，得到的转换结果

3. 问题来了，视觉VIO旋转方向为什么要转这样一个角度，旋转的角度是发生了什么。
4. 旋转的角度：根据上述的VINS方向，原VIO方向为右前上，进行(0, -math.pi/2, math.pi/2)
   转换后为前下左。（难道前下左为imu方向？？？）
   
5. 如果按照imu可能的前左上（确定是FLU了，根据https://zhuanlan.zhihu.com/p/410800248得到），转换回去的视觉是下前右。到底是什么呢，再来思考下。imu这边默认是东北天吧，在ENU坐标系中，X轴为正东方向，Y轴为正北方向，Z轴向上，单位为米。
6. 在px4官网中提到，将外部的定位方式转换为NED的方式，即北东地，但这个是PX4自己的，不是MAVROS的。同样上述链接中，存在存在mavros坐标系的描述，如下：。表明MAVROS的坐标是FRD，需要转换成这样吗？
   

四、总结

1. mavros使用RTK定位，坐标是ENU；
2. 正式确定，imu_data的坐标方向未FLU（同样也是ENU东北天，加速度和旋转），即前面为x，左边为y，上面为z。在初始化的时候，可能出现w为负数的情况，这里要注意了，当w为负数，所有的xyz因为都乘上了w才得到的，所有xyz的方向会被认为是反过来了。当绕z轴转360°就会发现所有的都回来了。
3. 上述对于四元素的定义有问题，实际为如下。这样四元素的平方和才为1，并且可以表示出不同的旋转方向。当w接近1时，表示角度为0，此时不管绕z轴逆时针还是正时针，w都会减小，但xyz中的sin则表现出正负号，体现出了旋转方向。
   
4. 明天看下vins的旋转坐标系，进行统一……