├── README.md ├── MonoCalib.py └── StereoCalib.py /README.md: -------------------------------------------------------------------------------- 1 | # CameraCalibration 2 | Python&OpenCV实现单目相机标定和双目相机标定 3 | -------------------------------------------------------------------------------- /MonoCalib.py: -------------------------------------------------------------------------------- 1 | #-*- coding:utf-8 -*- 2 | import os 3 | import numpy as np 4 | import cv2 5 | import glob 6 | import configparser 7 | 8 | class MonoCameraCalibrator(object): 9 | def __init__(self, 10 | data_root, 11 | corner_size=(7, 11), 12 | image_shape=(720, 1280), 13 | square_size=50, 14 | rectify_mode=0, 15 | cali_file="MonoCalib_Para_720P.ini", 16 | suffix="png"): 17 | super(MonoCameraCalibrator, self).__init__() 18 | self.data_root = data_root 19 | self.corner_h = corner_size[0] 20 | self.corner_w = corner_size[1] 21 | self.H, self.W = image_shape 22 | self.img_shape = (self.W, self.H) 23 | self.suffix = suffix 24 | self.cali_file = os.path.join(self.data_root, cali_file) 25 | self.square_size = square_size 26 | self.rectify_mode = rectify_mode 27 | 28 | def Run_Calibrator(self): 29 | 30 | if os.path.exists(self.cali_file): 31 | print("\n===> Read Calibration file from: {} ...".format(self.cali_file)) 32 | self.read_cali_file(self.cali_file) 33 | 34 | else: 35 | print("\n===> Start Calibration ...") 36 | self.mono_calibrate() 37 | # self.evaluate_calibrate(rectify_mode=self.rectify_mode, prefix="png") 38 | self.save_cali_file(self.cali_file) 39 | 40 | 41 | def mono_calibrate(self): 42 | 43 | ''' ========= 一、角点检测 ========= ''' 44 | # 寻找亚像素角点的参数，设置迭代终止条件,停止准则为最大循环次数30和最大误差容限0.001 45 | criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 46 | # 设置世界坐标系下棋盘角点坐标 object points, 形式为 (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) 47 | objp = np.zeros((self.corner_h*self.corner_w, 3), np.float32) 48 | objp[:, :2] = np.mgrid[0:self.corner_w, 0:self.corner_h].T.reshape(-1, 2) 49 | objp = objp * self.square_size # Create real world coords. Use your metric. 50 | # 用arrays存储所有图片的object points 和 image points 51 | objpoints = [] # 3d point in real world space 52 | imgpoints = [] # 2d points in image plane. 53 | 54 | # 用glob匹配文件夹/home/song/pic_1/right/下所有文件名含有“.jpg"的图片 55 | mono_images_path = os.path.join(self.data_root, "*.{}".format(self.suffix)) 56 | image_path_list = glob.glob(mono_images_path) 57 | for i, fname in enumerate(image_path_list): 58 | img = cv2.imread(fname) 59 | print("{:05d}: {}, {}".format(i, fname, img.shape)) 60 | 61 | h_, w_, _ = img.shape 62 | if h_ != self.H or w_ != self.W: 63 | img = cv2.resize(img, (self.W, self.H), interpolation=cv2.INTER_CUBIC) 64 | 65 | gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 66 | # 查找棋盘格角点 67 | ret, corners = cv2.findChessboardCorners(gray, (self.corner_w, self.corner_h), None) 68 | if ret and (corners[0, 0, 0] < corners[-1, 0, 0]): 69 | print("*"*5+"order of {} is inverse!".format(i)+"*"*5) 70 | corners = np.flip(corners, axis=0).copy() 71 | 72 | # 如果找到了就添加 object points, image points 73 | if ret == True: 74 | objpoints.append(objp) 75 | # 在原角点的基础上计算亚像素角点 76 | corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) 77 | imgpoints.append(corners) 78 | 79 | # 对角点连接画线加以展示 80 | cv2.drawChessboardCorners(img, (self.corner_w, self.corner_h), corners2, ret) 81 | cv2.imshow('img', img) 82 | cv2.waitKey(50) 83 | cv2.destroyAllWindows() 84 | 85 | self.objpoints = objpoints 86 | self.imgpoints = imgpoints 87 | 88 | 89 | ''' ========= 二、单目标定 ========= ''' 90 | ''' 91 | 第一个参数objectPoints，为世界坐标系中的三维点。需要依据棋盘上单个黑白矩阵的大小，计算出（初始化）每一个内角点的世界坐标； 92 | 第二个参数imagePoints，为每一个内角点对应的图像坐标点； 93 | 第三个参数imageSize，为图像的像素尺寸大小，在计算相机的内参和畸变矩阵时需要使用到该参数； 94 | 第四个参数cameraMatrix为相机的内参矩阵； 95 | 第五个参数distCoeffs为畸变矩阵； 96 | 第六个参数rvecs为旋转向量； 97 | 第七个参数tvecs为位移向量； 98 | 第八个参数flags为标定时所采用的算法。有如下几个参数： 99 |         CV_CALIB_USE_INTRINSIC_GUESS：使用该参数时，在cameraMatrix矩阵中应该有fx,fy,u0,v0的估计值。否则的话， 100 | 将初始化(u0,v0）图像的中心点，使用最小二乘估算出fx，fy。  101 |       CV_CALIB_FIX_PRINCIPAL_POINT：在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置， 102 | 光轴点将保持在中心或者某个输入的值。  103 |   CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只将fy作为可变量，进行优化计算。 104 | 当CV_CALIB_USE_INTRINSIC_GUESS没有被设置，fx和fy将会被忽略。 105 | 只有fx/fy的比值在计算中会被用到。  106 |         CV_CALIB_ZERO_TANGENT_DIST：设定切向畸变参数（p1,p2）为零。  107 |         CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6：对应的径向畸变在优化中保持不变。  108 |         CV_CALIB_RATIONAL_MODEL：计算k4，k5，k6三个畸变参数。如果没有设置，则只计算其它5个畸变参数。 109 | 第九个参数criteria是最优迭代终止条件设定。 110 | ''' 111 | # flags = None 112 | # criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 113 | flags = None 114 | criteria = None 115 | self.ret, self.mtx, self.dist, self.rvecs, self.tvecs = cv2.calibrateCamera(objpoints, 116 | imgpoints, 117 | gray.shape[::-1], 118 | cameraMatrix=None, 119 | distCoeffs=None) 120 | 121 | print("mtx: ", self.mtx) 122 | print("dist: ", self.dist) 123 | # print("rvecs: ", rvecs) 124 | # print("tvecs: ", tvecs) 125 | 126 | # 并且通过实验表明,distortion cofficients =(k_1,k_2,p_1,p_2,k_3) 127 | # 三个参数的时候由于k3所对应的非线性较为剧烈。估计的不好，容易产生极大的扭曲，所以k_3强制设置为0.0 128 | # self.dist[0,4]=0.0 129 | 130 | return self.ret, self.mtx, self.dist, self.rvecs, self.tvecs 131 | 132 | 133 | def rectify_image_list(self, mono_image_root, rectify_mode=0, prefix="png"): 134 | rectify_result_root = os.path.join(mono_image_root, "mono_rect") 135 | os.makedirs(rectify_result_root, exist_ok=True) 136 | 137 | # 对所有图片进行去畸变，有两种方法实现分别为： undistort()和remap() 138 | mono_images_path = os.path.join(self.data_root, "*.{}".format(self.suffix)) 139 | image_path_list = glob.glob(mono_images_path) 140 | for i, fname in enumerate(image_path_list): 141 | mono_rect_path = os.path.join(rectify_result_root, "rect_{:05d}.{}".format(i, prefix)) 142 | print("{:05d}:\n{}\n{}".format(i, fname, mono_rect_path)) 143 | 144 | img = cv2.imread(fname) 145 | img = cv2.resize(img, (self.W, self.H), interpolation=cv2.INTER_CUBIC) 146 | 147 | image_rec = self.mono_rectify(img, mode=rectify_mode) 148 | cv2.imwrite(mono_rect_path, image_rec) 149 | 150 | 151 | 152 | 153 | def evaluate_calibrate(self, rectify_mode=0, prefix="png"): 154 | rectify_result_root = os.path.join(self.data_root, "calib_rect") 155 | os.makedirs(rectify_result_root, exist_ok=True) 156 | 157 | if self.ret: 158 | # 对所有图片进行去畸变，有两种方法实现分别为： undistort()和remap() 159 | mono_images_path = os.path.join(self.data_root, "*.{}".format(self.suffix)) 160 | image_path_list = glob.glob(mono_images_path) 161 | for i, fname in enumerate(image_path_list): 162 | mono_rect_path = os.path.join(rectify_result_root, "rect_{:05d}.{}".format(i, prefix)) 163 | print("{:05d}:\n{}\n{}".format(i, fname, mono_rect_path)) 164 | 165 | img = cv2.imread(fname) 166 | img = cv2.resize(img, (self.W, self.H), interpolation=cv2.INTER_CUBIC) 167 | # h, w = img.shape[:2] 168 | # newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 0, (w, h)) 169 | # 170 | # # # 使用 cv.undistort()进行畸变校正 171 | # dst = cv2.undistort(img, mtx, dist, None, newcameramtx) 172 | # # # 对图片有效区域进行剪裁 173 | # # # x, y, w, h = roi 174 | # # # dst = dst[y:y+h, x:x+w] 175 | # # cv2.imwrite('/home/song/pic_1/undistort/'+prefix, dst) 176 | # 177 | # # 使用 remap() 函数进行校正 178 | # # mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5) 179 | # # dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR) 180 | # # 对图片有效区域进行剪裁 181 | # # x, y, w, h = roi 182 | # # dst = dst[y:y + h, x:x + w] 183 | image_rec = self.mono_rectify(img, mode=rectify_mode) 184 | cv2.imwrite(mono_rect_path, image_rec) 185 | 186 | # 重投影误差计算 187 | mean_error = 0 188 | for i in range(len(self.objpoints)): 189 | imgpoints2, _ = cv2.projectPoints(self.objpoints[i], self.rvecs[i], self.tvecs[i], self.mtx, self.dist) 190 | error = cv2.norm(self.imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2) 191 | mean_error += error 192 | 193 | print("total error: ", mean_error / len(self.objpoints)) 194 | 195 | 196 | else: 197 | raise ValueError("Mono calibrate failed! ") 198 | 199 | 200 | def mono_rectify(self, image, mode=0): 201 | if mode == 0: 202 | # 矫正单目图像：直接使用计算的相机内参数 203 | image_rec = cv2.undistort(image, self.mtx, self.dist, None, self.mtx) 204 | 205 | elif mode == 1: 206 | # 矫正单目图像：使用计算的相机内参数计算新的内参矩阵 207 | ''' 208 | 1、默认情况下，我们通常不会求取新的CameraMatrix，这样代码中会默认使用标定得到的Cameralatrix. 209 | 而这个摄像机矩阵是在理想情况下没有考虑畸变得到的，所以并不准确，重要的是fx和fy的值会比考虑畸变情况下的偏大， 210 | 会损失很多有效像素。我们可以通过这个函数getoptimaNewCameramatrix()求取一个新的摄像机内参矩阵。 211 | 2、cv2.getoptimalNewCameraMatrix()。如果参数alpha=0，它返回含有最小不需要像素的非扭曲图像， 212 | 所以它可能移除一些图像角点。如果alpha=1,所有像素都返回。还会返回一个 Ror图像，我们可以用来对结果进行裁剪。 213 | ''' 214 | # 使用 remap() 函数进行校正 215 | w, h = self.W, self.H 216 | newcameramtx, roi = cv2.getOptimalNewCameraMatrix(self.mtx, self.dist, (w, h), 0, (w, h)) 217 | image_rec = cv2.undistort(image, self.mtx, self.dist, None, newcameramtx) 218 | # 对图片有效区域进行剪裁 219 | # x, y, w, h = roi 220 | # image_rec = image_rec[y:y + h, x:x + w] 221 | 222 | elif mode == 2: 223 | # 使用 remap() 函数进行校正 224 | w, h = self.W, self.H 225 | newcameramtx, roi = cv2.getOptimalNewCameraMatrix(self.mtx, self.dist, (w, h), 1, (w, h)) 226 | mapx, mapy = cv2.initUndistortRectifyMap(self.mtx, self.dist, None, newcameramtx, (w, h), 5) 227 | image_rec = cv2.remap(image, mapx, mapy, cv2.INTER_LINEAR) 228 | # 对图片有效区域进行剪裁 229 | x, y, w, h = roi 230 | image_rec = image_rec[y:y + h, x:x + w] 231 | else: 232 | pass 233 | 234 | 235 | return image_rec 236 | 237 | 238 | def read_cali_file(self, cali_file): 239 | con = configparser.ConfigParser() 240 | con.read(cali_file, encoding='utf-8') 241 | sections = con.sections() 242 | # print(sections.items) 243 | calib = con.items('Calib') 244 | rectify = con.items('Rectify') 245 | calib = dict(calib) 246 | rectify = dict(rectify) 247 | 248 | self.u0 = calib['u0'] 249 | self.v0 = calib['v0'] 250 | self.fx = calib['fx'] 251 | self.fy = calib['fy'] 252 | 253 | self.mtx = np.array([[rectify['mtx_0'], rectify['mtx_1'], rectify['mtx_2']], 254 | [rectify['mtx_3'], rectify['mtx_4'], rectify['mtx_5']], 255 | [rectify['mtx_6'], rectify['mtx_7'], rectify['mtx_8']]]).astype('float32') 256 | 257 | self.dist = np.array([[rectify['dist_k1'], 258 | rectify['dist_k2'], 259 | rectify['dist_p1'], 260 | rectify['dist_p2'], 261 | rectify['dist_k3']]]).astype('float32') 262 | 263 | 264 | def save_cali_file(self, cali_file): 265 | self.u0 = self.mtx[0, 2] 266 | self.v0 = self.mtx[1, 2] 267 | self.fx = self.mtx[0, 0] 268 | self.fy = self.mtx[1, 1] 269 | self.Calib = {"u0": self.u0, 270 | "v0": self.v0, 271 | "fx": self.fx, 272 | "fy": self.fy, 273 | } 274 | # 相机内参矩阵 275 | # fx s u0 276 | # 0 fy v0 277 | # 0 0 1 278 | Rectify = {} 279 | for i in range(3): 280 | for j in range(3): 281 | Rectify["mtx_{}".format(i*3+j)] = self.mtx[i, j] 282 | 283 | # 相机畸变参数 distortion cofficients = (k_1, k_2, p_1, p_2, k_3) 284 | Rectify["dist_k1"] = self.dist[0, 0] 285 | Rectify["dist_k2"] = self.dist[0, 1] 286 | Rectify["dist_p1"] = self.dist[0, 2] 287 | Rectify["dist_p2"] = self.dist[0, 3] 288 | Rectify["dist_k3"] = self.dist[0, 4] 289 | 290 | self.Rectify = Rectify 291 | config = configparser.ConfigParser() 292 | config["Calib"] = self.Calib 293 | config["Rectify"] = self.Rectify 294 | with open(cali_file, 'w') as configfile: 295 | config.write(configfile) 296 | 297 | 298 | 299 | def main(): 300 | data_root = "./outputs/Data/Trinocular/Calib/mono" 301 | cameraCalibrator = MonoCameraCalibrator(data_root=data_root, 302 | corner_size=(7, 11), 303 | image_shape=(720, 1280), 304 | square_size=50, 305 | rectify_mode=1, 306 | cali_file="MonoCalib_Para_720P.ini", 307 | suffix="bmp") 308 | cameraCalibrator.Run_Calibrator() 309 | cameraCalibrator.rectify_image_list(mono_image_root=data_root, rectify_mode=0) 310 | 311 | 312 | 313 | if __name__ == '__main__': 314 | main() 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | -------------------------------------------------------------------------------- /StereoCalib.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | import os 4 | import numpy as np 5 | import glob 6 | import cv2 7 | import argparse 8 | import configparser 9 | 10 | 11 | class StereoCameraCalibrator(object): 12 | def __init__(self, 13 | data_root, 14 | imgL_root=None, 15 | imgR_root=None, 16 | corner_size=(7, 11), 17 | suffix="png", 18 | image_shape=(720, 1280), 19 | camera_param_file="StereoParameters_720P.ini"): 20 | super(StereoCameraCalibrator, self).__init__() 21 | self.data_root = data_root 22 | self.corner_h, self.corner_w = corner_size 23 | self.H, self.W = image_shape 24 | self.square_size = 50 25 | self.img_shape = (self.W, self.H) 26 | self.cali_file = os.path.join(data_root, camera_param_file) 27 | self.suffix = suffix 28 | 29 | if imgL_root == None and imgR_root == None: 30 | self.imgL_root = os.path.join(self.data_root, 'imgL/*.{}'.format(suffix)) 31 | self.imgR_root = os.path.join(self.data_root, 'imgR/*.{}'.format(suffix)) 32 | else: 33 | self.imgL_root = os.path.join(imgL_root, '*.{}'.format(suffix)) 34 | self.imgR_root = os.path.join(imgR_root, '*.{}'.format(suffix)) 35 | 36 | '''将左右图分开''' 37 | 38 | def split_imgLR(self, dir_name="imgLR", suffix="bmp"): 39 | imgLR_path_list = glob.glob(os.path.join(self.data_root, dir_name, "*.{}".format(suffix))) 40 | imgLR_path_list.sort() 41 | print(os.path.join(self.data_root, dir_name, "*.{}".format(suffix))) 42 | 43 | imgL_root = os.path.join(self.data_root, "imgL") 44 | imgR_root = os.path.join(self.data_root, "imgR") 45 | 46 | if not os.path.exists(imgL_root): 47 | os.makedirs(imgL_root) 48 | os.makedirs(imgR_root) 49 | 50 | for i, imgLR_path in enumerate(imgLR_path_list): 51 | imgLR = cv2.imread(imgLR_path) 52 | imgL, imgR = imgLR[:, :1920], imgLR[:, 1920:] 53 | 54 | imgL_path = os.path.join(imgL_root, "imgL_{:05d}.png".format(i)) 55 | imgR_path = os.path.join(imgR_root, "imgR_{:05d}.png".format(i)) 56 | 57 | cv2.imwrite(imgL_path, imgL) 58 | cv2.imwrite(imgR_path, imgR) 59 | print("{:05d}: {}".format(i, imgL_path)) 60 | print("{:05d}: {}\n".format(i, imgR_path)) 61 | 62 | '''立体标定''' 63 | def Run_Calibrator(self, alpha=0): 64 | if os.path.exists(self.cali_file): 65 | self.read_cali_file(self.cali_file) 66 | 67 | else: 68 | # calibration. 69 | self.read_images() 70 | self.camera_model = self.stereo_calibrate(self.img_shape, alpha=alpha) 71 | self.save_cali_file(self.cali_file) 72 | 73 | '''读取左右图角点''' 74 | 75 | def read_images(self): 76 | # termination criteria 77 | self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 78 | self.criteria_cal = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-5) 79 | 80 | # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) 81 | self.objp = np.zeros((self.corner_h * self.corner_w, 3), np.float32) 82 | self.objp[:, :2] = np.mgrid[0:self.corner_w, 0:self.corner_h].T.reshape(-1, 2) 83 | self.objp *= self.square_size 84 | # Arrays to store object points and image points from all the images. 85 | self.objpoints = [] # 3d point in real world space 86 | self.imgpoints_l = [] # 2d points in image plane. 87 | self.imgpoints_r = [] # 2d points in image plane. 88 | 89 | images_left = glob.glob(self.imgL_root) 90 | images_right = glob.glob(self.imgR_root) 91 | images_left.sort() 92 | images_right.sort() 93 | if len(images_left) == len(images_right): 94 | for i, fname in enumerate(images_left): 95 | print("\nRead image {}: \n{}\n{}".format(i, images_left[i], images_right[i])) 96 | img_l = cv2.imread(images_left[i]) 97 | img_r = cv2.imread(images_right[i]) 98 | img_l = cv2.resize(img_l, self.img_shape) 99 | img_r = cv2.resize(img_r, self.img_shape) 100 | 101 | gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY) 102 | gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY) 103 | 104 | # Find the chess board corners 105 | ret_l, corners_l = cv2.findChessboardCorners(gray_l, (self.corner_w, self.corner_h), None) 106 | ret_r, corners_r = cv2.findChessboardCorners(gray_r, (self.corner_w, self.corner_h), None) 107 | 108 | # If found, add object points, image points (after refining them) 109 | self.objpoints.append(self.objp) 110 | if ret_l is True: 111 | if (corners_l[0, 0, 0] < corners_l[-1, 0, 0]): 112 | print("*" * 5 + "order of {} is inverse!".format(i) + "*" * 5) 113 | corners_l = np.flip(corners_l, axis=0).copy() 114 | 115 | rt = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1), self.criteria) 116 | self.imgpoints_l.append(corners_l) 117 | 118 | # Draw and display the corners 119 | ret_l = cv2.drawChessboardCorners(img_l, (self.corner_w, self.corner_h), corners_l, ret_l) 120 | cv2.imshow("imgL", img_l) 121 | cv2.waitKey(50) 122 | 123 | if ret_r is True: 124 | if (corners_r[0, 0, 0] < corners_r[-1, 0, 0]): 125 | print("*" * 5 + "order of {} is inverse!".format(i) + "*" * 5) 126 | corners_r = np.flip(corners_r, axis=0).copy() 127 | 128 | rt = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1), self.criteria) 129 | self.imgpoints_r.append(corners_r) 130 | 131 | # Draw and display the corners 132 | ret_r = cv2.drawChessboardCorners(img_r, (self.corner_w, self.corner_h), corners_r, ret_r) 133 | cv2.imshow("imgR", img_r) 134 | cv2.waitKey(50) 135 | # self.img_shape = gray_l.shape[::-1] 136 | cv2.destroyAllWindows() 137 | else: 138 | raise ValueError("left image and right image must be same!") 139 | 140 | def stereo_calibrate(self, img_shape, alpha=0): 141 | flag1 = 0 142 | flag1 |= cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5 | cv2.CALIB_FIX_K6 143 | rt, self.M1, self.D1, self.r1, self.t1 = cv2.calibrateCamera(self.objpoints, 144 | self.imgpoints_l, 145 | self.img_shape, 146 | None, 147 | None, 148 | flags=flag1) 149 | rt, self.M2, self.D2, self.r2, self.t2 = cv2.calibrateCamera(self.objpoints, 150 | self.imgpoints_r, 151 | self.img_shape, 152 | None, 153 | None, 154 | flags=flag1) 155 | 156 | # flag2 |= cv2.CALIB_FIX_INTRINSIC 157 | # flags |= cv2.CALIB_FIX_PRINCIPAL_POINT 158 | # flag2 |= cv2.CALIB_USE_INTRINSIC_GUESS 159 | # flag2 |= cv2.CALIB_FIX_FOCAL_LENGTH 160 | # flags |= cv2.CALIB_FIX_ASPECT_RATIO 161 | # flag2 |= cv2.CALIB_ZERO_TANGENT_DIST 162 | # flags |= cv2.CALIB_RATIONAL_MODEL 163 | # flags |= cv2.CALIB_SAME_FOCAL_LENGTH 164 | # flags |= cv2.CALIB_FIX_K3 165 | # flags |= cv2.CALIB_FIX_K4 166 | # flags |= cv2.CALIB_FIX_K5 167 | ''' 168 | 1.objectPoints- vector 型的数据结构，存储标疋用息住世子 169 | 2.imagePoints1- vector>型的数据结构，存储标定角点在第一个摄像机下的投影后的亚像素坐标; 170 | 3.imagePoints2- vector>型的数据结构，存储标定角点在第二个摄像机下的投影后的亚像素坐标; 171 | 4.cameraMatrix1 输入/出的第个摄像机的相机矩阵。如果CALIB_USE_INTRINSIC_GUESS, CV CALIB FIX ASPECT RATIO 172 | CV CALIB FIX INTRINSICCVCALIB FIXFOCALLENGTH其中的一个或多个标志被没置，该摄像机矩阵的一些或全部参数需要被初始化; 173 | 5.distcoeffs1-第一个摄像机的辅入/输出型骑空问量，根据矫正模型的不同，输出向量长度由标志决定; 174 | 6.cameraMatrix2-输入/输出型的第二个摄像机的相机矩阵。参数意义同第一个相机矩阵相似; 175 | 7.distCoeffs2-第一个摄像机的输入/输出型变向量。根据矫正模型的不同，输出向量长度由标志决定; 176 | 8.imageSize-图像的大小; 177 | 9.R-输出型，第一和第二个摄像机之间的旋转矩阵; 178 | 10.T-输出型，第一和第二个摄像机之间的平移矩阵; 179 | 11.E-输出型，基本矩阵; 180 | 12.F-输出型，基础矩阵; 181 | flag:标定时的一些选项: 182 | CALIS USEINTRINSICGUESS:使用该参数时，在cameraMatrix矩阵中应该有fx,fy,ue,ve的估计值。否则的话，将初始化(ue,ve) 183 | 图像的中心点，使用最小二乘估算出fx.fy。 184 | CALIB FIXLPRINCIPAL_POINT:在进行优化时会固定光轴点。当CVCALIBUSE_INTRINSICGUESS参数被设置，光轴点将保持在中心或者某个输入的值。 CALIB FIX ASPECT RATI0:固定fx/fy的比值，只将fy作为可变量，进行优化计算。当CV CALIB USE_INTRINSIC GUESS没有被设置， 185 | fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到。 186 | CALIB ZERO_TANGENT_DIST:设定切向畸变参数(p1,p2)为零。 187 | CALIB FIXK1,...,CALIB FIX_K6:对应的径向畸变在优化中保持不变 188 | CALIB RATIONAL MODEL:计算k4，k5，k6三个畸变参数。如果没有设置，则只计算其它5个畸变参数。 189 | ''' 190 | flag2 = 0 191 | flag2 |= cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5 192 | stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5) 193 | ret, self.K1, self.D1, self.K2, self.D2, self.R, self.T, self.E, self.F = cv2.stereoCalibrate( 194 | self.objpoints, 195 | self.imgpoints_l, 196 | self.imgpoints_r, 197 | self.M1, 198 | self.D1, 199 | self.M2, 200 | self.D2, 201 | img_shape, 202 | criteria=stereocalib_criteria, 203 | flags=flag2) 204 | 205 | # 进行立体更正 206 | ''' 207 | cameraMatrix1-第一个摄像机的摄像机矩阵 distCoeffs1-第一个摄像机的畸变向量 208 | cameraMatrix2-第二个摄像机的摄像机矩阵 distCoeffs1-第二个摄像机的畸变向量 209 | imageSize-图像大小 210 | R- stereoCalibrate()求得的R矩阵 211 | T- stereoCalibrate()求得的T矩阵 212 | R1-输出矩阵，第一个摄像机的校正变换矩阵(旋转变换) 213 | R2-输出矩阵，第二个摄像机的校正变换矩阵(旋转矩阵) 214 | P1-输出矩阵，第一个摄像机在新坐标系下的投影矩阵 215 | P2-输出矩阵，第二个摄像机在想坐标系下的投影矩阵 216 | Q-4*4的深度差异映射矩阵 217 | flags-可选的标志有两种零或者CV_CALIB_ZERO_DISPARITY，如果设置CV_CALIB_ZERO_DISPARITY 的话， 218 | 该函数会让两幅校正后的图像的主点有相同的像素坐标。否则该函数会水平或垂直的移动图像，以使得其有用的范围最大 219 | alpha-拉伸参数。如果设置为负或忽略，将不进行拉伸。 220 | 如果设置为0，那么校正后图像只有有效的部分会被显示(没有黑色的部分) 221 | 如果设置为1，那么就会显示整个图像。设置为0~1之间的某个值，其效果也居于两者之间。 222 | newImageSize-校正后的图像分辨率，默认为原分辨率大小。 223 | validPixR0I1-可选的输出参数，Rect型数据。其内部的所有像素都有效 224 | validPixR012-可选的输出参数，Rect型数据。其内部的所有像素都有效 225 | ''' 226 | self.R1, self.R2, self.P1, self.P2, self.Q, validPixROI1, validPixROI2 = cv2.stereoRectify( 227 | self.K1, self.D1, 228 | self.K2, self.D2, 229 | self.img_shape, 230 | self.R, 231 | self.T, 232 | flags=cv2.CALIB_ZERO_DISPARITY, 233 | alpha=alpha) 234 | 235 | def get_coor(self, shape, axis=0): 236 | h, w = shape 237 | if axis == 0: 238 | coor = np.reshape(np.array(range(w)), [1, w]) 239 | coor = coor.repeat(h, axis=axis) 240 | else: 241 | coor = np.reshape(np.array(range(h)), [h, 1]) 242 | coor = coor.repeat(w, axis=axis) 243 | return coor.reshape(h, w) 244 | 245 | def undistort(self, image, K, D, R, P, interpolation=cv2.INTER_LINEAR): 246 | h, w = image.shape[:2] 247 | mapx, mapy = cv2.initUndistortRectifyMap(K, D, R, P, (w, h), cv2.CV_32FC1) 248 | image_Rectify = cv2.remap(image, mapx, mapy, interpolation) 249 | 250 | return image_Rectify 251 | 252 | def stereo_Rectify(self, imgL, imgR, shape=(720, 1280)): 253 | imgL = cv2.resize(imgL, (shape[1], shape[0])) 254 | imgL_rec = self.undistort(imgL, self.K1, self.D1, self.R1, self.P1) 255 | 256 | imgR = cv2.resize(imgR, (shape[1], shape[0])) 257 | imgR_rec = self.undistort(imgR, self.K2, self.D2, self.R2, self.P2) 258 | mapx = self.get_coor(shape=shape, axis=0) 259 | mapy = self.get_coor(shape=shape, axis=1) 260 | mapx = mapx - (float(self.Calib["u0l"]) - float(self.Calib["u0r"])) 261 | 262 | mapx = mapx.astype("float32") 263 | mapy = mapy.astype("float32") 264 | imgR_rec = cv2.remap(imgR_rec, mapx, mapy, cv2.INTER_LINEAR) 265 | 266 | # # 计算更正map 267 | # self.left_map1, self.left_map2 = cv2.initUndistortRectifyMap(self.K1, 268 | # self.D1, 269 | # self.R1, 270 | # self.P1, 271 | # self.img_shape, 272 | # cv2.CV_32FC1) 273 | # self.right_map1, self.right_map2 = cv2.initUndistortRectifyMap(self.K2, 274 | # self.D2, 275 | # self.R2, 276 | # self.P2, 277 | # self.img_shape, 278 | # cv2.CV_32FC1) 279 | # 280 | # imgL_Rectify = cv2.remap(imgL, self.left_map1, self.left_map2, cv2.INTER_LINEAR) 281 | # imgR_Rectify = cv2.remap(imgR, self.right_map1, self.right_map2, cv2.INTER_LINEAR) 282 | 283 | return imgL_rec, imgR_rec 284 | 285 | def rectify_image_list(self, stereo_image_root, shape=(720, 1280), prefix="png"): 286 | 287 | # 对所有图片进行去畸变，有两种方法实现分别为： undistort()和remap() 288 | imgL_path_root = os.path.join(self.data_root, "imgL/*.{}".format(self.suffix)) 289 | imgL_path_list = glob.glob(imgL_path_root) 290 | imgR_path_root = os.path.join(self.data_root, "imgR/*.{}".format(self.suffix)) 291 | imgR_path_list = glob.glob(imgR_path_root) 292 | imgL_path_list.sort() 293 | imgR_path_list.sort() 294 | if len(imgL_path_list) == len(imgR_path_list): 295 | rec_imgL_root = os.path.join(stereo_image_root, "imgL_rec") 296 | rec_imgR_root = os.path.join(stereo_image_root, "imgR_rec") 297 | os.makedirs(rec_imgL_root, exist_ok=True) 298 | os.makedirs(rec_imgR_root, exist_ok=True) 299 | 300 | for i in range(len(imgL_path_list)): 301 | imgL_path = imgL_path_list[i] 302 | imgR_path = imgR_path_list[i] 303 | imgL_rec_path = os.path.join(rec_imgL_root, "imgL_rec_{:05d}.{}".format(i, prefix)) 304 | imgR_rec_path = os.path.join(rec_imgR_root, "imgR_rec_{:05d}.{}".format(i, prefix)) 305 | print("{:05d}:\n{}\n{}".format(i, imgL_path, imgL_rec_path)) 306 | print("{:05d}:\n{}\n{}".format(i, imgR_path, imgR_rec_path)) 307 | 308 | imgL = cv2.imread(imgL_path) 309 | imgR = cv2.imread(imgR_path) 310 | imgL = cv2.resize(imgL, (self.W, self.H), interpolation=cv2.INTER_CUBIC) 311 | imgR = cv2.resize(imgR, (self.W, self.H), interpolation=cv2.INTER_CUBIC) 312 | 313 | imgL_rec, imgR_rec = self.stereo_Rectify(imgL, imgR, shape=shape) 314 | cv2.imwrite(imgL_rec_path, imgL_rec) 315 | cv2.imwrite(imgR_rec_path, imgR_rec) 316 | 317 | def read_cali_file(self, cali_file): 318 | con = configparser.RawConfigParser() 319 | con.optionxform = lambda option: option 320 | con.read(cali_file, encoding='utf-8') 321 | sections = con.sections() 322 | # print(sections.items) 323 | calib = con.items('Calib') 324 | rectify = con.items('Rectify') 325 | calib = dict(calib) 326 | rectify = dict(rectify) 327 | 328 | distort_left = np.array([rectify['KC0'], rectify['KC1'], rectify['KC2'], rectify['KC3'], 0.0]).astype('float32') 329 | distort_right = np.array( 330 | [rectify['KC_RIGHT0'], rectify['KC_RIGHT1'], rectify['KC_RIGHT2'], rectify['KC_RIGHT3'], 0.0]).astype( 331 | 'float32') 332 | 333 | R_left = np.array([rectify["R{}".format(i)] for i in range(9)], dtype="float32").reshape(3, 3) 334 | R_right = np.array([rectify["R_RIGHT{}".format(i)] for i in range(9)], dtype="float32").reshape(3, 3) 335 | 336 | K_left_old = np.array([[rectify['FC0'], 0.0, rectify['CC0']], 337 | [0.0, rectify['FC1'], rectify['CC1']], 338 | [0.0, 0.0, 1.0]]).astype('float32') 339 | K_left_new = np.array([[calib['focus'], 0.0, calib['u0l']], 340 | [0.0, calib['focus'], calib['v0']], 341 | [0.0, 0.0, 1.0]]).astype('float32') 342 | 343 | K_right_old = np.array([[rectify['FC_RIGHT0'], 0.0, rectify['CC_RIGHT0']], 344 | [0.0, rectify['FC_RIGHT1'], rectify['CC_RIGHT1']], 345 | [0.0, 0.0, 1.0]]).astype('float32') 346 | K_right_new = np.array([[calib['focus'], 0.0, calib['u0r']], 347 | [0.0, calib['focus'], calib['v0']], 348 | [0.0, 0.0, 1.0]]).astype('float32') 349 | 350 | KK_mtx_left = np.array([rectify["KK_inv{}".format(i)] for i in range(9)], dtype="float32").reshape(3, 3) 351 | KK_mtx_right = np.array([rectify["KK_RIGHT{}".format(i)] for i in range(9)], dtype="float32").reshape(3, 3) 352 | 353 | Rectify = { 354 | "K1": K_left_old, 355 | "D1": distort_left, 356 | "R1": R_left, 357 | "P1": K_left_new, 358 | "K2": K_right_old, 359 | "D2": distort_right, 360 | "R2": R_right, 361 | "P2": K_right_new, 362 | "mtx1": KK_mtx_left, 363 | "mtx2": KK_mtx_right, 364 | } 365 | 366 | Calib = { 367 | "u0l": float(calib["u0l"]), 368 | "u0r": float(calib["u0r"]), 369 | "v0": float(calib["v0"]), 370 | "bline": float(calib["bline"]), 371 | "focus": float(calib["focus"]), 372 | } 373 | 374 | self.Calib = Calib 375 | self.Rectify = Rectify 376 | 377 | self.u0l = Calib['u0l'] 378 | self.u0r = Calib['u0r'] 379 | self.v0 = Calib['v0'] 380 | self.baseline = Calib['bline'] 381 | self.focus = Calib['focus'] 382 | self.K1 = Rectify['K1'] 383 | self.D1 = Rectify['D1'] 384 | self.R1 = Rectify['R1'] 385 | self.P1 = Rectify['P1'] 386 | self.K2 = Rectify['K2'] 387 | self.D2 = Rectify['D2'] 388 | self.R2 = Rectify['R2'] 389 | self.P2 = Rectify['P2'] 390 | self.mtx1 = Rectify['mtx1'] 391 | self.mtx2 = Rectify['mtx2'] 392 | 393 | 394 | 395 | 396 | def save_cali_file(self, cali_file): 397 | self.u0l = self.P1[0, 2] 398 | self.u0r = self.P2[0, 2] 399 | self.v0 = self.P1[1, 2] 400 | self.baseline = 1.0 / self.Q[3, 2] 401 | self.focus = self.P1[0, 0] 402 | self.Calib = { 403 | "u0l": self.u0l, 404 | "u0r": self.u0r, 405 | "v0": self.v0, 406 | "bline": self.baseline, 407 | "focus": self.focus, 408 | } 409 | 410 | Rectify = {} 411 | R_left = np.reshape(self.R1, -1) 412 | for i in range(R_left.shape[0]): 413 | Rectify['R{}'.format(i)] = R_left[i] 414 | K_left_old = self.M1 415 | Rectify['FC0'] = K_left_old[0, 0] 416 | Rectify['FC1'] = K_left_old[1, 1] 417 | Rectify['CC0'] = K_left_old[0, 2] 418 | Rectify['CC1'] = K_left_old[1, 2] 419 | 420 | distort_left = self.D1 421 | for j in range(8): 422 | if j < 5: 423 | Rectify["KC{}".format(j)] = distort_left[0, j] 424 | else: 425 | Rectify["KC{}".format(j)] = 0.0 426 | 427 | for i in range(3): 428 | for j in range(3): 429 | Rectify["KK_inv{}".format(i * 3 + j)] = self.K1[i, j] 430 | 431 | R_right = np.reshape(self.R2, -1) 432 | for i in range(R_right.shape[0]): 433 | Rectify['R_RIGHT{}'.format(i)] = R_left[i] 434 | K_right_old = self.M2 435 | Rectify['FC_RIGHT0'] = K_right_old[0, 0] 436 | Rectify['FC_RIGHT1'] = K_right_old[1, 1] 437 | Rectify['CC_RIGHT0'] = K_right_old[0, 2] 438 | Rectify['CC_RIGHT1'] = K_right_old[1, 2] 439 | 440 | distort_right = self.D2 441 | for j in range(8): 442 | if j < 5: 443 | Rectify["KC_RIGHT{}".format(j)] = distort_left[0, j] 444 | else: 445 | Rectify["KC_RIGHT{}".format(j)] = 0.0 446 | 447 | for i in range(3): 448 | for j in range(3): 449 | Rectify["KK_RIGHT{}".format(i * 3 + j)] = self.K2[i, j] 450 | 451 | Rectify["KC4"] = 0.000000 452 | Rectify["KC2_0"] = 0.000000 453 | Rectify["KC2_1"] = 0.000000 454 | Rectify["KC2_2"] = 0.000000 455 | Rectify["KC2_RIGHT0"] = 0.000000 456 | Rectify["KC2_RIGHT1"] = 0.000000 457 | Rectify["KC2_RIGHT2"] = 0.000000 458 | 459 | self.Rectify = Rectify 460 | 461 | config = configparser.RawConfigParser() 462 | config.optionxform = lambda option: option 463 | config["Calib"] = self.Calib 464 | config["Rectify"] = self.Rectify 465 | with open(cali_file, 'w') as configfile: 466 | config.write(configfile) 467 | 468 | def rectify_video(self, video_path: str): 469 | self.load_params() 470 | cap = cv2.VideoCapture(video_path) 471 | if not cap.isOpened(): 472 | print("Unable to open video.") 473 | return False 474 | fourcc = int(cap.get(cv2.CAP_PROP_FOURCC)) 475 | out_format = video_path.split('.')[-1] 476 | fps = int(cap.get(cv2.CAP_PROP_FPS)) 477 | out = cv2.VideoWriter(filename='out.' + out_format, fourcc=0x00000021, fps=fps, frameSize=self.image_size) 478 | cv2.namedWindow("origin", cv2.WINDOW_NORMAL) 479 | cv2.namedWindow("dst", cv2.WINDOW_NORMAL) 480 | frame_count = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) 481 | for _ in range(frame_count): 482 | ret, img = cap.read() 483 | if ret: 484 | img = cv2.resize(img, (self.image_size[0], self.image_size[1])) 485 | cv2.imshow("origin", img) 486 | dst = self.rectify_image(img) 487 | cv2.imshow("dst", dst) 488 | out.write(dst) 489 | cv2.waitKey(1) 490 | cap.release() 491 | out.release() 492 | cv2.destroyAllWindows() 493 | return True 494 | 495 | 496 | if __name__ == '__main__': 497 | img_H, img_W = 720, 1280 # 720, 1280, 1080, 1920 498 | data_root = "./outputs/Data/Trinocular/Calib" 499 | StereoCalibrator = StereoCameraCalibrator(data_root=data_root, 500 | imgL_root=None, 501 | imgR_root=None, 502 | corner_size=(7, 11), 503 | suffix="png", 504 | image_shape=(img_H, img_W), 505 | camera_param_file="StereoParameters_{}P.ini".format(img_H)) 506 | 507 | StereoCalibrator.Run_Calibrator(alpha=1) 508 | # StereoCalibrator.rectify_image_list(stereo_image_root=data_root, shape=(img_H, img_W), prefix="png") 509 | 510 | images_right = glob.glob(os.path.join(data_root, 'imgL/*.png')) 511 | images_left = glob.glob(os.path.join(data_root, 'imgR/*.png')) 512 | images_left.sort() 513 | images_right.sort() 514 | imgL, imgR = cv2.imread(images_left[0]), cv2.imread(images_right[0]) 515 | 516 | imgL_Rectify, imgR_Rectify = StereoCalibrator.stereo_Rectify(imgL, imgR, shape=(img_H, img_W)) 517 | print("imgL_Rectify: ", imgL_Rectify.shape) 518 | print("imgR_Rectify: ", imgR_Rectify.shape) 519 | 520 | # 画圆，圆心为：(160, 160)，半径为：60，颜色为：point_color，实心线 521 | cv2.circle(imgL_Rectify, (int(img_W/2), int(img_H/2)), 3, (0, 0, 255), 0) 522 | cv2.circle(imgR_Rectify, (int(img_W/2), int(img_H/2)), 3, (0, 0, 255), 0) 523 | 524 | cv2.imshow("imgL_Rectify", imgL_Rectify) 525 | cv2.imshow("imgR_Rectify", imgR_Rectify) 526 | cv2.waitKey(0) 527 | --------------------------------------------------------------------------------