读取csv文件中的IMU数据并以sensor

读取csv文件中的IMU数据并以sensor_msgs/Imu格式发送 20210724

项目需要将所给的csv表格中IMU数据读取并转化为sensor_msgs/Imu格式，以便于后续使用数据跑定位算法。

csv表格：

读取csv文件

code：

ifstream inFile("(csv文件绝对路径)", ios::in);    //
vector<string> stamp,Yaw,Pitch,Roll,Ve,Vn,Vu;if (!inFile)
{cout << "打开CSV文件失败！" << endl;exit(1);
}
string line;
while (getline(inFile, line)){string field;istringstream sin(line);getline(sin, field, ',');stamp.push_back(field.c_str()); getline(sin, field, ',');Yaw.push_back(field.c_str()); getline(sin, field, ',');Pitch.push_back(field.c_str()); getline(sin, field, ',');Roll.push_back(field.c_str()); getline(sin, field, ',');Ve.push_back(field.c_str()); getline(sin, field, ',');Vn.push_back(field.c_str()); getline(sin, field); Vu.push_back(field.c_str());
}

IMU数据转换

由于csv中所给的数据并不是标准IMU格式，而是通过解算得到的处理过后的数据，所以需要通过逆变换反推出IMU的标准数据，得到gyro，accel，四元数等数据。

四元数

公式

