如何计算cv2.projectPoints生成点的间距及变换矩阵组合？

一、计算cv2.projectPoints生成点的距离 #

完全可以计算，需根据需求选择对应方式：

• 图像平面像素间距：直接对cv2.projectPoints输出的2D像素坐标（格式为(n,1,2)的numpy数组）计算欧氏距离。示例代码：

  import numpy as np
  # 假设p1_2d、p2_2d是projectPoints输出的单个点的2D坐标
  p1 = p1_2d[0][0]
  p2 = p2_2d[0][0]
  pixel_distance = np.linalg.norm(p1 - p2)
  

• 真实世界3D间距：不能用投影后的2D点计算（投影丢失深度信息），需先将两个点转换到同一坐标系（如相机系/世界系），再计算3D欧氏距离。转换逻辑见第二部分的矩阵运算，示例代码：

  # 假设world_p1、world_p2是转换到同一坐标系后的齐次坐标
  real_distance = np.linalg.norm(world_p1[:3] - world_p2[:3])
  

二、从四个变换矩阵推导T_point1_point2 #

首先明确变换矩阵的定义：统一采用右乘齐次坐标规则，即目标坐标 = 变换矩阵 @ 源坐标。假设你手上的四个矩阵是：

1. T_cam_A：ArUco标记A坐标系到相机坐标系的变换
2. T_cam_B：ArUco标记B坐标系到相机坐标系的变换
3. T_A_p1：点1相对于标记A坐标系的变换（通常是仅平移特定距离的4x4齐次矩阵）
4. T_B_p2：点2相对于标记B坐标系的变换

推导步骤 #

1. 计算点1到相机的变换：T_cam_p1 = T_cam_A @ T_A_p1
2. 计算相机到点2的变换：先求点2到相机的变换T_cam_p2 = T_cam_B @ T_B_p2，再取逆得到相机到点2的变换：T_p2_cam = np.linalg.inv(T_cam_p2)
3. 组合得到T_point1_point2：T_p1_p2 = T_p2_cam @ T_cam_p1（该矩阵可将点1坐标系下的点转换到点2坐标系下）

关键注意事项 #

• 若OpenCV返回的是旋转向量+平移向量，需先转成4x4齐次矩阵：

  def rvec_tvec_to_homogeneous(rvec, tvec):
      R, _ = cv2.Rodrigues(rvec)
      T = np.eye(4)
      T[:3, :3] = R
      T[:3, 3] = tvec.flatten()
      return T
  

• 矩阵乘法顺序不可颠倒（矩阵乘法不满足交换律），必须严格遵循“目标系 → 中间系 → 源系”的链式顺序。

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