关于传统视觉的SGBM双目测距算法

材料列表

双目摄像头：一般买回来的时候上面都是有一个胶盖的，需要拿下来再使用。如果是手动调焦的摄像头一定要小心，不要拧碎镜头（本人亲身经历）
开发环境：Matlab&&Python&&C++（三色绘恋）

双目摄像头长这样，一定要保护好镜头，很容易碎

我的双目是同时输出两个镜头的数据的，所以分辨率为：2560×720，需要拆分为两个1280×720，这里放一下双目摄像头拍照的C++代码。

#include <iostream>
#include "opencv2/opencv.hpp"
#include "string"

using namespace std;
using namespace cv;

int main() {
    // 双目视觉
    VideoCapture camera = VideoCapture(1); int time = 0;
    // 获取完成的双目图像
    camera.set(CAP_PROP_FRAME_WIDTH,2560);
    camera.set(CAP_PROP_FRAME_HEIGHT,720);
    while(!camera.isOpened())
        cout << "video is ending!" << endl;

    Mat frame,video;
    while(camera.read(frame)){
        int key = waitKeyEx(30);
		
        // 分割为左右摄像头
        resize(frame,video,Size(frame.cols/2,frame.rows/2));
        Rect left_rule = Rect(0,0,video.cols/2,video.rows);
        Rect right_rule = Rect(video.cols/2,0,video.cols/2,video.rows);
        // 展示图像
        imshow("left_camera",video(left_rule));
        imshow("right_camera",video(right_rule));

    	// 按S保存图片，这是极为必须的
        if(key == 's'){
            imwrite("C:/Users/28211/Desktop/camera1/" + to_string(time) + ".png",video(left_rule));
            imwrite("C:/Users/28211/Desktop/camera2/" + to_string(time) + ".png",video(right_rule));
            time++;
        }
        if (key == 27){
            break;
        }
    }

}

标定

首先知其然亦要知其所以然，我们首先需要知道摄像机的成像过程，我们知道摄像机通过凸透镜将事物的光折射到感光元件上形成了像。所谓摄像机的参数就是凸透镜的焦距、像距、畸变参数、旋转角、平移参数和畸变参数，通过标定像，就可以很容易的得到这些参数了，标定的目的就是如此。至于计算过程就是使用了凸透镜成像公式：

$\frac{1}{f}=\frac{1}{u}+\frac{1}{v}$

我们可以把这个过程看作是一个物理世界向量经过摄像机的矩阵变化为一个图片上的三维向量。我们有两种标定方式，使用线性标定就是进行比例缩放，而非线性标记就是进行谱分解。

双目摄像头使用前需要标定，也就是需要获取摄像机内参，我们需要一张标定图片，最好是将这张图片打印到一张A4纸上，同时打印结果也需要测量一下小方格的边长是否为25mm。

opencv

我们使用C++保存图片的项目进行拍摄（当然，用Python写一个也大同小异，不过我爱C++），随便拍一些就可以了，我们将左右目的照片保存到两个不同的文件夹内。

接下来我们打开Matlab，在命令行窗口内输入：

1	`stereoCameraCalibrator`

我们就会打开Matlab的标定程序。

我们在上面的Add image加入两个摄像头捕获的照片，然后按照下面图片的顺序进行点击：

按完三个按钮之后，我们就可以查看相关的参数了，我们关掉双目标定的界面，回到Matlab的主页面，我们可以看到多出来的参数：

我们把下面的内容复制到命令行窗口，我们就可以导出内参为表格了。

