ROS中通过欧拉角进行坐标变换

//先计算车辆在大地坐标下的初始时刻的roll,pitch,yaw(车辆到大地)
if(count==0){
tf::Quaternion orientation;
tf::quaternionMsgToTF(GnssInitMsg->pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(first_roll, first_pitch, first_yaw);

first_x = GnssInitMsg->pose.position.x;
first_y = GnssInitMsg->pose.position.y;
first_z = GnssInitMsg->pose.position.z;

count=1;
ROS_INFO_STREAM_ONCE("get first r , p ,y !!");
}
//将车辆在大地坐标系的位置转化到车辆初始时刻的坐标系下
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(GlobalVehicleMsg->pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

double x0=GlobalVehicleMsg->pose.pose.position.x - first_x;
double y0=GlobalVehicleMsg->pose.pose.position.y - first_y;
double z0=GlobalVehicleMsg->pose.pose.position.z - first_z;

double xl1 = x0;
double yl1 = y0 * cos(-first_roll) - z0 * sin(-first_roll);
double zl1 = y0 * sin(-first_roll) + z0 * cos(-first_roll);

double xl2 = zl1 * sin(-first_pitch) + xl1 * cos(-first_pitch);
double yl2 = yl1;
double zl2 = zl1 * cos(-first_pitch) - xl1 * sin(-first_pitch);

double xl3 = xl2 * cos(-first_yaw) - yl2 * sin(-first_yaw);
double yl3 = xl2 * sin(-first_yaw) + yl2 * cos(-first_yaw);
double zl3 = zl2;
---------------------