双目相机实现双目测距主要分为4个步骤：相机标定、双目校正、双目匹配、计算深度信息。

(1）相机标定：需要对双目相机进行标定，得到两个相机的内外参数、单应矩阵。
(2) 双目校正：根据标定结果对原始图像进行校正，校正后的两张图像位于同一平面且互相平行。
(3）双目匹配：对校正后的两张图像进行像素点匹配。
(4）计算深度图：根据匹配结果计算每个像素的三维坐标，从而获得深度图。

双目相机实现三维重建主要分为2个步骤：构建点云、显示点云。目前主要使用Open3D和PCL这两个库来进行三维重建，选用的库不同，构建点云和显示点云的方式也不同。

原理很简单，利用了相似三角形计算目标与基线的距离，所以 **双目测距的主要任务在于前期摄像头的定标、双目图像点的特征匹配上。 **

双目相机根据图像来分主要有RGB+RGB、RGB+IR、IR+IR三种，根据数据输出来分主要有单USB接口、双USB接口两种。

基线不太建议太小，作为测试，一般baseline在3~9cm就可以满足需求。
从双目三维重建原理中可知，左右摄像头的成像平面尽可能在一个平面内，成像平面不在同一个平面的，尽管可以立体矫正，其效果也差很多。

本项目的双目相机为RGB+RGB，基线是固定的5cm，单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)

• 采集数据前，调节相机焦距，尽可能保证左右视图清晰度一致（目的是为了让左右相机的焦距尽量一致）
• 采集棋盘格图像时，标定板一般占视图1/2到1/3左右
• 一般采集15~30张左右

width=9                
height=5
left_video=0
right_video=-1
save_dir="data/"
 
python get_stereo_images.py \
    --left_video 0 \
    --right_video -1 \
    --width 9  \
    --height 5  \
    --save_dir "data/" \

参数说明：

1.参数width指的是棋盘格宽方向黑白格子相交点个数
2.参数height指的是棋盘格长方向黑白格子相交点个数
3.参数left_video是左路相机ID，一般就是相机连接主板的USB接口号
4.参数right_video是右路相机ID，一般就是相机连接主板的USB接口号
5.如果你的双目相机是单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)，则设置left_video=相机ID，而right_video=-1，
6.参数detect建议设置True，这样可实时检测棋盘格，方面调整角度
7.按键盘s或者c保存左右视图图片

下面是采集双目摄像头标定板左右视图的Python代码：Get_Stereo_Imgs.py，仅需依赖OpenCV。

import os
import argparse
import cv2
 
 
class StereoCamera(object):
    """采集双目标定图片，按键盘【c】或【s】保存图片"""
 
    def __init__(self, chess_width, chess_height, detect=False):
        """
        :param chess_width: chessboard width size，即棋盘格宽方向黑白格子相交点个数，
        :param chess_height: chessboard height size，即棋盘格长方向黑白格子相交点个数
        :param detect: 是否实时检测棋盘格，方便采集数据
        """
        self.chess_width = chess_width
        self.chess_height = chess_height
        self.detect = detect
        self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 
    def detect_chessboard(self, image):
        """检测棋盘格并显示"""
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, (self.chess_width, self.chess_height), None)
        if ret:
            # 角点精检测
            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria)
            # Draw and display the corners
            image = cv2.drawChessboardCorners(image, (self.chess_width, self.chess_height), corners2, ret)
        return image
 
    def capture2(self, left_video, right_video, save_dir):
        """
        用于采集双USB连接线的双目摄像头
        :param left_video:int or str,左路视频路径或者摄像头ID
        :param right_video:int or str,右视频路径或者摄像头ID
        :param save_dir: str,保存左右图片的路径
        :return:
        """
        self.create_file(save_dir)
        capL = cv2.VideoCapture(left_video)
        capR = cv2.VideoCapture(right_video)
        widthL, heightL, numFramesL, fpsL = self.get_video_info(capL)
        widthR, heightR, numFramesR, fpsR = self.get_video_info(capR)
        print("capL:\n", widthL, heightL, numFramesL, fpsL)
        print("capR:\n", widthR, heightR, numFramesR, fpsR)
        save_videoL = self.create_file(save_dir, "video", "left_video.avi")
        save_videoR = self.create_file(save_dir, "video", "right_video.avi")
        writerL = self.get_video_writer(save_videoL, widthL, heightL, fpsL)
        writerR = self.get_video_writer(save_videoR, widthR, heightR, fpsR)
        i = 0
        while True:
            isuccessL, frameL = capL.read()
            isuccessR, frameR = capR.read()
            if not (isuccessL and isuccessR):
                print("No more frames")
                break
            if self.detect:
                l = self.detect_chessboard(frameL.copy())
                r = self.detect_chessboard(frameR.copy())
            else:
                l = frameL.copy()
                r = frameR.copy()
            cv2.imshow('left', l)
            cv2.imshow('right', r)
            key = cv2.waitKey(10)
            if key == ord('q'):
                break
            elif key == ord('c') or key == ord('s'):
                print("save image:{:0=3d}".format(i))
                cv2.imwrite(os.path.join(save_dir, "left_{:0=3d}.png".format(i)), frameL)
                cv2.imwrite(os.path.join(save_dir, "right_{:0=3d}.png".format(i)), frameR)
                i += 1
            writerL.write(frameL)
            writerR.write(frameR)
        capL.release()
        capR.release()
        cv2.destroyAllWindows()
 
    def capture1(self, video, save_dir):
        """
        用于采集单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)
        :param video:int or str,视频路径或者摄像头ID
        :param save_dir: str,保存左右图片的路径
        """
        self.create_file(save_dir)
        cap = cv2.VideoCapture(video)
        width, height, numFrames, fps = self.get_video_info(cap)
        print("capL:\n", width, height, numFrames, fps)
        save_videoL = self.create_file(save_dir, "video", "left_video.avi")
        save_videoR = self.create_file(save_dir, "video", "right_video.avi")
        writerL = self.get_video_writer(save_videoL, int(width / 2), height, fps)
        writerR = self.get_video_writer(save_videoR, int(width / 2), height, fps)
        i = 0
        while True:
            isuccess, frame = cap.read()
            if not isuccess:
                print("No more frames")
                break
            # 分离左右摄像头
            frameL = frame[:, :int(width / 2), :]
            frameR = frame[:, int(width / 2):, :]
            if self.detect:
                l = self.detect_chessboard(frameL.copy())
                r = self.detect_chessboard(frameR.copy())
            else:
                l = frameL.copy()
                r = frameR.copy()
            cv2.imshow('left', l)
            cv2.imshow('right', r)
            key = cv2.waitKey(10)
            if key == ord('q'):
                break
            elif key == ord('c') or key == ord('s'):
                print("save image:{:0=3d}".format(i))
                cv2.imwrite(os.path.join(save_dir, "left_{:0=3d}.png".format(i)), frameL)
                cv2.imwrite(os.path.join(save_dir, "right_{:0=3d}.png".format(i)), frameR)
                i += 1
            writerL.write(frameL)
            writerR.write(frameR)
        cap.release()
        cv2.destroyAllWindows()
 
    @staticmethod
    def get_video_info(video_cap):
        width = int(video_cap.get(cv2.CAP_PROP_FRAME_WIDTH))
        height = int(video_cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
        numFrames = int(video_cap.get(cv2.CAP_PROP_FRAME_COUNT))
        fps = int(video_cap.get(cv2.CAP_PROP_FPS))
        return width, height, numFrames, fps
 
    @staticmethod
    def get_video_writer(save_path, width, height, fps):
        if not os.path.exists(os.path.dirname(save_path)):
            os.makedirs(os.path.dirname(save_path))
        fourcc = cv2.VideoWriter_fourcc(*'XVID')
        frameSize = (int(width), int(height))
        video_writer = cv2.VideoWriter(save_path, fourcc, fps, frameSize)
        print("video:width:{},height:{},fps:{}".format(width, height, fps))
        return video_writer
 
    @staticmethod
    def create_file(parent_dir, dir1=None, filename=None):
        out_path = parent_dir
        if dir1:
            out_path = os.path.join(parent_dir, dir1)
        if not os.path.exists(out_path):
            os.makedirs(out_path)
        if filename:
            out_path = os.path.join(out_path, filename)
        return out_path
 
 
def get_parser():
    width = 9
    height = 5
    left_video = 0
    right_video = -1
    save_dir = "data/"
    parser = argparse.ArgumentParser(description='Camera calibration')
    parser.add_argument('--width', type=int, default=width, help='chessboard width size')
    parser.add_argument('--height', type=int, default=height, help='chessboard height size')
    parser.add_argument('--left_video', type=int, default=left_video, help='left video file or camera ID')
    parser.add_argument('--right_video', type=int, default=right_video, help='right video file or camera ID')
    parser.add_argument('--save_dir', type=str, default=save_dir, help='YML file to save calibrate matrices')
    return parser
 
 
if __name__ == '__main__':
    args = get_parser().parse_args()
    stereo = StereoCamera(args.width, args.height, detect=args.detect)
    if args.left_video > -1 and args.right_video > -1:
        # 双USB连接线的双目摄像头
        stereo.capture2(left_video=args.left_video, right_video=args.right_video, save_dir=args.save_dir)
    elif args.left_video > -1:
        # 单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)
        stereo.capture1(video=args.left_video, save_dir=args.save_dir)
    else:
        raise Exception("Error: Check your camera{}".format(args.left_video, args.right_video))

双目标定的目标是获得左右两个相机的内参、外参和畸变系数，其中内参包括左右相机的fx，fy，cx，cy，外参包括左相机相对于右相机的旋转矩阵和平移向量，畸变系数包括径向畸变系数（k1， k2，k3）和切向畸变系数（p1，p2）。

双目标定工具最常用的莫过于是MATLAB的工具箱： Stereo Camera Calibrator App。但该项目中使用OpenCV来实现双目相机的标定。

（1）单目相机标定(获取畸变校正和立体校正的矩阵）
参数说明：

width=9                     # 横向网格数
height=5                    # 纵向网格数
image_dir="data/"           # 棋盘格图片所在文件夹路径
save_dir="config/"          # 标定结果文件保存路径
file_name="camera_params"   # 标定结果保存文件名

python Stereo_Camera_Calibration.py \
    --width 9  \
    --height 5  \
    --image_dir "data/" \
    --save_dir "data/" \
    --file_name "camera_params"\

代码如下：

#-*- coding:utf-8 -*-
import os
import numpy as np
import cv2
import glob
import argparse

import json
import pickle



class Stereo_Camera_Calibration(object):
    def __init__(self, width, height):
        """
        :param width: chessboard width size，即棋盘格宽方向黑白格子相交点个数，
        :param height: chessboard height size，即棋盘格长方向黑白格子相交点个数
        """
        self.width       = width
        self.height      = height

        # 设置迭代终止条件
        self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        self.criteria_stereo = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    # =========================== 双目标定 =============================== #
    def stereo_calibration(self, file_L, file_R):
        # 设置 object points, 形式为 (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
        objp = np.zeros((self.width * self.height, 3), np.float32)  #我用的是6×7的棋盘格，可根据自己棋盘格自行修改相关参数
        objp[:, :2] = np.mgrid[0:self.width, 0:self.height].T.reshape(-1, 2)

        # 用arrays存储所有图片的object points 和 image points
        objpoints = []  # 3d points in real world space
        imgpointsR = []  # 2d points in image plane
        imgpointsL = []

        for i in range(len(file_L)):                   
            ChessImaR = cv2.imread(file_L[i], 0)  # 右视图
            ChessImaL = cv2.imread(file_R[i], 0)  # 左视图
            retR, cornersR = cv2.findChessboardCorners(ChessImaR,(self.width, self.height), None)  # 提取右图每一张图片的角点
            retL, cornersL = cv2.findChessboardCorners(ChessImaL,(self.width, self.height), None)  # # 提取左图每一张图片的角点
            if (True == retR) & (True == retL):
                objpoints.append(objp)
                cv2.cornerSubPix(ChessImaR, cornersR, (11, 11), (-1, -1), self.criteria)  # 亚像素精确化，对粗提取的角点进行精确化
                cv2.cornerSubPix(ChessImaL, cornersL, (11, 11), (-1, -1), self.criteria)  # 亚像素精确化，对粗提取的角点进行精确化
                imgpointsR.append(cornersR)
                imgpointsL.append(cornersL)

        # 相机的单双目标定、及校正
        #   右侧相机单独标定
        retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(objpoints,imgpointsR,ChessImaR.shape[::-1], None, None)

        #   获取新的相机矩阵后续传递给initUndistortRectifyMap，以用remap生成映射关系
        hR, wR = ChessImaR.shape[:2]
        OmtxR, roiR = cv2.getOptimalNewCameraMatrix(mtxR, distR,(wR, hR), 1, (wR, hR))

        #   左侧相机单独标定
        retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(objpoints,imgpointsL,ChessImaL.shape[::-1], None, None)

        #   获取新的相机矩阵后续传递给initUndistortRectifyMap，以用remap生成映射关系
        hL, wL = ChessImaL.shape[:2]
        OmtxL, roiL = cv2.getOptimalNewCameraMatrix(mtxL, distL, (wL, hL), 1, (wL, hL))

        # --------- 双目相机的标定 ----------#
        # 设置标志位为cv2.CALIB_FIX_INTRINSIC，这样就会固定输入的cameraMatrix和distCoeffs不变，只求解𝑅,𝑇,𝐸,𝐹
        flags = 0
        flags |= cv2.CALIB_FIX_INTRINSIC
        
        # 内参、畸变系数、平移向量、旋转矩阵
        retS, left_K,left_D, right_K, right_D, R, T, E, F = cv2.stereoCalibrate(objpoints,imgpointsL,imgpointsR,OmtxL,distL,OmtxR,distR,
                                                                ChessImaR.shape[::-1], self.criteria_stereo,flags)
        # 左内参矩阵、左畸变向量、右内参矩阵、右畸变向量、旋转矩阵、平移矩阵
        return left_K,left_D, right_K, right_D, R, T
    # ==================================================================== #

    # =========================== 双目校正 =============================== #
    # 获取畸变校正、立体校正、重投影矩阵
    def getRectifyTransform(self, width,height,left_K,left_D, right_K, right_D, R, T):
        #得出进行立体矫正所需要的映射矩阵
        # 左校正变换矩阵、右校正变换矩阵、左投影矩阵、右投影矩阵、深度差异映射矩阵
        R_l,R_r,P_l,P_r,Q, roi_left, roi_right = cv2.stereoRectify(left_K, left_D,right_K, right_D,
                                              (width, height),R, T,
                                              flags=cv2.CALIB_ZERO_DISPARITY, alpha=0 )

        # 计算畸变矫正和立体校正的映射变换。
        map_lx, map_ly = cv2.initUndistortRectifyMap(left_K, left_D, R_l, P_l, (width,height),cv2.CV_32FC1)
        map_rx, map_ry = cv2.initUndistortRectifyMap(right_K, right_D, R_r, P_r, (width,height),cv2.CV_32FC1)

        return map_lx, map_ly,map_rx, map_ry, Q

    # 得到畸变校正和立体校正后的图像
    def get_rectify_img(self, imgL, imgR,map_lx, map_ly,map_rx, map_ry):
        rec_img_L = cv2.remap(imgL,map_lx, map_ly, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)  # 使用remap函数完成映射
        rec_img_R = cv2.remap(imgR,map_rx, map_ry, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)

        return rec_img_L, rec_img_R

    # 立体校正检验——极线对齐
    def draw_line(self, rec_img_L,rec_img_R):
        #建立输出图像
        width  = max(rec_img_L.shape[1],rec_img_R.shape[1])
        height = max(rec_img_L.shape[0],rec_img_R.shape[0])

        output = np.zeros((height,width*2,3),dtype=np.uint8)
        output[0:rec_img_L.shape[0],0:rec_img_L.shape[1]] = rec_img_L
        output[0:rec_img_R.shape[0],rec_img_L.shape[1]:]  = rec_img_R

        # 绘制等间距平行线
        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 get_parser():
    parser = argparse.ArgumentParser(description='Camera calibration')
    parser.add_argument('--width', type=int, default=9, help='chessboard width size')
    parser.add_argument('--height', type=int, default=5, help='chessboard height size')
    parser.add_argument('--image_dir', type=str, default="data/", help='images path')
    parser.add_argument('--save_dir', type=str, default="config/", help='path to save file')
    parser.add_argument('--file_name', type=str, default="camera_params", help='camera params save file')
    return parser

def get_file(path):          #获取文件路径
    img_path = []
    for root, dirs, files in os.walk(path):
        for file in files:
            img_path.append(os.path.join(root,file))
    return img_path


if __name__ == "__main__":
    args = get_parser().parse_args()
    
    params_dict = {}

    file_L = get_file(args.image_dir + 'left')
    file_R = get_file(args.image_dir + 'right')

    imgL = cv2.imread(file_L[2])
    imgR = cv2.imread(file_R[2])
    height, width = imgL.shape[0:2]
    
    calibration = Stereo_Camera_Calibration(args.width,args.height)
    left_K,left_D, right_K, right_D, R, T = calibration.stereo_calibration(file_L, file_R)
    map_lx, map_ly,map_rx, map_ry, Q = calibration.getRectifyTransform(width,height,left_K,left_D,
                                                                       right_K, right_D, R, T)
    
    # 查看校正效果
    # rec_img_L, rec_img_R = calibration.get_rectify_img(imgL,imgR,map_lx, map_ly,map_rx, map_ry)
    # img_show = calibration.draw_line(rec_img_L,rec_img_R)
    # cv2.imshow("output",img_show)
    # cv2.waitKey(0)
    
    params_dict['size']        = [width, height]
    params_dict['K1']          = left_K.tolist()
    params_dict['D1']          = left_D.tolist()
    params_dict['K2']          = right_K.tolist()
    params_dict['D2']          = right_D.tolist()
    params_dict['left_map_x']  = map_lx.tolist()
    params_dict['left_map_y']  = map_ly.tolist()
    params_dict['right_map_x'] = map_rx.tolist()
    params_dict['right_map_y'] = map_ry.tolist()
    params_dict['R']           = R.tolist()
    params_dict['T']           = T.tolist()
    params_dict['Q']           = Q.tolist()
    

    # =========== 保存相机参数 =========== # 
    # 保存文件
    # file_path = args.save_dir + args.file_name + ".npy"
    # np.save(file_path, params_dict)


    # 保存为.json文件
    file_path = args.save_dir + args.file_name + ".json"
    with open(file_path,"w") as f:
        json.dump(params_dict, f, indent=1) # indent=1 设置自动换行

    # 保存为.pkl文件
    # file_path = args.save_dir + args.file_name + ".pkl"
    # with open(file_path,"wb") as f:
    #     pickle.dump(params_dict, f)

    print("ALL Make Done!")

注意：

• 标定代码会显示每一张图像的棋盘格的角点检测效果，如果发现有检测不到，或者角点检测出错，则需要自己手动删除这些图像，避免引入太大的误差
• 若误差超过0.1，建议重新调整摄像头并标定，不然效果会差很多

执行后，在save_dir目录下会生成camera_params.json参数文件,参数文件中保存有 图像尺寸、K1（左内参矩阵）、D1（左畸变系数矩阵）、K2（右内参矩阵）、D2（右畸变系数矩阵）、left_map_x、left_map_y（左校正变换矩阵）、right_map_x、right_map_y（右校正变换矩阵）、R（旋转矩阵）、T（平移矩阵）、Q（深度差异映射矩阵）等参数。

立体校正的目的是将拍摄于同一场景的左右两个视图进行数学上的投影变换，使得两个成像平面平行于基线，且同一个点在左右两幅图中位于同一行，简称共面行对准（满足对极约束）。只有达到共面行对准以后才可以应用三角原理计算距离。

    def get_rectify_img(self, imgL, imgR,map_lx, map_ly,map_rx, map_ry):
        rec_img_L = cv2.remap(imgL,map_lx, map_ly, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)  # 使用remap函数完成映射
        rec_img_R = cv2.remap(imgR,map_rx, map_ry, cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)

        return rec_img_L, rec_img_R

以此得到校正后的左右两幅图，由于左右两幅图像应该要满足对极约束，所以我们可以在两幅图像上绘制一些平行线，来检验立体校正的效果，如果同一个点处于同一条平行线上，则说明校正效果很好。

立体匹配的目的是为左图中的每一个像素点在右图中找到其对应点（世界中相同的物理点），这样就可以计算出视差：（xi和xj分别表示两个对应点在图像中的列坐标）。

大部分立体匹配算法的计算过程可以分成以下几个阶段：匹配代价计算、代价聚合、视差优化、视差细化。立体匹配是立体视觉中一个很难的部分，主要困难在于：

1.图像中可能存在重复纹理和弱纹理，这些区域很难匹配正确；
2.由于左右相机的拍摄位置不同，图像中几乎必然存在遮挡区域，在遮挡区域，左图中有一些像素点在右图中并没有对应的点，反之亦然；
3.左右相机所接收的光照情况不同；
4.过度曝光区域难以匹配；
5.倾斜表面、弯曲表面、非朗伯体表面；
6.较高的图像噪声等。

常用的立体匹配方法基本上可以分为两类：局部方法，例如BM、SGM、ELAS、Patch Match等，非局部的，即全局方法，例如Dynamic Programming、Graph Cut、Belief Propagation等，局部方法计算量小，匹配质量相对较低，全局方法省略了代价聚合而采用了优化能量函数的方法，匹配质量较高，但是计算量也比较大。

目前OpenCV中已经实现的方法有BM、binaryBM、SGBM、binarySGBM、BM(cuda)、Bellief Propogation(cuda)、Constant Space Bellief Propogation(cuda)这几种方法。比较好用的是SGBM算法，它的核心是基于SGM算法，但和SGM算法又有一些不同，比如匹配代价部分用的是BT代价（原图+梯度图）而不是HMI代价等等。

  def get_disparity(self, imgL, imgR, down_scale=True, use_wls=True):
        """
        :imgL: 畸变校正和立体校正后的左视图
        :imgR：畸变校正和立体校正后的右视图
        :use_wls：是否使用WLS滤波器对视差图进行滤波
        :down_scale：是否下采样
        :dispL:ndarray(np.float32),返回视差图
        """
        dispL,_ = StereoMatcher.stereoMatchSGBM(imgL, imgR, down_scale, use_wls)
        cv2.imwrite('./data/disaprity.png', dispL * 4)
        return dispL

这里有个地方需要注意，如果获得视差图像是CV_16S类型的，这样的视差图的每个像素值由一个16bit表示，其中低位的4位存储的是视差值得小数部分，所以真实视差值应该是该值除以16。在进行映射后应该乘以16，以获得毫米级真实位置。

在立体匹配生成视差图之后，还可以对视差图进行滤波后处理，例如Guided Filter、Fast Global Smooth Filter（一种快速WLS滤波方法）、Bilatera Filter、TDSR、RBS等。视差图滤波能够将稀疏视差转变为稠密视差，并在一定程度上降低视差图噪声，改善视差图的视觉效果，但是比较依赖初始视差图的质量。

得到了视差图之后，就可以计算像素深度了,在opencv中使用StereoRectify()函数可以得到一个重投影矩阵Q，它是一个4*4的视差图到深度图的映射矩阵(disparity-to-depth mapping matrix )，使用Q矩阵和cv2.reprojectImageTo3D即可实现将像素坐标转换为三维坐标，该函数会返回一个3通道的矩阵，分别存储X、Y、Z坐标（左摄像机坐标系下）。

def reprojectImageTo3D(disparity, Q, _3dImage=None, handleMissingValues=None, ddepth=None):
    """
    :param disparity: 输入视差图
    :param Q: 输入4*4的视差图到深度图的映射矩阵，即重投影矩阵 通过stereoRectify得到
            (disparity-to-depth mapping matrix)
    :param _3dImage: 映射后存储三维坐标的图像
             contains 3D coordinates of the point (x,y) computed from the disparity map
    :param handleMissingValues: 计算得到的非正常值是否给值，如果为true则给值10000
    :param ddepth: 输出类型 -1 即默认为CV_32FC3 还可以是 CV_16S, CV_32S, CV_32F
    :return:
    """

运算如下：

重投影矩阵 Q 中c_{x}和c_{y}为左相机主点在图像中的坐标，f为焦距，T_{x}为两台相机投影中心间的平移（负值），即基线baseline,相当于平移向量T_[0], c_{x}^{`} 是右相机主点在图像中的坐标。

其中Z即是深度距离depth：
其中 f 为焦距长度（像素焦距），b为基线长度，d为视差，c_{xl}与c_{xr}为两个相机主点的列坐标。

双目测距的精度说明：

根据上式可以看出，某点像素的深度精度取决于该点处估计的视差d的精度。假设视差d的误差恒定，当测量距离越远，得到的深度精度则越差，因此使用双目相机不适宜测量太远的目标。

如果想要对与较远的目标能够得到较为可靠的深度，一方面需要提高相机的基线距离，但是基线距离越大，左右视图的重叠区域就会变小，内容差异变大，从而提高立体匹配的难度，另一方面可以选择更大焦距的相机，然而焦距越大，相机的视域则越小，导致离相机较近的物体的距离难以估计。

理论上，深度方向的测量误差与测量距离的平方成正比，而X/Y方向的误差与距离成正比；而距离很近时，由于存在死角，会导致难以匹配的问题；想象一下，如果你眼前放置一块物体，那你左眼只能看到物体左侧表面，右眼同理只能看到物体右侧表面，这时由于配准失败，导致视差计算失败；这个问题在基线越长，问题就越严重

得到深度图后，就可以使用python-pcl和Open3D库显示点云图。PCL Python版比较难安装，如果安装不了，那可以采用Open3D替换。

Open3D是一个开源库，它支持处理3D数据的软件的快速开发。Open3D前端在C++和Python中公开了一组精心选择的数据结构和算法。后端经过高度优化，并设置为并行化。
1、利用Open3D生成RGB-D数据
2、利用Open3D读取内参
3、利用Open3D生成点云