双目相机三维定位踩坑实录

调用双目工业相机

import numpy as np
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" #参见https://github.com/opencv/opencv/issues/17687,另外环境变量设置要在import cv2之前
import cv2
cap = cv2.VideoCapture(1,cv2.CAP_MSMF)#打开内置摄像机,CAP_DSHOW很慢，CAP_MSMF在未设置OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS时打开很慢
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,3840)
flag = 1#播放视频
count = 0;#记录照相的次数
while cap.isOpened():#当摄像头打开时ret,frame=cap.read()#读取当前摄像头画面L=frame[:,0:1920]R=frame[:,1921:3840]cv2.imshow('l',L)#显示当前摄像头画面cv2.imshow('r',R)if cv2.waitKey(flag)==ord(' '):#按下空格键拍照c = str(count)#将countint转换成str型cv2.imwrite('L'+c+'.jpg',L)#保存图片cv2.imwrite('R'+c+'.jpg',R)#保存图片count=count+1#计数加一if cv2.waitKey(flag)==ord('q'):#退出循环break;
cv2.destroyAllWindows()#关闭所有窗口
cap.release()#释放摄像头

相机标定

import cv2
import numpy as np
import glob# 找棋盘格角点
# 阈值
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
#棋盘格模板规格
w = 6   #内角点个数，内角点是和其他格子连着的点
h = 4# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵
objp = np.zeros((w*h,3), np.float32)
objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
# 储存棋盘格角点的世界坐标和图像坐标对
objpoints = [] # 在世界坐标系中的三维点
imgpoints = [] # 在图像平面的二维点images = glob.glob('img/*.jpg')
for fname in images:img = cv2.imread(fname)gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)ret, corners = cv2.findChessboardCorners(gray, (w,h),None)if ret == True:cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)objpoints.append(objp)imgpoints.append(corners)cv2.drawChessboardCorners(img, (w,h), corners, ret)cv2.imshow('findCorners',img)cv2.waitKey(100)
cv2.destroyAllWindows()
#标定、去畸变
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
# mtx：内参数矩阵
# dist：畸变系数
# rvecs：旋转向量 （外参数）
# tvecs ：平移向量 （外参数）
print (("ret:"),ret)
print (("mtx:\n"),mtx)
print (("dist:\n"),dist)
print (("rvecs:\n"),rvecs)
print (("tvecs:\n"),tvecs)
# 去畸变
img2 = cv2.imread('img/0.jpg')
h,w = img2.shape[:2]
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),0,(w,h))
dst = cv2.undistort(img2, mtx, dist, None, newcameramtx)
x,y,w,h = roi
dst = dst[y:y+h, x:x+w]
cv2.imwrite('calibresult.jpg',dst)# 反投影误差
total_error = 0
for i in range(len(objpoints)):imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)total_error += error
print (("total error: "), total_error/len(objpoints))

注意：根据张正友的论文原文和翻译，标定板所在平面和相机的相对位置需要改变，否则视为同一组约束

calibrateCamera浅析

