[calib] update calibration

apps/calibration/calib_intri.py

@@ -2,17 +2,85 @@
   @ Date: 2021-03-02 16:12:59
   @ Author: Qing Shuai
   @ LastEditors: Qing Shuai
-  @ LastEditTime: 2021-03-02 16:12:59
+  @ LastEditTime: 2021-05-26 23:22:26
-  @ FilePath: /EasyMocap/scripts/calibration/calib_intri.py
+  @ FilePath: /EasyMocap/apps/calibration/calib_intri.py
 '''
 # This script calibrate each intrinsic parameters
-from easymocap.mytools import write_intri
+from easymocap.mytools.vis_base import plot_points2d
+from easymocap.mytools import write_intri, read_json, Timer
 import numpy as np
 import cv2
 import os
 from os.path import join
 from glob import glob
-from easymocap.mytools import read_json, Timer
+from easymocap.annotator.chessboard import get_lines_chessboard
+
+def read_chess(chessname):
+    data = read_json(chessname)
+    k3d = np.array(data['keypoints3d'], dtype=np.float32)
+    k2d = np.array(data['keypoints2d'], dtype=np.float32)
+    if k2d[:, -1].sum() < 0.01:
+        return False, k2d, k3d
+    return True, k2d, k3d
+
+def calib_intri_share(path, step):
+    camnames = sorted(os.listdir(join(path, 'images')))
+    imagenames = sorted(glob(join(path, 'images', '*', '*.jpg')))
+    chessnames = sorted(glob(join(path, 'chessboard', '*', '*.json')))
+    k3ds_, k2ds_, imgs = [], [], []
+    valid_idx = []
+    for i, chessname in enumerate(chessnames):
+        flag, k2d, k3d = read_chess(chessname)
+        k3ds_.append(k3d)
+        k2ds_.append(k2d)
+        if not flag:
+            continue
+        valid_idx.append(i)
+    MAX_ERROR_PIXEL = 1.
+    lines, line_cols = get_lines_chessboard()
+    valid_idx = valid_idx[::step]
+    len_valid = len(valid_idx)
+    cameras = {}
+    while True:
+        # sample
+        imgs = [imagenames[i] for i in valid_idx]
+        k3ds = [k3ds_[i] for i in valid_idx]
+        k2ds = [np.ascontiguousarray(k2ds_[i][:, :-1]) for i in valid_idx]
+        gray = cv2.imread(imgs[0], 0)
+        print('>> Detect {:3d} frames'.format(len(valid_idx)))
+        with Timer('calibrate'):
+            ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
+                k3ds, k2ds, gray.shape[::-1], None, None)
+        with Timer('check'):
+            removed = []
+            for i in range(len(imgs)):
+                img = cv2.imread(imgs[i])
+                points2d_repro, _ = cv2.projectPoints(
+                    k3ds[i], rvecs[i], tvecs[i], K, dist)
+                points2d_repro = points2d_repro.squeeze()
+                points2d = k2ds_[valid_idx[i]]
+                err = np.linalg.norm(points2d_repro - points2d[:, :2], axis=1).mean()
+                plot_points2d(img, points2d_repro, lines, col=(0, 0, 255), lw=1, putText=False)
+                plot_points2d(img, points2d, lines, lw=1, putText=False)
+                print(imgs[i], err)
+                # cv2.imshow('vis', img)
+                # cv2.waitKey(0)
+                if err > MAX_ERROR_PIXEL:
+                    removed.append(i)
+            for i in removed[::-1]:
+                valid_idx.pop(i)
+        if len_valid == len(valid_idx) or not args.remove:
+            print(K)
+            print(dist)
+            for cam in camnames:
+                cameras[cam] = {
+                    'K': K,
+                    'dist': dist  # dist: (1, 5)
+                }
+            break
+        len_valid = len(valid_idx)
+    write_intri(join(path, 'output', 'intri.yml'), cameras)
+
 
 def calib_intri(path, step):
     camnames = sorted(os.listdir(join(path, 'images')))
@@ -22,11 +90,8 @@ def calib_intri(path, step):
         chessnames = sorted(glob(join(path, 'chessboard', cam, '*.json')))
         k3ds, k2ds = [], []
         for chessname in chessnames[::step]:
-            data = read_json(chessname)
+            flag, k2d, k3d = read_chess(chessname)
-            k3d = np.array(data['keypoints3d'], dtype=np.float32)
+            if not flag:continue
-            k2d = np.array(data['keypoints2d'], dtype=np.float32)
-            if k2d[:, -1].sum() < 0.01:
-                continue
             k3ds.append(k3d)
             k2ds.append(np.ascontiguousarray(k2d[:, :-1]))
         gray = cv2.imread(imagenames[0], 0)
@@ -36,15 +101,21 @@ def calib_intri(path, step):
                 k3ds, k2ds, gray.shape[::-1], None, None)
             cameras[cam] = {
                 'K': K,
-                'dist': dist # dist: (1, 5)
+                'dist': dist  # dist: (1, 5)
             }
     write_intri(join(path, 'output', 'intri.yml'), cameras)
 
+
 if __name__ == "__main__":
     import argparse
     parser = argparse.ArgumentParser()
     parser.add_argument('path', type=str, default='/home/')
     parser.add_argument('--step', type=int, default=1)
+    parser.add_argument('--share_intri', action='store_true')
     parser.add_argument('--debug', action='store_true')
+    parser.add_argument('--remove', action='store_true')
     args = parser.parse_args()
-    calib_intri(args.path, step=args.step)
+    if args.share_intri:
+        calib_intri_share(args.path, step=args.step)
+    else:
+        calib_intri(args.path, step=args.step)

apps/calibration/check_calib.py

@@ -2,8 +2,8 @@
   @ Date: 2021-03-27 19:13:50
   @ Author: Qing Shuai
   @ LastEditors: Qing Shuai
-  @ LastEditTime: 2021-04-02 22:01:10
+  @ LastEditTime: 2021-04-15 22:53:23
-  @ FilePath: /EasyMocap/scripts/calibration/check_calib.py
+  @ FilePath: /EasyMocap/apps/calibration/check_calib.py
 '''
 import cv2
 import numpy as np
