
import cv2
import numpy as np
 
# 左相机内参
left_camera_matrix = np.array([[416.841180253704, 0.0, 338.485167779639],
                                         [0., 416.465934495134, 230.419201769346],
                                         [0., 0., 1.]])
 
# 左相机畸变系数:[k1, k2, p1, p2, k3]
left_distortion = np.array([[-0.0170280933781798, 0.0643596519467521, -0.00161785356900972, -0.00330684695473645, 0]])
 
# 右相机内参
right_camera_matrix = np.array([[417.765094485395, 0.0, 315.061245379892],
                                          [0., 417.845058291483, 238.181766936442],
                                            [0., 0., 1.]])
# 右相机畸变系数:[k1, k2, p1, p2, k3]                                          
right_distortion = np.array([[-0.0394089328586398, 0.131112076868352, -0.00133793245429668, -0.00188957913931929, 0]])
 
# om = np.array([-0.00009, 0.02300, -0.00372])
# R = cv2.Rodrigues(om)[0]
 
# 旋转矩阵
R = np.array([[0.999962872853149, 0.00187779299260463, -0.00840992323112715],
                           [ -0.0018408858041373, 0.999988651353238, 0.00439412154902114],
                           [ 0.00841807904053251, -0.00437847669953504, 0.999954981430194]])
 
# 平移向量
T = np.array([[-120.326603502087], [0.199732192805711], [-0.203594457929446]])
 
size = (640, 480)
 
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion,
                                                                  right_camera_matrix, right_distortion, size, R,
                                                                  T)
 
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)

双目定标可以参考：双目视觉定标+矫正（基于MATLAB）

双目数据转化可以参考：双目视觉三维重建、测距 ---准备工作（数据转化）

四、双目测试实现

完整代码 主要包括main.py、camera_config.py两个文件的代码；main.py是主函数，实现双目视觉测距。相机参数用 camera_config.py表示。

main.py代码如下：


# -*- coding: utf-8 -*-
 
import numpy as np
import cv2
import camera_config
import random
import math
 
 
cap = cv2.VideoCapture(0)
cap.set(3, 1280)
cap.set(4, 480)  #打开并设置摄像头
 
 
# 鼠标回调函数
def onmouse_pick_points(event, x, y, flags, param):
    if event == cv2.EVENT_LBUTTONDOWN:
        threeD = param
        print('\n像素坐标 x = %d, y = %d' % (x, y))
        # print("世界坐标是：", threeD[y][x][0], threeD[y][x][1], threeD[y][x][2], "mm")
        print("世界坐标xyz 是：", threeD[y][x][0]/ 1000.0 , threeD[y][x][1]/ 1000.0 , threeD[y][x][2]/ 1000.0 , "m")
 
        distance = math.sqrt( threeD[y][x][0] **2 + threeD[y][x][1] **2 + threeD[y][x][2] **2 ) 
        distance = distance / 1000.0  # mm -> m
        print("距离是：", distance, "m")
 
WIN_NAME = 'Deep disp'
cv2.namedWindow(WIN_NAME,  cv2.WINDOW_AUTOSIZE)
 
