第四章 Ubuntu16.04 Python/Opencv实现双目定位及测距

1、安装Python以及jupyter

本平台是Ubuntu16.04，需要安装python环境，我安装的是python3版本。

（1）首先安装python3

sudo apt-get install python3
#若要安装指定版本的python，这地方可以更改成你需要的版本,比如你要安装3.6版本：
sudo apt-get install python3.6 python3.6-dev

（2）切换python优先级

安装好后会在 /usr/bin/ 下出现 python3.6 这个可执行文件，现在需要切换版本(默认版本2.7)
如下就是设置python3.6的路径，并设置优先级为200，默认优先级最高的优先使用

sudo update-alternatives --install /usr/bin/python python /usr/bin/python3.6 200

所以如果想要使用系统默认的2.7版本，需要键入

sudo update-alternatives –install /usr/bin/python python /usr/bin/python2.7 250 

并设置优先级大于其他的版本 

（3）安装pip

当前python版本为3.x，输入如下命令：

sudo apt-get install python3-pip python3-dev

若python版本为2.7，则输入如下命令：

sudo apt-get install python-pip python-dev

在装tensorflow之前，不管是不是最新的pip版本，都要更新一下，具体命令如下：

python 2.7版本：
sudo pip install --upgrade pip
python 3.x版本：
sudo pip3 install --upgrade pip

（4）安装jupyter以及相关依赖包

若是Python3，把pip换成pip3

pip install pillow
pip install lxml
pip install jupyter
pip install matplotlib

在终端执行：

jupyter notebook

 会出现一个网址，如下图：

 打开网址，便进入到网页版的jupyter

注意：在哪个目录打开jupyter notebook，则该目录为根目录。 

2、安装opencv-python

这里不需要进入官网下载opencv源码进行繁琐的安装，安装opencv直接在终端执行：

sudo pip install opencv-python

查看opencv版本，执行：

pkg-config --modversion opencv

下图表安装成功 ，我的是3.3.1

下面写一个测试代码实现摄像头实时捕捉： 

import cv2
import numpy as np

cap = cv2.VideoCapture(0)
while(1):
    # get a frame
    ret, frame = cap.read()
    # show a frame
    cv2.imshow("capture", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
cap.release()
cv2.destroyAllWindows() 

 同样在jupyter上点击run all，或者按Shift+Eenter，就会显示出视频流

3、对双目摄像机进行标定

首先得买个双目摄像机吧！

我用的是matlab标定（棋盘格方法），我是先分别对左右两个图形一幅一幅的抓取，20*2=40张，然后按照官网步骤一步一步来，过程还是比较繁琐，没办法！标定的好坏决定最后的精度。

matlab标定参考官网：http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html

下面是抓取一张图片的代码,按q结束并抓拍一张图片。

#读取视频，按q结束并抓拍一张图片
import cv2
import numpy as np

cap = cv2.VideoCapture(0)
while(1):
    # get a frame
    ret, frame = cap.read()
    # show a frame
    cv2.imshow("capture", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        cv2.imwrite('camera1.jpg', frame)
        break
cap.release()
cv2.destroyAllWindows() 

标定完之后，新建一个camera_configs.py文件，里面专门用来存储两个摄像头标定好的参数。

import cv2
import numpy as np

#左摄像头参数
left_camera_matrix = np.array([[1394.26663, 0, 343.36581],
                               [0, 1393.25572, 206.56092],
                               [0, 0, 1]])
left_distortion = np.array([[-0.66451, 0.84137, -0.01001, -0.00274, 0]])

#右摄像头参数
right_camera_matrix = np.array([[1385.46346, 0, 344.38903],
                                [0, 1385.09596, 197.18927],
                                [0, 0, 1]])
right_distortion = np.array([[-0.63339, -0.68796, -0.00491, -0.00675, 0]])

om = np.array([0.00456, 0.01463, 0.00042])        # 旋转关系向量
R = cv2.Rodrigues(om)[0]                           # 使用Rodrigues变换将om变换为R
T = np.array([-59.63351, -0.15514, -0.35781])      # 平移关系向量

size = (640, 360) # 图像尺寸

# 进行立体更正
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion,
                                                                  right_camera_matrix, right_distortion, size, R,
                                                                  T)
# 计算更正map
left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)

4、利用BM匹配算法实现双目测距

双目测距原理就不说了，给一篇参考博客：https://blog.csdn.net/u013832707/article/details/53781810

BM匹配算法可参考博客：https://blog.csdn.net/lg1259156776/article/details/53103527

新建一个stereo.py，代码如下：

import cv2
import numpy as np
import camera_configs

cv2.namedWindow("depth")
cv2.moveWindow("left", 0, 0)
cv2.moveWindow("right", 640, 0)
cv2.createTrackbar("num", "depth", 2, 10, lambda x: None)
cv2.createTrackbar("blockSize", "depth", 5, 255, lambda x: None)

# 添加点击事件，打印当前点的距离q
def callbackFunc(e, x, y, f, p):
    if e == cv2.EVENT_LBUTTONDOWN:        
        print (threeD[y][x])

cv2.setMouseCallback("depth", callbackFunc, None)

cap = cv2.VideoCapture(0)   

while True:
    ret1, frame = cap.read()
    #ret2, frame2 = camera2.read()
    #if not ret1 or not ret2:
    if ret1 != True:
        break
    cv2.resize(frame,(2560,360),interpolation=cv2.INTER_LINEAR)  
    dsize = (1280, 360)
    imagedst = cv2.resize(frame,dsize,interpolation=cv2.INTER_LINEAR)
    
    frame1 = imagedst[0:360,0:640]
    frame2 = imagedst[0:360,640:1280]

    # 根据更正map对图片进行重构
    img1_rectified = cv2.remap(frame1, camera_configs.left_map1, camera_configs.left_map2, cv2.INTER_LINEAR)
    img2_rectified = cv2.remap(frame2, camera_configs.right_map1, camera_configs.right_map2, cv2.INTER_LINEAR)

    # 将图片置为灰度图，为StereoBM作准备
    imgL = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY)
    imgR = cv2.cvtColor(img2_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.StereoBM_create(numDisparities = 16*num, 
                                  blockSize = 31)
    
    disparity = stereo.compute(imgL, imgR)

    disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
    # 将图片扩展至3d空间中，其z方向的值则为当前的距离
    threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32)/16., camera_configs.Q)

    cv2.imshow("left", img1_rectified)
    cv2.imshow("right", img2_rectified)
    cv2.imshow("depth", disp)

    key = cv2.waitKey(1)
    if key == ord("q"):
        break
    elif key == ord("s"):
        cv2.imwrite(path_BM_left, imgL)
        cv2.imwrite(path_BM_right, imgR)
        cv2.imwrite(path_BM_depth, disp)
cap.release()
cv2.destroyAllWindows()

注意：需要把 camera_configs.py文件放在同一个目录。

测试结果：