add calibration code

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

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

scripts/calibration/Readme.md

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

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)

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