【c++】eigen 实现位姿绕指定轴旋转

• 代码假设位姿和指定旋转轴均在同一坐标系下，不涉及坐标系变换

// 位姿绕指定轴旋转
// - pose: 要旋转的位姿
// - axis_position: 指定旋转轴的位置
// - axis_direction: 指定旋转轴的方向
// - rotate_angle: 旋转角，单位为弧度
void rotate_pose_around_axis(Eigen::Isometry3d& pose, const Eigen::Vector3d& axis_position, const Eigen::Vector3d& axis_direction, double rotate_angle) {
    // 定义指定轴的旋转向量
    Eigen::AngleAxisd rotate_vector = Eigen::AngleAxisd(rotate_angle, axis_direction);

    // 位姿的位置绕指定轴旋转
    Eigen::Vector3d position = pose.translation() - axis_position;
    position                 = rotate_vector * position + axis_position;
    pose.translation()       = position;

    // 位姿的朝向绕指定轴旋转
    pose.linear() = rotate_vector * pose.linear();
    // 等同于 pose.prerotate(rotate_vector);
}