diff --git a/apps/calibration/calib_intri.py b/apps/calibration/calib_intri.py index 93fecd0..ed6639d 100644 --- a/apps/calibration/calib_intri.py +++ b/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 - @ FilePath: /EasyMocap/scripts/calibration/calib_intri.py + @ LastEditTime: 2021-05-26 23:22:26 + @ 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) - k3d = np.array(data['keypoints3d'], dtype=np.float32) - k2d = np.array(data['keypoints2d'], dtype=np.float32) - if k2d[:, -1].sum() < 0.01: - continue + flag, k2d, k3d = read_chess(chessname) + if not flag: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) \ No newline at end of file + if args.share_intri: + calib_intri_share(args.path, step=args.step) + else: + calib_intri(args.path, step=args.step) diff --git a/apps/calibration/check_calib.py b/apps/calibration/check_calib.py index 3f22e73..2a33d2e 100644 --- a/apps/calibration/check_calib.py +++ b/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 - @ FilePath: /EasyMocap/scripts/calibration/check_calib.py + @ LastEditTime: 2021-04-15 22:53:23 + @ 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) \ No newline at end of file diff --git a/apps/calibration/detect_chessboard.py b/apps/calibration/detect_chessboard.py index 271b786..ce2633a 100644 --- a/apps/calibration/detect_chessboard.py +++ b/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): - 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): +def create_chessboard(path, pattern, gridSize, ext): 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) - dataset = ImageFolder(path, annot='chessboard') + create_chessboard(path, pattern, gridSize, ext=args.ext) + 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, diff --git a/easymocap/annotator/chessboard.py b/easymocap/annotator/chessboard.py index 02f55e8..50628d2 100644 --- a/easymocap/annotator/chessboard.py +++ b/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)