Python双目相机计算三维坐标（使用opencv自带图片）_双目视觉求解物体的三维坐标编程

前言：虽然计算三维坐标已经很多大佬研究过了，但是网上能用的好少啊。原理不多解释了，直接上程序。

1.导入库

import numpy as np
import cv2

包括矩阵处理和图像处理的两个基本库

2.相机参数设置

由于本人太穷，根本搞不起较好的双目相机，于是借用opencv自带的图片。安装opencv后，在opencv\sources\samples\data下，有官方自带的图片，包括左右相机01-14（没有10）共26张双目相机照片，就以他们为例。

打开matlab进行双目相机标定（或者用opencv进行标定），由于未知标定板大小，据官方文件，其标定板可能是10mm*10mm（因为他默认大小参数是1.0，所以我猜是10mm，如有错误请大佬指出；其次，标定板大小只影响旋转和平移矩阵，对内参没影响）标定结果如下：

# 左/右相机内参数、旋转、平移矩阵
leftIntrinsic = np.array([[533.9634, 0, 342.6315],
                          [0, 533.9448, 234.3696],
                          [0, 0, 1]])
leftRotation = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
leftTranslation = np.array([[0],
                            [0],
                            [0]])
rightIntrinsic = np.array([[537.6219, 0, 326.7537],
                           [0, 537.2332, 250.4047],
                           [0, 0, 1]])
rightRotation = np.array([[1, -0.0033, -0.0055],
                          [0.0033, 1, 0.0094],
                          [0.0054, -0.0094, 0.9999]])
rightTranslation = np.array([[-33.2347],
                             [0.4254],
                             [0.0085]])

matlab中，我只设置了三参数模型，标定板大小为10mm，其他默认，点击Calibrate。matlab操作不多介绍。

以左相片建立空间坐标系，所以左相机旋转和平移矩阵为单位矩阵和零矩阵。右相机的旋转和平移矩阵使用两相机间的旋转和平移矩阵。

3.函数编写

# 函数参数为左右相片同名点的像素坐标，获取方式后面介绍
# lx，ly为左相机某点像素坐标，rx，ry为右相机对应点像素坐标
def uvToXYZ(lx, ly, rx, ry):
    mLeft = np.hstack([leftRotation, leftTranslation])
    mLeftM = np.dot(leftIntrinsic, mLeft)
    mRight = np.hstack([rightRotation, rightTranslation])
    mRightM = np.dot(rightIntrinsic, mRight)
    A = np.zeros(shape=(4, 3))
    for i in range(0, 3):
        A[0][i] = lx * mLeftM[2, i] - mLeftM[0][i]
    for i in range(0, 3):
        A[1][i] = ly * mLeftM[2][i] - mLeftM[1][i]
    for i in range(0, 3):
        A[2][i] = rx * mRightM[2][i] - mRightM[0][i]
    for i in range(0, 3):
        A[3][i] = ry * mRightM[2][i] - mRightM[1][i]
    B = np.zeros(shape=(4, 1))
    for i in range(0, 2):
        B[i][0] = mLeftM[i][3] - lx * mLeftM[2][3]
    for i in range(2, 4):
        B[i][0] = mRightM[i - 2][3] - rx * mRightM[2][3]
    XYZ = np.zeros(shape=(3, 1))
    # 根据大佬的方法，采用最小二乘法求其空间坐标
    cv2.solve(A, B, XYZ, cv2.DECOMP_SVD)
    print(XYZ)
    return XYZ

4.同名点的确定与空间坐标的检验

同名点是指左右相片对应的匹配点，在空间坐标中指的是同一个点，下面用opencv相机标定的方法输出并保存标定板上各个点的坐标。

# 导入库，包括opencv和openpyxl（excel操作库，安装方法：pip install openpyxl）
import cv2
import openpyxl
import os
 
# 两张图片位置，这里选择left01和right01两张，位置自行更改
img1 = cv2.imread('D:/SA/2021.3/6.1/l/left01.jpg')
img2 = cv2.imread('D:/SA/2021.3/6.1/r/right01.jpg')
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)
gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
# 标定板角点检测
ret1, corners1 = cv2.findChessboardCorners(gray1, (6, 9), None)
ret2, corners2 = cv2.findChessboardCorners(gray2, (6, 9), None)
# 亚像素角点检测，使其角点精度更高（反正我觉得没高多少）
corners11 = cv2.cornerSubPix(gray1, corners1, (5, 5), (-1, -1), criteria)
corners21 = cv2.cornerSubPix(gray2, corners2, (5, 5), (-1, -1), criteria)
# 将数据保存到excel中，excel路径自行修改，程序自动创建，无需手动操作
excel_path = 'D:/SA/AngularPointCoordinates.xlsx'
if os.path.exists(excel_path) is False:
    workbook = openpyxl.Workbook()
    worksheet = workbook.active
    worksheet.title = 'data'
    workbook.save(excel_path)
workbook = openpyxl.load_workbook(excel_path)
sheet = workbook['data']
for i in range(0, len(corners1)):
    sheet.cell(i+1, 1, corners11[i][0][0])
    sheet.cell(i+1, 2, corners11[i][0][1])
    sheet.cell(i+1, 3, corners21[i][0][0])
    sheet.cell(i+1, 4, corners21[i][0][1])
workbook.save(excel_path)

运行程序，完成后，打开excel文件，其中有54组角点坐标，如下图所示：

其并没有进行排列，所以我们要自己判断点的位置。其中，最左上角是xy都最小的（opencv像素坐标在左上，以右为x轴、下为y轴的正方向），可以得知第六行是最左上角点坐标，前两列为左相片，后两列为右相片坐标。所以坐标为

244.4273987, 94.16474152； 127.9022751, 110.3449249

此时我们找最左上角向下10mm的那个点，位于第五行，x与其大致相同，y稍大，其坐标为：244.9057465, 126.2445526；128.8562469, 142.0508575

以上面两组数据测试，添加代码

p1 = uvToXYZ(244.4273987, 94.16474152, 127.9022751, 110.3449249)
p2 = uvToXYZ(244.9057465, 126.2445526, 128.8562469, 142.0508575)
dp = p1-p2
print(dp)

结果如下

前两个为p1,p2点空间坐标（我们选的两个点），第三个为坐标差值，单位mm，除了在z方向有1.2mm误差较大外，其他方向误差较小。

5.精度分析

经多次测试，单方向最大误差小于2mm，造成这种误差可能是因为没有畸变校正。emm，下次再加上校正后在进行测试。

ps完整主代码如上，同名点确定属于另一个py文件中。

参考博客

https://blog.csdn.net/qq_15947787/article/details/53366592?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522162449922616780261926595%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=162449922616780261926595&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_v2~rank_v29-6-53366592.pc_search_result_before_js&utm_term=%E5%8F%8C%E7%9B%AE%E7%9B%B8%E6%9C%BA%E8%AE%A1%E7%AE%97%E4%B8%89%E7%BB%B4%E5%9D%90%E6%A0%87&spm=1018.2226.3001.4187

https://blog.csdn.net/weixin_43535573/article/details/88918400?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522162449922616780261926595%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=162449922616780261926595&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_v2~rank_v29-13-88918400.pc_search_result_before_js&utm_term=%E5%8F%8C%E7%9B%AE%E7%9B%B8%E6%9C%BA%E8%AE%A1%E7%AE%97%E4%B8%89%E7%BB%B4%E5%9D%90%E6%A0%87&spm=1018.2226.3001.4187