双目三维重建系统(双目标定+立体校正+双目测距+点云显示)Python

1.项目结构

├── config # 相机参数文件 ├── core # 相机核心算法包 ├── data # 相机采集的数据 ├── 演示demo # 演示demo文件 ├── libs # 第三方依赖(dependency)包 ├── scripts # 脚本 │ ├── mono_camera_calibration.sh # 单目相机校准(Calibration)脚本 │ └── stereo_camera_calibration.sh # 双目相机校准(Calibration)脚本 ├── get_stereo_images.py # 采集标定文件 ├── mono_camera_calibration.py # 单目相机标定 ├── stereo_camera_calibration.py # 双目相机标定 └── README.md

2. Environment

依赖(dependency)包，可参考 requirements.txt

python-pcl ( 安装python-pcl需要一丢丢耐心，实在不行，就用open3d吧 )

open3d-python=0.7.0.0

opencv-python

opencv-contrib-python

3.双目相机标定和校准(Calibration)

(0) 双目摄像头

下面这款双目摄像头（RGB+RGB）是在某宝购买的（几百元，链接就不发了，免得说我打广告），作为本项目的双目相机，其基线是固定的6cm，是单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)，基本满足我们测试需求。一般基线越长，可测量的距离越远，网友也可以根据自己需要购买。

一点注意事项 ：

双目相机三维重建也可以使用RGB+IR(红外)的摄像头，甚至IR+IR的也是可以，本人亲测，RGB+IR的相机，其效果也是杠杠的。

基线不太建议太小，作为测试，一般baseline在3~9cm就可以满足需求，有些无人车的双目基线更是恐怖到1~2米长

从双目三维重建原理中可知，左右摄像头的成像平面尽可能在一个平面内，成像平面不在同一个平面的，尽管可以立体矫正，其效果也差很多。

一分钱，一分货，相机的质量好坏，直接决定了你的成像效果

参数left_video是左路相机ID，一般就是相机连接主板的USB接口号

参数right_video是右路相机ID，一般就是相机连接主板的USB接口号

PS：如果你的双目相机是单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)，则设置left_video=相机ID，而right_video=-1，

参数


    detect

建议设置


    True

，这样可实时目标检测棋盘格，方面调整角度

按键盘

或者

保存左右视图图片

下面是采集双目摄像头标定板左右视图的Python代码：get_stereo_images.py，除了OpenCV，没啥依赖(dependency)，直接干就完事。

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 str2bool(v):
    return v.lower() in ('yes', 'true', 't', 'y', '1')
def get_parser():
    width = 8
    height = 11
    left_video = -1
    right_video = 0
    save_dir = "data/camera"
    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('--detect', type=str2bool, nargs='?', const=True, help='detect chessboard ')
    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)
    elif args.right_video > -1:
        # 单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)
        stereo.capture1(video=args.right_video, save_dir=args.save_dir)
    else:
        raise Exception("Error: Check your camera{}".format(args.left_video, args.right_video))
双目标定的目标是获得左右两个相机的内参、外参和畸变系数，其中内参包括左右相机的fx，fy，cx，cy，外参包括左相机相对于右相机的旋转矩阵(matrix)和平移向量，畸变系数包括径向畸变系数（k1， k2，k3）和切向畸变系数（p1，p2）。
双目标定工具最常用的莫过于是MATLAB的工具箱： Stereo Camera Calibrator App，网上有太多教程，我就不赘述了。
我采用的是Deepin系统，懒得去安装Matlab了，所以就参考各路神仙，使用OpenCV实现了单目和双目的标定程序。
(2) 单目相机标定和校准(Calibration)
bash scripts/mono_camera_calibration.sh
#!/usr/bin/env bash
image_dir=data/lenacv-camera # 棋盘格图片
save_dir=configs/lenacv-camera # 保存标定结果
width=8
height=11
square_size=20 #mm
image_format=png # 图片格式，如png,jpg
show=True # 是否显示目标检测结果
# left camera calibration
python mono_camera_calibration.py 
    --image_dir  $image_dir 
    --image_format $image_format  
    --square_size $square_size  
    --width $width  
    --height $height  
    --prefix left  
    --save_dir $save_dir 
    --show $show
# right camera calibration
python mono_camera_calibration.py 
    --image_dir  $image_dir 
    --image_format  $image_format  
    --square_size $square_size  
    --width $width  
    --height $height  
    --prefix right  
    --save_dir $save_dir 
    --show $show 
