使用sophus::so3d 和vector3d生成 se3d

Sophus库是一个用于处理三维数学变换，特别是旋转和平移（SE(3)）的C++库。`sophus::SO3d` 表示三维欧拉角、旋转矩阵或者旋转向量的表示，而 `vector3d` 则代表三维笛卡尔空间中的向量。

如果你想从 `vector3d` 创建一个 `se3d` 对象，即包含旋转和平移的 SE(3) 变换，你可以这样做：

#include <sophus/so3.hpp>
#include <sophus/se3.hpp>
#include <Eigen/Core>

// 假设你已经有了一个 vector3d 作为平移向量
Eigen::Vector3d translation = ...;

// 将平移向量转换为 Eigen::Matrix3d 形式，因为 SO3d 使用的是旋转矩阵
Eigen::Matrix3d rotation_matrix;
// 这里假设你有一个旋转角度或者旋转向量，将其转换为旋转矩阵
// （例如如果是绕Z轴旋转，可以使用 Eigen 的函数）
rotation_matrix = Sophus::angleAxisToRotationMatrix(angle);

// 现在你可以创建一个 se3d 变换
sophus::SE3d se3;
se3.setIdentity();
se3.translation() = translation;
se3.rotation() = rotation_matrix;

这里首先创建了一个 `SE3d` 的空实例 `se3`，然后设置了平移部分和旋转部分。如果你已经有现成的 `SO3d` 转换，也可以通过 `.setFromTwist()` 或者 `.fromTranslationAndRotation()` 来组合旋转和平移。