add calibration code

This commit is contained in:
shuaiqing 2021-03-02 17:17:47 +08:00
parent 745b725b7c
commit 9bde27ca60
13 changed files with 284 additions and 22 deletions

View File

@ -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
return F

Binary file not shown.

After

Width:  |  Height:  |  Size: 87 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 100 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 108 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 106 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 107 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 103 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 108 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 108 KiB

View File

@ -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. ]

View File

@ -0,0 +1,39 @@
<!--
* @Date: 2021-03-02 16:14:48
* @Author: Qing Shuai
* @LastEditors: Qing Shuai
* @LastEditTime: 2021-03-02 17:09:02
* @FilePath: /EasyMocap/scripts/calibration/Readme.md
-->
# 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.

View File

@ -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)

View File

@ -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
'''