@@ -14,7 +14,21 @@ from easymocap.mytools import read_camera, plot_points2d
 from easymocap.mytools import batch_triangulate, projectN3, Undistort
 from tqdm import tqdm
 
-def load_grids():
+POINTS_SQUARE = np.array([
+    [0., 0., 0.],
+    [1., 0., 0.],
+    [1., 1., 0.],
+    [0., 1., 0.]
+])
+
+LINES_SQUARE = np.array([
+    [0, 1],
+    [1, 2],
+    [2, 3],
+    [3, 0]
+])
+
+def load_cube():
     points3d = np.array([
         [0., 0., 0.],
         [1., 0., 0.],
@@ -42,6 +56,39 @@ def load_grids():
     points3d = np.hstack((points3d, np.ones((points3d.shape[0], 1))))
     return points3d, lines
 
+def merge_points_lines(points3d, lines):
+    dist = np.linalg.norm(points3d[:, None, :] - points3d[None, :, :], axis=-1)
+    mapid = np.arange(points3d.shape[0])
+    for i in range(dist.shape[0]):
+        if mapid[i] != i:
+            continue
+        equal = np.where(dist[i] < 1e-3)[0]
+        for j in equal:
+            if j == i:
+                continue
+            mapid[j] = i
+    newid = sorted(list(set(mapid)))
+    newpoints = points3d[newid]
+    for i, newi in enumerate(newid):
+        mapid[mapid==newi] = i
+    return newpoints, mapid[lines]
+
+def load_grid(xrange=10, yrange=10):
+    start = np.array([0., 0., 0.])
+    xdir = np.array([1., 0., 0.])
+    ydir = np.array([0., 1., 0.])
+    stepx = 1.
+    stepy = 1.
+    points3d, lines = [], []
+    for i in range(xrange):
+        for j in range(yrange):
+            base = start + xdir*i*stepx + ydir*j*stepy
+            points3d.append(POINTS_SQUARE+base)
+            lines.append(LINES_SQUARE+4*(i*yrange+j))
+    points3d = np.vstack(points3d)
+    lines = np.vstack(lines)
+    return merge_points_lines(points3d, lines)
+
 def check_calib(path, out, vis=False, show=False, debug=False):
     if vis:
         out_dir = join(out, 'check')
@@ -91,10 +138,9 @@ def check_calib(path, out, vis=False, show=False, debug=False):
             cv2.imwrite(outname, imgout)
     print('{:.2f}/{} = {:.2f} pixel'.format(total_sum, int(cnt), total_sum/cnt))
 
-def check_scene(path, out):
+def check_scene(path, out, points3d, lines):
     cameras = read_camera(join(out, 'intri.yml'), join(out, 'extri.yml'))
     cameras.pop('basenames')
-    points3d, lines = load_grids()
     nf = 0
     for cam, camera in cameras.items():
         imgname = join(path, 'images', cam, '{:06d}.jpg'.format(nf))
@@ -106,6 +152,37 @@ def check_scene(path, out):
         cv2.imshow('vis', img)
         cv2.waitKey(0)
 
+def check_match(path, out):
+    os.makedirs(out, exist_ok=True)
+    cameras = read_camera(join(path, 'intri.yml'), join(path, 'extri.yml'))
+    cams = cameras.pop('basenames')
+    annots = read_json(join(path, 'calib.json'))
+    points_global = annots['points_global']
+    points3d = np.ones((len(points_global), 4))
+    # first triangulate
+    points2d = np.zeros((len(cams), len(points_global), 3))
+    for i, record in enumerate(points_global):
+        for cam, (x, y) in record.items():
+            points2d[cams.index(cam), i] = (x, y, 1)
+    # 2. undistort
+    for nv in range(points2d.shape[0]):
+        camera = cameras[cams[nv]]
+        points2d[nv] = Undistort.points(points2d[nv], camera['K'], camera['dist'])
+    Pall = np.stack([cameras[cam]['P'] for cam in cams])
+    points3d = batch_triangulate(points2d, Pall)
+    lines = []
+    nf = 0
+    for cam, camera in cameras.items():
+        imgname = join(path, 'images', cam, '{:06d}.jpg'.format(nf))
+        assert os.path.exists(imgname), imgname
+        img = cv2.imread(imgname)
+        img = Undistort.image(img, camera['K'], camera['dist'])
+        kpts_repro = projectN3(points3d, camera['P'][None, :, :])[0]
+        plot_points2d(img, kpts_repro, lines, col=(0, 0, 255), lw=1, putText=True)
+        plot_points2d(img, points2d[cams.index(cam)], lines, col=(0, 255, 0), lw=1, putText=True)
+        outname = join(out, cam+'.jpg')
+        cv2.imwrite(outname, img)
+
 if __name__ == "__main__":
     import argparse
     parser = argparse.ArgumentParser()
@@ -117,9 +194,17 @@ if __name__ == "__main__":
     parser.add_argument('--show', action='store_true')
     parser.add_argument('--debug', action='store_true')
     parser.add_argument('--cube', action='store_true')
+    parser.add_argument('--grid', action='store_true')
+    parser.add_argument('--calib', action='store_true')
     
     args = parser.parse_args()
     if args.cube:
-        check_scene(args.path, args.out)
+        points, lines = load_cube()
+        check_scene(args.path, args.out, points, lines)
+    elif args.grid:
+        points, lines = load_grid(xrange=15, yrange=14)
+        check_scene(args.path, args.out, points, lines)
+    elif args.calib:
+        check_match(args.path, args.out)
     else:
         check_calib(args.path, args.out, args.vis, args.show, args.debug)

apps/calibration/detect_chessboard.py

@@ -1,32 +1,25 @@
 # detect the corner of chessboard
 from easymocap.annotator.file_utils import getFileList, read_json, save_json
 from tqdm import tqdm
-from easymocap.annotator import ImageFolder, findChessboardCorners
+from easymocap.annotator import ImageFolder
+from easymocap.annotator.chessboard import getChessboard3d, findChessboardCorners
 import numpy as np
 from os.path import join
 import cv2
 import os
 
-def get_object(pattern, gridSize):
+def create_chessboard(path, pattern, gridSize, ext):
-    object_points = np.zeros((pattern[1]*pattern[0], 3), np.float32)
-    # 注意：这里为了让标定板z轴朝上，设定了短边是x，长边是y
-    object_points[:,:2] = np.mgrid[0:pattern[0], 0:pattern[1]].T.reshape(-1,2)
-    object_points[:, [0, 1]] = object_points[:, [1, 0]]
-    object_points = object_points * gridSize
-    return object_points
-
-def create_chessboard(path, pattern, gridSize):
     print('Create chessboard {}'.format(pattern))
-    keypoints3d = get_object(pattern, gridSize=gridSize)
+    keypoints3d = getChessboard3d(pattern, gridSize=gridSize)
     keypoints2d = np.zeros((keypoints3d.shape[0], 3))
-    imgnames = getFileList(path, ext='.jpg')
+    imgnames = getFileList(path, ext=ext)
     template = {
         'keypoints3d': keypoints3d.tolist(),
         'keypoints2d': keypoints2d.tolist(),
         'visited': False
     }
     for imgname in tqdm(imgnames, desc='create template chessboard'):
-        annname = imgname.replace('images', 'chessboard').replace('.jpg', '.json')
+        annname = imgname.replace('images', 'chessboard').replace(ext, '.json')
         annname = join(path, annname)
         if os.path.exists(annname):
             # 覆盖keypoints3d
@@ -37,8 +30,8 @@ def create_chessboard(path, pattern, gridSize):
             save_json(annname, template)
 
 def detect_chessboard(path, out, pattern, gridSize):
-    create_chessboard(path, pattern, gridSize)
+    create_chessboard(path, pattern, gridSize, ext=args.ext)
-    dataset = ImageFolder(path, annot='chessboard')
+    dataset = ImageFolder(path, annot='chessboard', ext=args.ext)
     dataset.isTmp = False
     for i in tqdm(range(len(dataset))):
         imgname, annotname = dataset[i]
@@ -58,6 +51,7 @@ if __name__ == "__main__":
     parser = argparse.ArgumentParser()
     parser.add_argument('path', type=str)
     parser.add_argument('--out', type=str)
+    parser.add_argument('--ext', type=str, default='.jpg', choices=['.jpg', '.png'])
     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, 

easymocap/annotator/chessboard.py

@@ -1,6 +1,40 @@
+'''
+  @ Date: 2021-04-13 16:14:36
+  @ Author: Qing Shuai
+  @ LastEditors: Qing Shuai
+  @ LastEditTime: 2021-05-26 15:42:02
+  @ FilePath: /EasyMocap/easymocap/annotator/chessboard.py
+'''
 import numpy as np
 import cv2
 
+def getChessboard3d(pattern, gridSize):
+    object_points = np.zeros((pattern[1]*pattern[0], 3), np.float32)
+    # 注意：这里为了让标定板z轴朝上，设定了短边是x，长边是y
+    object_points[:,:2] = np.mgrid[0:pattern[0], 0:pattern[1]].T.reshape(-1,2)
+    object_points[:, [0, 1]] = object_points[:, [1, 0]]
+    object_points = object_points * gridSize
+    return object_points
+
+colors_chessboard_bar = [
+    [0, 0, 255],
+    [0, 128, 255],
+    [0, 200, 200],
+    [0, 255, 0],
+    [200, 200, 0],
+    [255, 0, 0],
+    [255, 0, 250]
+]
+
+def get_lines_chessboard(pattern=(9, 6)):
+    w, h = pattern[0], pattern[1]
+    lines = []
+    lines_cols = []
+    for i in range(w*h-1):
+        lines.append([i, i+1])
+        lines_cols.append(colors_chessboard_bar[i//w])
+    return lines, lines_cols
+
 def _findChessboardCorners(img, pattern):
     "basic function"
     criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)