python/opencv利用两个2D图像像素坐标测量3D世界坐标的真实距离

---

前言及参考资料

最近在做一个项目，使用摄像头去测量两个像素点之间的真实距离，我之前并未接触过相关知识，我查了大量的资料完成了简化版的测距问题，就是要求所有点在Z轴方向（深度方向）等于0,就是相当于在一个平面上求两个像素点之间的距离，因为我拍摄的所有照片都是使用我手机摄像头进行拍摄的，只有单个摄像头，无法获取深度方面的信息，我通过查阅资料得知单个摄像头是无法准确测量3D世界坐标的真实位置的，需要两个摄像头或者一个摄像头加一个深度传感器才能获知准确的3D世界坐标，所以以下代码是简化版本，仅仅在一个平面内适用，后续项目我应该会继续研究，现在的方案是一个摄像头加一个深度传感器去获取准确的3D世界坐标，后续如果研究明白的话，我会再写一篇博客。(注：我发现如果有深度信息的话（Z轴坐标），求3D世界坐标的公式跟现在的没啥太大区别，之前我把Z=0直接默认传入了，现在把Z轴坐标加上就可以了，我直接就在本片博客改了)

利用两个2D图像像素坐标测量3D世界坐标的真实距离，需要以下三个步骤：

1. 相机标定（求解相机内部参数，如焦距、畸变等）
2. 相机位姿估计（通过solvePnP函数求解相机坐标系关于世界坐标系的旋转矩阵R与平移矩阵T）
3. 根据数学公式的推倒求解出图像像素在世界坐标中的真实位置，然后进行测距

参考资料：

• OpenCV基础（28）使用OpenCV进行摄像机标定Python和C++：这篇博客主要讲了相机标定的问题，它翻译了一篇英文的博客，看起来应该是官方教程，我发现我查的资料以及chatgpt等大模型关于相机标定的代码基本相同，可以多找几篇对比一下代码，网上有很多，这个博客有Python和C++的相机标定代码。
• 相机标定之张正友标定法数学原理详解（含python源码）：这篇文章写了非常详细的相机标定的数学推倒过程，有兴趣的同学可以看一看，最后也给出了利用python写出的相机标定的代码。
• 【OpenCV】OpenCV-Python实现相机标定+利用棋盘格相对位姿估计：这篇文章给出了相机标定的代码，并给出了实时解算棋盘格位姿的代码，它利用的是摄像头，我修改了这篇文章的代码，因为我输入的都是手机摄像头拍摄的照片，所以我把摄像头相关的代码，修改成了输入照片的形式。（这边文章说他踩了个小坑，一开始用的是电脑的相机应用拍摄了20张照片，分辨率为1280×720。进行后面的步骤都没什么问题，但是测得的结果怎么都不准。这是因为这颗摄像头录像和拍照的分辨率不一致，因此编写一个小程序来获得标定所用的照片。我最开始已经看到了这篇文章，但是我还是踩了同样的坑，我在使用手机拍摄照片的时候，首先要使用专业模式，固定好焦距等数值，因为如果使用普通模式，手机现在会自动对焦、改变亮度等功能，会导致照片最后是由不同的焦距等数值拍摄出来的，其次就是我发现就是固定了专业模式的数值之后，手机还是会自动调整照片的分辨率，导致拍出来的照片分辨率是不一样的，所以同学们在进行实验的时候一定要好好看看自己照片的格式，我就是在这个地方耽误了不少时间）
• 计算机视觉中的坐标系之间的转换原理，以及OpenCV坐标原点的确定：这篇文章没有代码，仅仅讲了视觉中的坐标系之间的转换原理，但是其中的小孔成像原理我感觉还是需要看一看，虽然很简单，但是像我这种以前并没有接触过的人里说，确实不知道会用到小孔成像。
• c++/opencv利用相机位姿估计实现2D图像像素坐标到3D世界坐标的转换：这篇文章使用C++实现的图像像素坐标转换为真实世界坐标的过程，做的是做自动泊车项目中的车位线检测，这篇文章推到了像素坐标到世界坐标的数学过程，我编写的第三步代码就是通过这篇文章完成的，而且他写的其实已经很完整了，都是用C++写的，但是我很菜，我不懂C++，但是我需要看他的代码，我就用chatgpt翻译了他的c++代码，然后进行看的。
• 完整代码的github链接

---

相机标定

import cv2
import numpy as np
import glob
import json

# 参数设置
CHECKERBOARD = (9, 6)  # 内角点数量
square_size = 24.375  # 单位：mm（需实际测量）
images = glob.glob('calib_images/*.jpg')

