eigen及tf使用

tf: 欧拉角与旋转矩阵互转

   double roll = -0.25;
	double pitch =0.68;
	double yaw = -1.2;

tf::Matrix3x3 M;
M.setRPY(roll, pitch,yaw);

double roll1, pitch1, yaw1;
M.getRPY(roll1, pitch1, yaw1);  //roll1=-0.25  pitch1=0.68   yaw=-1.2

eigen:https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
https://blog.csdn.net/qingtian11112/article/details/105246247
eigen库函数从旋转矩阵转欧拉角可能有bug

double roll = -0.25;
double pitch =0.68;
double yaw = -1.2;

Eigen::Vector3d eulerAngle = {yaw, pitch,roll};  //0-pi,-pi-pi,-pi-pi
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Vector3d::UnitX())); //:UnitX() 指roll
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Vector3d::UnitY())); //Y pitch
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); //Z yaw

Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;  //外旋固定系，R-P-Y

//Z-Y-X，即RPY   X是0，Y 是1，Z是2 eulerAngle1={yaw,pitch,roll}
Eigen::Vector3d eulerAngle1=rotation_matrix.eulerAngles(2,1,0); //    结果存在+-pi的差
//if mat = Ry * Rz *Rx，则.eulerAngles(1,2,0)顺序求欧拉角  ={pitch,yaw,roll}

修正：自己做欧拉角回转

//对应  Rz * Ry *Rx的旋转  mat = Rz * Ry * Rx  或者 q = qz * qy * qx
 Eigen::Vector3d RelativeLocProcess::ToEulerAngles(const Eigen::Quaterniond &q)
{
    Eigen::Vector3d angles;  // 0-roll, 1-pitch, 2-yaw angles={roll, pitch , yaw}

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
    angles[0] = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w() * q.y() - q.z() * q.x());
    if (std::abs(sinp) >= 1)
        angles[1] = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles[1] = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
    angles[2] = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

Eigen::Vector3d eulerAngle = {yaw, pitch,roll};  //0-pi,-pi-pi,-pi-pi
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Vector3d::UnitX())); //x is roll
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Vector3d::UnitY())); //y is pitch
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); //z is yaw

Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;  //mat =   Rz * Ry *Rx


Eigen::Quaterniond q(rotation_matrix);
Eigen::Vector3d eulerAngle1=ToEulerAngles(q);
//注释：; 只要pitch在-pi/2 到pi/2 之间  **ToEulerAngles(q)和tf**  都没有问题，-0.5*pi《=pitch《=0.5*pi
//eulerAngle1= -0.25 0.68 -1.2

Guess you like