旋转矩阵
/** 四元数旋转矩阵 */
Eigen::Matrix3d quaternionToRotationMatrix(float quaternion[4]); // 四元数转换为旋转矩阵
Eigen::Matrix3d quaternionToRotationMatrix_buff(); // 四元数转换为旋转矩阵(BUFF)
Eigen::Matrix3d EulerAngle2RotationMatrix(double pitch,double roll,double yaw); // 解算旋转矩阵
quaternionToRotationMatrix函数
/**
* 函数名: quaternionToRotationMatrix
* 传入: 无
* 传出: Matrix3d R_x
* 功能: 通过电控发来的四元数,计算出旋转矩阵
*/
Eigen::Matrix3d AngleSolve::quaternionToRotationMatrix(float quaternion[4]) {
Eigen::Matrix3d R_x;
// 四元数
float w=quaternion[0],x=quaternion[1],y=quaternion[2],z=quaternion[3];
R_x << 1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w,
2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w,
2*x*z-2*y*w, 2*y*z+2*w*x, 1-2*x*x-2*y*y;
// float roll = atan2(2*y*z + 2*w*x,1 - 2*x*x - 2*y*y)/CV_PI * 180.0f;
// float pitch = asin(2*w*y - 2*x*z)/CV_PI*180.0f;
// float yaw = atan2(2*x*y + 2*w*z, 1 - 2*y*y - 2*z*z)/CV_PI*180.0f;
// std::cout<<"----------[quaternion_euler]-----------"<<std::endl;
// std::cout<<"[roll:] |"<<roll<<std::endl;
// std::cout<<"[pitch:] |"<<pitch<<std::endl;
// std::cout<<"[yaw:] |"<<yaw<<std::endl;
// std::cout<<"----------[get_from_euler]-----------"<<std::endl;
// std::cout<<"[get_roll:] |"<<ab_roll<<std::endl;
// std::cout<<"[get_pitch:] |"<<ab_pitch<<std::endl;
// std::cout<<"[get_yaw:] |"<<ab_yaw<<std::endl;
return R_x;
}
我们这里的四元数转换旋转矩阵，使用的是Hamilton四元数，四元数到旋转矩阵的推导有兴趣的可以去推导
我们这里直接将公式拿过来使用
EulerAngle2RotationMatrix函数
这个函数是为了解决一些内部需要的旋转，并没有怎么用到，这个函数在整车观测的时候是可以用到的，可以简化代码的写法以及计算量
缺点是使用起来会有一定的难度，需要你对坐标系有不错的理解
/**
* 函数名: EulerAngle2RotationMatrix
* 传入: double pitch, double roll, double yaw (pitch角度,roll角度,yaw角度)
* 传出: Eigen::Matrix3d R (旋转矩阵)
* 功能: 通过传入的欧拉角求出旋转矩阵
* PS: 为什么不使用静态欧拉角,因为计算次数过多,只要不传入两个90度就不会出现万向锁问题
*/
Eigen::Matrix3d AngleSolve::EulerAngle2RotationMatrix(double pitch, double roll, double yaw) {
// 弧度制
pitch *= CV_PI/180.0f;
roll *= CV_PI/180.0f;
yaw *= CV_PI/180.0f;
Eigen::Matrix3d R_x; // 计算旋转矩阵的X分量
R_x <<
1, 0, 0,
0, cos(pitch), -sin(pitch),
0, sin(pitch), cos(pitch);
Eigen::Matrix3d R_y; // 计算旋转矩阵的Y分量
R_y <<
cos(roll), 0, sin(roll),
0, 1, 0,
-sin(roll), 0, cos(roll);
Eigen::Matrix3d R_z; // 计算旋转矩阵的Z分量
R_z <<
cos(yaw), -sin(yaw), 0,
sin(yaw), cos(yaw), 0,
0, 0, 1;
// TODO: 特别注意左乘的顺序 R_z * R_y * R_x 是先 *R_x
Eigen::Matrix3d R = R_z * R_y * R_x;
// Eigen::Matrix3d R = R_x * R_y * R_z; // R_x * R_y * R_z 是先 *R_z
return R;
}
这里的解算也是比较简单的，但是左乘的顺序不一样会影响旋转的顺序，这点需要特别注意
你也可以单独对这个函数进行测试，只要自己规定一个坐标，比如（1，1，1）就可以看到结果了
如果你想要可视化，使用Python去显示结果是个不错的选择
你想直观的看到旋转的过程吗，那么运行看看下面的代码把：
import plotly.graph_objects as go
import numpy as np
x, y, z = [1],[0],[0]
pitch = 0 * 3.1415926/180
roll = 45 * 3.1415926/180
yaw = 45 * 3.1415926/180
Rx = np.array([[1, 0, 0],
[0, np.cos(pitch), -np.sin(pitch)],
[0, np.sin(pitch), np.cos(pitch)]])
Ry = np.array([[np.cos(roll), 0, np.sin(roll)],
[0, 1, 0],
[-np.sin(roll), 0, np.cos(roll)]])
Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]])
R = np.matmul(np.matmul(Rz,Ry),Rx)
xyz = np.matmul(R,np.array([[x[0]],
[y[0]],
[z[0]]]))
print(xyz)
x.append(xyz[0][0])
y.append(xyz[1][0])
z.append(xyz[2][0])
fig = go.Figure(data=[go.Scatter3d(
x=x, # 设置3个不同的坐标数据
y=y,
z=z,
mode='markers')])
# 添加3D坐标轴
fig.add_trace(go.Scatter3d(
x=[0, 2], y=[0, 0], z=[0, 0],
mode='lines',
line=dict(color='red', width=2)
))
fig.add_trace(go.Scatter3d(
x=[0, 0], y=[0, 2], z=[0, 0],
mode='lines',
line=dict(color='green', width=2)
))
fig.add_trace(go.Scatter3d(
x=[0, 0], y=[0, 0], z=[0, 2],
mode='lines',
line=dict(color='blue', width=2)
))
# 设置坐标轴标签
fig.update_layout(
scene=dict(
xaxis_title='X轴',
yaxis_title='Y轴',
zaxis_title='Z轴'
)
)
# 显示图形
fig.show()
这里可以自己调整旋转
x, y, z = [1],[0],[0]
pitch = 0 * 3.1415926/180
roll = 45 * 3.1415926/180
yaw = 45 * 3.1415926/180
对应的角度和坐标，xyz也就是这里的（1，0，0）是原坐标点，下面的三轴角度可以自己修改
这里的结果和你预期一样吗？红线是x轴，绿线是y轴，蓝线是z轴
