ROS中利用tf工具或Eigen将imu数据转换至世界坐标下

ROS中的imu数据（sensor_msgs/imu）一般为机体坐标系下，如果后续需要进行卡尔曼滤波等处理，需要先将其转换至世界坐标系中。借助ROS中tf工具里面的转换矩阵功能，可以较为简单的实现这一目的。（tf官方教程中是教用broadcaster与listener，个人感觉在这里有点大材小用且复杂化了）

代码全文

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Vector3.h"
#include "tf/tf.h"

void imu_fix_callback(const sensor_msgs::Imu::ConstPtr& data){
    //将imu传感器中的四元数转换为tf工具中的四元数。顺序为x,y,z,w。
    tf::Quaternion q1={data->orientation.x,data->orientation.y,data->orientation.z,data->orientation.w}; 
    //通过四元数得到旋转矩阵
    tf::Matrix3x3 r1;
    r1.setRotation(q1);

    geometry_msgs::Vector3 acc_world;//构建向量用于储存转换后的数据
    //旋转矩阵右乘列向量获得转换后的向量
    acc_world.x=(r1[0][0]*data->linear_acceleration.x)+(r1[0][1]*data->linear_acceleration.y)+(r1[0][2]*data->linear_acceleration.z);
    acc_world.y=(r1[1][0]*data->linear_acceleration.x)+(r1[1][1]*data->linear_acceleration.y)+(r1[1][2]*data->linear_acceleration.z);
    acc_world.z=(r1[2][0]*data->linear_acceleration.x)+(r1[2][1]*data->linear_acceleration.y)+(r1[2][2]*data->linear_acceleration.z);
}


int main(int argc,char **argv)
{
    ros::init(argc,argv,"test");
    ros::NodeHandle nh;

    ros::Subscriber sub=nh.subscribe("/mavros/imu/data",100,imu_fix_callback);

    ros::spin();
    return 0;
}

（新：）我发现Eigen版的更简单，所有在这里加一个。（代码是网页版手搓的，要是错了的话，根据提醒改一下就行，顺便告诉我一下）

#include <Eigen/Eigen>


void imu_fix_callback(const sensor_msgs::Imu::ConstPtr& data){
    //将imu传感器中的四元数转换为Eigen中的四元数。顺序为w,x,y,z。
    Eigen::Quaternionf q1={data->orientation.w,data->orientation.x,data->orientation.y,data->orientation.z}; 
    
    
    Eigen::Vector3f v1{data->linear_acceleration.x,data->linear_acceleration.y,data->linear_acceleration.x};   //加速度
    
    v1=q1.inverse()*v1;    //Eigen里面重载了四元数的乘法，实际数学意义为 p_new=q*p*q^(-1) .这里用q1的逆是因为公式为 ：q_world*v_world = q_body * v_body 。

    //这么简单的公式我相信大家都懂的，只是我自己笨笨的，之前搞错了，所以记录一下让大伙乐一乐。
    //这时候的v1就是世界坐标系下的加速度了，记得自己减去重力！！！此外，你要是姿态不准的话，去重力不够好，你的加速度信息就挺不好看的。    
}

代码比较简单，也带有注释，看到这里就看懂的就不需要继续往下看了。下面是代码更详细的解析与参考资料链接。

代码解析

int main(int argc,char **argv)
{
    ros::init(argc,argv,"test");
    ros::NodeHandle nh;

    ros::Subscriber sub=nh.subscribe("/mavros/imu/data",100,imu_fix_callback);

    ros::spin();
    return 0;
}

常规的消息订阅，没什么好讲的，刚入门的话建议去看 官方教程。

    //将imu传感器中的四元数转换为tf工具中的四元数。顺序为x,y,z,w。
    tf::Quaternion q1={data->orientation.x,data->orientation.y,data->orientation.z,data->orientation.w}; 
    //通过四元数得到旋转矩阵
    tf::Matrix3x3 r1;
    r1.setRotation(q1);

quaternion是tf工具里面储存四元数的一个类，除直接输入x,y,z,w以外，也可以通过输入欧拉角进行设置（里面的setRPY函数）。tf::Quaternion Class Reference

matrix3x3是tf工具里面的3x3多功能矩阵，这里利用的是其通过四元数转换旋转矩阵的功能（setRotation）。详细官方文档在此：tf::Matrix3x3 Class Reference

//旋转矩阵右乘列向量获得转换后的向量
    acc_world.x=(r1[0][0]*data->linear_acceleration.x)+(r1[0][1]*data->linear_acceleration.y)+(r1[0][2]*data->linear_acceleration.z);
    acc_world.y=(r1[1][0]*data->linear_acceleration.x)+(r1[1][1]*data->linear_acceleration.y)+(r1[1][2]*data->linear_acceleration.z);
    acc_world.z=(r1[2][0]*data->linear_acceleration.x)+(r1[2][1]*data->linear_acceleration.y)+(r1[2][2]*data->linear_acceleration.z);

这里的作用就是旋转矩阵与向量相乘。对应的矩阵运算为：

Matrix3x3对运算符[]进行了重载，可直接获得行向量（返回tf的vector类型），可以结合下图进行理解。

（图来源：ros tf 向量计算示例，欧拉角、四元数、转换矩阵之间的互转。里面还有更多情况下的转换，有兴趣的可以去看看原文。）

由于是小功能，代码就写的很随意与丑陋了，改进与优化的任务就留给读者自行解决了，希望能帮助到各位。如有疑问与纠错，欢迎在评论区留言。如需转载，标明出处且附上链接即可。

参考文献

ros tf 向量计算示例，欧拉角、四元数、转换矩阵之间的互转

 tf::Quaternion Class Reference

tf::Matrix3x3 Class Reference