使用Eigen进行角度转换和旋转平移计算

// 定义roll、pitch和yaw角度（单位：弧度）
double roll = 0.1; // 示例值
double pitch = 0.2; // 示例值
double yaw = 0.3; // 示例值

// 计算四元数
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
q.normalize();
Eigen::Matrix4d rt1_ = Eigen::Matrix4d::Identity();
odom_rt1_.block<3, 3>(0, 0) = q.toRotationMatrix(); // 设置旋转部分
Eigen::Vector3d t1_(x, y, z); //x,y,z 为自己设置的数值
odom_rt1_.block<3, 1>(0, 3) = odom_t1_;

多个 Eigen::Matrix4d rt 进行计算
Eigen::Matrix4d rt_ans_ = rt1_ * rt2_ * rt3_ * … ;