eigen欧拉角处理

Eigen的eulerAngles欧拉角转换

今天发现一个问题，有些旋转四元数利用eigen的eulerAngles转化出来的结果与ros的tf包的getRPY转化出来的结果不太一样，比如转化出来的yaw角中间差了$\pi$,比如ros转换为-0.1,eigen转化出来3.0415926;简单搜了下好像是eigen的转化有点问题，但是我又不想用弄ros，所以这里干脆自己重新弄了一个函数处理

static Eigen::Vector3d R2ypr(const Eigen::Matrix3d& R) {
        Eigen::Vector3d n = R.col(0);
        Eigen::Vector3d o = R.col(1);
        Eigen::Vector3d a = R.col(2);

        Eigen::Vector3d ypr(3);
        double y = atan2(n(1), n(0));
        double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
        double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
        ypr(0) = y;
        ypr(1) = p;
        ypr(2) = r;

        return ypr;
    }

这样就与ros的tf的getRPY的结果一样了。