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 = SubPix(gray1, corners1, (5, 5), (-1, -1), criteria)
corners21 = SubPix(gray2, corners2, (5, 5), (-1, -1), criteria)
# 将数据保存到excel中，excel路径⾃⾏修改，程序⾃动创建，⽆需⼿动操作
excel_path = 'D:/SA/AngularPointCoordinates.xlsx'
if ists(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)):
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)CH3(CH2)6COOH
dp = p1-p2
print(dp)
结果如下
舆情终端前两个为p1,p2点空间坐标（我们选的两个点），第三个为坐标差值，单位mm，除了在z⽅向有1.2mm误差较⼤外，其他⽅向误差较⼩。
5.精度分析
经多次测试，单⽅向最⼤误差⼩于2mm，造成这种误差可能是因为没有畸变校正。emm，下次再加上校正后在进⾏测试。
ps完整主代码如上，同名点确定属于另⼀个py⽂件中。
参考博客
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议