高斯坐标（东北高、东北天）转到RTK坐标

// Pw高斯坐标系坐标，vd 6个rtk参数，x,y,z,heading,pitch,roll
// 输出，RTK坐标系坐标
Mat getrtk(Mat Pw,vector vd)
{
    double r1 = (vd[3]/180)*3.1415926;
    double r2 = (vd[5]/180)*3.1415926;
    double r3 = (vd[4]/180)*3.1415926;
    Mat_ Rwi = (Mat_(3, 3)  <<  cos(r1)*cos(r3)+sin(r1)*sin(r2)*sin(r3) , sin(r1)*cos(r2) , cos(r1)*sin(r3)-sin(r1)*sin(r2)*cos(r3),
                                                -sin(r1)*cos(r3)+sin(r2)*cos(r1)*sin(r3), cos(r1)*cos(r2) , -sin(r1)*sin(r3)-sin(r2)*cos(r1)*cos(r3),
                                                -sin(r3)*cos(r2)                        , sin(r2)         , cos(r3)*cos(r2));

    Mat_ twi = (Mat_(3, 1) << vd[0], vd[1], vd[2]);
    Mat Pi = Rwi.t()*(Pw-twi);

    return Pi;
}

直接上代码，简单粗暴。网上各种都有，还不对。。。太乱。。。。