qB=[cos⁡(ψ/2)00sin⁡(ψ/2)][cos⁡(θ/2)0sin⁡(θ/2)0][cos⁡(ϕ/2)sin⁡(ϕ/2)00]=[cos⁡(ϕ/2)cos⁡(θ/2)cos⁡(ψ/2)+sin⁡(ϕ/2)sin⁡(θ/2)sin⁡(ψ/2)sin⁡(ϕ/2)cos⁡(θ/2)cos⁡(ψ/2)−cos⁡(ϕ/2)sin⁡(θ/2)sin⁡(ψ/2)cos⁡(ϕ/2)sin⁡(θ/2)cos⁡(ψ/2)+sin⁡(ϕ/2)cos⁡(θ/2)sin⁡(ψ/2)cos⁡(ϕ/2)cos⁡(θ/2)sin⁡(ψ/2)−sin⁡(ϕ/2)sin⁡(θ/2)cos⁡(ψ/2)]\begin{aligned} \mathbf{q}_{\mathrm{B}} &=\left[\begin{array}{c} \cos (\psi / 2) \\ 0 \\ 0 \\ \sin (\psi / 2) \end{array}\right]\left[\begin{array}{c} \cos (\theta / 2) \\ 0 \\ \sin (\theta / 2) \\ 0 \end{array}\right]\left[\begin{array}{c} \cos (\phi / 2) \\ \sin (\phi / 2) \\ 0 \\ 0 \end{array}\right] \\ &=\left[\begin{array}{l} \cos (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \sin (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)-\cos (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \cos (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \cos (\theta / 2) \sin (\psi / 2)-\sin (\phi / 2) \sin (\theta / 2) \cos (\psi / 2) \end{array}\right] \end{aligned} qB=⎣⎢⎢⎡cos(ψ/2)00sin(ψ/2)⎦⎥⎥⎤⎣⎢⎢⎡cos(θ/2)0sin(θ/2)0⎦⎥⎥⎤⎣⎢⎢⎡cos(ϕ/2)sin(ϕ/2)00⎦⎥⎥⎤=⎣⎢⎢⎡cos(ϕ/2)cos(θ/2)cos(ψ/2)+sin(ϕ/2)sin(θ/2)sin(ψ/2)sin(ϕ/2)cos(θ/2)cos(ψ/2)−cos(ϕ/2)sin(θ/2)sin(ψ/2)cos(ϕ/2)sin(θ/2)cos(ψ/2)+sin(ϕ/2)cos(θ/2)sin(ψ/2)cos(ϕ/2)cos(θ/2)sin(ψ/2)−sin(ϕ/2)sin(θ/2)cos(ψ/2)⎦⎥⎥⎤

code：

q0 = sin(roll/360.0f*M_PI) * cos(pitch/360.0f*M_PI) * cos(yaw/360.0f*M_PI) - cos(roll/360.0f*M_PI) *            sin(pitch/360.0f*M_PI) * sin(yaw/360.0f*M_PI);
q1 = cos(roll/360.0f*M_PI) * sin(pitch/360.0f*M_PI) * cos(yaw/360.0f*M_PI) + sin(roll/360.0f*M_PI) *          cos(pitch/360.0f*M_PI) * sin(yaw/360.0f*M_PI);
q2 = cos(roll/360.0f*M_PI) * cos(pitch/360.0f*M_PI) * sin(yaw/360.0f*M_PI) - sin(roll/360.0f*M_PI) *           sin(pitch/360.0f*M_PI) * cos(yaw/360.0f*M_PI);
q3 = cos(roll/360.0f*M_PI) * cos(pitch/360.0f*M_PI) * cos(yaw/360.0f*M_PI) + sin(roll/360.0f*M_PI) *          sin(pitch/360.0f*M_PI) * sin(yaw/360.0f*M_PI);

GYRO

公式

[gxgygz]=Mx⋅My⋅[00dydt]+Mx⋅[0dpdt0]+[drdt00]\left[\begin{array}{l} g_{x} \\ g_{y} \\ g_{z} \end{array}\right]=\mathrm{M}_{x} \cdot \mathrm{M}_{y} \cdot\left[\begin{array}{c} 0 \\ 0 \\ \frac{d y}{d t} \end{array}\right]+\mathrm{M}_{x} \cdot\left[\begin{array}{c} 0 \\ \frac{d p}{d t} \\ 0 \end{array}\right]+\left[\begin{array}{c} \frac{d r}{d t} \\ 0 \\ 0 \end{array}\right] ⎣⎡gxgygz⎦⎤=Mx⋅My⋅⎣⎡00dtdy⎦⎤+Mx⋅⎣⎡0dtdp0⎦⎤+⎣⎡dtdr00⎦⎤

=[1000cos⁡rsin⁡r0−sin⁡rcos⁡r]⋅[cos⁡p0−sin⁡p010sin⁡p0cos⁡p]⋅[00dydt]+[1000cos⁡rsin⁡r0−sin⁡rcos⁡r]⋅[0dpdt0]+[drdt00]=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos r & \sin r \\ 0 & -\sin r & \cos r \end{array}\right] \cdot\left[\begin{array}{ccc} \cos p & 0 & -\sin p \\ 0 & 1 & 0 \\ \sin p & 0 & \cos p \end{array}\right] \cdot\left[\begin{array}{c} 0 \\ 0 \\ \frac{d y}{d t} \end{array}\right]+\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos r & \sin r \\ 0 & -\sin r & \cos r \end{array}\right] \cdot\left[\begin{array}{c} 0 \\ \frac{d p}{d t} \\ 0 \end{array}\right]+\left[\begin{array}{c} \frac{d r}{d t} \\ 0 \\ 0 \end{array}\right] =⎣⎡1000cosr−sinr0sinrcosr⎦⎤⋅⎣⎡cosp0sinp010−sinp0cosp⎦⎤⋅⎣⎡00dtdy⎦⎤+⎣⎡1000cosr−sinr0sinrcosr⎦⎤⋅⎣⎡0dtdp0⎦⎤+⎣⎡dtdr00⎦⎤

=[10−sin⁡p0cos⁡rcos⁡p⋅sin⁡r0−sin⁡rcos⁡p⋅cos⁡r]⋅[drdtdpdtdydt]=\left[\begin{array}{ccc} 1 & 0 & -\sin p \\ 0 & \cos r & \cos p \cdot \sin r \\ 0 & -\sin r & \cos p \cdot \cos r \end{array}\right] \cdot\left[\begin{array}{l} \frac{d r}{d t} \\ \frac{d p}{d t} \\ \frac{d y}{d t} \end{array}\right] =⎣⎡1000cosr−sinr−sinpcosp⋅sinrcosp⋅cosr⎦⎤⋅⎣⎡dtdrdtdpdtdy⎦⎤

code

Eigen::Vector3f deltaAngle;
Eigen::Vector3f gyro;
Eigen::Matrix3f tfmatrix;
yaw   = atof(Yaw[i].c_str());
pitch = atof(Pitch[i].c_str());
roll  = atof(Roll[i].c_str());
tfmatrix<<1,0,-sin(pitch/180.0f*M_PI),0,cos(roll/180.0f*M_PI),cos(pitch/180.0f*M_PI)*sin(roll/180.0f*M_PI),0,-sin(roll/180.0f*M_PI),cos(pitch/180.0f*M_PI)*cos(roll/180.0f*M_PI);
deltaAngle<<(roll-last_roll)/0.01f,(pitch-last_pitch)/0.01f,(yaw-last_yaw)/0.01f;
gyro=tfmatrix*deltaAngle;

Accel

公式：

[axayaz]=Mx⋅My⋅Mz⋅[dvndveg+dvu/dt]\left[\begin{array}{l} a_{x} \\ a_{y} \\ a_{z} \end{array}\right]=\mathrm{M}_{x} \cdot \mathrm{M}_{y} \cdot \mathrm{M}_{z} \cdot\left[\begin{array}{l} d vn \\ d ve \\ g+dvu/dt \end{array}\right] ⎣⎡axayaz⎦⎤=Mx⋅My⋅Mz⋅⎣⎡dvndveg+dvu/dt⎦⎤

=[1000cos⁡rsin⁡r0−sin⁡rcos⁡r]⋅[cos⁡p0−sin⁡p010sin⁡p0cos⁡p]⋅[cos⁡ysin⁡y0−sin⁡ycos⁡y0001]⋅[dvndveg+dvu/dt]=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos r & \sin r \\ 0 & -\sin r & \cos r \end{array}\right] \cdot\left[\begin{array}{ccc} \cos p & 0 & -\sin p \\ 0 & 1 & 0 \\ \sin p & 0 & \cos p \end{array}\right] \cdot\left[\begin{array}{ccc} \cos y & \sin y & 0 \\ -\sin y & \cos y & 0 \\ 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{l} d vn \\ d ve \\ g+dvu/dt \end{array}\right] =⎣⎡1000cosr−sinr0sinrcosr⎦⎤⋅⎣⎡cosp0sinp010−sinp0cosp⎦⎤⋅⎣⎡cosy−siny0sinycosy0001⎦⎤⋅⎣⎡dvndveg+dvu/dt⎦⎤

