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

调用双目工业相机

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浅析

$$mtx=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$
$f_x,f_y$ 是焦距，单位为像素，理论上是相等的，$c_x,c_y$ 是光轴的像素坐标
$$\overrightarrow{r_1}=\left[\begin{array}{c}z{x}_1\\ z{y}_1\\ z\end{array}\right]=\left[\begin{array}{c}z(\frac{f_{x}x}{z}+c_x)\\ z(\frac{f_{y}y}{z}+c_y)\\ z\end{array}\right]=mtx\times \vec{r}$$

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

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

计算z

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

有
$$\frac{x_1}{f_1}=\frac{x}{z}$$
$$-\frac{x_2}{f_2}=\frac{B-x}{z}$$
联立可得
$$z=\frac{B}{\frac{x_1}{f_1}-\frac{x_2}{f_2}}$$

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=6
    h=4
    gray = 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$，精度还可以

计算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=6
    h=4
    gray = 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.f
            y=z*a[1]/L.f
            if 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'):
            break
    except:
        pass    

一般情况