咨询：如何从图像计算相机到地面平面点的距离及OpenCV相关函数

核心原理梳理 #

我来一步步帮你拆解这个问题——本质上，我们需要把图像里的像素点反向投影到3D世界坐标系，找到它和地面平面的交点，再计算相机光心到这个交点的欧氏距离。整个流程需要结合内参、位姿参数完成坐标系的转换。

先明确已知参数的定义：

• 相机内参矩阵 K：包含焦距 fx, fy（像素单位）、主点坐标 cx, cy，形式为：

  [fx, 0, cx]
  [0, fy, cy]
  [0,  0,  1]
  

• 相机高度 H：相机光心到地面的垂直距离（单位要统一，比如都用米）
• 俯仰角 pitch：相机光轴与水平线的夹角（注意方向：通常我们把向下倾斜的角度设为正，后续旋转矩阵要对应这个定义）

具体计算步骤 #

1. 像素坐标转相机坐标系方向向量

首先把图像上的像素点 (u, v) 转换成相机坐标系下的归一化方向向量：

• 构造齐次像素坐标 [u, v, 1]^T
• 用内参矩阵的逆矩阵 K⁻¹ 左乘该坐标，得到归一化平面（相机坐标系Z=1平面）上的点 (x_c, y_c, 1)
• 这个点对应的就是相机光心指向该像素的射线方向（相机坐标系下）

2. 方向向量转换到世界坐标系

我们需要把相机坐标系的方向向量旋转到世界坐标系（设定地面为XY平面，Z轴垂直地面向上，相机光心初始位置为 (0, 0, H)）：

• 对应俯仰角 pitch（向下为正）的绕Y轴旋转矩阵：

  [cos(pitch),  0,  sin(pitch)]
  [     0,      1,      0     ]
  [-sin(pitch), 0,  cos(pitch)]
  

• 用这个旋转矩阵左乘相机坐标系的方向向量 (x_c, y_c, 1)，得到世界坐标系下的射线方向 (Xw_dir, Yw_dir, Zw_dir)

3. 求射线与地面的交点

地面平面的方程是 Z=0，射线的参数方程为：
X = t*Xw_dir, Y = t*Yw_dir, Z = H + t*Zw_dir
令 Z=0，解出参数 t：
t = -H / Zw_dir（注意：如果 Zw_dir >=0，说明射线向上倾斜，不会和地面相交，直接返回无效结果）

4. 计算距离

交点的世界坐标为 (t*Xw_dir, t*Yw_dir, 0)，相机光心到该点的欧氏距离就是：
distance = sqrt( (t*Xw_dir)^2 + (t*Yw_dir)^2 + H^2 )
（如果方向向量是归一化过的，直接用 |t| 作为距离结果也是一致的）

OpenCV相关实现 #

OpenCV没有直接一步到位计算这个距离的函数，但它提供了所有必需的基础工具来搭建计算流程：

• 用 cv::invert（C++）或 np.linalg.inv（Python）计算内参矩阵的逆
• 用 cv::Rodrigues 可以将俯仰角（旋转向量）转换为旋转矩阵（如果需要处理多轴旋转的话）
• 用矩阵乘法函数（如 cv::gemm 或Python的 @ 运算符）完成坐标转换

这里给一个Python + OpenCV的示例代码：

import cv2
import numpy as np

def get_ground_distance(u, v, K, camera_height, pitch_rad):
    # 1. 像素转归一化平面坐标
    pixel_homo = np.array([u, v, 1], dtype=np.float32)
    K_inv = np.linalg.inv(K)
    norm_point = K_inv @ pixel_homo
    x_c, y_c, _ = norm_point

    # 2. 构建俯仰角旋转矩阵（绕Y轴，向下为正）
    R = np.array([
        [np.cos(pitch_rad), 0, np.sin(pitch_rad)],
        [0, 1, 0],
        [-np.sin(pitch_rad), 0, np.cos(pitch_rad)]
    ], dtype=np.float32)

    # 3. 转换到世界坐标系方向
    camera_dir = np.array([x_c, y_c, 1], dtype=np.float32)
    world_dir = R @ camera_dir
    Xw, Yw, Zw = world_dir

    # 4. 判断是否与地面相交
    if Zw >= 0:
        print("该射线向上倾斜，无法到达地面")
        return None, None

    # 5. 计算交点和距离
    t = -camera_height / Zw
    intersection = np.array([t*Xw, t*Yw, 0], dtype=np.float32)
    distance = np.linalg.norm(intersection - np.array([0, 0, camera_height]))
    return distance, intersection

# 示例参数
# 内参矩阵：fx=1000, fy=1000, cx=500, cy=300
K = np.array([[1000, 0, 500], [0, 1000, 300], [0, 0, 1]], dtype=np.float32)
camera_h = 1.8  # 相机高度1.8米
pitch = np.deg2rad(25)  # 俯仰角25度向下

# 测试图像点
u, v = 500, 450
dist, point = get_ground_distance(u, v, K, camera_h, pitch)
if dist is not None:
    print(f"相机到地面点的距离：{dist:.2f} 米")
    print(f"地面交点世界坐标：{point}")

关键注意事项 #

• 俯仰角定义：一定要确认你的pitch角是向上还是向下为正，旋转矩阵的符号必须对应，否则结果会完全错误
• 单位统一：所有参数的单位要一致，比如高度用米，计算出的距离也是米
• 其他位姿参数：如果相机还有roll或yaw角，需要在旋转矩阵中加入对应的旋转分量
• 内参准确性：内参矩阵必须是经过标定的准确值，否则投影结果会有明显误差

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