=[cos⁡p⋅cos⁡ycos⁡p⋅sin⁡y−sin⁡pcos⁡y⋅sin⁡p⋅sin⁡r−cos⁡r⋅sin⁡ycos⁡r⋅cos⁡y+sin⁡p⋅sin⁡r⋅sin⁡ycos⁡p⋅sin⁡rsin⁡r⋅sin⁡y+cos⁡r⋅cos⁡y⋅sin⁡pcos⁡r⋅sin⁡p⋅sin⁡y−cos⁡y⋅sin⁡rcos⁡p⋅cos⁡r]⋅[dvndveg+dvu/dt]=\left[\begin{array}{ccc} \cos p \cdot \cos y & \cos p \cdot \sin y & -\sin p \\ \cos y \cdot \sin p \cdot \sin r-\cos r \cdot \sin y & \cos r \cdot \cos y+\sin p \cdot \sin r \cdot \sin y & \cos p \cdot \sin r \\ \sin r \cdot \sin y+\cos r \cdot \cos y \cdot \sin p & \cos r \cdot \sin p \cdot \sin y-\cos y \cdot \sin r & \cos p \cdot \cos r \end{array}\right] \cdot\left[\begin{array}{l} dvn \\ dve \\ g+dvu/dt \end{array}\right] =⎣⎡cosp⋅cosycosy⋅sinp⋅sinr−cosr⋅sinysinr⋅siny+cosr⋅cosy⋅sinpcosp⋅sinycosr⋅cosy+sinp⋅sinr⋅sinycosr⋅sinp⋅siny−cosy⋅sinr−sinpcosp⋅sinrcosp⋅cosr⎦⎤⋅⎣⎡dvndveg+dvu/dt⎦⎤

