目录
3. 欧拉角和旋转矩阵可同样表示刚体在三维空间的旋转,下面分享这两者互相转换的方法和核心代码
0. Eigen/四元数/欧拉角/旋转矩阵 相关系列文章
1. SLAM——之Eigen入门(矩阵运算及几何模块)
2. [SLAM——之Eigen函数库,一个相对复杂的EIgen使用实例]
3. [SLAM——Eigen函数库:矩阵块运算,block操作]
4. [欧拉角和旋转矩阵相互转换]
5. [四元数与三维向量相乘运算]
6. [四元数求导]
1.参考资料
2.变换矩阵/F/H的svd分解或者旋转矩阵、平移矩阵求解
- 欧拉角转旋转矩阵
欧拉角通过将刚体绕过原点的轴(i,j,k)旋转θ,分解成三步,如下图(蓝色是起始坐标系,而红色的是旋转之后的坐标系) ,12种方法的其中一种分解示意图!
如果将每一个角度用旋转矩阵表示如下:
所以,容易得到,欧拉角转旋转矩阵如下:
- 旋转矩阵转欧拉角
将旋转矩阵表示如下: -
则可以如下表示欧拉角:
3. 欧拉角和旋转矩阵可同样表示刚体在三维空间的旋转,下面分享这两者互相转换的方法和核心代码
- 欧拉角转旋转矩阵
/**
欧拉角计算对应的旋转矩阵
**/
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// 计算旋转矩阵的X分量
Mat R_x = (Mat_<double>(3,3) <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0])
);
// 计算旋转矩阵的Y分量
Mat R_y = (Mat_<double>(3,3) <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1])
);
// 计算旋转矩阵的Z分量
Mat R_z = (Mat_<double>(3,3) <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1);
// 合并
Mat R = R_z * R_y * R_x;
return R;
}
- 旋转矩阵转欧拉角
/*** 功能: 检查是否是旋转矩阵**/
bool isRotationMatrix(Mat &R)
{
Mat Rt;
transpose(R, Rt);
Mat shouldBeIdentity = Rt * R;
Mat I = Mat::eye(3,3, shouldBeIdentity.type());
return norm(I, shouldBeIdentity) < 1e-6;
}
/**
* 功能: 通过给定的旋转矩阵计算对应的欧拉角**/
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
assert(isRotationMatrix(R));
float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) + R.at<double>(1,0) * R.at<double>(1,0) );
bool singular = sy < 1e-6; // If
float x, y, z;
if (!singular) {
x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
y = atan2(-R.at<double>(2,0), sy);
z = atan2(R.at<double>(1,0), R.at<double>(0,0));
} else {
x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
y = atan2(-R.at<double>(2,0), sy);
z = 0;
}
return Vec3f(x, y, z);
}
程序运行结果展示:
参考
1. 欧拉角详解
2. 欧拉角
暂做记录,后续补充