• 双目测距代码
千次阅读
2018-10-07 08:54:13
#coding:utf-8
#生成黑白标定图及定标代码

import cv2
import numpy as np
import os
import time
import sys

#单目校正后的npy文件保存路径
Leftmtx_path='/home/nvidia/AI_Robot/twilight/usb_camera/output/LeftcameraMatrix.npy'    #左摄像头内参数矩阵
Leftdist_path='/home/nvidia/AI_Robot/twilight/usb_camera/output/LeftdistCoeff.npy'      #左摄像头畸变矩阵
Rightmtx_path='/home/nvidia/AI_Robot/twilight/usb_camera/output/RightcameraMatrix.npy'  #右摄像头内参数矩阵
Rightdist_path='/home/nvidia/AI_Robot/twilight/usb_camera/output/RightdistCoeff.npy'     #右摄像头畸变矩阵

#立体校正后的npy文件保存路径
LeftCameraMtx_path='./output/LeftMatrix.npy'
RightCameraMtx_path='./output/RightMatrix.npy'

left_map1_path='./output/left_map1.npy'
left_map2_path='./output/left_map2.npy'
right_map1_path='./output/right_map1.npy'
right_map2_path='./output/right_map2.npy'
Q_path='./output/Q.npy'
cal_path='./picture/cal_picture/'       #标定图片存储文件夹

#生成一张标准尺寸为width*height的标定图,格子尺寸为length
def Calibration_create(width=450,height=350,length=50,path='./Calibration.jpg'):
image = np.zeros((width,height),dtype = np.uint8)
for j in range(height):
for i in range(width):
if((int)(i/length) + (int)(j/length))%2:
image[i,j] = 255;
cv2.imwrite(path,image)
print 'create successfully'

#计算图中可以检测到几个角点
def CornersDetect(path,shape):
gray=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

for i in range(shape[0],2,-1):
for j in range(shape[1],2,-1):
ret,corners=cv2.findChessboardCorners(gray,(i,j),None)

if ret==True:
print shape
else:
print (i,j),'falied match'
sys.exit(0)

#验证shape大小是否能够检测成功
def Corners_Verification(path,shape):
gray=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

ret,corners=cv2.findChessboardCorners(gray,shape,None)
if ret==True:
return True
else:
print 'detect failed'
sys.exit(0)

#加载标定图片并对两个摄像头分别进行单摄像头标定,shape为标定板方格横向和纵向个数
def SoloCalibration(shape=(11,8)):

cal_result_path='./picture/cal_result/'

pic_list=os.listdir(cal_path)
Leftpic_list=[]
Rightpic_list=[]

for name in pic_list:
if 'L' in name:
Leftpic_list.append(name)
elif 'R' in name:
Rightpic_list.append(name)

#合格的图片数
img_num=0
#检测是不是所有图片符合检测要求，不符合结束程序
for name in pic_list:
status=Corners_Verification(path=cal_path+name,shape=shape)
if status==True:
img_num+=1
elif status==None:
print 'detect failed'
sys.exit(0)
print 'all pirtures corners detected successfully'

img_num/=2

# 阈值
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标,
# 记为二维矩阵
objp = np.zeros((shape[0]*shape[1],3), np.float32)

objp[:,:2] = np.mgrid[0:shape[0],0:shape[1]].T.reshape(-1,2)    #一个框边长5mm

# 储存棋盘格角点的世界坐标和图像坐标对
LeftObjpoints=[]
LeftImgpoints=[]
RightObjpoints=[]
RightImgpoints=[]

#左摄像头定标
for name in Leftpic_list:
gray=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

ret,corners=cv2.findChessboardCorners(gray,shape,None)

if ret==True:
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) #精确检测点坐标
LeftObjpoints.append(objp)
LeftImgpoints.append(corners)

cv2.drawChessboardCorners(img, shape , corners,ret)
cv2.imwrite(cal_result_path+name,img)
#print name,' OK'
else:
print 'detect failed'
return None
# 标定
#返回标定结果、相机的内参数矩阵、畸变系数、旋转矩阵和平移向量
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(LeftObjpoints,
LeftImgpoints, gray.shape[::-1], None, None)

print '左摄像头定标结果'
print '内参数矩阵、畸变矩阵尺寸: ',mtx.shape,dist.shape

np.save(Leftmtx_path,mtx)
np.save(Leftdist_path,dist)

print '标定结果:'
print ret
print '相机内参数矩阵:'
print mtx
print '畸变系数'
print dist
print '旋转矩阵'
print rvecs
print '平移向量'
print tvecs

#右摄像头定标
for name in Rightpic_list:
gray=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

ret,corners=cv2.findChessboardCorners(gray,shape,None)

if ret==True:
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) #精确检测点坐标
RightObjpoints.append(objp)
RightImgpoints.append(corners)

cv2.drawChessboardCorners(img, shape , corners,ret)
cv2.imwrite(cal_result_path+name,img)
else:
print 'detect failed'
return None
# 标定
#返回标定结果、相机的内参数矩阵、畸变系数、旋转矩阵和平移向量
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(RightObjpoints,
RightImgpoints, gray.shape[::-1], None, None)

print '右摄像头定标结果'
print '右摄像头内参数矩阵、畸变矩阵尺寸: ',mtx.shape,dist.shape

np.save(Rightmtx_path,mtx)
np.save(Rightdist_path,dist)

print '标定结果:'
print ret
print '相机内参数矩阵:'
print mtx
print '畸变系数'
print dist
print '旋转矩阵'
print rvecs
print '平移向量'
print tvecs
#单目拍摄并去畸变
def SoloShot(cameraID=1):
camera=cv2.VideoCapture(cameraID)
print pic.shape
camera.release()
if cameraID==0:
else:

pic=cv2.undistort(pic,mtx,dist)
return pic
#左右摄像头各拍摄一张图片并使用单目摄像头定标得到的内参数矩阵和畸变矩阵对图形进行校准，输出校准前和校准后结果
def SoloCompare(output_path='./picture/compare/'):
print 'soloCompare start'
#获取两张图片
video0=cv2.VideoCapture(0)
video0.release()

video1=cv2.VideoCapture(1)
video1.release()
#保存原图片
cv2.imwrite(output_path+'LeftSoloOrigin.jpg',pic1)
cv2.imwrite(output_path+'RightSoloOrigin.jpg',pic2)

#加载两个摄像头的内参数矩阵和畸变矩阵

#进行校准
cal_pic1=cv2.undistort(pic1,Leftmtx,Leftdist)

cal_pic2=cv2.undistort(pic2,Rightmtx,Rightdist)

#保存校准后图片
cv2.imwrite(output_path+'./LeftSoloOut.jpg',cal_pic1)
cv2.imwrite(output_path+'./RightSoloOut.jpg',cal_pic2)

print 'OK'

#双目校准,shape为标定图规格,Lpath为左摄像头拍摄图像,Rpath为右摄像头拍摄图像
def CameraCalibration(shape=(11,8)):
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

objRealPoint=[] #图片角点实际物理坐标集合

imgPointsL=[] #左边摄像头某一照片角点集合
imgPointsR=[] #右边摄像头某一照片角点集合

#获取图片列表
pic_list=os.listdir(cal_path)
Leftpic_list=[]
Rightpic_list=[]
#分为左摄像头和右摄像头两个列表
for name in pic_list:
if 'L' in name:
Leftpic_list.append(name)
elif 'R' in name:
Rightpic_list.append(name)

#合格的图片数
img_num=0
#检测是不是所有图片符合检测要求，不符合结束程序
for name in pic_list:
status=Corners_Verification(path=cal_path+name,shape=shape)
if status==True:
img_num+=1
elif status==None:
return None
print 'all pictures corners detected successfully'

img_num/=2

# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标,
# 记为二维矩阵
objp = np.zeros((shape[0]*shape[1],3), np.float32)
objp[:,:2] = np.mgrid[0:shape[0],0:shape[1]].T.reshape(-1,2)

#加载单摄像头标定所得内参数和畸变矩阵

#图片预处理
for i in range(img_num):
if i==29:
continue
LGray=cv2.cvtColor(LImage,cv2.COLOR_BGR2GRAY)
RGray=cv2.cvtColor(RImage,cv2.COLOR_BGR2GRAY)

Lret,Lcorners=cv2.findChessboardCorners(LGray,shape,None)
Rret,Rcorners=cv2.findChessboardCorners(RGray,shape,None)

if Lret==True and Rret==True:
cv2.cornerSubPix(LGray,Lcorners,(11,11),(-1,-1),criteria)
cv2.cornerSubPix(RGray,Rcorners,(11,11),(-1,-1),criteria)

objRealPoint.append(objp) #实际物理坐标
imgPointsL.append(Lcorners)
imgPointsR.append(Rcorners)
else:
print 'detect failed'
return None

#drawChessboardCorners(LImage, shape, Lcorners,Lret)
#cv2.drawChessboardCorners(RImage, shape, Rcorners,Rret)
#print cv2.CALIB_USE_INTRINSIC_GUESS
#计算立体标定，输出标定结果为左摄像头内参数矩阵，右摄像头内参数矩阵，左畸变系数，右畸变系数，两摄像头之间旋转矩阵及平移矩阵，本质矩阵，基本矩阵
retval,cameraMatrixL,distCoeffL,cameraMatrixR,distCoeffR,R,T,E,F=cv2.stereoCalibrate(objRealPoint,imgPointsL,imgPointsR,cameraMatrixL,distCoeffL,cameraMatrixR,distCoeffR,LGray.shape[::-1],flags=cv2.CALIB_USE_INTRINSIC_GUESS)

print '基于单目定标后的双目摄像头立体定标输出参数:'

print '左摄像头内参数矩阵、畸变矩阵尺寸: ',cameraMatrixL.shape,distCoeffL.shape
print '右摄像头内参数矩阵、畸变矩阵尺寸: ',cameraMatrixR.shape,distCoeffR.shape

print '左摄像头内参数矩阵:'
print cameraMatrixL
print '左摄像头畸变矩阵:'
print distCoeffL
print '右摄像头内参数矩阵:'
print cameraMatrixR
print '右摄像头畸变矩阵:'
print distCoeffR

#存储立体标定后获得的内参数矩阵和畸变矩阵
np.save(LeftCameraMtx_path,cameraMatrixL)
np.save(RightCameraMtx_path,cameraMatrixR)

#计算旋转校准矩阵和投影矩阵，使得两张图共面对准
R1,R2,P1,P2,Q,validPixROI1,validPixROI2=cv2.stereoRectify(cameraMatrixL,distCoeffL,cameraMatrixR,distCoeffR,LGray.shape[::-1],R,T)

#计算更正map
left_map1,left_map2=cv2.initUndistortRectifyMap(cameraMatrixL,distCoeffL,R1,P1,LGray.shape[::-1],cv2.CV_16SC2)
right_map1,right_map2=cv2.initUndistortRectifyMap(cameraMatrixR,distCoeffR,R2,P2,LGray.shape[::-1],cv2.CV_16SC2)

print '映射map:'
print '左摄像头映射矩阵1:'

print left_map1
print '左摄像头映射矩阵2:'
print left_map2
print '右摄像头映射矩阵1:'
print right_map1
print '右摄像头映射矩阵2:'
print right_map2

np.save(left_map1_path,left_map1)
np.save(left_map2_path,left_map2)
np.save(right_map1_path,right_map1)
np.save(right_map2_path,right_map2)
np.save(Q_path,Q)

#左右摄像头各拍摄一张图片并使用双目立体定标得到的内参数矩阵和畸变矩阵对图形进行校准，输出校准前和校准后结果
def CameraCompare(output_path='./picture/compare/'):
#获取两张图片
video0=cv2.VideoCapture(0)
video0.release()

video1=cv2.VideoCapture(1)
video1.release()
#保存原图片
cv2.imwrite(output_path+'LeftCameraOrigin.jpg',pic1)
cv2.imwrite(output_path+'RightCameraOrigin.jpg',pic2)

#加载两个摄像头的内参数矩阵和畸变矩阵

#进行校准
cal_pic1=cv2.undistort(pic1,Leftmtx,Leftdist)

cal_pic2=cv2.undistort(pic2,Rightmtx,Rightdist)

#保存校准后图片
cv2.imwrite(output_path+'./LeftCameraOut.jpg',cal_pic1)
cv2.imwrite(output_path+'./RightCameraOut.jpg',cal_pic2)

print 'OK'

#计算立体更正获得深度图
def depth_map( output_path='./picture/compare/' ):

start_time=time.time()

#获取两张图片
video0=cv2.VideoCapture(0)
video1=cv2.VideoCapture(1)
#a=time.time()
#a=time.time()
video0.release()

#video1=cv2.VideoCapture(1)
#b=time.time()
#print b-a
#video0.release()
video1.release()
#保存原图片
cv2.imwrite(output_path+'LeftDepthOrigin.jpg',pic1)
cv2.imwrite(output_path+'RightDepthOrigin.jpg',pic2)

#加载两个摄像头的内参数矩阵和畸变矩阵

#进行校准

#cal_pic1=cv2.undistort(pic1,Leftmtx,Leftdist)
#cal_pic2=cv2.undistort(pic2,Rightmtx,Rightdist)

#根据map对图片进行重映射
LeftImg_Rectified=cv2.remap(pic1,left_map1,left_map2,cv2.INTER_LINEAR)
RightImg_Rectified=cv2.remap(pic2,right_map1,right_map2,cv2.INTER_LINEAR)

#LeftImg_Rectified=LeftImg

#RightImg_Rectified=RightImg

imgL=cv2.cvtColor(LeftImg_Rectified,cv2.COLOR_BGR2GRAY)
imgR=cv2.cvtColor(RightImg_Rectified,cv2.COLOR_BGR2GRAY)

# 两个trackbar用来调节不同的参数查看效果
'''num = cv2.getTrackbarPos("num", "depth")
blockSize = cv2.getTrackbarPos("blockSize", "depth")
if blockSize % 2 == 0:
blockSize += 1
if blockSize < 5:
blockSize = 5'''

# 根据Block Maching方法生成差异图（opencv里也提供了SGBM/Semi-Global Block Matching算法，有兴趣可以试试）

disparity = stereo.compute(imgL, imgR)

disp = cv2.normalize(disparity,  alpha=0,beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_16UC1)

#disp=cv2.threshold(disp,30,200)
disp[disp>220]=0

#膨胀操作
kernel1=np.ones((2,2),np.uint8)
kernel2=np.ones((5,5),np.uint8)

erobe_disp=disp
#对图像进行开运算先膨胀再腐蚀
erobe_disp=cv2.morphologyEx(erobe_disp,cv2.MORPH_OPEN,kernel1,iterations=1)  #去噪点
erobe_disp=cv2.morphologyEx(erobe_disp,cv2.MORPH_CLOSE,kernel2,iterations=1)    #填充

erobe_disp[erobe_disp<25]=0

erobe_disp=erobe_disp.astype(np.uint8)

#双边滤波器
erobe_disp=cv2.bilateralFilter(erobe_disp,d=10,sigmaColor=10,sigmaSpace=15)

erobe_disp[erobe_disp<20]=0
# 将图片扩展至3d空间中，其z方向的值则为当前的距离
#threeD = cv2.reprojectImageTo3D(disp.astype(np.float32)/16., Q)

#disp=disp.reshape(disp.shape[0],disp.shape[1],1)
#disp=cv2.merge([disp,disp,disp])