一点注意事项： 
标定代码会显示每一张图像的棋盘格的角点目标检测效果，如果发现有目标检测不到，或者角点目标检测出错，则需要自己手动删除这些图像，避免引入太大的误差
若误差超过0.1，建议重新调整摄像头并标定，不然效果会差很多
 执行后，在$save_dir目录下会生成left_cam.yml和right_cam.yml左右相机参数文件
%YAML:1.0
K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 7.6327773983725410e+02, 0., 2.8768149780495781e+02, 0.,
       7.6350419482076416e+02, 2.1897333674659842e+02, 0., 0., 1. ]
D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ 3.5020967324140520e-02, -4.0770563420764315e-02,
       -4.4231047037511739e-04, -1.0552565305999332e-03,
       -9.7750323762439514e-02 ]
其中K是相机内参矩阵(matrix)，D是畸变系数矩阵(matrix)
这是完整的代码
(3) 双目相机标定和校准(Calibration)
bash scripts/stereo_camera_calibration.sh
image_dir=data/lenacv-camera # 棋盘格图片
save_dir=configs/lenacv-camera # 保存标定结果
width=8
height=11
square_size=20 #mm
image_format=png # 图片格式，如png,jpg
#show=True # 是否显示目标检测结果
show=False # 是否显示目标检测结果
# stereo camera calibration
python stereo_camera_calibration.py 
    --left_file $save_dir/left_cam.yml 
    --right_file $save_dir/right_cam.yml 
    --left_prefix left 
    --right_prefix right 
    --width $width 
    --height $height 
    --left_dir $image_dir 
    --right_dir $image_dir 
    --image_format  $image_format  
    --square_size $square_size 
    --save_dir $save_dir 
一点注意事项：  
若误差超过0.1，建议重新调整摄像头并标定
执行后，在$save_dir目录下会生成stereo_cam.yml相机参数文件，这个文件，包含了左右相机的相机内参矩阵(matrix)(K1,K2)，畸变系数矩阵(matrix)(D1,D2)，左右摄像机之间的旋转矩阵(matrix)R和平移矩阵(matrix)T，以及本质矩阵(matrix)E和基本矩阵(matrix)F等.
有了双目相机内外参数信息(stereo_cam.yml)，下面就可以进行立体矫正，计算视差了
4.视差图和深度图
(1) 立体校正
这部分基础知识来源于：https://blog.csdn.net/dulingwen/article/details/100115157
立体校正的目的是将拍摄于同一场景的左右两个视图进行数学上的投影变换，使得两个成像平面平行于基线，且同一个点在左右两幅图中位于同一行，简称共面行对准。只有达到共面行对准以后才可以应用三角原理计算距离。
    def get_rectify_image(self, imgL, imgR):
        畸变校正和立体校正
        根据更正map对图片进行重构
        获取用于畸变校正和立体校正的映射矩阵(matrix)以及用于计算像素空间坐标的重投影矩阵(matrix)
        :param imgL:
        :param imgR:
        :return:
        # camera_params.get_rectify_transform(K1, D1, K2, D2, R, T, image_size)
        left_map_x, left_map_y = self.camera_config["left_map_x"], self.camera_config["left_map_y"]
        right_map_x, right_map_y = self.camera_config["right_map_x"], self.camera_config["right_map_y"]
        rectifiedL = cv2.remap(imgL, left_map_x, left_map_y, cv2.INTER_LINEAR, borderValue=cv2.BORDER_CONSTANT)
        rectifiedR = cv2.remap(imgR, right_map_x, right_map_y, cv2.INTER_LINEAR, borderValue=cv2.BORDER_CONSTANT)
        return rectifiedL, rectifiedR 
(2) 立体匹配与视差图计算
立体匹配的目的是为左图中的每一个像素点在右图中找到其对应点（世界中相同的物理点），这样就可以计算出视差：（xi和xj分别表示两个对应点在图像中的列坐标）。




    

