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;