rowName = cell(1,10);
rowName{1,1} = '平移矩阵';
rowName{1,2} = '旋转矩阵';
rowName{1,3} = '相机1内参矩阵';
rowName{1,4} = '相机1径向畸变';
rowName{1,5} = '相机1切向畸变';
rowName{1,6} = '相机2内参矩阵';
rowName{1,7} = '相机2径向畸变';
rowName{1,8} = '相机2切向畸变';
rowName{1,9} = '相机1畸变向量';
rowName{1,10} = '相机2畸变向量';
xlswrite('out.xlsx',rowName(1,1),1,'A1');
xlswrite('out.xlsx',rowName(1,2),1,'A2');
xlswrite('out.xlsx',rowName(1,3),1,'A5');
xlswrite('out.xlsx',rowName(1,4),1,'A8');
xlswrite('out.xlsx',rowName(1,5),1,'A9');
xlswrite('out.xlsx',rowName(1,6),1,'A10');
xlswrite('out.xlsx',rowName(1,7),1,'A13');
xlswrite('out.xlsx',rowName(1,8),1,'A14');
xlswrite('out.xlsx',rowName(1,9),1,'A15');
xlswrite('out.xlsx',rowName(1,10),1,'A16');
xlswrite('out.xlsx',stereoParams.TranslationOfCamera2,1,'B1');  % 平移矩阵
xlswrite('out.xlsx',stereoParams.RotationOfCamera2.',1,'B2');  % 旋转矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters1.IntrinsicMatrix.',1,'B5');  % 相机1内参矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters1.RadialDistortion,1,'B8');  % 相机1径向畸变(1,2,5)
xlswrite('out.xlsx',stereoParams.CameraParameters1.TangentialDistortion,1,'B9');  % 相机1切向畸变(3,4)
xlswrite('out.xlsx',stereoParams.CameraParameters2.IntrinsicMatrix.',1,'B10');  % 相机2内参矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters2.RadialDistortion,1,'B13');  % 相机2径向畸变(1,2,5)
xlswrite('out.xlsx',stereoParams.CameraParameters2.TangentialDistortion,1,'B14');  % 相机2切向畸变(3,4)
xlswrite('out.xlsx',[stereoParams.CameraParameters1.RadialDistortion(1:2), stereoParams.CameraParameters1.TangentialDistortion,...
    stereoParams.CameraParameters1.RadialDistortion(3)],1,'B15');  % 相机1畸变向量
xlswrite('out.xlsx',[stereoParams.CameraParameters2.RadialDistortion(1:2), stereoParams.CameraParameters2.TangentialDistortion,...
    stereoParams.CameraParameters2.RadialDistortion(3)],1,'B16');  % 相机2畸变向量

我们拖出out.xlsx文件，然后打开即可。这样我们完成了双目标定。

使用参数

我所购买的摄像机是无畸变的（并不是绝对没有畸变，只是芯片预处理了），我们按项填入下面的代码内：

import numpy as np
import cv2
import pcl


class stereoCamera(object):
    def __init__(self):
        # 左相机内参
        self.cam_matrix_left = np.array([[368.675898674749, -1.50245100976057, 308.223174252045],
                                         [0, 369.172983810346, 184.420046614954],
                                         [0, 0, 1]])
        # 右相机内参
        self.cam_matrix_right = np.array([[374.452750889965, -0.933263166338735, 284.273373661522],
                                          [0, 374.79807385038, 196.866606672554],
                                          [0, 0, 1]])

        # 左右相机畸变系数:[k1, k2, p1, p2, k3],其中k为径向畸变，p为切向畸变，k3为畸变系数，没有的话写0.
        self.distortion_l = np.array([[-0.0169072200281734, -0.0393656307016634, 0.00315587347941657, -0.00142406381437646, 0]])

        self.distortion_r = np.array([[-0.137714949099853, 0.347043476849145,
                                       0.00882677131371728, -0.0137088374523692, 0]])

        # 旋转矩阵
        self.R = np.array([[0.997827201730772, 0.000619636065025224, 0.0658824068874556],
                           [0.00205989330848618, 0.999173532016587, -0.0405956864340979],
                           [-0.0658531117388919, 0.0406431909259702, 0.997001253111379]])

        # 平移矩阵
        self.T = np.array([[-60.1391668881605], [-0.22679212052219], [4.53161173378334]])

        # 焦距
        self.focal_length = 859.367

        # 基线距离
        self.baseline = 61


# 预处理
def preprocess(img1, img2):
    # 彩色图->灰度图
    if (img1.ndim == 3):  # 判断为三维数组
        img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)  # 通过OpenCV加载的图像通道顺序是BGR
    if (img2.ndim == 3):
        img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

    # 直方图均衡
    img1 = cv2.equalizeHist(img1)
    img2 = cv2.equalizeHist(img2)

    return img1, img2


# 消除畸变
def undistortion(image, camera_matrix, dist_coeff):
    undistortion_image = cv2.undistort(image, camera_matrix, dist_coeff)

    return undistortion_image


# 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵
# @param：config是一个类，存储着双目标定的参数:config = stereoconfig.stereoCamera()
def getRectifyTransform(height, width, config):
    # 读取内参和外参
    left_K = config.cam_matrix_left
    right_K = config.cam_matrix_right
    left_distortion = config.distortion_l
    right_distortion = config.distortion_r
    R = config.R
    T = config.T

    # 计算校正变换
    R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion,
                                                      (width, height), R, T, alpha=0)

    map1x, map1y = cv2.initUndistortRectifyMap(left_K, left_distortion, R1, P1, (width, height), cv2.CV_32FC1)
    map2x, map2y = cv2.initUndistortRectifyMap(right_K, right_distortion, R2, P2, (width, height), cv2.CV_32FC1)

    return map1x, map1y, map2x, map2y, Q


# 畸变校正和立体校正
def rectifyImage(image1, image2, map1x, map1y, map2x, map2y):
    rectifyed_img1 = cv2.remap(image1, map1x, map1y, cv2.INTER_AREA)
    rectifyed_img2 = cv2.remap(image2, map2x, map2y, cv2.INTER_AREA)

    return rectifyed_img1, rectifyed_img2


