Eigen四元数和欧拉角相互转换以及增量转换为四元数
因为loam的优化是基于欧拉角的,这不符合我下一步的操作目的,所以我决定将欧拉角的部分都替换为四元数。但是目前来看只替换优化部分效果很差,所以我决定将整个代码做一个修改。
以下,将用到过的知识记录一下。
// 将欧拉角转换为四元数
Eigen::AngleAxisf roll(transformTobeMapped[0], Eigen::Vector3f::UnitX());
Eigen::AngleAxisf pitch(transformTobeMapped[1], Eigen::Vector3f::UnitY());
Eigen::AngleAxisf yaw(transformTobeMapped[2], Eigen::Vector3f::UnitZ());
Eigen::Quaternionf quater;
quater = yaw * pitch * roll;//完成转换
下面将四元数转换为欧拉角
Eigen::Vector3f eulerAngle = quater.toRotationMatrix().eulerAngles(2,1,0);
//eulerAngle(0)为yaw
//eulerAngle(1)为pitch
//eulerAngle(2)为roll
下面记录几个功能函数
//取反对称矩阵
Eigen::Matrix3f SkewSymmetric(const Eigen::Vector3f &vec)
{
Eigen::Matrix3f mat;
mat << 0.0, -vec(2), vec(1),
vec(2), 0.0, -vec(0),
-vec(1), vec(0), 0.0;
return mat;
}
//将微小增量转换为四元数
Eigen::Quaternionf DeltaQ(const Eigen::Vector3f &_vec)
{
Eigen::Quaternionf quater;
quater.w() = 1.0;
quater.x() = _vec(0) / 2;
quater.y() = _vec(1) / 2;
quater.z() = _vec(2) / 2;
return quater;
}