Eigen四元数和欧拉角相互转换以及增量转换为四元数

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;
    }