使用qua.toRotationMatrix().eulerAngles(2, 1, 0)来得到的欧拉角可能不是我们所希望的,因为4元数可以对应俩个欧拉角,以下是wiki百科中所给出的优化方法。
#include <iostream>
#include <Eigen/Dense>
#include <pcl/pcl_macros.h>
using namespace Eigen;
using namespace std;
struct EulerAngles {
double roll, pitch, yaw;
};
EulerAngles ToEulerAngles(Quaterniond q);
int main()
{
Eigen::Matrix<float, 4, 4> transformation = Eigen::Matrix<float, 4, 4>::Identity();
Eigen::Quaterniond quaternion;
float yaw = M_PI / 2; // 弧度角
//float pitch = M_PI / 4; // 弧度角
//float roll = M_PI / 4; // 弧度角
float pitch = -M_PI; // 弧度角
float roll = -M_PI / 2; // 弧度角
//float pitch = M_PI / 2; // 弧度角
//float roll = 0; // 弧度角
quaternion = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
cout << "4元数w:" << endl << quaternion.w() << endl;
cout << "4元数x:" << endl << quaternion.x() << endl;
cout << "4元数y:" << endl << quaternion.y() << endl;
cout << "4元数z:" << endl << quaternion.z() << endl;
quaternion.w() = 0.00392036;
quaternion.x() = -0.00511095;
quaternion.y() = -0.613622;
quaternion.z() = 0.789573;
cout << "4元数:" << endl << quaternion.matrix() << endl;
//Eigen::Isometry3d iso = Eigen::Translation3d(1, 2, 3) * quaternion;
Eigen::Isometry3d iso = Eigen::Translation3d(2,1,0) * quaternion;
Eigen::Matrix4d res = iso.matrix();
cout << "等距映射:" << endl << res << endl;
// 从旋转矩阵构造四元数
Eigen::Matrix<double, 3, 3> rot;
rot = quaternion.matrix();
Eigen::Quaterniond qua(rot);
// 1,从四元数转换为欧拉角
Eigen::Vector3d euler = qua.toRotationMatrix().eulerAngles(2, 1, 0);
cout << "欧拉角:" << endl << euler << endl; //弧度单位
//=======================研究四元数与欧拉角转换是超过2*M_PI=============================//
//2,保证出来的欧拉角不会超过2PI
EulerAngles a = ToEulerAngles(qua);
cout << "优化后的欧拉角yaw:" << endl << a.yaw << endl; //弧度单位
cout << "优化后的欧拉角pitch:" << endl << a.pitch << endl; //弧度单位
cout << "优化后的欧拉角roll:" << endl << a.roll << endl; //弧度单位
return 0;
}
//https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
EulerAngles ToEulerAngles(Quaterniond q) {
EulerAngles angles;
// 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.roll = 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.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
angles.pitch = 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.yaw = std::atan2(siny_cosp, cosy_cosp);
return angles;
}
输出:
4元数x:
0.5
4元数y:
-0.5
4元数z:
-0.5
4元数:
-0.999915 8.15619e-05 -0.0128822
0.0124632 -0.246903 -0.968959
-0.0032597 -0.969039 0.246884
等距映射:
-0.999915 8.15619e-05 -0.0128822 2
0.0124632 -0.246903 -0.968959 1
-0.0032597 -0.969039 0.246884 0
0 0 0 1
欧拉角:
3.12913
0.00325971
-1.32133
优化后的欧拉角yaw:
3.12913
优化后的欧拉角pitch:
0.0032597
优化后的欧拉角roll:
-1.32133
网友评论