# 生成三维点
objp = np.zeros((CHECKERBOARD[0] * CHECKERBOARD[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2) * square_size

# 存储标定数据
obj_points = []
img_points = []

# 处理所有图片
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    ret, corners = cv2.findChessboardCorners(gray, CHECKERBOARD, None)
    if ret:
        # 亚像素优化
        corners_sub = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
                                       (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))
        obj_points.append(objp)
        img_points.append(corners_sub)

# 标定相机
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    obj_points, img_points, gray.shape[::-1], None, None)

# 计算重投影误差
mean_error = 0
for i in range(len(obj_points)):
    imgpoints2, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(img_points[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
    mean_error += error
print(f"平均重投影误差: {mean_error / len(obj_points):.4f} 像素")


def save_para(filename, matrix, dist):
    camera_para_dict = {"mtx": matrix, "dist": dist}
    temp = json.dumps(camera_para_dict, indent=4)
    with open(filename, 'w') as json_file:
        json_file.write(temp)


# 保存参数
save_para("camera_intr_opt.json", mtx.tolist(), dist.tolist())

---

相机位姿估计

import cv2
import numpy as np
import math
import os
import json


def load_para(json_path):
    assert os.path.exists(json_path), json_path + " does not exist."
    json_file = open(json_path, 'r')
    para = json.load(json_file)
    return para


para = load_para("camera_intr_opt.json")

mtx, dist = np.array(para["mtx"]), np.array(para["dist"])

# 3D 真实世界坐标系的点（单位 mm, 确保单位一致）
square_size = 24.375  # 24.375mm 每格
objp = np.zeros((9 * 6, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2) * square_size  # 计算棋盘格点的 3D 坐标
obj_points = objp  # 存储3D点

# 读取棋盘格照片
img = cv2.imread("./test.jpg")
if img is None:
    print("无法读取图片，请检查文件路径！")
    exit()

gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

# 检测棋盘格角点
ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)

if ret:
    img_points = np.array(corners)

    # 在图片上绘制棋盘格角点
    cv2.drawChessboardCorners(img, (9, 6), corners, ret)

    # 解算位姿（求解旋转向量和平移向量）
    _, rvec, tvec = cv2.solvePnP(obj_points, img_points, mtx, dist)

    # 计算距离（单位 cm）
    distance = math.sqrt(tvec[0] ** 2 + tvec[1] ** 2 + tvec[2] ** 2) / 10.0

    # 旋转向量 -> 旋转矩阵
    rvec_matrix = cv2.Rodrigues(rvec)[0]

    # 组合投影矩阵
    proj_matrix = np.hstack((rvec_matrix, tvec))

    # 计算欧拉角
    eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6]
    pitch, yaw, roll = eulerAngles[0], eulerAngles[1], eulerAngles[2]

    # 在图像上绘制距离 & 角度信息
    cv2.putText(img, "dist: %.2fcm, yaw: %.2f, pitch: %.2f, roll: %.2f" % (distance, yaw, pitch, roll),
                (10, 500), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)

    # 显示结果
    cv2.imshow('Chessboard Detection', img)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

    # 保存结果
    RT_dict = {"R": rvec_matrix.tolist(), "T": tvec.tolist()}
    RT = json.dumps(RT_dict, indent=4)
    with open('RT.json', 'w') as json_file:
        json_file.write(RT)
else:
    print("未检测到棋盘格，请检查图片内容！")

---

获取图像中的像素点

运行代码之后，点击对应像素点，可以在显示终端中看到打印出来的像素坐标。

import cv2


# 鼠标点击回调函数
def get_pixel(event, x, y, flags, param):
    if event == cv2.EVENT_LBUTTONDOWN:  # 左键点击
        print(f"坐标: ({x}, {y}) 像素值: {image[y, x]}")  # 注意 OpenCV 的索引是 (y, x)


# 读取图像
image = cv2.imread("test.jpg")

# 创建窗口并绑定鼠标事件
cv2.namedWindow("Image")
cv2.setMouseCallback("Image", get_pixel)

while True:
    cv2.imshow("Image", image)
    if cv2.waitKey(1) & 0xFF == 27:  # 按 ESC 退出
        break

cv2.destroyAllWindows()

---

显示获取像素点的位置

要判断一下上一个步骤获取的像素点是否正确，所以写了如下代码，会在对应的像素点上画一个红色的小点。

import cv2

# 读取棋盘格照片
img = cv2.imread("./test.jpg")

# 指定像素点坐标 (x, y)
pixel_point = (1086, 1279)
pixel_point1 = (1339, 1285)

# 在该像素点位置画两个半径为 2 的红色圆圈
cv2.circle(img, pixel_point, radius=2, color=(0, 0, 255), thickness=-1)
cv2.circle(img, pixel_point1, radius=2, color=(0, 0, 255), thickness=-1)

# 显示图像
cv2.imshow("Image with Circle", img)
cv2.waitKey(0)
cv2.destroyAllWindows()

---

像素到真实世界坐标的转换，并测量两个像素点之间的真实距离

import numpy as np
import os
import json


def load_para(json_path):
    assert os.path.exists(json_path), json_path + " does not exist."
    json_file = open(json_path, 'r')
    para = json.load(json_file)
    return para


RT = load_para('./RT.json')
R, T = RT['R'], RT['T']

para = load_para("camera_intr_opt.json")
mtx, dist = np.array(para["mtx"]), np.array(para["dist"])


def pixel_to_world(u, v, z_world, mtx, R, T):
    # 计算相机坐标 (归一化)
    pixel_coords = np.array([u, v, 1], dtype=np.float32).reshape(3, 1)
    Z_C = (z_world + (np.linalg.inv(R) @ T)[2]) / \
          (np.linalg.inv(R) @ np.linalg.inv(mtx) @ pixel_coords)[2]
    camera_coords = Z_C * np.linalg.inv(mtx) @ pixel_coords

    # 计算世界坐标
    world_coords = np.linalg.inv(R) @ (camera_coords - T)
    X, Y, Z = world_coords.flatten()

    return np.array([X, Y, Z])


def distance_between_pixels(u1, v1, z1, u2, v2, z2, mtx, R, T):
    world_p1 = pixel_to_world(u1, v1, z1, mtx, R, T)
    world_p2 = pixel_to_world(u2, v2, z2, mtx, R, T)

    # 计算两点之间的欧几里得距离
    distance = np.linalg.norm(world_p1 - world_p2)
    return distance


# 示例：两个像素点
u1, v1, z1 = 819.17, 1635, -25
u2, v2, z2 = 1837, 1635, -25

distance = distance_between_pixels(u1, v1, z1, u2, v2, z2, mtx, R, T)
print(f"像素坐标 ({u1}, {v1}, {z1}) 和 ({u2}, {v2}, {z2}) 之间的实际距离: {distance:.2f} mm")

---

像素到真实世界坐标的转换，并测量两个像素点之间的真实距离（第二种计算方式）

此方法是deepseek直接给出的，没想到直接就能用，算得很准，而且还加上了去畸变的过程，下述代码就是把上面第一种方法的代码对应的矩阵给提取出来了，本质上是一样的，代码细节不懂可以直接复制代码问deepseek。

import numpy as np
import cv2
import os
import json


def load_para(json_path):
    assert os.path.exists(json_path), json_path + " does not exist."
    with open(json_path, 'r') as json_file:
        para = json.load(json_file)
    return para


# 加载参数
RT = load_para('./RT.json')
R = np.array(RT['R'])
T = np.array(RT['T'])

para = load_para("camera_intr_opt.json")
mtx = np.array(para["mtx"])
dist = np.array(para["dist"])


def pixel_to_world_with_z(u, v, z_world, mtx, R, T):
    """带Z轴坐标的像素到世界坐标转换"""
    # 去畸变
    undistorted = cv2.undistortPoints(np.array([[[u, v]]], dtype=np.float32), mtx, dist, None, mtx)
    u_undist, v_undist = undistorted[0][0]

    # 检查去畸变结果
    if not np.isfinite([u_undist, v_undist]).all():
        raise ValueError(f"去畸变结果无效: {u_undist}, {v_undist}")

    fx, fy = mtx[0, 0], mtx[1, 1]
    cx, cy = mtx[0, 2], mtx[1, 2]

    r11, r12, r13 = R[0]
    r21, r22, r23 = R[1]
    r31, r32, r33 = R[2]
    tx, ty, tz = T

    # 线性方程组 AX = B
    A = np.array([
        [fx * r11 - (u_undist - cx) * r31, fx * r12 - (u_undist - cx) * r32],
        [fy * r21 - (v_undist - cy) * r31, fy * r22 - (v_undist - cy) * r32]
    ])
    B = np.array([
        (u_undist - cx) * (r33 * z_world + tz) - fx * (r13 * z_world + tx),
        (v_undist - cy) * (r33 * z_world + tz) - fy * (r23 * z_world + ty)
    ])

    # 确保 A 可解
    if np.linalg.det(A) == 0:
        raise ValueError("A 矩阵是奇异的，无法求解")

    X, Y = np.linalg.solve(A, B)

    return np.array([float(X), float(Y), float(z_world)])


def distance_between_pixels_with_z(u1, v1, z1, u2, v2, z2, mtx, R, T):
    world_p1 = pixel_to_world_with_z(u1, v1, z1, mtx, R, T)
    world_p2 = pixel_to_world_with_z(u2, v2, z2, mtx, R, T)
    return np.linalg.norm(world_p1 - world_p2)


# 测试示例
u1, v1, z1 = 819.17, 1635, -25
u2, v2, z2 = 1837, 1635, -25

distance = distance_between_pixels_with_z(u1, v1, z1, u2, v2, z2, mtx, R, T)
print(f"三维空间距离: {distance:.2f} mm")