一、VINS算法

vins算法，使用D435I相机。
该坐标方向为：右前上分别为xyz。
角度：由于是四元素，暂时不考虑
使用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 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

二、MAVROS坐标

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

三、四元素和欧拉角

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

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

imu的四元素的坐标又是以什么的坐标系来的呢，这值得思考。初步发现，w值居然不是为1，而是0.7，这是为什么呢？默认猜测是如下，imu已经将绕z轴旋转了90°了。
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进行一个坐标转换，得到的转换结果

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

四、总结

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的旋转坐标系，进行统一……