eigen Quaterniond与sophus so3互相转换

Eigen是一个C++模板库，主要用于线性代数运算，而 Sophus 是一个C++库，专注于数学优化，特别是用于处理SE3（三维欧拉变换）和SO3（旋转群）。Quaterniond是Eigen库中表示四元数的数据类型，它是复数的扩展，用于描述三维空间中的旋转变换。

Sophus中的`SO3`则专门处理三维旋转，它通常会提供一些特定于旋转的操作，如构造旋转矩阵、乘法、逆等。

要将两者互相转换，你可以这样做：

1. **从Eigen的Quaterniond到Sophus的SO3**:
- Eigen的Quaterniond有一个成员函数`toRotationMatrix()`，可以将其转换为一个旋转矩阵。
- 使用Sophus的`SO3::expmap()`函数，可以从旋转矩阵计算出对应的SO3对象。

2. **从Sophus的SO3到Eigen的Quaterniond**:
- 如果你已经有Sophus的SO3实例，可以先获取其旋转矩阵，然后用Eigen的`Quaterniond::fromRotationMatrix()`函数从矩阵生成四元数。

以下是伪代码示例：

// Eigen Quaterniond to SO3
Eigen::Quaterniond quat = ...;
Eigen::Matrix3f rotationMat = quat.toRotationMatrix();
 Sophus::SO3 R(rotationMat);

// SO3 to Quaterniond
const Sophus::SO3& sO3 = ...;
Eigen::Matrix3f mat = sO3.matrix();
Eigen::Quaterniond quatFromSO3(mat);