m t x = [ f x 0 c x 0 f y c y 0 0 1 ] (2) mtx=\left[ \begin{matrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{matrix} \right] \tag{2} mtx= fx000fy0cxcy1 (2)
f x , f y f_x,f_y fx,fy 是焦距，单位为像素，理论上是相等的， c x , c y c_x,c_y cx,cy 是光轴的像素坐标
r 1 ⃗ = [ z x 1 z y 1 z ] = [ z ( f x x z + c x ) z ( f y y z + c y ) z ] = m t x × r ⃗ \vec{r_1}=\left[ \begin{matrix} zx_1 \\ zy_1 \\ z \end{matrix} \right]= \left[ \begin{matrix} z(\frac{f_xx}{z}+c_x) \\ z(\frac{f_yy}{z}+c_y) \\ z \end{matrix} \right] =mtx\times \vec{r} r1 = zx1zy1z = z(zfxx+cx)z(zfyy+cy)z =mtx×r

r ⃗ \vec{r} r 是摄像机坐标系下的坐标 ( x , y , z ) T (x,y,z)^T (x,y,z)T或齐次坐标 ( k x , k y , k z ) T (kx,ky,kz)^T (kx,ky,kz)T， r 1 ⃗ \vec{r_1} r1 是图像中(并非图像坐标系，因为加上了中心点)的二维齐次坐标 ( z x , z y , z ) T (zx,zy,z)^T (zx,zy,z)T

两相机光轴 z z z，相机坐标系 x , y x,y x,y轴都平行的情况

计算z

理论上两个相机的焦距应当相同，但是实践中无法做到，设其为 f 1 , f 2 f_1,f_2 f1,f2. 像点在两个图像坐标系中的 x x x坐标为 x 1 , x 2 x_1,x_2 x1,x2

有
x 1 f 1 = x z \frac{x_1}{f_1}=\frac{x}{z} f1x1=zx
− x 2 f 2 = B − x z -\frac{x_2}{f_2}=\frac{B-x}{z} −f2x2=zB−x
联立可得
z = B x 1 f 1 − x 2 f 2 z=\frac{B}{\frac{x_1}{f_1}-\frac{x_2}{f_2}} z=f1x1−f2x2B

import numpy as np
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
cap = cv2.VideoCapture(1,cv2.CAP_MSMF)#打开内置摄像机
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,3840)class camera:def __init__(self,filename):self.imtx=np.loadtxt(filename)self.fx=self.imtx[0,0]self.fy=self.imtx[1,1]self.cx=self.imtx[0,2]self.cy=self.imtx[1,2]self.f=(self.fx+self.fy)/2
def corner(img):w=6h=4gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)ret, corners = cv2.findChessboardCorners(gray, (w,h),None)if ret == True:cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)return corners
L=camera("L.txt")
R=camera("R.txt")
print(L.f,R.f)
while cap.isOpened():#当摄像头打开时try:ret,frame=cap.read()#读取当前摄像头画面l=frame[:,0:1920]r=frame[:,1921:3840]#cv2.imshow('r',r)#显示当前摄像头画面q#cv2.waitKey(1)l_corners=corner(l)cv2.drawChessboardCorners(l, (6,4), l_corners, True)cv2.imshow('l',l)r_corners=corner(r)cv2.drawChessboardCorners(r, (6,4), r_corners, True)cv2.imshow('r',r)cv2.waitKey(1)a=(l_corners[0,0,0]-L.cx,l_corners[0,0,1]-L.cy)b=(r_corners[0,0,0]-R.cx,l_corners[0,0,1]-R.cy)print('red',a,b)print(120/(a[0]/L.f-b[0]/R.f))a=(l_corners[23,0,0]-L.cx,l_corners[23,0,1]-L.cy)b=(r_corners[23,0,0]-R.cx,l_corners[23,0,1]-R.cy)print('green',a,b)print(120/(a[0]/L.f-b[0]/R.f))except:pass

测量了两个点的 z z z，精度还可以

计算xy

import numpy as np
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
import math
cap = cv2.VideoCapture(1,cv2.CAP_MSMF)#打开内置摄像机
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,3840)class camera:def __init__(self,filename):self.imtx=np.loadtxt(filename)self.fx=self.imtx[0,0]self.fy=self.imtx[1,1]self.cx=self.imtx[0,2]self.cy=self.imtx[1,2]self.f=(self.fx+self.fy)/2
def corner(img):w=6h=4gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)ret, corners = cv2.findChessboardCorners(gray, (w,h),None)if ret == True:cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)return corners
L=camera("L.txt")
R=camera("R.txt")
print(L.f,R.f)
while cap.isOpened():#当摄像头打开时try:ret,frame=cap.read()#读取当前摄像头画面l=frame[:,0:1920]r=frame[:,1921:3840]#cv2.imshow('r',r)#显示当前摄像头画面q#cv2.waitKey(1)l_corners=corner(l)cv2.drawChessboardCorners(l, (6,4), l_corners, True)#cv2.imshow('l',l)r_corners=corner(r)cv2.drawChessboardCorners(r, (6,4), r_corners, True)#cv2.imshow('r',r)#cv2.waitKey(1)pos=[]for i in range(24):a=(l_corners[i,0,0]-L.cx,l_corners[i,0,1]-L.cy)b=(r_corners[i,0,0]-R.cx,r_corners[i,0,1]-R.cy)z=120/(a[0]/L.f-b[0]/R.f)x=z*a[0]/L.fy=z*a[1]/L.fif i!=0:print(math.sqrt((x-pos[-1][0])**2+(y-pos[-1][1])**2+(z-pos[-1][2])**2))pos.append((x,y,z))x=round(x,1)y=round(y,1)z=round(z,1)print(i,a,b,x,y,z)cv2.putText(l,str(x)+' '+str(y)+' '+str(z),(int(l_corners[i,0,0]),int(l_corners[i,0,1])),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)cv2.imshow('l',l)if cv2.waitKey(1)==ord('q'):breakexcept:pass

一般情况