需要使用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;
}
}