大部分立体匹配算法的计算过程可以分成以下几个阶段：匹配代价(cost)计算、代价(cost)聚合、视差优化、视差细化。立体匹配是立体视觉中一个很难的部分，主要困难在于：
1.图像中可能存在重复纹理和弱纹理，这些区域很难匹配正确；
2.由于左右相机的拍摄位置不同，图像中几乎必然存在遮挡区域，在遮挡区域，左图中有一些像素点在右图中并没有对应的点，反之亦然；
3.左右相机所接收的光照情况不同；
4.过度曝光区域难以匹配；
5.倾斜表面、弯曲表面、非朗伯体表面；
6.较高的图像噪声等。
常用的立体匹配方法基本上可以分为两类：局部方法，例如BM、SGM、ELAS、Patch Match等，非局部的，即全局方法，例如Dynamic Programming、Graph Cut、Belief Propagation等，局部方法计算量小，匹配质量相对较低，全局方法省略了代价(cost)聚合而采用了优化能量函数(energy function)的方法，匹配质量较高，但是计算量也比较大。
目前OpenCV中已经实现的方法有BM、binaryBM、SGBM、binarySGBM、BM(cuda)、Bellief Propogation(cuda)、Constant Space Bellief Propogation(cuda)这几种方法。比较好用的是SGBM算法，它的核心是基于SGM算法，但和SGM算法又有一些不同，比如匹配代价(cost)部分用的是BT代价(cost)（原图+梯度(gradient)图）而不是HMI代价(cost)等等。有关SGM算法的原理解释，可以参考另一篇博客 : 《双目立体匹配算法：SGM 》https://blog.csdn.net/dulingwen/article/details/104142149
    def get_disparity(self, imgL, imgR, use_wls=True):
        :param imgL: 畸变校正和立体校正后的左视图
        :param imgR：畸变校正和立体校正后的右视图
        :param use_wls：是否使用WLS滤波器对视差图进行滤波
        :return dispL:ndarray(np.float32),返回视差图
        dispL = disparity.get_disparity_filter(imgL, imgR, use_wls=use_wls)
        # dispL = disparity.get_disparity_simple(imgL, imgR)
        return dispL 
(3) Demo
运行Demo进行立体矫正，计算视差图并恢复深度图,
Demo的参数说明如下
calibration_file
双目相机的配置文件，如"configs/lenacv-camera/stereo_cam.yml"
left_video
左路相机ID或者视频文件
right_video
右路相机ID或者视频文件
left_file
左路测试图像文件
right_file
右路测试图像文件
filter
是否对视差图进行滤波
python 演示demo.py  
  --stereo_file "configs/lenacv-camera/stereo_cam.yml" 
  --left_video "data/lenacv-video/left_video.avi" 
  --right_video "data/lenacv-video/right_video.avi" 
  --filter True 
视差图滤波:
在立体匹配生成视差图之后，还可以对视差图进行滤波后处理，例如Guided Filter、Fast Global Smooth Filter（一种快速WLS滤波方法）、Bilatera Filter、TDSR、RBS等。 视差图滤波能够将稀疏视差转变为稠密视差，并在一定程度上降低视差图噪声，改善视差图的视觉效果，但是比较依赖(dependency)初始视差图的质量。
5.双目测距
得到了视差图之后，就可以计算像素深度了,在opencv中使用StereoRectify()函数可以得到一个重投影矩阵(matrix)Q，它是一个4*4的视差图到深度图的映射矩阵(matrix)(disparity-to-depth mapping matrix )，使用Q矩阵(matrix)和cv2.reprojectImageTo3D即可实现将像素坐标转换为三维坐标，该函数会返回一个3通道的矩阵(matrix)，分别存储X、Y、Z坐标（左摄像机坐标系下）。
def reprojectImageTo3D(disparity, Q, _3dImage=None, handleMissingValues=None, ddepth=None):
    :param disparity: 输入(input)视差图
    :param Q: 输入(input)4*4的视差图到深度图的映射矩阵(matrix)，即重投影矩阵(matrix) 通过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:
