EUROC数据集，IMU数据的使用和坐标系问题，与根据IMU数据集还原位置和姿态的尝试
少说废话。去搜索EUROC数据集的解读，收费的我不看，坐标轴旋转大家拧来拧去把自己都说蒙了暂且不提，直说两个最关键的也是最多人止步的问题。举个例子：这样的错误我发现不止三篇博文出现，不点名了，简单的英文是这样的：就是说， b_a是bias，偏差，不是测量值！！！这个弱智问题相信有读下原文的人都不会误解。
一、20241010：
数据来源为kmavvisualinertialdatasets – ASL Datasets
Vicon Room 1 01 的 ASL Dataset Format格式，即zip。
如图
去搜索EUROC数据集的解读，收费的我不看，IMU的坐标系旋转大家拧来拧去把自己都说蒙了暂且不提，直说两个最关键的也是最多人止步的问题。
1、低级翻译错误和错误反复转载：
举个例子groundtruth数据包含：
这样的错误我发现不止三篇博文出现，不点名了，简单的英文是这样的：
就是说，groundtruth数据中 b_a是bias，偏差，不是测量值！！！ 这个弱智问题相信有读下原文的人都不会误解。
2. IMU 在 data.csv中的数据格式比较奇怪，不是我们想象中的[ 0,0,9.8]
2.1 IMU数据为什么“不对”
data.csv中给出的IMU数据例子为：
很奇怪是不是，甚至Z轴还有-3这样吊诡的数据，这加速度是什么？
答案很简单：因为机器人初始状态IMU就不是理想状态下的[ 0,0,9.8]，更简单来说，IMU初始状态就是歪的（不与地面坐标系一致）！
同时也引出另一个结论：
不管如何解读“a_RS_S_x”中的坐标系是谁和谁，至少，这个“a_RS_S_x”数值代表了其自身的读数，加速度层面，就是IUM坐标系下的数值，角速度层面，那自然就是在IMU坐标系下的随体角速度。
OK，既然初始状态是歪的，那么要做的事情就是，找出IMU初始状态歪了多少，然后想办法跟[ 0,0,9.8]这样的数值相减，就得到了真实的IMU运动数值，想干什么都行了。
那么要怎么做呢？ 有些无聊的人可能就要开另一篇博文或者说不定还要收费啦。我恰巧是另一种无聊。
2.2 如何确认IMU（传感器组）的初始旋转
可以参考VINS的代码或者MSCKF_VIO / MSCKF_MONO的代码思想，这里以MSCKF为例：
msckf_vio.cpp文件中：
void MsckfVio::imuCallback(
const sensor_msgs::ImuConstPtr& msg) {
// IMU msgs are pushed backed into a buffer instead of
// being processed immediately. The IMU msgs are processed
// when the next image is available, in which way, we can
// easily handle the transfer delay.
imu_msg_buffer.push_back(*msg);
if (!is_gravity_set) {
if (imu_msg_buffer.size() < 200) return;
//if (imu_msg_buffer.size() < 10) return;
initializeGravityAndBias();
is_gravity_set = true;
}
return;
}
void MsckfVio::initializeGravityAndBias() {
// Initialize gravity and gyro bias.
Vector3d sum_angular_vel = Vector3d::Zero();
Vector3d sum_linear_acc = Vector3d::Zero();
for (const auto& imu_msg : imu_msg_buffer) {
Vector3d angular_vel = Vector3d::Zero();
Vector3d linear_acc = Vector3d::Zero();
tf::vectorMsgToEigen(imu_msg.angular_velocity, angular_vel);
tf::vectorMsgToEigen(imu_msg.linear_acceleration, linear_acc);
sum_angular_vel += angular_vel;
sum_linear_acc += linear_acc;
}
state_server.imu_state.gyro_bias =
sum_angular_vel / imu_msg_buffer.size();
//IMUState::gravity =
// -sum_linear_acc / imu_msg_buffer.size();
// This is the gravity in the IMU frame.
Vector3d gravity_imu =
sum_linear_acc / imu_msg_buffer.size();
// Initialize the initial orientation, so that the estimation
// is consistent with the inertial frame.
double gravity_norm = gravity_imu.norm();
IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);
Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
gravity_imu, -IMUState::gravity);
state_server.imu_state.orientation =
rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());
return;这段代码的主要意思就是这几步：
1、程序开始对IMU的未知旋转状态录取200次数据；
2、把线加速度和角速度的累加200次并除以200取均值；
3、200次角速度的累加均值，直接作为bias偏差使用；（也可以根据其他方式获取）
4、线加速度的200次累加值均值，对三元素向量直接求模，因为显然初始静置的话，假设IMU测量值为[X,Y,Z]，那么就是自然重力加速度的最大值gravity_norm。
5、定义自然重力向量[0，0，-gravity_norm],求这个向量和上述[X,Y,Z]的夹角，作为Q_I_W初始值，也就是IMU坐标系在绝对世界坐标系W中的旋转角，也就是我们想知道的IMU初始歪了多少。EUROC文中提到了W坐标系就是这个W，当然也可以等价为R（视觉测量坐标系）。
6、根据Q_I_W初始值，后续所有的IMU坐标系内的测量值，都可以从坐标系 I 旋转到世界坐标系W中，当然这时随着角速度的变化，Q_I_W也会进行更新，比如常用的小角度假设更新。
小结：至此，数据集的事情基本就说清楚了，搞清楚数据测得都是什么坐标系下的数之后，根据EUROC论文给出坐标关系图，至少在数据理解上就不会有问题。
研究了一下午，看了一堆不知所云的博文，为防止后来者浪费青春，特此成文。
二、20241015
为了防止大家走弯路，我把这几天的对数据集尝试得到几个IMU和数据处理的结论放到这里：
一、由于真实IMU数据的抖动，直接积分累积的位置误差极大，数量级的差距（第三部分进行了仿真），完全无法接受，即使采用四阶龙格库塔积分。你会难以分清是你的旋转出了问题还是哪边编程错了，网上有直接积分的例程是自己微分出来的虚拟值。这也是为何必须加上滤波算法或者相机矫正等策略。
二、四元数在转换姿态角时会出现角度跳变，发生在pi/180°附近，画图时候可以后处理转化到即可，不用纠结。
三、怀疑：matlab提供的quat2angle函数与机器人工具箱toolbox中的UnitQuaternion.torpy方法坐标系不同，直观上就是ROLL和YAW恰好相反。此外我怀疑机器人工具箱toolbox中的四元数构造函数UnitQuaternion.rpy(...)有问题，与matlab提供的angle2quat结果不同。
接下来给出仿照MSCKF的IMU数据处理方式，对IMU数据进行直接的四阶龙格库塔积分的MATLAB程序，默认已有机器人工具箱，初步分析，我是仿照msckf的程序直接套用的，其中在大OMEGA矩阵的生成部分，可能轴的处理出了问题。
%仿照MSCKF的IMU数据处理方式，对IMU数据进行直接的四阶龙格库塔积分，默认已有机器人工具箱
clc;
close all;
clear all;
load imu0_r1;%！matlab导入数据集的IMU0测量数据处理成mat
imu0_data = table2array(data);
imu_time_orin = imu0_data(:,1);%ns
imu_time = imu_time_orin - imu_time_orin(1,1);
imu_time = imu_time/1e9;%200Hz
%imu角速度，IMU坐标系内
omi_RS_S_x = imu0_data(:,2);
omi_RS_S_y = imu0_data(:,3);
omi_RS_S_z = imu0_data(:,4);
omi_RS_S = [omi_RS_S_x,omi_RS_S_y,omi_RS_S_z];
%imu加速度，IMU坐标系内
ai_RS_S_x = imu0_data(:,5);
ai_RS_S_y = imu0_data(:,6);
ai_RS_S_z = imu0_data(:,7);
ai_RS_S = [ai_RS_S_x,ai_RS_S_y,ai_RS_S_z];
load ground_truth_r1;%！导入数据集的groundtruth真实测量数据处理成mat
gt_data = table2array(data1);
gt_time_orin = gt_data(:,1);%ns
gt_time = gt_time_orin - gt_time_orin(1,1);
gt_time = gt_time/1e9;%20Hz
%真实位置，单位m 根据世界/测量坐标系R的
p_RS_R_x = gt_data(:,2);
p_RS_R_y = gt_data(:,3);
p_RS_R_z = gt_data(:,4);
p_RS_R = [p_RS_R_x,p_RS_R_y,p_RS_R_z];
%真实姿态，四元数，根据世界/测量坐标系R的
q_RS_w = gt_data(:,5);
q_RS_x = gt_data(:,6);
q_RS_y = gt_data(:,7);
q_RS_z = gt_data(:,8);
q_RS = [q_RS_w,q_RS_x,q_RS_y,q_RS_z];
%真实线速度，m/s，根据世界/测量坐标系R的
v_RS_R_x = gt_data(:,9);
v_RS_R_y = gt_data(:,10);
v_RS_R_z = gt_data(:,11);
v_RS_R = [v_RS_R_x,v_RS_R_y,v_RS_R_z];
%真实角速度偏差，rad/s，根据世界/测量坐标系R的
b_w_RS_S_x = gt_data(:,12);
b_w_RS_S_y = gt_data(:,13);
b_w_RS_S_z = gt_data(:,14);
b_w_RS_S = [b_w_RS_S_x,b_w_RS_S_y,b_w_RS_S_z];
%真实现行加速度偏差，m/s^2，根据世界/测量坐标系R的
b_a_RS_S_x = gt_data(:,15);
b_a_RS_S_y = gt_data(:,16);
b_a_RS_S_z = gt_data(:,17);
b_a_RS_S = [b_a_RS_S_x,b_a_RS_S_y,b_a_RS_S_z];
%
figure;
plot3(p_RS_R_x,p_RS_R_y,p_RS_R_z);
%确认初始的IMU姿态。200组取均值。ai_RS_S(x,y,z),初始代表x上，y右，z前。
%a0_i_w = [0,0,-ai_RS_S_200_norm],坐标系变为z上，x，前，y 右？
ai_RS_S_200= sum(ai_RS_S(1:200,:),1)/200;
ai_RS_S_200_norm = norm(ai_RS_S_200);
gravity = [0,0,-ai_RS_S_200_norm];
%实现MSCKF的uaterniond q0_i_w = Quaterniond::FromTwoVectors(
% gravity_imu, -IMUState::gravity)
% matlab没有对应的函数
u = cross(-gravity,ai_RS_S_200);
u = u/norm(u);
theta = acos(dot(-gravity,ai_RS_S_200)/(norm(ai_RS_S_200)*norm(-gravity)));
% q = [cos(theta) sin(theta)*u];
q0_a_i_w = UnitQuaternion.angvec( theta,u);%IMU initial rotation
R0_a_i_w = q0_a_i_w.R;
% test = R0_a_i_w' * ai_RS_S_200';
% 应用MSCKF的4阶积分，根据predictNewState函数改写
%但是感觉误差依然非常大，没有滤波加上偏差的原因吗？未解之谜？！
% q_v(1,:) = [ q0_a_i_w.s, q0_a_i_w.v(1), q0_a_i_w.v(2), q0_a_i_w.v(3)];
% q_v(2,:) = [0,0,0,0];
q_r(1,:) = q0_a_i_w.conj;
q_r(2,:) = UnitQuaternion([1,0,0,0]);
V_t(1,:) = [0,0,0];
P_t(1,:) = [0,0,0];
d_t = 0.005;%200hz
a_i_i= [ai_RS_S(:,1),ai_RS_S(:,2),ai_RS_S(:,3)] ;
omi_i_i = [omi_RS_S(:,1),omi_RS_S(:,2),omi_RS_S(:,3)] ;
b_a_i = [b_a_RS_S(:,1),b_a_RS_S(:,2),b_a_RS_S(:,3)] ;
b_w_i = [b_w_RS_S(:,1),b_w_RS_S(:,2),b_w_RS_S(:,3)] ;
for i =1: (length(b_a_i)-1)
%%%%%%%%%%
% a_t_w(i,:) = a_i_i(i,:) - b_a_i(i,:);
a_t_w(i,:) = a_i_i(i,:);
% omi = (omi_i_i(i,:)-b_w_i(i,:))';
omi = (omi_i_i(i,:))'; %可能是这里顺序有问题，懒得折腾了
omi_norm = norm(omi);
OMEGA = [ -skew(omi),omi; ...,
-omi',0;];%4*4
if i ==1
q_r_new(1,:) = q_r(1,:);
else
q_r(i,:) = q_r_new(i-1,:) * UnitQuaternion([1, 0.5*omi(1)*d_t, 0.5*omi(2)*d_t , 0.5*omi(3)*d_t ]);
end
q_v = [ q_r(i,:).s, q_r(i,:).v(1), q_r(i,:).v(2), q_r(i,:).v(3)];
if omi_norm> 1e-5
dq_dt = ( cos(omi_norm*d_t*0.5)*eye(4) ...
+ 1/omi_norm*sin(omi_norm*d_t*0.5)*OMEGA) * q_v';
dq_dt2 = ( cos(omi_norm*d_t*0.25)*eye(4) ...
+ 1/omi_norm*sin(omi_norm*d_t*0.25)*OMEGA) * q_v' ;
else
dq_dt = (eye(4) + d_t*0.5*OMEGA ) * ...
+ cos(omi_norm*d_t*0.5)* q_v' ;
dq_dt2 = (eye(4) + d_t*0.25*OMEGA ) * ...
+ cos(omi_norm*d_t*0.25)* q_v' ;
end
dq_dt_qua = Quaternion(dq_dt);
dq_dt2_qua = Quaternion(dq_dt2);
dq_dt_qua_uni = UnitQuaternion(dq_dt_qua);
dR_dt_transpose = dq_dt_qua_uni.R();
dR_dt_transpose = dR_dt_transpose';
dq_dt2_qua_uni = UnitQuaternion(dq_dt2_qua);
dR_dt2_transpose = dq_dt2_qua_uni.R();
dR_dt2_transpose = dR_dt2_transpose';
%%%%
q_v_qua = q_r(i,:) ;
R_q_v_trans= q_v_qua.R;
R_q_v_trans = R_q_v_trans';
%k1 = f(tn, yn)
k1_v_dot = R_q_v_trans * a_t_w(i,:)' + gravity';
a_test(i,:) = k1_v_dot; %记录矫正旋转之后的IMU"真实值"
k1_p_dot = V_t(i,:)';
%k2 = f(tn+dt/2, yn+k1*dt/2)
k1_v = V_t(i,:)'+ k1_v_dot*d_t/2;
k2_v_dot = dR_dt2_transpose* a_t_w(i,:)'+ gravity';
k2_p_dot = k1_v;
%k3 = f(tn+dt/2, yn+k2*dt/2)
k2_v = V_t(i,:)'+ k2_v_dot*d_t/2;
k3_v_dot = dR_dt2_transpose* a_t_w(i,:)' + gravity';
k3_p_dot = k2_v;
%k4 = f(tn+dt, yn+k3*dt)
k3_v = V_t(i,:)'+ k3_v_dot*d_t;
k4_v_dot = dR_dt_transpose* a_t_w(i,:)' + gravity';
k4_p_dot = k3_v;
% yn+1 = yn + dt/6*(k1+2*k2+2*k3+k4)
% q_v_qua(i,:) = dq_dt_qua_uni;
temp= V_t(i,:)' + d_t/6*(k1_v_dot+2*k2_v_dot+2*k3_v_dot+k4_v_dot);
V_t(i+1,:) = temp';
temp=P_t(i,:)' + d_t/6*(k1_p_dot+2*k2_p_dot+2*k3_p_dot+k4_p_dot);
P_t(i+1,:) =temp';
q_r_new(i,:) = dq_dt_qua;
end
figure;
plot3(P_t(:,1),P_t(:,2),P_t(:,3)) ;
%直接用小角度假设的实现的四元数积分，换算出RPY
% for i = 1:length(q_r)
% rpytest(i,:) = q_r(i).torpy;
% end
for i = 1:length(q_r)
rpytest(i,:) = q_r_new(i).torpy;%默认顺序ZYX
end
figure;
plot(rpytest(:,1)) ;hold on;
plot(rpytest(:,2)) ;hold on;
plot(rpytest(:,3)) ;hold on;
legend("yaw","pitch","roll");
title("直接用小角度假设的实现的四元数积分，换算出RPY");
% 画出groundtruth 的RPY
for i = 1:length(q_r)
q_RS_qua = UnitQuaternion([q_RS(i,1),q_RS(i,2),q_RS(i,3),q_RS(i,4)]);
rpytest1(i,:) = q_RS_qua.torpy;
end
figure;
[temp1,temp2] = find(rpytest1>=(pi-0.5));%只是为了画图不要跳变而已，没有实际物理意义
% 换言之转换成rpy最好从四元数手动转换，不要用自带的
rpytest1(temp1,temp2) = rpytest1(temp1,temp2) -2*pi;
plot(rpytest1(:,1)) ;hold on;
plot(rpytest1(:,2)) ;hold on;
plot(rpytest1(:,3)) ;hold on;
legend("yaw","pitch","roll");
title("画出groundtruth 的RPY，注意与小角度假设的坐标系不同");
（根据后来分析，直接使用小角度估计的结果与直接测量的结果是近似的，通过画出q_r并对曲线跳变部分进行+2pi或者-2pi的操作就可以看出趋势基本一致，直接看图有很多2pi的波动，这里面之所以出现如此大的波动，可能还是四阶积分我这里编程有误，问题可能出在大OMEGA的计算……）
三、20241016
今天通过进一步分析和尝试，给出以下结论：
1、IMU加速度直接积分获得位置就是会导致较大的发散，哪怕角度相对准确，这也是为什么需要滤波。而且数据集给出的ground truth的bias值，也并不算准确。
2、明确了EUROC数据集中IMU的坐标系S（同时也是B），ground truth（视觉测量系统R，也就是世界W）的坐标系的关系，如下图所示：
根据上述坐标系，我们直接使用测量的世界姿态进行转化并积分，这样一来，就可以避免前面所说的角度的偏差较大影响姿态进而影响加速度的估计了。
IMU直接积分matlab程序如下，可见IMU加速度直接积分还是飘到了姥姥家，我个人也用4阶积分尝试，依然不行，也可能我程序编的不对，感兴趣可以尝试。
%% IMU坐标系按照euroc的，上x，前z，右y,世界坐标系按照上Z，前X，右Y
% 对IMU数据进行直接的积分，默认已有机器人工具箱
clc;
close all;
clear all;
load imu0_r1;%！matlab导入数据集的IMU0测量数据处理成mat
imu0_data = table2array(data);
imu_time_orin = imu0_data(:,1);%ns
imu_time = imu_time_orin - imu_time_orin(1,1);
imu_time = imu_time/1e9;%200Hz
%imu角速度，IMU坐标系内
omi_RS_S_x = imu0_data(:,2);
omi_RS_S_y = imu0_data(:,3);
omi_RS_S_z = imu0_data(:,4);
omi_RS_S = [omi_RS_S_x,omi_RS_S_y,omi_RS_S_z];
%imu加速度，IMU坐标系内
ai_RS_S_x = imu0_data(:,5);
ai_RS_S_y = imu0_data(:,6);
ai_RS_S_z = imu0_data(:,7);
ai_RS_S = [ai_RS_S_x,ai_RS_S_y,ai_RS_S_z];
load ground_truth_r1;%！导入数据集的groundtruth真实测量数据处理成mat
gt_data = table2array(data1);
gt_time_orin = gt_data(:,1);%ns
gt_time = gt_time_orin - gt_time_orin(1,1);
gt_time = gt_time/1e9;%20Hz
%真实位置，单位m 根据世界/测量坐标系R的
p_RS_R_x = gt_data(:,2);
p_RS_R_y = gt_data(:,3);
p_RS_R_z = gt_data(:,4);
p_RS_R = [p_RS_R_x,p_RS_R_y,p_RS_R_z];
%真实姿态，四元数，根据世界/测量坐标系R的
q_RS_w = gt_data(:,5);
q_RS_x = gt_data(:,6);
q_RS_y = gt_data(:,7);
q_RS_z = gt_data(:,8);
q_RS = [q_RS_w,q_RS_x,q_RS_y,q_RS_z];
%真实线速度，m/s，根据世界/测量坐标系R的
v_RS_R_x = gt_data(:,9);
v_RS_R_y = gt_data(:,10);
v_RS_R_z = gt_data(:,11);
v_RS_R = [v_RS_R_x,v_RS_R_y,v_RS_R_z];
%真实角速度偏差，rad/s，根据世界/测量坐标系R的
b_w_RS_S_x = gt_data(:,12);
b_w_RS_S_y = gt_data(:,13);
b_w_RS_S_z = gt_data(:,14);
b_w_RS_S = [b_w_RS_S_x,b_w_RS_S_y,b_w_RS_S_z];
%真实现行加速度偏差，m/s^2，根据世界/测量坐标系R的
b_a_RS_S_x = gt_data(:,15);
b_a_RS_S_y = gt_data(:,16);
b_a_RS_S_z = gt_data(:,17);
b_a_RS_S = [b_a_RS_S_x,b_a_RS_S_y,b_a_RS_S_z];
%
figure;
plot3(p_RS_R_x,p_RS_R_y,p_RS_R_z);
%确认初始的IMU姿态。200组取均值。ai_RS_S(x,y,z),初始代表x上，y右，z前。
%a0_i_w = [0,0,-ai_RS_S_200_norm],坐标系变为z上，x，前，y 右？
ai_RS_S_200= sum(ai_RS_S(1:200,:),1)/200;
ai_RS_S_200_norm = norm(ai_RS_S_200);
gravity = [0,0,-ai_RS_S_200_norm];
V_t(1,:) = [0,0,0];
P_t(1,:) = [0,0,0];
d_t = 0.005;%200hz
%数据集提供的bias仍然偏差较大，根据前200组数据自行评估一个bias
for i = 1:200
ai_RS_S_1 = ai_RS_S(i,:);
q_RS_1 = q_RS(i,:) ;
q_RS_1_qua = UnitQuaternion([q_RS_1]);
q_RS_1_R = q_RS_1_qua.R;
a1test(i,:) = q_RS_1_R*ai_RS_S_1' + gravity';
end
ai_bias_200 = sum(a1test)/200;
%姿态测量结果中，z上，y右，x前。
for i = 1:length(q_RS)
ai_RS_S_1 = ai_RS_S(i,:);
q_RS_1 = q_RS(i,:) ;
q_RS_1_qua = UnitQuaternion([q_RS_1]);
q_RS_1_R = q_RS_1_qua.R;
a1test(i,:) = q_RS_1_R*ai_RS_S_1';
a1test_cali(i,:) = a1test(i,:) -ai_bias_200 + gravity; %矫正后的imu测试数据
end
for i = 1: length(a1test_cali)
a_t_w = a1test_cali(i,:);
V_t(i+1,:) = V_t(i,:) + a_t_w*d_t;%v1 = v0 + a*t;
P_t(i+1,:) = P_t(i,:) + V_t(i,:)*d_t + 0.5 * a_t_w*d_t*d_t;%s1 = s0 +v0*t+ 0.5*a*t*t;
end
figure;
plot3(P_t(:,1),P_t(:,2),P_t(:,3)) ;
title("直接矩形积分的位置结果");
补充一点数据分析，看看我们imu数据处理对不对。记录根据前200次加速度均值估算的偏差值和根据实际姿态进行旋转矫正处理的加速度值，程序中记为"a1test_cali"。初始的值可以看出还是基本符合初始起飞前状态的：
然后再最后降落时，发现IMU的数值已经出现了偏差：
这说明IMU在使用过程中又发生了一定的漂移（怀疑）。读者可以尝试使用数据集给出的所谓ground truth的bias值，最终数值上并没有变好。这也说明直接用IMU漂移就是很大，采集频率越高，漂移的越显著。
至此搞清楚了EUROC数据集中的IMU数据转换，以及坐标系之间的关系，那么相关的论文就应该少了一些理解壁垒。
四、20241021
今天继续开展工作的时候，发现本文第二部分中的程序可能有误，我是仿照原始C代码编写的，可能存在轴没有对应上的问题。
但，按照第三部分的直接使用世界坐标系中的测量结果，没有使用IMU的角速度，积分的漂移还是那样，不影响第三部分的结论。
五、2024.11.21
生活所迫，都是为了水论文啊，前几年还信誓旦旦要把论文写在祖国大地，被一顿暴打之后现在为了生活还得想办法憋洋屁，过阵子再找块地继续写吧。
自己补充一下本博文关于matlab中四元数欧拉角可能的误区：
quat2angle函数与机器人工具箱toolbox中的UnitQuaternion.torpy，给出的结果中ROLL和YAW的顺序是相反，但都是按照ZYX旋转进行解析的，这是因为机器人工具箱手册中的描述有歧义，会让人以为是跟ZYX对应的，输出[YAW,PITCH,ROLL]
实际上，quat2angle函数输出的是[YAW,PITCH,ROLL] ，而toolbox中的UnitQuaternion.torpy输出的[ROLL,PITCH,YAW]，人家就是这样的，接受就好。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
26
0- 0
已为社区贡献1条内容
所有评论(0)