#disp=cv2.cvtColor(disp,cv2.COLOR_BGR2RGB)
cv2.imwrite(output_path+'deep_output.jpg',disp)
cv2.imwrite(output_path+'erode_deep_output.jpg',erobe_disp)

#输出重映射对齐后的图像
cv2.imwrite(output_path+'LeftImage_Rectified.jpg',LeftImg_Rectified)
cv2.imwrite(output_path+'RightImage_Rectified.jpg',RightImg_Rectified)

print disp.shape

end_time=time.time()

print 'time consume:%.4f' %(end_time-start_time)

print 'max:',np.max(disp),'min',np.min(disp),'mean',np.mean(disp)

#拍摄并计算出深度图保存为output_name
def depth_shot(output_name):

start_time=time.time()

#获取两张图片
video0=cv2.VideoCapture(0)
video1=cv2.VideoCapture(1)

video0.release()

video1.release()

print 'shot OK'

#加载两个摄像头的内参数矩阵和畸变矩阵

#根据map对图片进行重映射
LeftImg_Rectified=cv2.remap(pic1,left_map1,left_map2,cv2.INTER_LINEAR)
RightImg_Rectified=cv2.remap(pic2,right_map1,right_map2,cv2.INTER_LINEAR)

imgL=cv2.cvtColor(LeftImg_Rectified,cv2.COLOR_BGR2GRAY)
imgR=cv2.cvtColor(RightImg_Rectified,cv2.COLOR_BGR2GRAY)

# 根据Block Maching方法生成差异图（opencv里也提供了SGBM/Semi-Global Block Matching算法，有兴趣可以试试）

disparity = stereo.compute(imgL, imgR)

disp = cv2.normalize(disparity,  alpha=0,beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_16UC1)

#disp=cv2.threshold(disp,30,200)
disp[disp>220]=0

#膨胀操作
kernel1=np.ones((2,2),np.uint8)
kernel2=np.ones((5,5),np.uint8)

erobe_disp=disp
#对图像进行开运算先膨胀再腐蚀
erobe_disp=cv2.morphologyEx(erobe_disp,cv2.MORPH_OPEN,kernel1,iterations=1)  #去噪点
erobe_disp=cv2.morphologyEx(erobe_disp,cv2.MORPH_CLOSE,kernel2,iterations=1)    #填充

erobe_disp[erobe_disp<25]=0

erobe_disp=erobe_disp.astype(np.uint8)

#双边滤波器
erobe_disp=cv2.bilateralFilter(erobe_disp,d=10,sigmaColor=10,sigmaSpace=15)

erobe_disp[erobe_disp<20]=0

cv2.imwrite('./car_data/ori.jpg',imgL)
cv2.imwrite(output_name,erobe_disp)

print 'OK'

if  __name__=='__main__':
print 'start'
#Calibration_create()
#CornersDetect('./picture/cal_picture/9R.jpg',(11,8))
#SoloCalibration()
#CameraCalibration()
#depth_map()
SoloCompare()
#CameraCompare()



更多相关内容
• opencv自带的SGBM算法，简单实现双目测距功能，可以使用自己标定的双目相机参数
• 文章名称：3-D Point Cloud ...文章（英文）详细讲解了双目视觉的一些基本原理，已经如何使用两个普通的网络摄像头来实现双目视觉，以及如何根据两个摄像头来计算物体深度信息。 代码为文章中附带的代码 仅供参考学习
• 1、双目相机拍照后使用Matlab进行双目标定 主要参考：https://blog.csdn.net/dulingwen/article/details/98071584 感谢大佬的分享！！！(*≧ω≦)！！ Python-openCV 中cv2.StereoSGBM_create()参数的含义 参考：...

# 一、双目相机拍照后使用Matlab进行双目标定

主要参考：https://blog.csdn.net/dulingwen/article/details/98071584
感谢大佬的分享！！！(*≧ω≦)！！

# 二、标定后生成深度图，之后在进行测距

## 1、导入相关库和相机的标定参数

首先导入需要的相关库以及双目相机标定的各项参数：

 # -*- coding: utf-8 -*-
import cv2
import numpy as np
import stereoconfig_040_2 #导入相机标定的参数
import pcl
import pcl.pcl_visualization


首先对导入的左右相机的图片进行预处理，一般可以削弱光照不均的影响，使得两个图片的曝光值差距缩小。
小知识：python-opencv读取的灰度图像是二维列表（数组）,彩色图像是三位列表（数组）。
.ndim返回的是数组的维度，返回的只有一个数，该数即表示数组的维度。
参考：https://blog.csdn.net/mokeding/article/details/17599585

#像素的访问和访问numpy中ndarray的方法完全一样
img[j,i] = 255  # 灰度图访问;j,i 分别表示图像的行和列 即 j * i 二维

#BGR 图像访问 彩色 BGR 图像 为 j * i * 3 三维
img[j,i,0] = 255   # 0 -- 为通道，指B通道
img[j,i,1] = 255   # 1 -- 为通道，指G通道
img[j,i,2] = 255   # 2 -- 为通道，指R通道


## 2、图像预处理

# 预处理
def preprocess(img1, img2):
# 彩色图->灰度图
if(img1.ndim == 3): #判断是否为三维数组
img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)  # 通过OpenCV加载的图像通道顺序是BGR
if(img2.ndim == 3):#判断是否为三维数组
img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

# 直方图均衡
# 当然这里也可以按需对图像进行处理
img1 = cv2.equalizeHist(img1)
img2 = cv2.equalizeHist(img2)

return img1, img2


## 3、消除图像的畸变

cv.undistort()消除畸变函数参数含义
参考OpenCV官网：https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d

cv.undistort(	src, cameraMatrix, distCoeffs[, dst[, newCameraMatrix]]	) ->	dst


各个参数的含义：

参数名含义
src输入未畸变矫正的图片
dst输出矫正之后的图片，与src输入的图片具有相同的大小和类型
cameraMatrix输入相机的内参数矩阵 A = [ [fx, 0, cx], [0, fy, cy], [0, 0, 1] ]
distCoeffs输入相机的畸变参数向量 （ (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy ] ] ] ] ) ）中的4, 5, 8, 12 或 14 个元素。如果向量为NULL /空，则假定失真系数为零。一般取标定后相机的[k1, k2, p1, p2, k3]
newCameraMatrix畸变图像的相机矩阵。 默认情况下，它与cameraMatrix相同，但是您还可以使用其他矩阵来缩放和移动结果。

消除畸变函数的定义：

# 消除畸变
def undistortion(image, camera_matrix, dist_coeff):
undistortion_image = cv2.undistort(image, camera_matrix, dist_coeff)

return undistortion_image


调用示例，不能直接使用（stereoconfig_040_2.py完整代码见文末）：

 # 读取相机内参和外参
config = stereoconfig_040_2.stereoCamera()
i = 3
string = 'Val'
# 读取数据集的图片
iml = cv2.imread('./%sLeft%d.bmp' %(string,i) )  # 左图
imr = cv2.imread('./%sRight%d.bmp'%(string,i) )  # 右图
iml = undistortion(iml, config.cam_matrix_left, config.distortion_l)
imr = undistortion(imr, config.cam_matrix_right, config.distortion_r)

#cv2.undistort（）的dist_coeff参数的形式
# 左右相机畸变系数:[k1, k2, p1, p2, k3]
#config.distortion_l = np.array([[-0.0806, 0.3806, -0.0033, 0.0005148, -0.5229]])
#config.distortion_r = np.array([[-0.0485, 0.2200, -0.002,  0.0017,    -0.2876]])


## 4、立体校正


# 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵
# @param：config是一个类，存储着双目标定的参数:config = stereoconfig.stereoCamera()
def getRectifyTransform(height, width, config):
# 读取内参和外参
left_K = config.cam_matrix_left
right_K = config.cam_matrix_right
left_distortion = config.distortion_l
right_distortion = config.distortion_r
R = config.R
T = config.T

