基于OpenCV相机校准，用勾股定理计算棋盘格角点深度

问题背景 #

已完成OpenCV相机校准，获取了相机矩阵、投影矩阵等参数，需要通过勾股定理计算棋盘格每个角点的深度（相机到角点的三维空间距离），已知相机与棋盘格平面的距离为20000mm，作为OpenCV新手，不清楚如何实现。

现有校准代码 #

import numpy as np
import cv2 as cv
import glob
import matplotlib.pyplot as plt

# 寻找棋盘格角点
SizeOfChessBoard = (8,6)
SizeOfFrame = (1440,1080)

# 终止准则
TerminationCriteria= (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# 准备世界坐标系下的点，如(0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
ObjectPoint = np.zeros((SizeOfChessBoard[0] * SizeOfChessBoard[1], 3), np.float32)
ObjectPoint[:,:2] = np.mgrid[0:SizeOfChessBoard[0],0:SizeOfChessBoard[1]].T.reshape(-1,2)

size_of_chessboard_squares_mm = 30
ObjectPoint = ObjectPoint * size_of_chessboard_squares_mm

# 创建数组存储所有图像对应的世界点和图像点
# 世界点是三维真实世界坐标，图像点是二维图像平面坐标
ObjectPoints = []
ImagePoints = []

images = glob.glob('myimages\\*.jpg')

for image in images:
    img = cv.imread(image)
    grayScaleImage = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

    # 寻找棋盘格角点
    CameraCalibrated, Corners = cv.findChessboardCorners(grayScaleImage, SizeOfChessBoard, None)

    # 如果找到角点，优化后添加世界点和图像点
    if CameraCalibrated == True:
        ObjectPoints.append(ObjectPoint)
        NewCorners = cv.cornerSubPix(grayScaleImage, Corners, (11,11), (-1,-1), TerminationCriteria)
        ImagePoints.append(Corners)

        # 绘制并显示棋盘格角点
        cv.drawChessboardCorners(img, SizeOfChessBoard, NewCorners, CameraCalibrated)
        RGB_img = cv.cvtColor(img, cv.COLOR_BGR2RGB)
        plt.imshow(RGB_img)
        plt.show()

cv.destroyAllWindows()

# 执行相机校准
CameraCalibrated, CameraMatrix, DistortionParam, RotationVectors, TranslationVectors = cv.calibrateCamera(ObjectPoints, ImagePoints, SizeOfFrame, None, None)
np.savez('CameraParams.npz', CameraMatrix=CameraMatrix, DistortionParam=DistortionParam, RotationVectors=RotationVectors, TranslationVectors=TranslationVectors)
print("CameraCalibrated: ", CameraCalibrated)
print("\nCamera Matrix: \n", CameraMatrix)
print("\nDistortion Parameters: \n", DistortionParam)
print("\nRotation Vectors: \n", RotationVectors)
print("\nTranslation Vectors: \n", TranslationVectors)

R = cv.Rodrigues(RotationVectors[0])[0]
T = TranslationVectors[0]
RT = np.concatenate([R,T], axis=-1)
P=np.matmul(CameraMatrix, RT)

print("\nProjection Matrix: \n", P)

# 根据OpenCV文档，可对图像进行去畸变处理
# OpenCV提供两种方法
# 首先，使用cv.getOptimalNewCameraMatrix()基于自由缩放参数优化相机矩阵
# 若缩放参数alpha=0，返回的去畸变图像会去除多余像素，可能裁掉边角像素；若alpha=1，保留所有像素但会有黑边
# 该函数还返回图像ROI，可用于裁剪结果

img = cv.imread('myimages/1.jpg')
h, w = img.shape[:2]
newCameraMatrix, RegionOfInterest = cv.getOptimalNewCameraMatrix(CameraMatrix, DistortionParam, (w,h), 1, (w,h))

# 方法1：使用cv.undistort()去畸变
# 这是最简单的方法，调用函数后用上述ROI裁剪结果
distortion = cv.undistort(img, CameraMatrix, DistortionParam, None, newCameraMatrix)

# 裁剪图像
x, y, w, h = RegionOfInterest
distortion = distortion[y:y+h, x:x+w]
cv.imwrite('CalibratedResult\\caliResult1.jpg', distortion)

# 方法2：通过重映射去畸变
# 该方法稍复杂，先找到从畸变图像到去畸变图像的映射函数，再使用重映射函数
mapx, mapy = cv.initUndistortRectifyMap(CameraMatrix, DistortionParam, None, newCameraMatrix, (w,h), 5)
distortion = cv.remap(img, mapx, mapy, cv.INTER_LINEAR)

# 裁剪图像
x, y, w, h = RegionOfInterest
distortion = distortion[y:y+h, x:x+w]
cv.imwrite('CalibratedResult\\caliResult2.jpg', distortion)

# 计算重投影误差
mean_error = 0

for i in range(len(ObjectPoints)):
    NewImagePoints, _ = cv.projectPoints(ObjectPoints[i], RotationVectors[i], TranslationVectors[i], CameraMatrix, DistortionParam)
    error = cv.norm(ImagePoints[i], NewImagePoints, cv.NORM_L2)/len(NewImagePoints)
    mean_error += error

print( "total error: {}".format(mean_error/len(ObjectPoints)) )

计算棋盘格角点深度的实现步骤 #

要计算每个角点的深度，核心是将世界坐标系下的角点转换到相机坐标系，再用勾股定理计算点到相机原点（光心）的三维距离：

1. 世界点转相机坐标系
   相机校准得到的RotationVectors和TranslationVectors是单张图像对应的旋转向量和平移向量，将旋转向量转换为旋转矩阵R后，可通过以下公式将世界点P_world转换为相机点P_cam：

   P_cam = R @ P_world.T + T
   

   （注：@是矩阵乘法，.T表示转置，确保维度匹配）

2. 用勾股定理计算深度
   相机坐标系下，点的坐标为(x, y, z)，到原点的距离即为深度：

   depth = sqrt(x² + y² + z²)
   

代码实现（添加到现有代码末尾） #

# 计算第一张图像对应的棋盘格角点深度
# 取第一张图像的旋转矩阵和平移向量
R = cv.Rodrigues(RotationVectors[0])[0]
T = TranslationVectors[0]

# 将世界点转换为相机坐标系下的点
# ObjectPoint是Nx3的数组，转换后得到Nx3的相机点数组
camera_points = (R @ ObjectPoint.T).T + T

# 用勾股定理计算每个角点的深度
depths = np.sqrt(np.sum(camera_points ** 2, axis=1))

# 打印前5个角点的深度（单位：mm）
print("\n前5个棋盘格角点的深度（mm）：")
for i in range(5):
    print(f"角点{i+1}: {depths[i]:.2f} mm")

# 验证棋盘格平面到相机的距离（理论上应为20000mm左右）
# 棋盘格在世界坐标系z=0平面，平面到相机的距离是T的z分量绝对值（当棋盘格平面与相机光轴垂直时）
print(f"\n棋盘格平面到相机的距离（理论值）：{abs(T[2]):.2f} mm")

说明 #

• 若棋盘格平面与相机光轴垂直，棋盘格上所有点的深度应接近20000mm，边缘点因位置差异会有微小偏差；
• 代码中使用了第一张图像的旋转和平移参数，若要处理其他图像，只需替换RotationVectors[0]和TranslationVectors[0]为对应索引即可。

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