diff --git a/code/mytools/camera_utils.py b/code/mytools/camera_utils.py index 838a075..e182d0e 100644 --- a/code/mytools/camera_utils.py +++ b/code/mytools/camera_utils.py @@ -6,7 +6,7 @@ import os class FileStorage(object): def __init__(self, filename, isWrite=False): version = cv2.__version__ - self.version = int(version.split('.')[0]) + self.version = '.'.join(version.split('.')[:2]) if isWrite: self.fs = cv2.FileStorage(filename, cv2.FILE_STORAGE_WRITE) else: @@ -19,7 +19,8 @@ class FileStorage(object): if dt == 'mat': cv2.FileStorage.write(self.fs, key, value) elif dt == 'list': - if self.version == 4: # 4.4 + # import ipdb;ipdb.set_trace() + if self.version == '4.4': # 4.4 # self.fs.write(key, '[') # for elem in value: # self.fs.write('none', elem) @@ -67,39 +68,98 @@ def _FindChessboardCorners(img, patternSize = (9, 6)): corners = cv2.cornerSubPix(img, corners, (11, 11), (-1, -1), criteria) return True, corners -def FindChessboardCorners(image_names, patternSize=(9, 6), gridSize=0.1, debug=False): +def FindChessboardCorners(image_names, patternSize=(9, 6), gridSize=0.1, debug=False, remove=False, resize_rate = 1): # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) object_points = np.zeros((patternSize[1]*patternSize[0], 3), np.float32) object_points[:,:2] = np.mgrid[0:patternSize[0], 0:patternSize[1]].T.reshape(-1,2) object_points = object_points * gridSize # Arrays to store object points and image points from all the images. - objpoints = [] # 3d point in real world space - imgpoints = [] # 2d points in image plane. - images = [] - for fname in tqdm(image_names): + infos = [] + for fname in tqdm(image_names, desc='detecting chessboard'): + assert os.path.exists(fname), fname + tmpname = fname+'.corners.txt' img = cv2.imread(fname) - gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) - # Find the chess board corners - ret, corners = _FindChessboardCorners(gray, patternSize) + img = cv2.resize(img, None, fx=resize_rate, fy=resize_rate) + if debug: + if img.shape[0] > 1000: + show = cv2.resize(img, None, fx=0.5, fy=0.5) + else: + show = img + cv2.imshow('img', show) + cv2.waitKey(10) + - # If found, add object points, image points (after refining them) - if ret: - objpoints.append(object_points) - imgpoints.append(corners) - # Draw and display the corners - images.append(img) - if debug: - img = cv2.drawChessboardCorners(img, patternSize, corners, ret) - cv2.imshow('img',img) - cv2.waitKey(10) + if os.path.exists(tmpname) and not debug: + ret = True + tmp = np.loadtxt(tmpname) + if len(tmp.shape) < 2: + ret = False + else: + corners = tmp.reshape((-1, 1, 2)) + corners = np.ascontiguousarray(corners.astype(np.float32)) else: + gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) + # Find the chess board corners + ret, corners = _FindChessboardCorners(gray, patternSize) + if not ret: + ret, corners = _FindChessboardCorners(gray, patternSize, False) + print('Not found in adaptive mode, but retry = {}'.format(ret)) + # If found, add object points, image points (after refining them) + if ret: + np.savetxt(tmpname, corners[:, 0]) + else: + np.savetxt(tmpname, np.empty((0, 2), dtype=np.float32)) + if ret: + infos.append({ + 'point_object': object_points, + 'point_image': corners, + 'image': img, + 'image_name': fname + }) + if debug: + show = img.copy() + show = cv2.drawChessboardCorners(show, patternSize, corners, ret) + if show.shape[0] > 1000: + show = cv2.resize(show, None, fx=0.5, fy=0.5) + cv2.imshow('img', show) + cv2.waitKey(10) + elif remove: os.remove(fname) - return imgpoints, objpoints, images + os.remove(tmpname) + return infos + def safe_mkdir(path): if not os.path.exists(path): os.makedirs(path) +def read_intri(intri_name): + assert os.path.exists(intri_name), intri_name + intri = FileStorage(intri_name) + camnames = intri.read('names', dt='list') + from collections import OrderedDict + cameras = OrderedDict() + for key in camnames: + cam = {} + cam['K'] = intri.read('K_{}'.format(key)) + cam['invK'] = np.linalg.inv(cam['K']) + cam['dist'] = intri.read('dist_{}'.format(key)) + cameras[key] = cam + return cameras + +def write_extri(camera, path, base='extri.yml'): + extri_name = os.path.join(path, base) + extri = FileStorage(extri_name, True) + results = {} + camnames = [key_.split('.')[0] for key_ in camera.keys()] + extri.write('names', camnames, 'list') + for key_, val in camera.items(): + key = key_.split('.')[0] + extri.write('R_{}'.format(key), val['Rvec']) + extri.write('Rot_{}'.format(key), val['R']) + extri.write('T_{}'.format(key), val['T']) + return 0 + def read_camera(intri_name, extri_name, cam_names=[0,1,2,3]): assert os.path.exists(intri_name), intri_name assert os.path.exists(extri_name), extri_name @@ -243,4 +303,4 @@ def get_fundamental_matrix(cameras, basenames): F[(icam, jcam)] += fundamental_RT_op(cameras[icam]['K'], cameras[icam]['RT'], cameras[jcam]['K'], cameras[jcam]['RT']) if F[(icam, jcam)].sum() == 0: F[(icam, jcam)] += 1e-12 # to avoid nan - return F \ No newline at end of file + return F diff --git a/data/examples/calibration/extri_images/1.jpg b/data/examples/calibration/extri_images/1.jpg new file mode 100644 index 0000000..6782d22 Binary files /dev/null and b/data/examples/calibration/extri_images/1.jpg differ diff --git a/data/examples/calibration/extri_images/2.jpg b/data/examples/calibration/extri_images/2.jpg new file mode 100644 index 0000000..5fd789b Binary files /dev/null and b/data/examples/calibration/extri_images/2.jpg differ diff --git a/data/examples/calibration/extri_images/3.jpg b/data/examples/calibration/extri_images/3.jpg new file mode 100644 index 0000000..6a8b9aa Binary files /dev/null and b/data/examples/calibration/extri_images/3.jpg differ diff --git a/data/examples/calibration/extri_images/4.jpg b/data/examples/calibration/extri_images/4.jpg new file mode 100644 index 0000000..62184fc Binary files /dev/null and b/data/examples/calibration/extri_images/4.jpg differ diff --git a/data/examples/calibration/extri_images/5.jpg b/data/examples/calibration/extri_images/5.jpg new file mode 100644 index 0000000..4227b77 Binary files /dev/null and b/data/examples/calibration/extri_images/5.jpg differ diff --git a/data/examples/calibration/extri_images/6.jpg b/data/examples/calibration/extri_images/6.jpg new file mode 100644 index 0000000..a50a033 Binary files /dev/null and b/data/examples/calibration/extri_images/6.jpg differ diff --git a/data/examples/calibration/extri_images/7.jpg b/data/examples/calibration/extri_images/7.jpg new file mode 100644 index 0000000..e9cc9cf Binary files /dev/null and b/data/examples/calibration/extri_images/7.jpg differ diff --git a/data/examples/calibration/extri_images/8.jpg b/data/examples/calibration/extri_images/8.jpg new file mode 100644 index 0000000..198017e Binary files /dev/null and b/data/examples/calibration/extri_images/8.jpg differ diff --git a/data/examples/calibration/intri.yml b/data/examples/calibration/intri.yml new file mode 100644 index 0000000..f9a4db4 --- /dev/null +++ b/data/examples/calibration/intri.yml @@ -0,0 +1,99 @@ +%YAML:1.0 +--- +names: + - "1" + - "2" + - "3" + - "4" + - "5" + - "6" + - "7" + - "8" +K_1: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 9.0927224982003611e+02, 0., 9.6062674039097942e+02, 0., + 9.2444288845626772e+02, 5.4293008226872905e+02, 0., 0., 1. ] +dist_1: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 0., 0., 0., 0., 0. ] +K_2: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 9.0927224982003611e+02, 0., 9.6062674039097942e+02, 0., + 9.2444288845626772e+02, 5.4293008226872905e+02, 0., 0., 1. ] +dist_2: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 0., 0., 0., 0., 0. ] +K_3: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 9.0927224982003611e+02, 0., 9.6062674039097942e+02, 0., + 9.2444288845626772e+02, 5.4293008226872905e+02, 0., 0., 1. ] +dist_3: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 0., 0., 0., 0., 0. ] +K_4: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 9.1665997341043840e+02, 0., 9.5547887626827935e+02, 0., + 9.8855809176036394e+02, 5.5038850358905097e+02, 0., 0., 1. ] +dist_4: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 0., 0., 0., 0., 0. ] +K_5: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 9.0156851427245795e+02, 0., 9.6223657355953253e+02, 0., + 8.8178917785495423e+02, 5.6788041632010697e+02, 0., 0., 1. ] +dist_5: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 0., 0., 0., 0., 0. ] +K_6: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 9.0088200265010209e+02, 0., 9.5815011666263433e+02, 0., + 9.3840029880200143e+02, 5.5017825887724632e+02, 0., 0., 1. ] +dist_6: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 0., 0., 0., 0., 0. ] +K_7: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 8.9861671187902732e+02, 0., 9.7219362635132836e+02, 0., + 9.0366361957942956e+02, 5.4668133454126132e+02, 0., 0., 1. ] +dist_7: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 0., 0., 0., 0., 0. ] +K_8: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 9.0204703596907223e+02, 0., 9.7263731333736860e+02, 0., + 8.7830208920235123e+02, 5.4022734518354935e+02, 0., 0., 1. ] +dist_8: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [ 0., 0., 0., 0., 0. ] diff --git a/scripts/calibration/Readme.md b/scripts/calibration/Readme.md new file mode 100644 index 0000000..720b393 --- /dev/null +++ b/scripts/calibration/Readme.md @@ -0,0 +1,39 @@ + +# Camera Calibration + +## 0. Prepare your chessboard + +## 1. Distortion and Intrinsic Parameter Calibration +TODO + +## 2. Extrinsic Parameter Calibration +Prepare your images as following: +```bash +./data/examples/calibration +├── extri_images +│   ├── 1.jpg +│   ├── 2.jpg +│   ├── 3.jpg +│   ├── 4.jpg +│   ├── 5.jpg +│   ├── 6.jpg +│   ├── 7.jpg +│   └── 8.jpg +└── intri.yml +``` +The basename of the images must be same as the name of cameras in `intri.yml`. + +```bash +python3 scripts/calibration/calib_extri.py -i ./data/examples/calibration/extri_images -o ./data/examples/calibration --debug +``` +To specify your chessboard, add the option `--pattern`, `--grid` +```bash +python3 scripts/calibration/calib_extri.py -i ./data/examples/calibration/extri_images -o ./data/examples/calibration --debug --pattern 9,6 --grid 0.1 +``` +More details can be found in the code. \ No newline at end of file diff --git a/scripts/calibration/calib_extri.py b/scripts/calibration/calib_extri.py new file mode 100644 index 0000000..4ebe173 --- /dev/null +++ b/scripts/calibration/calib_extri.py @@ -0,0 +1,57 @@ +''' + @ Date: 2021-03-02 16:13:03 + @ Author: Qing Shuai + @ LastEditors: Qing Shuai + @ LastEditTime: 2021-03-02 17:06:41 + @ FilePath: /EasyMocap/scripts/calibration/calib_extri.py +''' +import os +from glob import glob +from os.path import join +import cv2 +import sys +code_path = join(os.path.dirname(__file__), '..', '..', 'code') +sys.path.append(code_path) +from mytools.camera_utils import read_intri, write_extri, FindChessboardCorners + +def calib_extri_pipeline(path, out, resize_rate, debug, args): + assert os.path.exists(path), path + intri = read_intri(join(out, 'intri.yml')) + cameras = [i.split('.')[0] for i in sorted(os.listdir(path))] + if cameras[0].isdigit(): + cameras.sort(key=lambda x:int(x)) + total = 0 + extri = {} + for cam in cameras: + image_names = glob(join(path, '{}.jpg'.format(cam))) + assert len(image_names) >= 1, '{}/{} has no images'.format(path, cameras) + infos = FindChessboardCorners([image_names[0]], + patternSize=args.pattern, gridSize=args.grid, + debug=debug, remove=False, resize_rate=resize_rate) + if len(infos) < 1: + continue + info = infos[0] + ret, rvecs, tvecs = cv2.solvePnP(info['point_object'], info['point_image'], intri[cam]['K'], intri[cam]['dist']) + extri[cam] = {} + extri[cam]['Rvec'] = rvecs + extri[cam]['R'] = cv2.Rodrigues(rvecs)[0] + extri[cam]['T'] = tvecs + extri[cam]['center'] = -extri[cam]['R'].T @ tvecs + write_extri(extri, out, 'extri.yml') + +if __name__ == "__main__": + import argparse + parser = argparse.ArgumentParser() + parser.add_argument('-i', '--path', type=str,dest='path', + help='the directory contains the extrinsic images') + parser.add_argument('-o', '--out', type=str, + help='output path') + parser.add_argument('--pattern', type=lambda x: (int(x.split(',')[0]), int(x.split(',')[1])), + help='The pattern of the chessboard', default=(9, 6)) + parser.add_argument('--grid', type=float, default=0.1, + help='The length of the grid size (unit: meter)') + parser.add_argument('--rate', type=float, default=1, + help='scale the original image') + parser.add_argument('--debug', action='store_true') + args = parser.parse_args() + calib_extri_pipeline(args.path, args.out, args.rate, args.debug, args) \ No newline at end of file diff --git a/scripts/calibration/calib_intri.py b/scripts/calibration/calib_intri.py new file mode 100644 index 0000000..b1bfe27 --- /dev/null +++ b/scripts/calibration/calib_intri.py @@ -0,0 +1,7 @@ +''' + @ Date: 2021-03-02 16:12:59 + @ Author: Qing Shuai + @ LastEditors: Qing Shuai + @ LastEditTime: 2021-03-02 16:12:59 + @ FilePath: /EasyMocap/scripts/calibration/calib_intri.py +'''