while True:
  ret, frame = cap.read()
  frame1 = frame[0:480, 0:640]
  frame2 = frame[0:480, 640:1280]  #割开双目图像
 
  imgL = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)  # 将BGR格式转换成灰度图片
  imgR = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY)
 
  # cv2.remap 重映射，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。
  # 依据MATLAB测量数据重建无畸变图片
  img1_rectified = cv2.remap(imgL, camera_config.left_map1, camera_config.left_map2, cv2.INTER_LINEAR)
  img2_rectified = cv2.remap(imgR, camera_config.right_map1, camera_config.right_map2, cv2.INTER_LINEAR)  
 
  imageL = cv2.cvtColor(img1_rectified, cv2.COLOR_GRAY2BGR)  
  imageR = cv2.cvtColor(img2_rectified, cv2.COLOR_GRAY2BGR)
 
  # BM
  numberOfDisparities = ((640 // 8) + 15) & -16  # 640对应是分辨率的宽
 
  stereo = cv2.StereoBM_create(numDisparities=16, blockSize=9)  #立体匹配
  stereo.setROI1(camera_config.validPixROI1)
  stereo.setROI2(camera_config.validPixROI2)
  stereo.setPreFilterCap(31)
  stereo.setBlockSize(15)
  stereo.setMinDisparity(0)
  stereo.setNumDisparities(numberOfDisparities)
  stereo.setTextureThreshold(10)
  stereo.setUniquenessRatio(15)
  stereo.setSpeckleWindowSize(100)
  stereo.setSpeckleRange(32)
  stereo.setDisp12MaxDiff(1)
 
  disparity = stereo.compute(img1_rectified, img2_rectified) # 计算视差
 
  disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)  #归一化函数算法
 
  threeD = cv2.reprojectImageTo3D(disparity, camera_config.Q, handleMissingValues=True)  #计算三维坐标数据值
  threeD = threeD * 16 
 
  # threeD[y][x] x:0~640; y:0~480;   !!!!!!!!!!
  cv2.setMouseCallback(WIN_NAME, onmouse_pick_points, threeD)
 
  cv2.imshow("left", frame1)
  # cv2.imshow("right", frame2)
  # cv2.imshow("left_r", imgL)
  # cv2.imshow("right_r", imgR)
  cv2.imshow(WIN_NAME, disp)  #显示深度图的双目画面
 
  key = cv2.waitKey(1)
  if key == ord("q"):
    break
 
cap.release()
cv2.destroyAllWindows()

五、BM算法

BM，全称Bidirectional Matching，一种匹配算法。优点就是快，缺点是深度图的效果不是很好。

它是进行双向匹配的，首先通过匹配代价在右图中计算得出匹配点。然后相同的原理及计算在左图中的匹配点。比较找到的左匹配点和源匹配点是否一致，如果是，则匹配成功。

原理：将两个摄像头的的帧分成很多的小方块来机型匹配，通过移动小方块来匹配另一个图中的小方块，通过发现不同小方块在另一个图像中的像素点位置在结合两个摄像头的关系数据（标定的参数中的translate 和rotation矩阵）来计算出物体的实际深度从而生成相应的深度图。

参考：立体视觉BM算法原理一看就懂 - 知乎

下面将一些实用性的，如何调整BM中参数，达到不同环境有好的效果。

OpenCV中创建BM函数：

参数含义:

numDisparities

数量差异

视差搜索范围。对于每个像素算法都会找到从 0（默认最小视差）到 numDisparities 的最佳视差。然后可以通过更改最小视差来移动搜索范围。

blockSize

块大小

算法比较的块的线性大小。大小应该是奇数（因为块以当前像素为中心）。更大的块大小意味着更平滑但不太准确的视差图。较小的块大小提供更详细的视差图，但算法找到错误对应关系的机会更高。

该函数创建StereoBM对象。然后调用StereoBM::compute()来计算特定立体对的视差。

还想设置其他一些参数，部分如下：

详细参考官方的：OpenCV: cv::StereoBM Class Reference

BM算法示例：


numberOfDisparities = ((640 // 8) + 15) & -16  # 640对应是分辨率的宽
 
stereo = cv2.StereoBM_create(numDisparities=16, blockSize=9)  #立体匹配
stereo.setROI1(camera_config.validPixROI1)
stereo.setROI2(camera_config.validPixROI2)
stereo.setPreFilterCap(31)
stereo.setBlockSize(15)
stereo.setMinDisparity(0)
stereo.setNumDisparities(numberOfDisparities)
stereo.setTextureThreshold(10)
stereo.setUniquenessRatio(15)
stereo.setSpeckleWindowSize(100)
stereo.setSpeckleRange(32)
stereo.setDisp12MaxDiff(1)
 
disparity = stereo.compute(img1_rectified, img2_rectified) # 计算视差