- 代码假设位姿和指定旋转轴均在同一坐标系下,不涉及坐标系变换
// 位姿绕指定轴旋转
// - 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);
}