# 立体校正检验----画线
def draw_line(image1, image2):
    # 建立输出图像
    height = max(image1.shape[0], image2.shape[0])
    width = image1.shape[1] + image2.shape[1]

    output = np.zeros((height, width, 3), dtype=np.uint8)
    output[0:image1.shape[0], 0:image1.shape[1]] = image1
    output[0:image2.shape[0], image1.shape[1]:] = image2

    # 绘制等间距平行线
    line_interval = 50  # 直线间隔：50
    for k in range(height // line_interval):
        cv2.line(output, (0, line_interval * (k + 1)), (2 * width, line_interval * (k + 1)), (0, 255, 0), thickness=2,
                 lineType=cv2.LINE_AA)

    return output


# 视差计算
def stereoMatchSGBM(left_image, right_image, down_scale=False):
    # SGBM匹配参数设置
    if left_image.ndim == 2:
        img_channels = 1
    else:
        img_channels = 3
    blockSize = 3
    paraml = {'minDisparity': 0,
              'numDisparities': 128,
              'blockSize': blockSize,
              'P1': 8 * img_channels * blockSize ** 2,
              'P2': 32 * img_channels * blockSize ** 2,
              'disp12MaxDiff': 1,
              'preFilterCap': 63,
              'uniquenessRatio': 15,
              'speckleWindowSize': 100,
              'speckleRange': 1,
              'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
              }

    # 构建SGBM对象
    left_matcher = cv2.StereoSGBM_create(**paraml)
    paramr = paraml
    paramr['minDisparity'] = -paraml['numDisparities']
    right_matcher = cv2.StereoSGBM_create(**paramr)

    # 计算视差图
    size = (left_image.shape[1], left_image.shape[0])
    if down_scale == False:
        disparity_left = left_matcher.compute(left_image, right_image)
        disparity_right = right_matcher.compute(right_image, left_image)

    else:
        left_image_down = cv2.pyrDown(left_image)
        right_image_down = cv2.pyrDown(right_image)
        factor = left_image.shape[1] / left_image_down.shape[1]

        disparity_left_half = left_matcher.compute(left_image_down, right_image_down)
        disparity_right_half = right_matcher.compute(right_image_down, left_image_down)
        disparity_left = cv2.resize(disparity_left_half, size, interpolation=cv2.INTER_AREA)
        disparity_right = cv2.resize(disparity_right_half, size, interpolation=cv2.INTER_AREA)
        disparity_left = factor * disparity_left
        disparity_right = factor * disparity_right

    # 真实视差（因为SGBM算法得到的视差是×16的）
    trueDisp_left = disparity_left.astype(np.float32) / 16.
    trueDisp_right = disparity_right.astype(np.float32) / 16.

    return trueDisp_left, trueDisp_right


if __name__ == '__main__':

    i = 3

    video = cv2.VideoCapture(1)

    video.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
    video.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
    if video.isOpened():
        while True:
            ret, frame = video.read()
            if not ret:
                print("Video End!")
                break
            iml = frame[0:720, 0:1280]
            imr = frame[0:720, 1280:2560]

            height, width = iml.shape[0:2]

            # 读取相机内参和外参
            config = stereoCamera()

            # 立体校正
            map1x, map1y, map2x, map2y, Q = getRectifyTransform(height, width,
                                                                config)  # 获取用于畸变校正和立体校正的映射矩阵以及用于计算像素空间坐标的重投影矩阵
            iml_rectified, imr_rectified = rectifyImage(iml, imr, map1x, map1y, map2x, map2y)

            # 绘制等间距平行线，检查立体校正的效果
            line = draw_line(iml_rectified, imr_rectified)

            # 立体匹配
            # iml_, imr_ = preprocess(iml, imr)  # 预处理，一般可以削弱光照不均的影响，不做也可以
            # iml_rectified_l, imr_rectified_r = rectifyImage(iml_, imr_, map1x, map1y, map2x, map2y)
            disp, _ = stereoMatchSGBM(iml_, imr_, True)

            # 计算像素点的3D坐标（左相机坐标系下）
            points_3d = cv2.reprojectImageTo3D(disp, Q)  # 可以使用上文的stereo_config.py给出的参数


            # 鼠标点击事件
            def onMouse(event, x, y, flags, param):
                if event == cv2.EVENT_LBUTTONDOWN:
                    dis = ((points_3d[y, x, 0] ** 2 + points_3d[y, x, 1] ** 2 + points_3d[y, x, 2] ** 2) ** 0.5) / 500
                    print('点的相对距离为 %0.3f cm' % dis)


            cv2.namedWindow("disparity", 0)
            cv2.imshow("disparity", disp)
            cv2.setMouseCallback("disparity", onMouse, 0)

            # 实拍参考图
            cv2.namedWindow("iml", 0)
            cv2.imshow("iml", iml)

            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
            pass
        pass