双目测距 BM算法 Python版

目录

一、双目测距 效果

二、双目测距 流程思路

三、双目测距 前提准备

四、双目测试 实现

五、BM算法

小结

参考文献

一、双目测距 效果

2)双目摄像头校准；获得的参数:

左相机内参和左相机畸变系数：[k1,k2,p1,p2,k3]

右相机内参和右相机畸变系数：[k1,k2,p1,p2,k3]

旋转矩阵，左右相机之间的平移向量。并命名为：camera_config.py，需要使用以下测距。

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函数： 

 参数含义:

双目视觉 定标+矫正 （基于MATLAB）

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

欢迎交流；