# 计算校正变换
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion,
(width, height), R, T, alpha=0)

map1x, map1y = cv2.initUndistortRectifyMap(left_K, left_distortion, R1, P1, (width, height), cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(right_K, right_distortion, R2, P2, (width, height), cv2.CV_32FC1)

return map1x, map1y, map2x, map2y, Q



cv.stereoRectify()参数含义：
官网定义的形式：

cv.stereoRectify(
cameraMatrix1,
distCoeffs1,
cameraMatrix2,
distCoeffs2,
imageSize,
R,
T[, R1
[, R2
[, P1
[, P2
[, Q
[, flags
[, alpha
[, newImageSize]]]]]]]]
) ->	R1, R2, P1, P2, Q, validPixROI1, validPixROI2


cv.stereoRectify()参数的含义：

参数名含义
cameraMatrix1第一个相机的内参数矩阵。
distCoeffs1第一个相机的畸变参数。
cameraMatrix2第二个相机的内参数矩阵。
distCoeffs2第二个相机的畸变参数。
imageSize双目相机标定的图像的尺寸大小。
R从第一个摄像头的坐标系到第二个摄像头的坐标系的旋转矩阵，请参见stereoCalibrate。（输出旋转矩阵。连同平移矢量T一起，此矩阵将第一个摄像机坐标系中给定的点带到第二个摄像机坐标系中的点。用更专业的术语来说，R和T的元组执行的基础是从第一相机的坐标系到第二相机的坐标系的变化。由于其二元性，该元组等效于第一摄像机相对于第二摄像机坐标系的位置。）
T（输出转换向量，请参见上面的描述。）从第一个摄像机的坐标系到第二个摄像机的坐标系的平移向量，请参见stereoCalibrate
R1为第一个摄像机输出3x3立体矫正的变换（旋转矩阵）。 该矩阵将未校正的第一照相机的坐标系中给定的点带到校正的第一照相机的坐标系中的点。 用更多的专业术语来说就是，它执行了从未校正的第一摄像机的坐标系到校正了的第一摄像机的坐标系的基准的更改。
R2为第二个摄像机输出3x3立体矫正的变换（旋转矩阵）。 该矩阵将未校正的第二照相机的坐标系中给定的点带到校正的第二照相机的坐标系中的点。 用更多的专业术语来说就是，它执行了从未校正的第二摄像机的坐标系到校正了的第二摄像机的坐标系的基准的更改。
P1在第一个摄像机的新（校正）坐标系中输出3x4投影矩阵，即它将在校正后的第一摄像机坐标系中给定的点，投影到校正后的第一摄像机的图像中。
P2在第二个摄像机的新（校正）坐标系中输出3x4投影矩阵，即它将在校正后的第二摄像机坐标系中给定的点，投影到校正后的第二摄像机的图像中。
Q输出4×4视差深度映射矩阵（请参阅reprojectImageTo3D）。
flags可能为零或CALIB_ZERO_DISPARITY的操作标志。如果设置了该标志，则该功能使每个摄像机的主要的像素点在校正后的视图中具有相同的像素坐标。并且，如果未设置该标志，则该功能仍可以在水平或垂直方向上移动图像（取决于对极线的方向），以最大化可用图像区域。
alpha自由缩放参数。如果它是-1或缺省，则该函数将执行默认缩放。否则，该参数应在0到1之间。alpha = 0表示对校正的图像已经经过缩放和移动了，以便仅有效像素可见（在校正之后的非黑色区域）。 alpha = 1表示对校正后的图像进行抽取和移位，以便将来自摄像机的原始图像中的所有像素保留在校正后的图像中（不丢失任何源图像像素）。任何中间值（0～1）都会在这两种极端情况之间产生中间结果。
newImageSize校正后的新图像的分辨率。应将相同的大小传递给initUndistortRectifyMap（请参阅OpenCV示例目录中的stereo_calib.cpp示例）。传递（0,0）（默认值）时，将其设置为原始imageSize。将其设置为较大的值可以帮助您保留原始图像中的细节，尤其是在径向变形较大的情况下。
validPixROI1在所有像素均有效的已校正图像内的可选输出矩形。如果alpha = 0，则ROI覆盖整个图像。否则，它们可能会更小（请参见下图）。
validPixROI2在所有像素均有效的已校正图像内的可选输出矩形。如果alpha = 0，则ROI覆盖整个图像。否则，它们可能会更小（请参见下图）。
cv.initUndistortRectifyMap()函数的定义：
cv.initUndistortRectifyMap(
cameraMatrix,
distCoeffs,
R,
newCameraMatrix,
size,
m1type[, map1
[, map2]
]
) ->	map1, map2


cv.initUndistortRectifyMap()函数各个参数的含义：
参考OpenCV官网：
https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga7dfb72c9cf9780a347fbe3d1c47e5d5a

参数名含义
cameraMatrix相机的内参数矩阵。
distCoeffs输入相机的畸变参数向量 （一般取标定后相机的[k1, k2, p1, p2, k3] ）。
R对象空间中的可选的校正变换（3x3矩阵）。由stereoRectify计算的R1或R2可以在此处传递。如果矩阵为空，则假定身份转换。在cvInitUndistortMap中，R是一个单位矩阵。
newCameraMatrix新相机内参数的矩阵。
size未校正的图片的尺寸。
m1type第一个输出映射的类型可以是CV_32FC1，CV_32FC2或CV_16SC2，请参见convertMaps
map1第一个输出图。
map2第二个输出图。

## 5、视差计算

# 视差计算
def stereoMatchSGBM(left_image, right_image, down_scale=False):
# SGBM匹配参数设置
if left_image.ndim == 2:
img_channels = 1
else:
img_channels = 3

blockSize = 3
paraml = {'minDisparity': 0,
'numDisparities': 128,
'blockSize': blockSize,
'P1': 8 * img_channels * blockSize ** 2,
'P2': 32 * img_channels * blockSize ** 2,
'disp12MaxDiff': 1,
'preFilterCap': 63,
'uniquenessRatio': 15,
'speckleWindowSize': 100,
'speckleRange': 1,
'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
}

# 构建SGBM对象
left_matcher = cv2.StereoSGBM_create(**paraml)
paramr = paraml
paramr['minDisparity'] = -paraml['numDisparities']
right_matcher = cv2.StereoSGBM_create(**paramr)

# 计算视差图
size = (left_image.shape[1], left_image.shape[0])
if down_scale == False:
disparity_left = left_matcher.compute(left_image, right_image)
disparity_right = right_matcher.compute(right_image, left_image)

else:
left_image_down = cv2.pyrDown(left_image)
right_image_down = cv2.pyrDown(right_image)
factor = left_image.shape[1] / left_image_down.shape[1]

disparity_left_half = left_matcher.compute(left_image_down, right_image_down)
disparity_right_half = right_matcher.compute(right_image_down, left_image_down)
disparity_left = cv2.resize(disparity_left_half, size, interpolation=cv2.INTER_AREA)
disparity_right = cv2.resize(disparity_right_half, size, interpolation=cv2.INTER_AREA)
disparity_left = factor * disparity_left
disparity_right = factor * disparity_right

# 真实视差（因为SGBM算法得到的视差是×16的）
trueDisp_left = disparity_left.astype(np.float32) / 16.
trueDisp_right = disparity_right.astype(np.float32) / 16.

return trueDisp_left, trueDisp_right



Python-openCV 中cv2.StereoSGBM_create()参数的含义：

参考：https://docs.opencv.org/trunk/d2/d85/classcv_1_1StereoSGBM.html
参考OpenCV官网：https://docs.opencv.org/trunk/d2/d85/classcv_1_1StereoSGBM.html
cv2.StereoSGBM_create()的SGBM算法的定义：

cv2.StereoSGBM_create(
[，minDisparity
[，numDisparities
[，blockSize
[，P1
[，P2
[，disp12MaxDiff
[，preFilterCap
[，uniquenessRatio
[，speckleWindowSize
[，speckleRange
[，mode]]]]]]]]]]]]
)


