空间坐标系转换矩阵计算

1. 基本概念

在三维空间中，两个坐标系之间的转换通常包括旋转和平移两部分。假设有坐标系 $A$ 和坐标系 $B$，我们需要将点 $P$ 在 $B$ 中的坐标 $(x_B,y_B,z_B)$ 转换为在 $A$ 中的坐标 $(x_A,y_A,z_A)$。

2. 旋转矩阵

旋转矩阵 $R$ 是一个 $3\times 3$ 的正交矩阵，表示从坐标系 $B$ 到坐标系 $A$ 的旋转关系。其列向量是 $B$ 的坐标轴在 $A$ 中的单位方向向量。例如：
$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$
其中：

• 第一列是 $B$ 的 $x$ 轴在 $A$ 中的方向向量
• 第二列是 $B$ 的 $y$ 轴在 $A$ 中的方向向量
• 第三列是 $B$ 的 $z$ 轴在 $A$ 中的方向向量

旋转矩阵满足正交性：$R^{T}R=I$，且 $\det (R)=1$。

3. 平移向量

平移向量 $T$ 是一个 $3\times 1$ 的列向量，表示坐标系 $B$ 的原点在坐标系 $A$ 中的坐标：
$$T=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$$

4. 齐次变换矩阵

为统一表示旋转和平移，通常使用 $4\times 4$ 的齐次变换矩阵：
$$H=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ r_{31}& r_{32}& r_{33}& t_z\\ 0& 0& 0& 1\end{array}\right]$$

5. 坐标转换公式

点 $P$ 在坐标系 $B$ 中的齐次坐标 ${\mathbf{P}}_B=\left[\begin{array}{c}{x}_B\\ y_B\\ z_B\\ 1\end{array}\right]$，转换到坐标系 $A$ 的公式为：
$${\mathbf{P}}_A=H\cdot {\mathbf{P}}_B$$
展开为：
$$\left[\begin{array}{c}{x}_A\\ y_A\\ z_A\\ 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ r_{31}& r_{32}& r_{33}& t_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_B\\ y_B\\ z_B\\ 1\end{array}\right]$$

6. 逆变换

从坐标系 $A$ 到 $B$ 的逆变换矩阵为：
$$H^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}T\\ 0& 1\end{array}\right]$$

7. 示例计算

假设坐标系 $B$ 相对于 $A$ 绕 $z$ 轴旋转 $90°$，且原点在 $A$ 中的坐标为 $(1,2,3)$：

• 旋转矩阵 $R$（绕 $z$ 轴）：
  $$R=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$$
• 平移向量 $T$：
  $$T=\left[\begin{array}{c}1\\ 2\\ 3\end{array}\right]$$
• 齐次变换矩阵 $H$：
  $$H=\left[\begin{array}{cccc}0& -1& 0& 1\\ 1& 0& 0& 2\\ 0& 0& 1& 3\\ 0& 0& 0& 1\end{array}\right]$$

若点 $P$ 在 $B$ 中坐标为 $(1,0,0)$，则在 $A$ 中的坐标为：
$${\mathbf{P}}_A=H\cdot \left[\begin{array}{c}1\\ 0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}1\\ 3\\ 3\\ 1\end{array}\right]$$

---

空间坐标系转换矩阵计算

空间坐标系转换通常涉及平移、旋转和缩放操作。以下代码示例演示如何构建一个 4x4 的齐次变换矩阵，用于将点从一个坐标系转换到另一个坐标系。

import numpy as npdef translation_matrix(tx, ty, tz):"""创建平移矩阵"""return np.array([[1, 0, 0, tx],[0, 1, 0, ty],[0, 0, 1, tz],[0, 0, 0, 1]])def rotation_matrix_x(theta):"""绕X轴旋转矩阵"""c = np.cos(theta)s = np.sin(theta)return np.array([[1, 0, 0, 0],[0, c, -s, 0],[0, s, c, 0],[0, 0, 0, 1]])def rotation_matrix_y(theta):"""绕Y轴旋转矩阵"""c = np.cos(theta)s = np.sin(theta)return np.array([[c, 0, s, 0],[0, 1, 0, 0],[-s, 0, c, 0],[0, 0, 0, 1]])def rotation_matrix_z(theta):"""绕Z轴旋转矩阵"""c = np.cos(theta)s = np.sin(theta)return np.array([[c, -s, 0, 0],[s, c, 0, 0],[0, 0, 1, 0],[0, 0, 0, 1]])def scale_matrix(sx, sy, sz):"""缩放矩阵"""return np.array([[sx, 0, 0, 0],[0, sy, 0, 0],[0, 0, sz, 0],[0, 0, 0, 1]])def compose_transform(translation, rotation, scale):"""组合平移、旋转和缩放矩阵"""T = translation_matrix(*translation)R = rotation_matrix_x(rotation[0]) @ rotation_matrix_y(rotation[1]) @ rotation_matrix_z(rotation[2])S = scale_matrix(*scale)return T @ R @ Sdef transform_point(point, transform_matrix):"""应用变换矩阵到点"""# 将点转换为齐次坐标homogeneous_point = np.append(point, 1)# 应用变换transformed_point = transform_matrix @ homogeneous_point# 转换回3D坐标return transformed_point[:3]# 示例使用
translation = (2, 3, 5)
rotation = (np.pi/4, np.pi/6, np.pi/3)  # 绕X,Y,Z轴旋转角度
scale = (1, 1, 1)  # 不缩放transform = compose_transform(translation, rotation, scale)
point = np.array([1, 2, 3])
transformed_point = transform_point(point, transform)print("原始点:", point)
print("变换矩阵:\n", transform)
print("变换后的点:", transformed_point)

变换矩阵的数学基础

3D空间中的变换通常使用4x4齐次变换矩阵表示，其一般形式如下：

$$\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& t_x\\ R_{21}& R_{22}& R_{23}& t_y\\ R_{31}& R_{32}& R_{33}& t_z\\ 0& 0& 0& 1\end{array}\right]$$

其中R是3x3旋转矩阵部分，t是平移向量。

变换顺序的重要性

变换的顺序会影响最终结果。通常遵循缩放(S)→旋转®→平移(T)的顺序。代码中的compose_transform函数使用了这种顺序：

• 首先应用缩放
• 然后应用旋转
• 最后应用平移

扩展功能

如果需要更复杂的变换，可以考虑添加以下功能：

• 支持任意轴旋转
• 支持剪切变换
• 支持透视投影
• 支持矩阵求逆运算

欢迎关注交流与指正！

相关文章：

• matlab实现：https://blog.csdn.net/sinat_29886521/article/details/77506426