生成符合指定4×4形式的齐次变换矩阵列表的高效方法

针对你需要从形状为[n,3]的数组a生成对应4×4齐次变换矩阵列表的需求，以下是几种最高效的实现方式：

• 向量化Numpy实现（首选）
  利用Numpy的广播与向量化运算特性，完全避免Python层面的循环，借助底层C级别的运算逻辑实现最高效率。

  代码示例：

  import numpy as np
  
  def generate_homogeneous_matrices(a):
      n = a.shape[0]
      # 预分配[n,4,4]形状的矩阵数组
      mats = np.zeros((n, 4, 4))
      # 批量计算所有角度的cos和sin值
      cos_vals = np.cos(a[:, 0])
      sin_vals = np.sin(a[:, 0])
      
      # 填充旋转和平移元素
      mats[:, 0, 0] = cos_vals
      mats[:, 0, 1] = -sin_vals
      mats[:, 0, 3] = a[:, 1]
      
      mats[:, 1, 0] = sin_vals
      mats[:, 1, 1] = cos_vals
      mats[:, 1, 3] = a[:, 2]
      
      # 设置固定的单位行
      mats[:, 2, 2] = 1.0
      mats[:, 3, 3] = 1.0
      
      return mats
  

• 矩阵块组合实现（兼顾可读性与效率）
  将旋转矩阵块和平移向量分开构建，再组合为完整的齐次矩阵，代码结构更清晰，效率与第一种方法接近：

  import numpy as np
  
  def generate_homogeneous_matrices(a):
      n = a.shape[0]
      # 批量构建2×2旋转矩阵
      rot_block = np.stack([
          np.cos(a[:,0]), -np.sin(a[:,0]),
          np.sin(a[:,0]), np.cos(a[:,0])
      ], axis=1).reshape(n, 2, 2)
      # 提取平移向量部分
      trans_vec = a[:, 1:3]
      
      # 初始化单位齐次矩阵数组
      mats = np.eye(4)[np.newaxis, :, :].repeat(n, axis=0)
      # 替换对应矩阵块
      mats[:, :2, :2] = rot_block
      mats[:, :2, 3] = trans_vec
      
      return mats
  

• 需要避免的低效方式
  不要使用Python原生for循环逐个生成矩阵，这种方式在n较大时性能会显著下降：

  # 低效示例，不推荐
  import numpy as np
  
  def generate_homogeneous_matrices(a):
      mats = []
      for theta, tx, ty in a:
          mat = np.array([
              [np.cos(theta), -np.sin(theta), 0, tx],
              [np.sin(theta), np.cos(theta), 0, ty],
              [0,0,1,0],
              [0,0,0,1]
          ])
          mats.append(mat)
      return np.array(mats)
  

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