=[−sin⁡pcos⁡p⋅sin⁡rcos⁡p⋅cos⁡r]⋅[dvndveg+dvu/dt]=\left[\begin{array}{c} -\sin p \\ \cos p \cdot \sin r \\ \cos p \cdot \cos r \end{array}\right] \cdot\left[\begin{array}{l} d vn \\ d ve \\ g+dvu/dt \end{array}\right] =⎣⎡−sinpcosp⋅sinrcosp⋅cosr⎦⎤⋅⎣⎡dvndveg+dvu/dt⎦⎤

code:

Eigen::Vector3f deltaV;
Eigen::Vector3f accel;
Eigen::Matrix3f tfmatrix2;
vx = atof(Vn[i].c_str());
vy = atof(Ve[i].c_str());
vz = atof(Vu[i].c_str());tfmatrix2<<cos(pitch/180.0f*M_PI)*cos(yaw/180.0f*M_PI),cos(pitch/180.0f*M_PI)*sin(yaw/180.0f*M_PI),-sin(pitch/180.0f*M_PI),cos(yaw/180.0f*M_PI)*sin(pitch/180.0f*M_PI)*sin(roll/180.0f*M_PI)-                                           cos(roll/180.0f*M_PI)*sin(yaw/180.0f*M_PI),cos(roll/180.0f*M_PI)*cos(yaw/180.0f*M_PI)+sin(pitch/180.0f*M_PI)*sin(roll/180.0f*M_PI)*sin(yaw/180.0f*M_PI),cos(pitch/180.0f*M_PI)*sin(roll/180.0f*M_PI),sin(roll/180.0f*M_PI)*sin(yaw/180.0f*M_PI)+cos(roll/180.0f*M_PI)*cos(yaw/180.0f*M_PI)*sin(pitch/180.0f*M_PI),cos(roll/180.0f*M_PI)*sin(pitch/180.0f*M_PI)*sin(yaw/180.0f*M_PI)-cos(yaw/180.0f*M_PI)*sin(roll/180.0f*M_PI),cos(pitch/180.0f*M_PI)*cos(roll/180.0f*M_PI);
deltaV<<(vx-last_vx)/0.01f,(vy-last_vy)/0.01f,(vz-last_vz)/0.01f+9.8f;
accel=tfmatrix2*deltaV;

节点发布

ros::init(argc, argv, "imu2rosbag");
ros::NodeHandle nh;
ros::Publisher IMU_pub = nh.advertise<sensor_msgs::Imu>("IMU_data", 20);
ros::Rate loop_rate(10);
sensor_msgs::Imu imu_data;
imu_data.header.frame_id = "base_link";
imu_data.header.stamp = ros::Time(atoi(stamp[i].c_str()));imu_data.orientation.x = q0;
imu_data.orientation.y = q1;
imu_data.orientation.z = q2;
imu_data.orientation.w = q3;imu_data.linear_acceleration.x = accel(0);
imu_data.linear_acceleration.y = accel(1);
imu_data.linear_acceleration.z = accel(2);imu_data.angular_velocity.x = gyro(0);
imu_data.angular_velocity.y = gyro(1);
imu_data.angular_velocity.z = gyro(2);

ref: https://blog.csdn.net/weixin_39719127/article/details/110299079 （姿态解算）

https://blog.csdn.net/chenlin41204050/article/details/78429437 （C++读CSV文件）