8、双目测距及3D重建python_双目视觉三维重建代码github

---

1、简介

1.1 双目测距

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

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

1.2 三维重建

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

---

2、双目测距

2.1、双目测距原理

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

---

2.2、双目相机标定和校准

2.2.1 双目相机选择

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

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

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

2.2.2 采集标定板的左右视图

• 采集数据前，调节相机焦距，尽可能保证左右视图清晰度一致（目的是为了让左右相机的焦距尽量一致）
• 采集棋盘格图像时，标定板一般占视图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
2
3
4
5
6
7
8
9
10
11
12

参数说明：

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))
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178

2.2.3 相机标定和校准

双目标定的目标是获得左右两个相机的内参、外参和畸变系数，其中内参包括左右相机的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"\
1
2
3
4
5
6
7
8
9
10
11
12

代码如下：

#-*- 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不变，只求解
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/IT小白/article/detail/152902