如何利用相机内参外参及已知尺寸计算物体与相机的距离

问题描述 #

我已经通过OpenCV完成相机标定，得到了相机的内参矩阵（intrinsic matrix）和外参矩阵（extrinsic matrix）。请问结合这些参数以及物体的已知尺寸，能否计算出物体与相机之间的距离？目前我已获取了一组像素点对应的世界坐标，相关代码如下：

# Given point in image coordinates (in pixels)
image_point = np.array([1232, 552, 1])  # Replace with your actual x, y pixel coordinates

# Given intrinsic matrix K
K = np.array([
   [[2947.2353515625, 0.0, 2059.852294921875], [0.0, 2947.2353515625, 1039.6634521484375], [0.0, 0.0, 1.0]]
])  # Replace fx, fy, cx, cy with your actual intrinsic parameters
# or you can get it using device.readCalibration().getCameraIntrinsics()

# Invert the intrinsic matrix
K_inv = np.linalg.inv(K)

# Convert to normalized camera coordinates
normalized_camera_point = K_inv @ image_point

# Given CAM_to_WORLD transformation matrix
CAM_to_WORLD = np.array([[ 0.99475127,  0.01237097,  0.10157198,  0.08476004],
 [ 0.01436607,  0.96594656, -0.25834254,  0.14530805],
 [-0.10130906,  0.25844576,  0.96069884, -0.28102537],
 [ 0.,          0. ,         0. ,         1.,        ]])  # Replace with your actual transformation matrix

# Transform to world coordinates
homogeneous_world_point = CAM_to_WORLD @ np.append(normalized_camera_point, 1)

# Convert to Cartesian coordinates
world_point = homogeneous_world_point[:3] / homogeneous_world_point[3]

print("World Point:", world_point)

问题解答 #

核心结论 #

结合内参、外参和物体已知尺寸，完全可以计算物体与相机的距离，分两种场景处理：

场景1：已知像素点对应的真实世界坐标

如果已经明确图像中某像素点对应的真实世界坐标，直接计算相机光心到该世界点的欧氏距离即可：

1. 从相机到世界的变换矩阵CAM_to_WORLD中提取相机光心的世界坐标：该矩阵最后一列的前三个元素就是相机光心在世界坐标系中的位置。
2. 用欧氏距离公式计算两点间距。

修改后的代码示例（修正内参维度并添加距离计算）：

# 修正内参矩阵为标准3x3维度
K = np.array([
    [2947.2353515625, 0.0, 2059.852294921875],
    [0.0, 2947.2353515625, 1039.6634521484375],
    [0.0, 0.0, 1.0]
])

image_point = np.array([1232, 552, 1])
K_inv = np.linalg.inv(K)
normalized_camera_point = K_inv @ image_point

CAM_to_WORLD = np.array([[ 0.99475127,  0.01237097,  0.10157198,  0.08476004],
 [ 0.01436607,  0.96594656, -0.25834254,  0.14530805],
 [-0.10130906,  0.25844576,  0.96069884, -0.28102537],
 [ 0.,          0. ,         0. ,         1.,        ]])

# 替换为你已获取的真实世界坐标
real_world_point = np.array([...])

# 获取相机光心的世界坐标
camera_center = CAM_to_WORLD[:3, 3]

# 计算相机到物体点的距离
distance = np.linalg.norm(real_world_point - camera_center)
print("相机到物体点的距离:", distance)

场景2：仅知物体真实尺寸，未知世界坐标

如果只有物体的真实尺寸（比如物体宽度为real_width米），可以通过图像中物体的像素宽度pixel_width结合内参计算距离：

• 利用相似三角形原理，距离公式为：

fx = K[0, 0]  # 内参矩阵中的x方向焦距（像素单位）
distance = (real_width * fx) / pixel_width

• 注意：该公式适用于物体平面与相机光轴垂直的情况；若物体有倾斜，需要结合外参的旋转矩阵修正角度影响，计算实际深度。

代码误区说明 #

原代码中直接将归一化相机坐标（本质是相机坐标系下的射线方向向量）乘外参得到的世界点，是深度为1的虚拟点，并非物体的真实世界坐标——单像素点本身无法确定深度，必须结合物体尺寸或已知世界坐标才能得到真实位置。

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