旋转矩阵转换欧拉角

#ifndef IMGPOSTCC_HPP
#define IMGPOSTCC_HPP

#include "Eigen/Dense"

class imgpostCC {
    //
public:
    void Demo(){
        //using namespace Eigen;

        double pin, rin, yin, pout, rout, yout;
        //初始化
        setCameraCaliPara(-90, 0, 0);

        pin = 0;
        rin = 45;
        yin = 0;
        //计算
        getRotationENUtoIMU(pin, rin, yin, pout, rout, yout);


#if 0
        //zxy
        Eigen::Vector3d v = printEulerAngles(imu_pry_R,2,0,1);
        printf("imu_pry_R: x:%.2f y:%.2f z:%.2f\r\n",
               v[1] * 180.0 / M_PI,
               v[2] * 180.0 / M_PI,
               v[0] * 180.0 / M_PI);

        //zyx
        v = printEulerAngles(cam_cali_R, 2, 1, 0);
        printf("cam_cali_R: x:%.2f y:%.2f z:%.2f\r\n",
               v[2] * 180.0 / M_PI,
               v[1] * 180.0 / M_PI,
               v[0] * 180.0 / M_PI);

        //zxy
        v = printEulerAngles(enu_cam_R,2,0,1);
        printf("enu_cam_R: pitch:%.2f roll:%.2f yaw:%.2f\r\n",
               v[1] * 180.0 / M_PI,
               v[2] * 180.0 / M_PI,
               v[0] * 180.0 / M_PI);
#endif

    }


    void setCameraCaliPara(double rx, double ry, double rz){
        Eigen::AngleAxisd rx_cam,ry_cam,rz_cam;
        rx_cam = Eigen::AngleAxisd(rx / 180.0 * M_PI, Eigen::Vector3d::UnitX());
        ry_cam = Eigen::AngleAxisd(ry / 180.0 * M_PI, Eigen::Vector3d::UnitY());
        rz_cam = Eigen::AngleAxisd(rz / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
        //z - y - x
        cam_cali_R = Eigen::AngleAxisd(rz_cam * ry_cam * rx_cam);
    }

    void getRotationENUtoIMU(double pitch, double roll, double yaw, double& p, double& r, double& y){
        //zxy
        Eigen::AngleAxisd rx_imu,ry_imu,rz_imu;
        rx_imu = Eigen::AngleAxisd(pitch / 180.0 * M_PI, Eigen::Vector3d::UnitX());
        ry_imu = Eigen::AngleAxisd(roll / 180.0 * M_PI, Eigen::Vector3d::UnitY());
        //西偏为正
        rz_imu = Eigen::AngleAxisd(-yaw / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
        //z - x - y
        imu_pry_R = Eigen::AngleAxisd(rz_imu * rx_imu * ry_imu);
        //R(enu2camera)=R(enu2imu)*R(imu2camera)
        enu_cam_R = Eigen::AngleAxisd(imu_pry_R * cam_cali_R);
        //enu2camera - z - x - y
        Eigen::Vector3d eulerAngle = enu_cam_R.matrix().eulerAngles(2,0,1);

        p = eulerAngle[1] * 180.0 / M_PI;
        r = eulerAngle[2] * 180.0 / M_PI;
        y = eulerAngle[0] * 180.0 / M_PI;
        printf("enu_cam_R: pitch:%.2f roll:%.2f yaw:%.2f\r\n", p, r, y);
    }

private:

    Eigen::Vector3d printEulerAngles(Eigen::AngleAxisd aa, int id0, int id1, int id2){
        //zxy
        Eigen::Vector3d eulerAngle = aa.matrix().eulerAngles(id0,id1,id2);
        return eulerAngle;
    }

public:
    Eigen::AngleAxisd cam_cali_R, imu_pry_R, enu_cam_R;
};

#endif // IMGPOSTCC_HPP