各个参数的含义：

参数名含义
minDisparity最小可能的差异值。通常情况下，它是零，但有时整流算法可能会改变图像，所以这个参数需要作相应的调整。
numDisparities最大差异减去最小差异。该值总是大于零。在当前的实现中，该参数必须可以被16整除。
blockSize匹配的块大小。它必须是> = 1的奇数。通常情况下，它应该在3～11的范围内。
P1控制视差平滑度的第一个参数。见下文。
P2第二个参数控制视差平滑度。值越大，差异越平滑。P1是相邻像素之间的视差变化加或减1的惩罚。P2是相邻像素之间的视差变化超过1的惩罚。该算法需要P2> P1。请参见stereo_match.cpp示例，其中显示了一些相当好的P1和P2值（即分别设置为 P1 = 8 * number_of_image_channels * blockSize * blockSize和 P2 = 32 * number_of_image_channels * blockSize * blockSize）。
disp12MaxDiff左右视差检查中允许的最大差异（以整数像素为单位）。将其设置为非正值以禁用检查。
preFilterCap预滤波图像像素的截断值。该算法首先计算每个像素的x导数，并通过[-preFilterCap，preFilterCap]间隔剪切其值。结果值传递给Birchfield-Tomasi像素成本函数。
uniquenessRatio最佳（最小）计算成本函数值应该“赢”第二个最佳值以考虑找到的匹配正确的百分比保证金。通常，5-15范围内的值就足够了。
speckleWindowSize平滑视差区域的最大尺寸，以考虑其噪声斑点和无效。将其设置为0可禁用斑点过滤。否则，将其设置在50-200的范围内。
speckleRange每个连接组件内的最大视差变化。如果你做斑点过滤，将参数设置为正值，它将被隐式乘以16.通常，1或2就足够好了。
mode将其设置为StereoSGBM :: MODE_HH以运行全尺寸双通道动态编程算法。它将消耗O（W * H * numDisparities）字节，这对640x480立体声很大，对于HD尺寸的图片很大。默认情况下，它被设置为false。

Python-openCV 中cv2.pyrDown()中的参数和含义：
参考OpenCV官网：https://docs.opencv.org/master/d4/d86/group__imgproc__filter.html#gaf9bba239dfca11654cb7f50f889fc2ff

cv.pyrDown(	src[, dst[, dstsize[, borderType]]]	) ->	dst

参数名含义
src输入图像
dst输出图像；与上面的输入图像具有相同的尺寸大小和类型
dstsize输出图像的大小
borderType像素外推方法，请参见BorderTypes（不支持BORDER_CONSTANT

## 6、其他函数的参数以及含义

### reprojectImageTo3D的定义：

cv.reprojectImageTo3D(
disparity,
Q
[, _3dImage
[, handleMissingValues
[, ddepth]]]
) ->	_3dImage


cv2.reprojectImageTo3D()参数以及其含义：
参考官网：https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga1bc1152bd57d63bc524204f21fde6e02

参数名含义
disparity输入单通道8位无符号，16位有符号，32位有符号或32位浮点视差图像。 假定8位/ 16位带符号格式的值没有小数位。 如果视差是由StereoBM或StereoSGBM以及其他算法计算出的16位带符号格式，则应在此处使用之前将其除以16（并缩放为浮点数）。
_3dImage输出与视差大小相同的3通道浮点图像。 _3dImage（x，y）的每个元素都包含根据视差图计算的点（x，y）的3D坐标。 如果使用通过stereoRectify获得的Q，则返回的点将在第一个摄像机的校正坐标系中表示。
Q可以使用stereoRectify获得的4×4透视变换矩阵。
handleMissingValues指示函数是否应处理缺失值（即未计算视差的点）。如果handleMissingValues = true，则具有与异常值相对应的最小视差的像素（请参见StereoMatcher :: compute）将转换为Z值非常大（当前设置为10000）的3D点。
ddepth可选的输出数组深度。如果为-1，则输出图像将具有CV_32F深度。 ddepth也可以设置为CV_16S，CV_32S或CV_32F。

# 三、双目测距代码的实现

## 1、stereoconfig_040_2.py–相机标定的参数

import numpy as np

####################仅仅是一个示例###################################

# 双目相机参数
class stereoCamera(object):
def __init__(self):
# 左相机内参
self.cam_matrix_left = np.array([   [830.5873,   -3.0662,  658.1007],
[       0,  830.8116,  482.9859],
[       0,         0,         1]
])
# 右相机内参
self.cam_matrix_right = np.array([  [830.4255,   -3.5852,  636.8418],
[       0,  830.7571,  476.0664],
[       0,         0,         1]
])

# 左右相机畸变系数:[k1, k2, p1, p2, k3]
self.distortion_l = np.array([[-0.0806, 0.3806, -0.0033, 0.0005148, -0.5229]])
self.distortion_r = np.array([[-0.0485, 0.2200, -0.002,  0.0017,    -0.2876]])

# 旋转矩阵
self.R = np.array([ [      1,  0.0017, -0.0093],
[-0.0018,  1.0000, -0.0019],
[ 0.0093,  0.0019,  1.0000]
])

# 平移矩阵
self.T = np.array([[-119.9578], [0.1121], [-0.2134]])

# 焦距
self.focal_length = 859.367 # 默认值，一般取立体校正后的重投影矩阵Q中的 Q[2,3]

# 基线距离
self.baseline = 119.9578 # 单位：mm， 为平移向量的第一个参数（取绝对值）



## 2、dianyuntu.py–测距实现代码

 # -*- coding: utf-8 -*-
import cv2
import numpy as np
import stereoconfig_040_2   #导入相机标定的参数
import pcl
import pcl.pcl_visualization

# 预处理
def preprocess(img1, img2):
# 彩色图->灰度图
if(img1.ndim == 3):#判断为三维数组
img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)  # 通过OpenCV加载的图像通道顺序是BGR
if(img2.ndim == 3):
img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

# 直方图均衡
img1 = cv2.equalizeHist(img1)
img2 = cv2.equalizeHist(img2)

return img1, img2

# 消除畸变
def undistortion(image, camera_matrix, dist_coeff):
undistortion_image = cv2.undistort(image, camera_matrix, dist_coeff)

return undistortion_image

# 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵
# @param：config是一个类，存储着双目标定的参数:config = stereoconfig.stereoCamera()
def getRectifyTransform(height, width, config):
# 读取内参和外参
left_K = config.cam_matrix_left
right_K = config.cam_matrix_right
left_distortion = config.distortion_l
right_distortion = config.distortion_r
R = config.R
T = config.T

# 计算校正变换
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion,
(width, height), R, T, alpha=0)