运算如下：
重投影矩阵(matrix)Q中和为左相机主点在图像中的坐标，f为焦距，为两台相机投影中心间的平移（负值），即基线baseline,相当于平移向量T[0], 是右相机主点在图像中的坐标。
其中Z即是深度距离depth：
其中 f 为焦距长度（像素焦距），b为基线长度，d为视差，与为两个相机主点的列坐标。
这里有个地方需要注意，如果获得视差图像是CV_16S类型的，这样的视差图的每个像素值由一个16bit表示，其中低位的4位存储的是视差值得小数部分，所以真实视差值应该是该值除以16。在进行映射后应该乘以16，以获得毫米级真实位置。
    def get_depth(self, disparity, Q, scale=1.0, method=False):
        reprojectImageTo3D(disparity, Q),输入(input)的Q,单位必须是毫米(mm)
        :param disparity: 视差图
        :param Q: 重投影矩阵(matrix)Q=[[1, 0, 0, -cx]
                           [0, 1, 0, -cy]
                           [0, 0, 0,  f]
                           [1, 0, -1/Tx, (cx-cx`)/Tx]]
            其中f为焦距，Tx相当于平移向量T的第一个参数
        :param scale: 单位变换尺度,默认scale=1.0,单位为毫米
        :return depth:ndarray(np.uint16),depth返回深度图, 即距离
        # 将图片扩展至3d空间中，其z方向的值则为当前的距离
        if method:
            points_3d = cv2.reprojectImageTo3D(disparity, Q)  # 单位是毫米(mm)
            x, y, depth = cv2.split(points_3d)
        else:
            # baseline = abs(camera_config["T"][0])
            baseline = 1 / Q[3, 2]  # 基线也可以由T[0]计算
            fx = abs(Q[2, 3])
            depth = (fx * baseline) / disparity
        depth = depth * scale
        # depth = np.asarray(depth, dtype=np.uint16)
        depth = np.asarray(depth, dtype=np.float32)
        return depth 
运行演示demo.py后，鼠标点击图像任意区域，终端会打印对应距离
鼠标点击手部区域会打印距离摄像头的距离约633mm,即0.63米，还是比较准的
(x,y)=(203,273),depth=633.881653mm
(x,y)=(197,329),depth=640.386047mm
(x,y)=(222,292),depth=631.549072mm
(x,y)=(237,270),depth=630.389221mm
(x,y)=(208,246),depth=652.560669mm
 双目测距的精度 说明：
根据上式可以看出，某点像素的深度精度取决于该点处估计的视差d的精度。假设(Hypothesis)视差d的误差恒定，当测量距离越远，得到的深度精度则越差，因此使用双目相机不适宜测量太远的目标。
如果想要对与较远的目标能够得到较为可靠的深度，一方面需要提高相机的基线距离，但是基线距离越大，左右视图的重叠区域就会变小，内容差(tolerance)异变大，从而提高立体匹配的难度，另一方面可以选择更大焦距的相机，然而焦距越大，相机的视域则越小，导致离相机较近的物体的距离难以估计。
6.3D点云显示
恢复三维坐标后，就可以使用python-pcl和Open3D库显示点云图
PCL Python版比较难安装，如果安装不了，那可以采用Open3D勉强凑合使用吧
如下图所示，你可以用鼠标旋转坐标轴，放大点云
2D-RGB
Open3D点云显示
PCL点云显示
        if self.use_open3d:
            x, y, depth = cv2.split(points_3d)  # depth = points_3d[:, :, 2]
            self.open3d_viewer.show(color_image=frameL, depth_image=depth)
    def show_3dcloud_for_pcl(self, frameL, frameR, points_3d):
        使用PCL显示点云
        :param frameL:
        :param frameR:
        :param points_3d:
        :return:
        if self.use_pcl:
            self.pcl_viewer.add_3dpoints(points_3d, frameL)
            self.pcl_viewer.show() 
7.Demo代码
【完整的项目代码】：https://download.csdn.net/download/guyuealian/42517006
# -*-coding: utf-8 -*-
    @Author : panjq
    @E-mail : pan_jinquan@163.com
    @Date   : 2020-04-10 18:24:06
import os
import cv2
import argparse
import numpy as np
from core.utils import image_utils, file_utils
from core import camera_params, stereo_matcher
class StereoDepth(object):
    """双目测距"""
    def __init__(self, stereo_file, width=640, height=480, filter=True, use_open3d=True, use_pcl=True):
        :param stereo_file: 双目相机内外参数配置文件
        :param width: 相机分辨率width
        :param height:相机分辨率height
        :param filter: 是否使用WLS滤波器对视差图进行滤波
        :param use_open3d: 是否使用open3d显示点云
        :param use_pcl: 是否使用PCL显示点云
        self.count = 0
        self.filter = filter
        self.camera_config = camera_params.get_stereo_coefficients(stereo_file)
        self.use_pcl = use_pcl
        self.use_open3d = use_open3d
        # 初始化3D点云
        if self.use_pcl:
            # 使用open3d显示点云
            from core.utils_pcl import pcl_tools
            self.pcl_viewer = pcl_tools.PCLCloudViewer()
        if self.use_open3d:
            # 使用PCL显示点云
            from core.utils_3d import open3d_visual
            self.open3d_viewer = open3d_visual.Open3DVisual(camera_intrinsic=self.camera_config["K1"],
                                                            depth_width=width,
                                                            depth_height=height)
            self.open3d_viewer.show_image_pcd(True)
            self.open3d_viewer.show_origin_pcd(True)
            self.open3d_viewer.show_image_pcd(True)
        assert (width, height) == self.camera_config["size"], Exception("Error:{}".format(self.camera_config["size"]))
    def test_pair_image_file(self, left_file, right_file):
        测试一对左右图像
        :param left_file: 左路图像文件
        :param right_file: 右路图像文件
        :return:
        frameR = cv2.imread(left_file)
        frameL = cv2.imread(right_file)
        self.task(frameR, frameL, waitKey=0)
    def capture1(self, video):
        用于采集单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)
        :param video:int or str,视频路径或者摄像头ID
        :param save_dir: str,保存左右图片的路径
        cap = image_utils.get_video_capture(video)
        width, height, numFrames, fps = image_utils.get_video_info(cap)
        self.count = 0
        while True:
            success, frame = cap.read()
            if not success:
                print("No more frames")
                break
            frameL = frame[:, :int(width / 2), :]
            frameR = frame[:, int(width / 2):, :]
            self.count += 1
            self.task(frameL, frameR, waitKey=5)
            if cv2.waitKey(1) & 0xFF == ord('q'):  # Get key to stop stream. Press q for exit
                break
        cap.release()
        cv2.destroyAllWindows()
    def capture2(self, left_video, right_video):
        用于采集双USB连接线的双目摄像头
        :param left_video:int or str,左路视频路径或者摄像头ID
        :param right_video:int or str,右视频路径或者摄像头ID
        :return:
        capL = image_utils.get_video_capture(left_video)
        capR = image_utils.get_video_capture(right_video)
        width, height, numFrames, fps = image_utils.get_video_info(capL)
        width, height, numFrames, fps = image_utils.get_video_info(capR)
        self.count = 0
        while True:
            successL, frameL = capL.read()
            successR, frameR = capR.read()
            if not (successL and successR):
                print("No more frames")
                break
            self.count += 1
            self.task(frameL, frameR, waitKey=50)
            if cv2.waitKey(1) & 0xFF == ord('q'):  # Get key to stop stream. Press q for exit
                break
        capL.release()
        capR.release()
        cv2.destroyAllWindows()
    def get_3dpoints(self, disparity, Q, scale=1.0):
        计算像素点的3D坐标（左相机坐标系下）
        reprojectImageTo3D(disparity, Q),输入(input)的Q,单位必须是毫米(mm)
        :param disparity: 视差图
        :param Q: 重投影矩阵(matrix)Q=[[1, 0, 0, -cx]
                           [0, 1, 0, -cy]
                           [0, 0, 0,  f]
                           [1, 0, -1/Tx, (cx-cx`)/Tx]]
            其中f为焦距，Tx相当于平移向量T的第一个参数
        :param scale: 单位变换尺度,默认scale=1.0,单位为毫米
        :return points_3d:ndarray(np.float32),返回三维坐标points_3d，三个通道分布(Distribution)表示(X,Y,Z)
                    其中Z是深度图depth, 即距离,单位是毫米(mm)
        # 返回三维坐标points_3d，三个通道分布(Distribution)表示(X,Y,Z)
        # depth = stereo_matcher.get_depth(disparity, Q, scale=1.0)
        points_3d = cv2.reprojectImageTo3D(disparity, Q)
        # x, y, depth = cv2.split(points_3d)
        # baseline = abs(camera_config["T"][0])
        # baseline = 1 / Q[3, 2]  # 基线也可以由T[0]计算
        # fx = abs(Q[2, 3])
        # depth = (fx * baseline) / disparity
        points_3d = points_3d * scale
        points_3d = np.asarray(points_3d, dtype=np.float32)
        return points_3d
    def get_disparity(self, imgL, imgR, use_wls=True):
        :param imgL: 畸变校正和立体校正后的左视图
        :param imgR：畸变校正和立体校正后的右视图
        :param use_wls：是否使用WLS滤波器对视差图进行滤波
        :return dispL:ndarray(np.float32),返回视差图
        dispL = stereo_matcher.get_filter_disparity(imgL, imgR, use_wls=use_wls)
        # dispL = disparity.get_simple_disparity(imgL, imgR)
        return dispL
    def get_rectify_image(self, imgL, imgR):
        畸变校正和立体校正
        根据更正map对图片进行重构
        获取用于畸变校正和立体校正的映射矩阵(matrix)以及用于计算像素空间坐标的重投影矩阵(matrix)
        :param imgL:
        :param imgR:
        :return:
        # camera_params.get_rectify_transform(K1, D1, K2, D2, R, T, image_size)
        left_map_x, left_map_y = self.camera_config["left_map_x"], self.camera_config["left_map_y"]
        right_map_x, right_map_y = self.camera_config["right_map_x"], self.camera_config["right_map_y"]
        rectifiedL = cv2.remap(imgL, left_map_x, left_map_y, cv2.INTER_LINEAR, borderValue=cv2.BORDER_CONSTANT)
        rectifiedR = cv2.remap(imgR, right_map_x, right_map_y, cv2.INTER_LINEAR, borderValue=cv2.BORDER_CONSTANT)
        return rectifiedL, rectifiedR
    def task(self, frameL, frameR, waitKey=5):
        :param frameL: 左路视频帧图像(BGR)
        :param frameR: 右路视频帧图像(BGR)
        # 畸变校正和立体校正
        rectifiedL, rectifiedR = self.get_rectify_image(imgL=frameL, imgR=frameR)
        # 绘制等间距平行线，检查立体校正的效果
        # calibrate_tools.draw_line_rectify_image(rectifiedL, rectifiedR)
        # We need grayscale for disparity map.
        grayL = cv2.cvtColor(rectifiedL, cv2.COLOR_BGR2GRAY)
        grayR = cv2.cvtColor(rectifiedR, cv2.COLOR_BGR2GRAY)
        # Get the disparity map
        dispL = self.get_disparity(grayL, grayR, self.filter)
        points_3d = self.get_3dpoints(disparity=dispL, Q=self.camera_config["Q"])
        self.show_3dcloud_for_open3d(frameL, frameR, points_3d)
        self.show_3dcloud_for_pcl(frameL, frameR, points_3d)
        self.show_2dimage(frameL, frameR, points_3d, dispL, waitKey=waitKey)
    def show_3dcloud_for_open3d(self, frameL, frameR, points_3d):
        使用open3d显示点云
        :param frameL:
        :param frameR:
        :param points_3d:
        :return:
        if self.use_open3d:
            x, y, depth = cv2.split(points_3d)  # depth = points_3d[:, :, 2]
            self.open3d_viewer.show(color_image=frameL, depth_image=depth)
    def show_3dcloud_for_pcl(self, frameL, frameR, points_3d):
        使用PCL显示点云
        :param frameL:
        :param frameR:
        :param points_3d:
        :return:
        if self.use_pcl:
            self.pcl_viewer.add_3dpoints(points_3d, frameL)
            self.pcl_viewer.show()
    def show_2dimage(self, frameL, frameR, points_3d, dispL, waitKey=0):
        :param frameL:
        :param frameR:
        :param dispL:
        :param points_3d:
        :return:
        x, y, depth = cv2.split(points_3d)  # depth = points_3d[:, :, 2]
        depth_colormap = stereo_matcher.get_visual_depth(depth)
        dispL_colormap = stereo_matcher.get_visual_disparity(dispL)
        image_utils.addMouseCallback("left", depth, info="depth=%fmm")
        image_utils.addMouseCallback("right", depth, info="depth=%fmm")
        image_utils.addMouseCallback("disparity-color", depth, info="depth=%fmm")
        image_utils.addMouseCallback("depth-color", depth, info="depth=%fmm")
        result = {"frameL": frameL, "frameR": frameR, "disparity": dispL_colormap, "depth": depth_colormap}
        cv2.imshow('left', frameL)
        cv2.imshow('right', frameR)
        cv2.imshow('disparity-color', dispL_colormap)
        cv2.imshow('depth-color', depth_colormap)
        key = cv2.waitKey(waitKey)
        self.save_images(result, self.count, key)
        if self.count <= 2:
            cv2.moveWindow("left", 700, 0)
            cv2.moveWindow("right", 1400, 0)
            cv2.moveWindow("disparity-color", 700, 700)
            cv2.moveWindow("depth-color", 1400, 700)
    def save_images(self, result, count, key, save_dir="./data/temp"):
        :param result:
        :param count:
        :param key:
        :param save_dir:
        :return:
        if key == ord('q'):
            exit(0)
        elif key == ord('c') or key == ord('s'):
            file_utils.create_dir(save_dir)
            print("save image:{:0=4d}".format(count))
            cv2.imwrite(os.path.join(save_dir, "left_{:0=4d}.png".format(count)), result["frameL"])
            cv2.imwrite(os.path.join(save_dir, "right_{:0=4d}.png".format(count)), result["frameR"])
            cv2.imwrite(os.path.join(save_dir, "disparity_{:0=4d}.png".format(count)), result["disparity"])
            cv2.imwrite(os.path.join(save_dir, "depth_{:0=4d}.png".format(count)), result["depth"])
