c++中eigen Quaterniond与sophus so3互相转换的直接函数

在C++中，Eigen库是一个用于线性代数操作的强大工具，特别是对于数值计算和矩阵处理。而Sophus库则专注于旋转群的数学表示，如SO(3)代表三维空间中的旋转。

Eigen库中的Quaterniond是一种四元素的复数，用于表示三维空间中的旋转变换。Sophus库中通常使用SE3或SO3数据结构来表示旋转部分，其中SO3专门表示纯旋转。

Eigen到Sophus的转换通常是这样的：
1. Eigen的Quaterniond包含了一个旋转和一个平移分量。如果你只需要纯旋转部分，你需要提取出旋转部分。这可以通过`quaternion().coeff()`访问各个分量，然后构造一个Sophus::Rot3对象。

#include <Eigen/Eigen>
#include < Sophus/Sophus.hpp>

Eigen::Quaterniond eigen_quat;
Sophus::RotationMatrix<3> sophus_rot;

// 提取旋转部分
sophus_rot = Sophus::fromEigen(eigen_quat.rotation());

2. 要从Sophus的SO3转换回Eigen的Quaterniond，你可以这样做：

Sophus::SO3< double > sophus_so3;
Eigen::Quaterniond eigen_quat_from_sophus;

// 将Sophus SO3转换为Eigen Quaterniond
eigen_quat_from_sophus = Eigen::Quaterniond::FromAngleAxis(sophus_so3.angle(), sophus_so3.axis());

注意这里假设了SO3已经转化为旋转轴和角度的形式。