围绕指定点旋转4x4变换矩阵T0，如何计算新位姿T1？

要围绕指定点center旋转初始位姿矩阵T0，需通过齐次变换的三步流程实现，核心是将旋转操作转换为坐标系平移+旋转+逆平移的组合变换：

变换原理 #

围绕任意点的旋转无法直接通过旋转矩阵作用于位姿，需要先将坐标系原点对齐到旋转中心，完成旋转后再还原坐标系位置，具体步骤：

1. 构造平移矩阵，将初始位姿的原点平移至旋转中心center（把世界坐标系转换为以center为原点的局部坐标系）；
2. 在局部坐标系下应用3x3旋转矩阵R（扩展为4x4齐次旋转矩阵）；
3. 构造逆平移矩阵，将坐标系原点移回原位置，得到最终位姿T1。

数学推导与代码实现 #

将上述步骤转换为4x4齐次矩阵运算，完整代码如下：

import numpy as np

T0 = np.eye(4)
R = np.array([[0.98480775, 0., 0.17364818],
              [0., 1., 0.],
              [-0.17364818, 0., 0.98480775]])
center = np.array([-2.00628613e-02, -1.26855529e+00, -3.45331795e+01])

# 1. 构造平移到旋转中心的齐次矩阵
T_trans = np.eye(4)
T_trans[:3, 3] = -center

# 2. 构造旋转的齐次矩阵（扩展3x3旋转矩阵到4x4）
T_rot = np.eye(4)
T_rot[:3, :3] = R

# 3. 构造逆平移矩阵（平移回原位置）
T_trans_inv = np.eye(4)
T_trans_inv[:3, 3] = center

# 计算最终位姿T1：按顺序相乘矩阵
T1 = T_trans_inv @ T_rot @ T_trans @ T0

关键说明 #

• 矩阵乘法顺序不可颠倒：齐次变换的顺序是从右到左对应变换的执行顺序，即先执行T_trans（平移到中心），再执行T_rot（旋转），接着执行T_trans_inv（平移回原位），最后作用于初始位姿T0。
• 当T0为单位矩阵时，T1表示世界坐标系中围绕center点旋转后的位姿；若T0是某个物体的位姿矩阵，T1则是该物体围绕center点旋转后的新位姿。

内容的提问来源于stack exchange，提问作者Florian Ludewig