def str2bool(v):
    return v.lower() in ('yes', 'true', 't', 'y', '1')
def get_parser():
    stereo_file = "configs/lenacv-camera/stereo_cam.yml"
    # left_video = None
    # right_video = None
    left_video = "data/lenacv-video/left_video.avi"
    right_video = "data/lenacv-video/right_video.avi"
    left_file = "docs/left.png"
    right_file = "docs/right.png"
    parser = argparse.ArgumentParser(description='Camera calibration')
    parser.add_argument('--stereo_file', type=str, default=stereo_file, help='stereo calibration file')
    parser.add_argument('--left_video', default=left_video, help='left video file or camera ID')
    parser.add_argument('--right_video', default=right_video, help='right video file or camera ID')
    parser.add_argument('--left_file', type=str, default=left_file, help='left image file')
    parser.add_argument('--right_file', type=str, default=right_file, help='right image file')
    parser.add_argument('--filter', type=str2bool, nargs='?', default=True, help='use disparity filter')
    return parser
if __name__ == '__main__':
    args = get_parser().parse_args()
    stereo = StereoDepth(args.stereo_file, filter=args.filter)
    if args.left_video is not None and args.right_video is not None:
        # 双USB连接线的双目摄像头
        stereo.capture2(left_video=args.left_video, right_video=args.right_video)
    elif args.left_video is not None:
        # 单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)
        stereo.capture1(video=args.left_video)
    elif args.right_video is not None:
        # 单USB连接线的双目摄像头(左右摄像头被拼接在同一个视频中显示)
        stereo.capture1(video=args.right_video)
    if args.left_file and args.right_file:
        # 测试一对左右图像
        stereo.test_pair_image_file(args.left_file, args.right_file)
