【代码】四元数、RPY、旋转矩阵、so3之间的转换

 需要使用Eigen库

using Vec3 = Eigen::Matrix<double, 3, 1>;
using Quat = Eigen::Matrix<double, 4, 1>;
using Mat3 = Eigen::Matrix<double, 3, 3>;

namespace MyMathTool{

enum class CoordinateAxis {X, Y, Z};

Mat3 CoordinateRotation(CoordinateAxis axis, double theta){
    
    double s = std::sin(theta);
    double c = std::cos(theta);

    Mat3 R;

    if (axis == MyMathTool::CoordinateAxis::X) {
        R << 1, 0, 0, 0, c, s, 0, -s, c;
    } 
    else if (axis == MyMathTool::CoordinateAxis::Y) {
        R << c, 0, -s, 0, 1, 0, s, 0, c;
    } 
    else if (axis == MyMathTool::CoordinateAxis::Z) {
        R << c, s, 0, -s, c, 0, 0, 0, 1;
    }

    return R;
}


double square(const double a){return a*a;}


Mat3 quaternionToRotationMatrix(const Quat& q){
      
  double e0 = q(0);
  double e1 = q(1);
  double e2 = q(2);
  double e3 = q(3);

  Mat3 R;

  R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
      2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
      1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
      2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
      1 - 2 * (e1 * e1 + e2 * e2);
  R.transposeInPlace();
  return R;
}


Vec3 quatToRPY(const Quat& q){
        
    Vec3 rpy;
    double as = std::min(-2. * (q[1] * q[3] - q[0] * q[2]), .99999);
    rpy(2) =    std::atan2(2 * (q[1] * q[2] + q[0] * q[3]),
                square(q[0]) + square(q[1]) - square(q[2]) - square(q[3]));
    rpy(1) = std::asin(as);
    rpy(0) =
    std::atan2(2 * (q[2] * q[3] + q[0] * q[1]),
                square(q[0]) - square(q[1]) - square(q[2]) + square(q[3]));
    return rpy;
}



Vec3 quaternionToso3(const Quat quat){
      
    Vec3 so3;
    // Vec3 Zero;  Zero.setZero();
    so3[0] = quat[1];
    so3[1] = quat[2];
    so3[2] = quat[3];

    double theta =
    2.0 * asin(sqrt(so3[0] * so3[0] + so3[1] * so3[1] + so3[2] * so3[2]));

    if (fabs(theta) < 0.0000001) {
        so3.setZero();
        return so3;
    }
    so3 /= sin(theta / 2.0);
    so3 *= theta;

    return so3;
}

Quat so3ToQuat(const Vec3& so3){
     
    Quat quat;

    double theta = sqrt(so3[0] * so3[0] + so3[1] * so3[1] + so3[2] * so3[2]);

    if (fabs(theta) < 1.e-6) {
        quat.setZero();
        quat[0] = 1.;
        return quat;
    }
    quat[0] = cos(theta / 2.);
    quat[1] = so3[0] / theta * sin(theta / 2.);
    quat[2] = so3[1] / theta * sin(theta / 2.);
    quat[3] = so3[2] / theta * sin(theta / 2.);
    return quat;
}

Quat rpyToQuat(const Vec3& rpy){
        
    Mat3 R = rpyToRotMat(rpy);
    Quat Q = rotationMatrixToQuaternion(R);

    return Q;
}

Quat rotationMatrixToQuaternion(const Mat3& r1){
    
    Quat q;
    Mat3 r = r1.transpose();
    double tr = r.trace();

    if(tr > 0.0){
        double S = sqrt(tr + 1.0) * 2.0;
        q(0) = 0.25 * S;
        q(1) = (r(2, 1) - r(1, 2)) / S;
        q(2) = (r(0, 2) - r(2, 0)) / S;
        q(3) = (r(1, 0) - r(0, 1)) / S;       
    }
    else if ((r(0, 0) > r(1, 1)) && (r(0, 0) > r(2, 2))) {
        double S = sqrt(1.0 + r(0, 0) - r(1, 1) - r(2, 2)) * 2.0;
        q(0) = (r(2, 1) - r(1, 2)) / S;
        q(1) = 0.25 * S;
        q(2) = (r(0, 1) + r(1, 0)) / S;
        q(3) = (r(0, 2) + r(2, 0)) / S;
    } 
    else if (r(1, 1) > r(2, 2)) {
        double S = sqrt(1.0 + r(1, 1) - r(0, 0) - r(2, 2)) * 2.0;
        q(0) = (r(0, 2) - r(2, 0)) / S;
        q(1) = (r(0, 1) + r(1, 0)) / S;
        q(2) = 0.25 * S;
        q(3) = (r(1, 2) + r(2, 1)) / S;
    } else {
        double S = sqrt(1.0 + r(2, 2) - r(0, 0) - r(1, 1)) * 2.0;
        q(0) = (r(1, 0) - r(0, 1)) / S;
        q(1) = (r(0, 2) + r(2, 0)) / S;
        q(2) = (r(1, 2) + r(2, 1)) / S;
        q(3) = 0.25 * S;
    }

    return q;
}
   
}