map1x, map1y = cv2.initUndistortRectifyMap(left_K, left_distortion, R1, P1, (width, height), cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(right_K, right_distortion, R2, P2, (width, height), cv2.CV_32FC1)

return map1x, map1y, map2x, map2y, Q

# 畸变校正和立体校正
def rectifyImage(image1, image2, map1x, map1y, map2x, map2y):
rectifyed_img1 = cv2.remap(image1, map1x, map1y, cv2.INTER_AREA)
rectifyed_img2 = cv2.remap(image2, map2x, map2y, cv2.INTER_AREA)

return rectifyed_img1, rectifyed_img2

# 立体校正检验----画线
def draw_line(image1, image2):
# 建立输出图像
height = max(image1.shape[0], image2.shape[0])
width = image1.shape[1] + image2.shape[1]

output = np.zeros((height, width, 3), dtype=np.uint8)
output[0:image1.shape[0], 0:image1.shape[1]] = image1
output[0:image2.shape[0], image1.shape[1]:] = image2

# 绘制等间距平行线
line_interval = 50  # 直线间隔：50
for k in range(height // line_interval):
cv2.line(output, (0, line_interval * (k + 1)), (2 * width, line_interval * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA)

return output

# 视差计算
def stereoMatchSGBM(left_image, right_image, down_scale=False):
# SGBM匹配参数设置
if left_image.ndim == 2:
img_channels = 1
else:
img_channels = 3
blockSize = 3
paraml = {'minDisparity': 0,
'numDisparities': 128,
'blockSize': blockSize,
'P1': 8 * img_channels * blockSize ** 2,
'P2': 32 * img_channels * blockSize ** 2,
'disp12MaxDiff': 1,
'preFilterCap': 63,
'uniquenessRatio': 15,
'speckleWindowSize': 100,
'speckleRange': 1,
'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
}

# 构建SGBM对象
left_matcher = cv2.StereoSGBM_create(**paraml)
paramr = paraml
paramr['minDisparity'] = -paraml['numDisparities']
right_matcher = cv2.StereoSGBM_create(**paramr)

# 计算视差图
size = (left_image.shape[1], left_image.shape[0])
if down_scale == False:
disparity_left = left_matcher.compute(left_image, right_image)
disparity_right = right_matcher.compute(right_image, left_image)

else:
left_image_down = cv2.pyrDown(left_image)
right_image_down = cv2.pyrDown(right_image)
factor = left_image.shape[1] / left_image_down.shape[1]

disparity_left_half = left_matcher.compute(left_image_down, right_image_down)
disparity_right_half = right_matcher.compute(right_image_down, left_image_down)
disparity_left = cv2.resize(disparity_left_half, size, interpolation=cv2.INTER_AREA)
disparity_right = cv2.resize(disparity_right_half, size, interpolation=cv2.INTER_AREA)
disparity_left = factor * disparity_left
disparity_right = factor * disparity_right

# 真实视差（因为SGBM算法得到的视差是×16的）
trueDisp_left = disparity_left.astype(np.float32) / 16.
trueDisp_right = disparity_right.astype(np.float32) / 16.

return trueDisp_left, trueDisp_right

# 将h×w×3数组转换为N×3的数组
def hw3ToN3(points):
height, width = points.shape[0:2]

points_1 = points[:, :, 0].reshape(height * width, 1)
points_2 = points[:, :, 1].reshape(height * width, 1)
points_3 = points[:, :, 2].reshape(height * width, 1)

points_ = np.hstack((points_1, points_2, points_3))

return points_

# 深度、颜色转换为点云
def DepthColor2Cloud(points_3d, colors):
rows, cols = points_3d.shape[0:2]
size = rows * cols

points_ = hw3ToN3(points_3d)
colors_ = hw3ToN3(colors).astype(np.int64)

# 颜色信息
blue = colors_[:, 0].reshape(size, 1)
green = colors_[:, 1].reshape(size, 1)
red = colors_[:, 2].reshape(size, 1)

rgb = np.left_shift(blue, 0) + np.left_shift(green, 8) + np.left_shift(red, 16)

# 将坐标+颜色叠加为点云数组
pointcloud = np.hstack((points_, rgb)).astype(np.float32)

# 删掉一些不合适的点
X = pointcloud[:, 0]
Y = -pointcloud[:, 1]
Z = -pointcloud[:, 2]

remove_idx1 = np.where(Z <= 0)
remove_idx2 = np.where(Z > 15000)
remove_idx3 = np.where(X > 10000)
remove_idx4 = np.where(X < -10000)
remove_idx5 = np.where(Y > 10000)
remove_idx6 = np.where(Y < -10000)
remove_idx = np.hstack((remove_idx1[0], remove_idx2[0], remove_idx3[0], remove_idx4[0], remove_idx5[0], remove_idx6[0]))

pointcloud_1 = np.delete(pointcloud, remove_idx, 0)

return pointcloud_1

# 点云显示
def view_cloud(pointcloud):
cloud = pcl.PointCloud_PointXYZRGBA()
cloud.from_array(pointcloud)

try:
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorACloud(cloud)
v = True
while v:
v = not (visual.WasStopped())
except:
pass

if __name__ == '__main__':

i = 3
string = 're'
# 读取数据集的图片
iml = cv2.imread('./%sLift%d.bmp' %(string,i) )  # 左图
imr = cv2.imread('./%sRight%d.bmp'%(string,i) ) # 右图
height, width = iml.shape[0:2]

print("width = %d \n"  % width)
print("height = %d \n" % height)

# 读取相机内参和外参
config = stereoconfig_040_2.stereoCamera()

# 立体校正
map1x, map1y, map2x, map2y, Q = getRectifyTransform(height, width, config)  # 获取用于畸变校正和立体校正的映射矩阵以及用于计算像素空间坐标的重投影矩阵
iml_rectified, imr_rectified = rectifyImage(iml, imr, map1x, map1y, map2x, map2y)

print("Print Q!")
print(Q)

# 绘制等间距平行线，检查立体校正的效果
line = draw_line(iml_rectified, imr_rectified)
cv2.imwrite('./%s检验%d.png' %(string,i), line)

# 消除畸变
iml = undistortion(iml, config.cam_matrix_left, config.distortion_l)
imr = undistortion(imr, config.cam_matrix_right, config.distortion_r)

# 立体匹配
iml_, imr_ = preprocess(iml, imr)  # 预处理，一般可以削弱光照不均的影响，不做也可以

iml_rectified_l, imr_rectified_r = rectifyImage(iml_, imr_, map1x, map1y, map2x, map2y)

disp, _ = stereoMatchSGBM(iml_rectified_l, imr_rectified_r, True)
cv2.imwrite('./%s视差%d.png' %(string,i), disp)

# 计算像素点的3D坐标（左相机坐标系下）
points_3d = cv2.reprojectImageTo3D(disp, Q)  # 可以使用上文的stereo_config.py给出的参数

#points_3d = points_3d

# 鼠标点击事件
def onMouse(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
print('点 (%d, %d) 的三维坐标 (%f, %f, %f)' % (x, y, points_3d[y, x, 0], points_3d[y, x, 1], points_3d[y, x, 2]))
dis = ( (points_3d[y, x, 0] ** 2 + points_3d[y, x, 1] ** 2 + points_3d[y, x, 2] **2) ** 0.5) / 1000
print('点 (%d, %d) 距离左摄像头的相对距离为 %0.3f m' %(x, y, dis) )

# 显示图片
cv2.namedWindow("disparity",0)
cv2.imshow("disparity", disp)
cv2.setMouseCallback("disparity", onMouse, 0)

# 构建点云--Point_XYZRGBA格式
pointcloud = DepthColor2Cloud(points_3d, iml)

# 显示点云
view_cloud(pointcloud)

cv2.waitKey(0)
cv2.destroyAllWindows()


## 3、代码打包下载地址

本人瞎写的代码，如有错误，还请见谅！

## 4、查看实现的效果

直接使用python运行上边打包的代码的dianyuntu.py即可，如果报错，就是环境没有搭建好，自行解决吧。

PS. 我的摄像头的分辨率是1920*720，所以效果一般。
得到的视差图：

得到的点云图：

展开全文
• 最近高摄像头项目，顺便扩展学习...代码： #!/usr/bin/python # -*- coding: utf-8 -*- import cv2 import time AUTO = True # 自动拍照，或手动按s键拍照 INTERVAL = 2 # 自动拍照间隔 cv2.namedWindow("left")...

最近高摄像头项目，顺便扩展学习python+opencv的图片处理和视频处理。

拍照效果：

代码：

#!/usr/bin/python
# -*- coding: utf-8 -*-
import cv2
import time

AUTO = True  # 自动拍照，或手动按s键拍照
INTERVAL = 2 # 自动拍照间隔

cv2.namedWindow("left")
cv2.namedWindow("right")
cv2.moveWindow("left", 0, 0)
cv2.moveWindow("right", 400, 0)
left_camera = cv2.VideoCapture(0)
#cv2.waitKey(50)
#left_camera.set(cv2.CV_CAP_PROP_FRAME_WIDTH,320)
#left_camera.set(cv2.CV_CAP_PROP_FRAME_HEIGHT,240)
right_camera = cv2.VideoCapture(1)
#right_camera.set(cv2.CV_CAP_PROP_FRAME_WIDTH,320)
#right_camera.set(cv2.CV_CAP_PROP_FRAME_HEIGHT,240)

counter = 0
utc = time.time()
pattern = (12, 8) # 棋盘格尺寸
folder = "./snapshot/" # 拍照文件目录

def shot(pos, frame):
global counter
path = folder + pos + "_" + str(counter) + ".jpg"

cv2.imwrite(path, frame)
print("snapshot saved into: " + path)

while True:

cv2.imshow("left", left_frame)
cv2.imshow("right", right_frame)

now = time.time()
if AUTO and now - utc >= INTERVAL:
shot("left", left_frame)
shot("right", right_frame)
counter += 1
utc = now

key = cv2.waitKey(1)
if key == ord("q"):
break
elif key == ord("s"):
shot("left", left_frame)
shot("right", right_frame)
counter += 1

left_camera.release()
right_camera.release()
cv2.destroyWindow("left")
cv2.destroyWindow("right")

展开全文
• 基于C++的双摄像头的立体双目测距算法。可以实现对物体目标的捕捉 。
• 利用VS2013+OpenCV3.2+C++语言，实现了双目立体匹配和测距
• 已经测试过的Opencv3.2版本双目测距代码，效果还行。得出的结果与实际距离有一点差值，经过线性拟合后效果很不错，误差在几个厘米内。
• 可以先+Q，在下载哦～ 首先pip install -r rerequirements.txt 搭建好yolov5的环境 搭建好yolov5的环境后直机运行 python detect_and_stereo_video_003.py
• 关于视觉双目的测量，网上虽然有很多资料，但是但是你懂的，网上很多资源都讲的很模糊，不完整。我这个代码完整的计算出了深度信息。前提是你标定作准了。
• 实现双目测距功能，需要python环境，安装OpenCV，自行标定
• 使用双目摄像头，基于opencv的测距程序
• 双目测距的操作流程有四步：相机标定——双目校正——双目匹配——计算深度，具体内容参考 ： https://blog.csdn.net/qq_38236355/article/details/88933839 其中相机标定通常用matlab现成的工具箱进行标定，具体...

双目测距的操作流程有四步：相机标定——双目校正——双目匹配——计算深度，具体内容参考 ： https://blog.csdn.net/qq_38236355/article/details/88933839

其中相机标定通常用matlab现成的工具箱进行标定，具体操作参考： https://blog.csdn.net/qq_38236355/article/details/89280633

我们接下来在完成相机标定的基础上，用标定得到的数据，按上述流程对双目深度进行计算。如果用的是任意的两个单目摄像头拼成的双目相机，则需要按上述所说进行双目校正；如果用的是成品的双目相机，则不需要进行双目校正，商家已经对相机校正好了。

一、双目校正+BM算法双目匹配生成视差图+生成深度图

#include <opencv2\opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;

const int imagewidth = 752;
const int imageheigh = 480; // 摄像头分辨率752*480
Size imageSize = Size(imagewidth, imageheigh);

Mat grayImageL;
Mat grayImageR;
Mat rectifyImageL, rectifyImageR;
Mat disp, disp8;//视差图

Rect vaildROIL;
Rect vaildROIR;//图像校正后，会对图像进行裁剪，这里的vaildROIR就是指裁剪之后的区域

Mat maplx, maply, mapRx, mapRy;//映射表
Mat Rl, Rr, Pl, Pr, Q; //校正旋转矩阵R，投影矩阵P，重投影矩阵Q
Mat xyz;
int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr<StereoBM>bm = StereoBM::create(16, 9);

//事先用matlab标定好的相机参数
//matlab里的左相机标定参数矩阵
Mat cameraMatrixL = (Mat_<float>(3, 3) << 165.9419, -0.2471, 372.8349, 0, 164.8281, 325.8182, 0, 0, 1);
//matlab里左相机畸变参数
Mat distCoeffL = (Mat_<double>(5, 1) << -0.0218, -0.0014, -0.0104, -0.0025, -0.000024286);
//matlab右相机标定矩阵
Mat cameraMatrixR = (Mat_<double>(3, 3) << 168.2781, 0.1610, 413.2010, 0, 167.7710, 304.7487, 0, 0, 1);
//matlab右相机畸变参数
Mat distCoffR = (Mat_<double>(5, 1) << -0.0332, 0.0033, -0.0090, -0.0029, -0.00038324);
//matlab T 平移参数
Mat T = (Mat_<double>(3, 1) << -117.2789, -0.8970, 0.9281);
//旋转矩阵
Mat R = (Mat_<double>(3, 3) << 1.0000, -0.0040, -0.000052, 0.0040, 1.0000, -0.0041, 0.0000683, 0.0041, 1.0000);
//立体匹配 BM算法
void stereo_match(int, void*)
{
bm->setROI1(vaildROIL);
bm->setROI2(vaildROIR);
bm->setPreFilterCap(31);
bm->setMinDisparity(0);//最小视差，默认值为0
bm->setNumDisparities(numDisparities * 16 + 16);//视差窗口，最大视差值与最小视差值之差
bm->setTextureThreshold(10);
bm->setUniquenessRatio(uniquenessRatio);//可以防止误匹配
bm->setSpeckleWindowSize(100);
bm->setSpeckleRange(32);
bm->setDisp12MaxDiff(-1);
bm->compute(rectifyImageL, rectifyImageR, disp);//生成视差图
disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16) * 16.));//计算出的视差是CV_16S格式,将其转化为CV_8U的形式
reprojectImageTo3D(disp, xyz, Q, true);
xyz = xyz * 16;//在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
imshow("disp image", disp8);
}
//生成深度图
void disp2Depth(int, void*)
{
Mat depthMap = Mat(disp.size(), CV_16UC1, 255);

int type = disp.type();
float fx = cameraMatrixL.at<float>(0, 0);
float fy = cameraMatrixL.at<float>(1, 1);
float cx = cameraMatrixL.at<float>(0, 2);
float cy = cameraMatrixL.at<float>(1, 2);

float baseline = 40; //基线距离40mm

if (type == CV_16S)
{
int height = disp.rows;
int width = disp.cols;
short* dispData = (short*)disp.data;
ushort* depthData = (ushort*)depthMap.data;
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
int id = i * width + j;
if (!dispData[id])
{
continue;
}
depthData[id] = ushort((float)fx*baseline / ((float)dispData[id]));
}
}
}
else
{
cout << "please onfirm dispImage's type" << endl;
}
Mat depthMap8;
depthMap.convertTo(depthMap8, CV_8U);//计算出的深度图是CV_16U格式
imshow("depth image", depthMap8);
}
int main()
{
//双目校正
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, -1, imageSize, &vaildROIL, &vaildROIR);
initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl , imageSize, CV_32FC1, maplx, maply);
initUndistortRectifyMap(cameraMatrixR, distCoffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
//读取图像
if (grayImageR.empty() || grayImageL.empty())
{
return -1;
}
imshow("imageL before rectify", grayImageL);
imshow("imageR bedore rectify", grayImageR);
//经过remap之后，左右相机的图像已经共面并且行对准了
remap(grayImageL, rectifyImageL, maplx, maply, INTER_LINEAR);
remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
//把校正结果显示出来
imshow("imageL after rectify", rectifyImageL);
imshow("imageR after rectify", rectifyImageR);
//显示在同一张图上
Mat canvas;
double sf;
int w, h;
sf = 600. / MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width*sf);
h = cvRound(imageSize.height*sf);
canvas.create(h, w * 2, CV_8UC1);
//左图像画到画布上
Mat canvaspart = canvas(Rect(w * 0, 0, w, h));
resize(rectifyImageL, canvaspart, canvaspart.size(), 0, 0, INTER_AREA);
Rect vroil(cvRound(vaildROIL.x*sf), cvRound(vaildROIL.y*sf), cvRound(vaildROIL.width*sf), cvRound(vaildROIL.height*sf));
cout << "Painted imagel" << endl;
//右图像画到画布上
canvaspart = canvas(Rect(w, 0, w, h));
resize(rectifyImageR, canvaspart, canvaspart.size(), 0, 0, INTER_LINEAR);
Rect vroiR(cvRound(vaildROIR.x*sf), cvRound(vaildROIR.y*sf), cvRound(vaildROIR.width*sf), cvRound(vaildROIR.height*sf));
cout << "Painted imageR" << endl;
//画上对应线条
for (int i = 0; i < canvas.rows; i += 16)
{
line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
imshow("rectified", canvas);
}
//立体匹配
namedWindow("disp image", CV_WINDOW_AUTOSIZE);
createTrackbar("blocksize:\n", "disp image", &blockSize, 8, stereo_match);
createTrackbar("UniquenessRatio:\n", "disp image", &uniquenessRatio, 50, stereo_match);
createTrackbar("NumDisparities:\n", "disp image", &numDisparities, 16, stereo_match);
stereo_match(0, 0);
disp2Depth(0, 0);
waitKey(0);
return 0;
}