8.参考资料
<真实场景的双目立体匹配（Stereo Matching）获取深度图详解> : 真实场景的双目立体匹配（Stereo Matching）获取深度图详解 – 一度逍遥 – 博客园
<双目测距理论及其python实现> 双目测距理论及其python实现！_dulingwen的博客-CSDN博客_python双目测距
	版权声明：本文为博主pan_jinquan原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
	原文链接：https://blog.csdn.net/guyuealian/article/details/121301896
                                                                
                                        
                                        
                                        
                                        
              
    
            
      共计人评分，平均分    
          到目前为止还没有投票！成为第一位评论此文章。    
      
          
                                                                                
                        Open3D三维重建双目标定双目测距、显示点云立体校正
                             赞 (0)
        【语义SLAM论文】Incremental Dense Semantic Stereo Fusion for Large-Scale Semantic Scene Reconstruction    
    2022年5月30日
        三维重建之三角测量计算双目相机坐标系下三维坐标    
    2022年5月8日
        【视觉SLAM】SO-SLAM: Semantic Object SLAM With Scale Proportional and Symmetrical Texture Constraints    
    2023年4月5日
        三维点云重建 — open3d python    
    2023年3月9日
        双目三维重建系统(双目标定+立体校正+双目测距+点云显示)Python    
    2023年3月4日
        火爆科研圈的三维重建技术：Neural radiance fields (NeRF)    
    2022年2月22日
        立体匹配入门指南（8）：视差图、深度图、点云    
    2022年5月30日
        Colmap论文——《Structure-from-Motion Revisited》论文阅读笔记    
    2022年3月28日
        利用PCL库构建Mesh三维模型    
    2022年4月27日
        神经辐射场 3D 重建——NeRF    
    2023年2月25日