双目测距代码

#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'
LeftCameraDist_path='./output/LeftCoeff.npy'
RightCameraMtx_path='./output/RightMatrix.npy'
RightCameraDist_path='./output/RightCoeff.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):
    img=cv2.imread(path)
    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):
    img=cv2.imread(path)
    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:
        img=cv2.imread(cal_path+name)
        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:
        img=cv2.imread(cal_path+name)
        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)
    ret,pic=camera.read()
    print pic.shape
    camera.release()
    if cameraID==0:
	mtx=np.load(Leftmtx_path)
	dist=np.load(Leftdist_path)
    else:
	mtx=np.load(Rightmtx_path)
	dist=np.load(Rightdist_path)

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

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

    #加载两个摄像头的内参数矩阵和畸变矩阵
    Leftmtx=np.load(Leftmtx_path)
    Leftdist=np.load(Leftdist_path)
    Rightmtx=np.load(Rightmtx_path)
    Rightdist=np.load(Rightdist_path)
    
    #进行校准
    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)

    #加载单摄像头标定所得内参数和畸变矩阵
    cameraMatrixL=np.load(Leftmtx_path)
    distCoeffL=np.load(Leftdist_path)
    cameraMatrixR=np.load(Rightmtx_path)
    distCoeffR=np.load(Rightdist_path)
    
    #图片预处理
    for i in range(img_num):
	if i==29:
		continue
        LImage=cv2.imread(cal_path+str(i)+'L.jpg')
        RImage=cv2.imread(cal_path+str(i)+'R.jpg')
        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(LeftCameraDist_path,distCoeffL)
    np.save(RightCameraMtx_path,cameraMatrixR)
    np.save(RightCameraDist_path,distCoeffR)

    #计算旋转校准矩阵和投影矩阵，使得两张图共面对准
    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)
    ret,pic1=video0.read()
    video0.release()
        

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

    #加载两个摄像头的内参数矩阵和畸变矩阵
    Leftmtx=np.load(LeftCameraMtx_path)
    Leftdist=np.load(LeftCameraDist_path)
    Rightmtx=np.load(RightCameraMtx_path)
    Rightdist=np.load(RightCameraDist_path)
    
    #进行校准
    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()
    ret,pic1=video0.read()
    #a=time.time()
    video0.release()

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

    left_map1=np.load(left_map1_path)
    left_map2=np.load(left_map2_path)
    right_map1=np.load(right_map1_path)
    right_map2=np.load(right_map2_path)
    Q=np.load(Q_path)

    #加载两个摄像头的内参数矩阵和畸变矩阵
    Leftmtx=np.load(LeftCameraMtx_path)
    Leftdist=np.load(LeftCameraDist_path)
    Rightmtx=np.load(RightCameraMtx_path)
    Rightdist=np.load(RightCameraDist_path)
    
    #进行校准
        
    #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算法，有兴趣可以试试）
    stereo = cv2.StereoSGBM(minDisparity=0,numDisparities=256, SADWindowSize=3) 

    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)
    
    ret,pic1=video0.read()
    video0.release()

    ret2,pic2=video1.read()
    video1.release()

    print 'shot OK'
    
    left_map1=np.load(left_map1_path)
    left_map2=np.load(left_map2_path)
    right_map1=np.load(right_map1_path)
    right_map2=np.load(right_map2_path)
    Q=np.load(Q_path)

    #加载两个摄像头的内参数矩阵和畸变矩阵
    Leftmtx=np.load(LeftCameraMtx_path)
    Leftdist=np.load(LeftCameraDist_path)
    Rightmtx=np.load(RightCameraMtx_path)
    Rightdist=np.load(RightCameraDist_path)
    
    #根据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算法，有兴趣可以试试）
    stereo = cv2.StereoSGBM(minDisparity=0,numDisparities=256, SADWindowSize=3) 

    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()