VINS、MAVROS等的坐标系统一（草稿，未得出明确结果）

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

一、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 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坐标

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

5. 确定了，对于mavros来说，坐标系是FLU，同时也是东北天，对应着XYZ。但是为什么VINS启动之后，往前是Y呢，往右边是X呢，注意看四元素，它已经绕Z轴旋转了90°了，所以原本向前是X轴的，现在变成了Y轴。

6. 当没有VINS，只使用GPS和imu的时候，角度就是朝向东方的0°。而使用VINS之后，因为VINS坐标系是右前上，与MAVROS的坐标系前左上不对应，所以就在MAVROS中给其定义了旋转了90°。

7. 为什么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. 上述的转换可以看出只是将初始角度换过去了，难道这样就可以了吗？是的，因为前面提到坐标系是一致的了，都是右前上，因此转换之后的坐标就是符合后面的角度了。
   

8. 上述的分析稍微有点问题，MAVROS的初始化会根据vision_pose进行自动调整，并不是和vision_pose一模一样的。比如上述的flu转换文件中，即使这样旋转了，但是后续的旋转仍然以左前上的方向进行，在vins输出的四元素和vision_pose的四元素都可以看出来，但是在local_position中就有了变化。猜测原因是，只要定义好了初始值，mavros的local_position就会自动进行转换过来为对应的方位。比如上述flu转换之后，转换到的vision_pose就是0°了，这样在吗，mavros的local_position中就会认为前面是x轴，并且按照右手螺旋定则进行yz的分配。

9. 综上，进行修改的时候，因综合考虑pose和四元素的转换，在思考时，应该先考虑四元素的转换，再进一步考虑pose的转换！