四元数to变换矩阵
Eigen::Quaterniond quaternion(w, x, y, z);
Eigen::Matrix3d rotation_matrix;
rotation_matrix = quaternion.matrix();
transformation.rotate(rotation_matrix);
transformation.pretranslate(Eigen::Vector3d(tx, ty, tz));
cout << transformation.matrix() << endl;
变换矩阵to四元数
transfor为已知的变换矩阵,从icp配准得到的就是
//获取变换矩阵,求四元数q和位移t_vector
Eigen::Matrix4f transfor;
transfor = icp.getFinalTransformation();
Eigen::Matrix3f r_matrix = Eigen::Matrix3f::Identity();
for(int i=0;i<3;i++)
{
for(int j=0;j<3;j++)
{
r_matrix(i,j) = transfor(i,j);
}
}
Eigen::Vector3f t_vector(transfor(0,3),transfor(1,3),transfor(2,3));
Eigen::Quaternionf q;
q = Eigen::Quaternionf ( r_matrix );
欧拉角转换to旋转矩阵
参考:https://zhuanlan.zhihu.com/p/144032401
https://blog.csdn.net/shyjhyp11/article/details/111701127
Eigen库:
Eigen::Vector3d eulerAngle(1.0, 2.0, 3.0);//欧拉角
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix1; //旋转矩阵
rotation_matrix1 = yawAngle*pitchAngle*rollAngle;
//旋转矩阵to变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(rotation_matrix1);
T.pretranslate(Eigen::Vector3d(11, 3, 4));
cout << T.matrix() <<endl;
自己写:
Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta)
{
Eigen::Matrix3d R_x; // 计算旋转矩阵的X分量
R_x <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0]);
Eigen::Matrix3d R_y; // 计算旋转矩阵的Y分量
R_y <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1]);
Eigen::Matrix3d R_z; // 计算旋转矩阵的Z分量
R_z <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1;
Eigen::Matrix3d R = R_z * R_y * R_x;
return R;
}
旋转矩阵to欧拉角
eigen:
Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2,1,0); // ZYX顺序,yaw,pitch,roll
cout << "roll_1 pitch_1 yaw_1 = " << eulerAngle1[2] << " " << eulerAngle1[1]
<< " " << eulerAngle1[0] << endl << endl;
自己写:
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
assert(isRotationMatirx(R));
double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
bool singular = sy < 1e-6;
double x, y, z;
if (!singular)
{
x = atan2( R(2,1), R(2,2));
y = atan2(-R(2,0), sy);
z = atan2( R(1,0), R(0,0));
}
else
{
x = atan2(-R(1,2), R(1,1));
y = atan2(-R(2,0), sy);
z = 0;
}
return {x, y, z};
}
python版
https://blog.csdn.net/shyjhyp11/article/details/111701127
import numpy as np
import math
from scipy.spatial.transform import Rotation as R
Rq=[-0.71934025092983234, 1.876085535681999e-06, 3.274841213980097e-08, 0.69465790385533299]
# 四元数到旋转矩阵
r = R.from_quat(Rq)
Rm = r.as_matrix()
# 0:array([ 1.00000000e+00, -2.74458557e-06, 2.55936079e-06])
# 1:array([-2.65358979e-06, -3.49007932e-02, 9.99390782e-01])
# 2:array([-2.65358979e-06, -9.99390782e-01, -3.49007932e-02])
# 符号相反的四元数, 仍表示同一个旋转
Rq1= [0.71934025092983234, -1.876085535681999e-06, -3.274841213980097e-08, -0.69465790385533299]
# 四元数到旋转矩阵
r1 = R.from_quat(Rq1)
Rm1 = r1.as_matrix()
# 0:array([ 1.00000000e+00, -2.74458557e-06, 2.55936079e-06])
# 1:array([-2.65358979e-06, -3.49007932e-02, 9.99390782e-01])
# 2:array([-2.65358979e-06, -9.99390782e-01, -3.49007932e-02])
# 四元数到欧拉角
euler0 = r.as_euler('xyz', degrees=True)
# ([-9.20000743e+01, 1.52039496e-04, -1.52039496e-04])
euler3 = r.as_euler('xzy', degrees=True)
#([-9.20000743e+01, -1.52039496e-04, 1.52039496e-04])
euler1 = r.as_euler('zxy', degrees=True)
#([-179.99564367, -87.99992566, 179.99579836])
euler2 = r.as_euler('zyx', degrees=True)
#([ 1.57253169e-04, 1.46640571e-04, -9.20000743e+01])
euler4 = r.as_euler('yxz', degrees=True)
#([179.99564367, -87.99992566, 179.99549428])
euler5 = r.as_euler('yzx', degrees=True)
#([ 1.46640571e-04, 1.57253169e-04, -9.20000743e+01])
# 旋转矩阵到四元数
r3 = R.from_matrix(Rm)
qua = r3.as_quat()
#[0.7193402509298323, -1.8760855356819988e-06, -3.2748412139801076e-08, -0.694657903855333] #与原始相反,但等价
# 旋转矩阵到欧拉角
euler_1 = r3.as_euler('zxy', degrees=True)
#([-179.99564367, -87.99992566, 179.99579836])
# 欧拉角到旋转矩阵
r4 = R.from_euler('zxy', [-179.99564367, -87.99992566, 179.99579836], degrees=True)
rm = r4.as_matrix()
# 0:array([ 1.00000000e+00, -2.74452529e-06, 2.55936075e-06])
# 1:array([-2.65358765e-06, -3.49007933e-02, 9.99390782e-01])
# 2:array([-2.65352955e-06, -9.99390782e-01, -3.49007933e-02])
# 欧拉角到四元数
qua1 = r4.as_quat()
#([-7.19340251e-01, 1.87606384e-06, 3.27274889e-08, 6.94657904e-01])
#----测试--------------------------------------------------------------------
theta=[-116, 0. , -105]
r6 = R.from_euler('xyz', theta, degrees=True)
rm = r6.as_matrix()
# 0:array([-0.25881905, -0.42343401, 0.86816838])
# 1:array([-0.96592583, 0.1134588 , -0.23262502])
# 2:array([ 0. , -0.89879405, -0.43837115])
qua3 = r6.as_quat()
#array([-0.52720286, 0.68706415, -0.39667667, 0.30438071])
print(qua3)