二、成品双目无双目校正，BM算法双目匹配生成视差图+生成深度图

#include <opencv2/opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;

const int imagewidth = 752;
const int imageheigh = 480; // 摄像头分辨率752*480
Size imageSize = Size(imagewidth, imageheigh);

int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr<StereoBM> bm = StereoBM::create(16, 9); //用Block Matching算法,实现双目匹配

Mat grayImageL, grayImageR , disp, disp8;

Rect vaildROIL = Rect(0, 0, 752, 480);
Rect vaildROIR = Rect(0, 0, 752, 480);

Mat cameraMatrixL = (Mat_<float>(3, 3) << 165.9419, -0.2471, 372.8349, 0, 164.8281, 325.8182, 0, 0, 1);

void stereo_match(int, void*); //双目匹配,生成视差图（xl-xr）
void disp2Depth(int, void*); //视差图转为深度图

int main()
{
if (grayImageL.empty() || grayImageR.empty())
{
}
imshow("input ImageL", grayImageL);
imshow("input ImageR", grayImageR);

//显示在同一张图上
Mat canvas;
double sf;
int w, h;
sf = 600. / MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width*sf);
h = cvRound(imageSize.height*sf);
canvas.create(h, w * 2, CV_8UC1);
//左图像画到画布上
Mat canvaspart = canvas(Rect(w * 0, 0, w, h));
resize(grayImageL, canvaspart, canvaspart.size(), 0, 0, INTER_AREA);
Rect vroil(cvRound(vaildROIL.x*sf), cvRound(vaildROIL.y*sf), cvRound(vaildROIL.width*sf), cvRound(vaildROIL.height*sf));
cout << "Painted imagel" << endl;
//右图像画到画布上
canvaspart = canvas(Rect(w, 0, w, h));
resize(grayImageR, canvaspart, canvaspart.size(), 0, 0, INTER_LINEAR);
Rect vroiR(cvRound(vaildROIR.x*sf), cvRound(vaildROIR.y*sf), cvRound(vaildROIR.width*sf), cvRound(vaildROIR.height*sf));
cout << "Painted imageR" << endl;
//画上对应线条
for (int i = 0; i < canvas.rows; i += 16)
{
line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
imshow("rectified", canvas);
}

