opencv双目避障那些事

• 公司因为成本原因，让我做一个传统的双目避障。没办法磕一下吧。

• 双目避障流程

  • 拿到相机第一步肯定要标定了，最简单的方法，matlab2021b的工具箱就可以。这里面我把 Skew （Tangential Distortion上边那个）参数也勾选上了。
  • 因为是双目，所以要做正畸、矫正（对齐）。都是API看代码就懂了。
  • 读取到视频流后，通过SGBM的API计算视差图，从视差图得到深度图，并通过API反映射到三维坐标系，得到三维坐标，便可计算出Z坐标（距离）。
  • 因为是避障，所以光有距离是不够的，你还要对深度图进行一些列处理，得到障碍物的形状。
  • 我这里用的一些API就看代码吧，不介绍了。最后可以算出障碍物的信息。

• **问题：**速度特别慢，我发现，comput这一步（左右相机计算视差）会特别慢，还在想加速的办法。

• 下面上代码：
  这个代码，在深度图上点击鼠标可以输出坐标和距离（网上抄的（doge））。

import cv2
import numpy as np
import time
import random
import math


left_camera_matrix = np.array([[1.041391360906410e+03, -0.497373958828725, 6.529633882697765e+02], 
                                [0, 1.041945256148336e+03, 5.011346138132716e+02], 
                                [0, 0, 1]])
left_distortion = np.array([[-0.038865939454924, 0.141434746390239, 0.002010222102729, 0.002010222102729, 0]])

right_camera_matrix = np.array([[1.040740304791424e+03, -0.745746463223811, 6.214512440664031e+02], 
                                [0, 1.041706604301214e+03, 4.936376604155090e+02], 
                                [0, 0, 1]])
right_distortion = np.array([[-0.042478429626748, 0.149801715580196, 4.679743572389913e-04, 3.157700181865255e-04, 0]])

R = np.array([[0.999998129286225, -8.830788262094638e-04, -0.001720928771787],
              [8.814580091946381e-04, 0.999999167496347, -9.423578887644861e-04],
              [0.001721759515406, 9.408391994334367e-04, 0.999998075181034]])
T = np.array([-1.202972148785817e+02, 0.244005292586265, 0.294722278253404])

size = (1280, 720)

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

capture = cv2.VideoCapture("./Capture00004.avi")
ret,img = capture.read()

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("世界坐标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")

fps = 0.0
flag = 0
while True:
    t1 = time.time()
    ret,img = capture.read()
    imgl = img[0:720,0:1280]
    imgr = img[0:720,1280:2560]

    imgl = cv2.cvtColor(imgl,cv2.COLOR_BGR2GRAY)
    imgr = cv2.cvtColor(imgr,cv2.COLOR_BGR2GRAY)

    imgl_rectified = cv2.remap(imgl,left_map1,left_map2,cv2.INTER_LINEAR)
    imgr_rectified = cv2.remap(imgr,right_map1,right_map2,cv2.INTER_LINEAR)

    imagel = cv2.cvtColor(imgl_rectified,cv2.COLOR_GRAY2BGR)
    imager = cv2.cvtColor(imgr_rectified,cv2.COLOR_GRAY2BGR)

    blockSize = 8
    img_channels = 3
    stereo = cv2.StereoSGBM_create(minDisparity=1,numDisparities=112,
                                    blockSize=blockSize,
                                    P1=8 * img_channels * blockSize * blockSize,
                                    P2=32 * img_channels * blockSize * blockSize,
                                    disp12MaxDiff=-1,preFilterCap=1,uniquenessRatio=10,
                                    speckleWindowSize=100,speckleRange=100,
                                    mode=cv2.STEREO_SGBM_MODE_HH)


    disparity = stereo.compute(imgl_rectified,imgr_rectified)
    disp = cv2.normalize(disparity,disparity,alpha=0,beta=255,norm_type=cv2.NORM_MINMAX,dtype=cv2.CV_8U)
    threeD = cv2.reprojectImageTo3D(disparity,Q,handleMissingValues=True)
    threeD = threeD * 16

    cv2.setMouseCallback("before_threeD", onmouse_pick_points, threeD)


    for i in range(disp.shape[0]):
        for j in range(disp.shape[1]):
            Y = threeD[i][j][1] / 1000.0
            if Y < -1.0 or Y > 0.55:
                disp[i][j] = 0

    k = np.ones((10,10),dtype=np.uint8)
    disp = cv2.morphologyEx(disp,op=cv2.MORPH_CLOSE,kernel=k)
    disp = cv2.morphologyEx(disp,op=cv2.MORPH_OPEN,kernel=k)
    # disparity = cv2.GaussianBlur(disparity,(15,15),0,0)
    disp = cv2.medianBlur(disp,15,-1)

    before_threeD = disp.copy()
    ret,disp = cv2.threshold(disp,20,255,type=cv2.THRESH_BINARY)
    contours,hierarchy = cv2.findContours(disp,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)


    for i in range(len(contours)):
        area = cv2.contourArea(contours[i])
        if 15000 < area < 230400:
            x,y,w,h = cv2.boundingRect(contours[i])
            distance = math.sqrt(threeD[y+int(h/2)][x+int(w/2)][0] ** 2 + threeD[y+int(h/2)][x+int(w/2)][1] ** 2 + threeD[y+int(h/2)][x+int(w/2)][2] ** 2)
            distance = distance / 1000.0
            cv2.rectangle(imagel,(x,y),(x + w,y + h),(255,255,0),2)
            cv2.putText(imagel,str(distance),(x,y+50),fontFace=cv2.FONT_HERSHEY_COMPLEX_SMALL,fontScale=2,color=(225,0,0),thickness=2)
            cv2.circle(imagel,(x+int(w/2),y+int(h/2)),3,(255,0,0),-1)

    fps = (fps + (1. / (time.time() - t1))) / 2
    
    cv2.namedWindow("dis",cv2.WINDOW_NORMAL)
    cv2.namedWindow("imagel",cv2.WINDOW_NORMAL)
    cv2.namedWindow("before_threeD",cv2.WINDOW_NORMAL)
    cv2.imshow("dis",disp)
    cv2.imshow("imagel",imagel)
    cv2.imshow("before_threeD",before_threeD)
    if cv2.waitKey(10) & 0xff == ord('q'):
        break
    print(fps)
cv2.destroyAllWindows()

• 后续改了C++版我再更新，希望各路大佬多多提建议~