EUROC数据集，IMU数据的使用和坐标系问题

少说废话。

去搜索EUROC数据集的解读，收费的我不看，坐标轴旋转大家拧来拧去把自己都说蒙了暂且不提，直说两个最关键的也是最多人止步的问题。

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运动数值，想干什么都行了。

这一点原文说的有点晦涩：

标黄部分说的有个unkown旋转在世界坐标系S（但前面又说S是传感器坐标系，此处按照世界坐标系理解就行了）和B（理解为机器人的运动中心就好，数据集中，同时也是IMU的坐标系）之间。

这个UNKOWN旋转就是我们想知道的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]，那么 $\sqrt{X^2+Y^2+Z^2}$ 就是自然重力加速度的最大值gravity_norm。