//立体匹配
namedWindow("disparity", CV_WINDOW_AUTOSIZE);
createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);//创建视差唯一性百分比窗口 Trackbar
createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);// 创建视差窗口 Trackbar
stereo_match(0, 0);
disp2Depth(0, 0);

waitKey(0);
return 0;
}
void stereo_match(int, void*)
{
bm->setBlockSize(2 * blockSize + 5); //SAD窗口大小，5-21之间为宜
bm->setROI1(vaildROIL);
bm->setROI2(vaildROIR);
bm->setPreFilterCap(31);
bm->setMinDisparity(0); //最小视差
bm->setNumDisparities(numDisparities * 16 + 16);
bm->setTextureThreshold(10);
bm->setUniquenessRatio(uniquenessRatio); //uniquenessRatio主要可以防止误匹配
bm->setSpeckleRange(32);
bm->setSpeckleWindowSize(100);
bm->setDisp12MaxDiff(-1);
bm->compute(grayImageL, grayImageR, disp); //输入图像必须为灰度图,输出视差图
disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
imshow("disparity", disp8);
}
void disp2Depth(int, void*)
{
Mat depthMap = Mat(disp.size(), CV_16UC1, 255);

int type = disp.type();
float fx = cameraMatrixL.at<float>(0, 0);
float fy = cameraMatrixL.at<float>(1, 1);
float cx = cameraMatrixL.at<float>(0, 2);
float cy = cameraMatrixL.at<float>(1, 2);

float baseline = 40; //基线距离40mm

if (type == CV_16S)
{
int height = disp.rows;
int width = disp.cols;
short* dispData = (short*)disp.data;
ushort* depthData = (ushort*)depthMap.data;
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
int id = i * width + j;
if (!dispData[id])
{
continue;
}
depthData[id] = ushort((float)fx*baseline / ((float)dispData[id]));
}
}
}
else
{
cout << "please onfirm dispImage's type" << endl;
}
Mat depthMap8;
depthMap.convertTo(depthMap8, CV_8U);//计算出的深度图是CV_16U格式
imshow("depth image", depthMap8);
}


我拍了一幅电脑的图片，效果如下图所示：

左右原图：

校正后放到同一平面：

视差图：

深度图：

展开全文
• 双目测距的基本流程 双目标定-->立体校正（含消除畸变）-->立体匹配-->视差计算-->深度计算/3D坐标计算 二 双目标定 参照之前的文章求出相册的内参，外参以及畸变函数，其中内参包括左右相机的fx...
• 修改内容：屏蔽了用cvblobslib实现的功能，但是主要功能标定以及测距都可以实现，而且不用安装那反人类的cvblobslib扩展库。 实现环境： 1.windows10 2.opencv 2.4.9 3.visual studio 2013 4.两颗微软HD-...
• 需求:使用双目摄像头得出物体3D坐标，本质就是利用双目来得到深度信息。 github代码 0 知识扫盲 相机模型 四大坐标关系及其关系 1 相机标定 Q1:用MATLAB标定还是opencv标定？ A1:两种我都试了。总结来说，直接...
• 好久没有写博客了，手都感觉好生疏，最近一直在研究工控机，水下机器人等，好不容易闲下来，没办法呀，继续前行吧，前面一直说双目测距，但没有高速大家如何获得这个双目标注得图像，我在这把这代码贴一下吧，获得...
• 博主在复现下面的代码时，遇到了一系列的问题，在Google在徘徊了好久才解决，先大概列出几个问题以及解决方法。https://github.com/jzbontar/mc-cnn问题一：运行下面代码时出错：./main.lua kitti fast -a predict -...
• 使用双目摄像头，基于opencv的测距程序
• 两个部分，一个是相机的参数设置，一个是测距运用matlab里面的stereo Camera Calibrator APP进行拍照 拍个30多张，然后拉线，留个10-20张进行计算，把双目摄像机的数据填到camera_configs.py里面camera_configs.py...
• 基于LINUX系统的双目测距系统的开发文档、双目摄像头的测距算法及相关注释