''' @ Date: 2021-03-27 19:13:50 @ Author: Qing Shuai @ LastEditors: Qing Shuai @ LastEditTime: 2022-10-11 16:47:10 @ FilePath: /EasyMocapPublic/apps/calibration/check_calib.py ''' from easymocap.mytools.debug_utils import myerror, mywarn from easymocap.mytools.file_utils import myarray2string import cv2 import numpy as np import os from os.path import join from easymocap.mytools import read_json, merge from easymocap.mytools import read_camera, plot_points2d from easymocap.mytools import batch_triangulate, projectN3, Undistort from tqdm import tqdm 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(grid_size=1, **kwargs): min_x, min_y, min_z = (0, 0, 0.) max_x, max_y, max_z = (grid_size, grid_size, grid_size) # min_x, min_y, min_z = (-0.75, -0.9, 0.) # max_x, max_y, max_z = (0.75, 0.7, 0.9) # # 灯光球场篮球: # min_x, min_y, min_z = (-7.5, -2.89, 0.) # max_x, max_y, max_z = (7.5, 11.11, 2.) # # 4d association: # min_x, min_y, min_z = (-1.6, -1.6, 0.) # max_x, max_y, max_z = (1.5, 1.6, 2.4) # min_x, min_y, min_z = (-2.45, -4., 0.) # max_x, max_y, max_z = (1.65, 2.45, 2.6) points3d = np.array([ [min_x, min_y, min_z], [max_x, min_y, min_z], [max_x, max_y, min_z], [min_x, max_y, min_z], [min_x, min_y, max_z], [max_x, min_y, max_z], [max_x, max_y, max_z], [min_x, max_y, max_z], ]) lines = np.array([ [0, 1], [1, 2], [2, 3], [3, 0], [4, 5], [5, 6], [6, 7], [7, 4], [0, 4], [1, 5], [2, 6], [3, 7] ], dtype=np.int64) 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=28, yrange=15, step=1, two=False, **kwargs): start = np.array([0., 0., 0.]) xdir = np.array([1., 0., 0.]) ydir = np.array([0., 1., 0.]) stepx = step stepy = step points3d, lines = [], [] if two: start_x = -xrange start_y = -yrange else: start_x = 0 start_y = 0 for i in range(start_x, xrange): for j in range(start_y, yrange): base = start + xdir*i*stepx + ydir*j*stepy points3d.append(POINTS_SQUARE+base) lines.append(LINES_SQUARE+4*((i-start_x)*(yrange-start_y)+(j-start_y))) points3d = np.vstack(points3d) lines = np.vstack(lines) return merge_points_lines(points3d, lines) def load_human(path, pid, nf=0, camnames=[], annot='annots'): points = [] nvs = [] annot_ = annot for nv, sub in enumerate(camnames): annotname = join(path, annot_, sub, '{:06d}.json'.format(nf)) if not os.path.exists(annotname): print('[Warn] Not exist ', annotname) continue annots = read_json(annotname) if isinstance(annots, dict): annots = annots['annots'] annot = [d for d in annots if d['personID'] == pid] if len(annot) == 0: continue pts = np.array(annot[0]['keypoints']) if args.hand: handl = np.array(annot[0]['handl2d']) handr = np.array(annot[0]['handr2d']) pts = np.vstack([pts, handl, handr]) points.append(pts) nvs.append(nv) points = np.stack(points) results = np.zeros((len(camnames), *points.shape[1:])) results[nvs] = points from easymocap.dataset.config import CONFIG lines = CONFIG['body25']['kintree'] return results, lines class BaseCheck: def __init__(self, path, out, mode='cube', ext='.jpg', sub=[]) -> None: cameras = read_camera(join(out, 'intri.yml'), join(out, 'extri.yml')) cameras.pop('basenames') self.outdir = join(out, mode) self.cameras = cameras if len(sub) == 0: self.camnames = sorted(list(cameras.keys())) else: self.camnames = sub if args.prefix is not None: for c in self.camnames: self.cameras[c.replace(args.prefix, '')] = self.cameras.pop(c) self.camnames = [c.replace(args.prefix, '') for c in self.camnames] print('[check] cameras: ', self.camnames) zaxis = np.array([0., 0., 1.]).reshape(3, 1) for cam in self.camnames: camera = cameras[cam] center = -camera['R'].T @ camera['T'] # lookat = camera['R'].T @ (zaxis - camera['T']) print(' - {}: center = {}, look at = {}'.format(cam, np.round(center.T, 3), np.round(lookat.T, 3))) self.path = path self.kpts2d = None self.ext = ext self.errors = [] def check(self, points3d, lines, nf = 0, show=False, write=True): if write: os.makedirs(self.outdir, exist_ok=True) conf3d = points3d[:, -1] p3d = np.ascontiguousarray(points3d[:, :3]) errors = [] for nv, cam in enumerate(self.camnames): camera = self.cameras[cam] if show or write: imgname = join(self.path, 'images', cam, '{:06d}{}'.format(nf, self.ext)) if not os.path.exists(imgname): imgname = join(self.path, 'images', cam, '{:08d}{}'.format(nf, self.ext)) if not os.path.exists(imgname): print('[WARN] Not exist', imgname) continue assert os.path.exists(imgname), imgname img = cv2.imread(imgname) img = Undistort.image(img, camera['K'], camera['dist']) if False: points2d_repro, xxx = cv2.projectPoints(p3d, cv2.Rodrigues(camera['R'])[0], camera['T'], camera['K'], camera['dist']) kpts_repro = points2d_repro.squeeze() else: kpts_repro = projectN3(p3d, [camera['P']])[0] if self.kpts2d is not None: k2d = self.kpts2d[nv] k2d = Undistort.points(k2d, camera['K'], camera['dist']) valid = (conf3d > 0.)&(k2d[:, 2] > 0.) # print(kpts_repro) # import ipdb; ipdb.set_trace() if k2d[:, 2].sum() > 0.: diff = np.linalg.norm(k2d[:, :2] - kpts_repro[:, :2], axis=1) * valid print('[Check] {}: {} points, {:3.2f} pixels， max is {}, {:3.2f} pixels'.format(cam, valid.sum(), diff.sum()/valid.sum(), diff.argmax(), diff.max())) diff = diff.sum()/valid.sum() errors.append(diff) self.errors.append((diff, nv, nf)) if show or write: plot_points2d(img, k2d, lines, col=(0, 255, 0), lw=1, putText=False) else: k2d = np.zeros((10, 3)) if show or write: if points3d.shape[-1] == 4: conf = points3d[..., -1:] > 0.01 elif points3d.shape[-1] == 3: conf = np.ones_like(points3d[..., -1:]) kpts_vis = np.hstack((kpts_repro[:, :2], conf)) # for i in range(kpts_vis.shape[0]): # print('{}: {}, {}, {}'.format(i, *kpts_vis[i])) plot_points2d(img, kpts_vis, lines, col=(0, 0, 255), lw=1, putText=args.text, style='+') for i in range(kpts_vis.shape[0]): if k2d[i][-1] < 0.1:continue cv2.line(img, (int(kpts_vis[i][0]), int(kpts_vis[i][1])), (int(k2d[i][0]), int(k2d[i][1])), (0,0,0), thickness=2) not_skip_unvis = True if show and (k2d[:, 2].sum()>0 or not_skip_unvis): vis = img if vis.shape[0] > 1000: vis = cv2.resize(vis, None, fx=1000/vis.shape[0], fy=1000/vis.shape[0]) cv2.imshow('vis', vis) cv2.waitKey(0) if write: outname = join(self.outdir, '{}_{:06d}.jpg'.format(cam, nf)) cv2.imwrite(outname, img) if len(errors) > 0: print('[Check] Mean error: {:3.2f} pixels'.format(sum(errors)/len(errors))) def summary(self): errors = self.errors if len(errors) > 0: errors.sort(key=lambda x:-x[0]) print('[Check] Total {} frames Mean error: {:3.2f} pixels, max: {:3.2f} in cam "{}" frame {}'.format(len(errors), sum([e[0] for e in errors])/len(errors), errors[0][0], self.camnames[errors[0][1]], self.errors[0][2])) class QuanCheck(BaseCheck): def __init__(self, path, out, mode, ext, sub=[]) -> None: super().__init__(path, out, mode, ext, sub) def triangulate(self, k2ds, gt=None): # k2ds: (nViews, nPoints, 3) self.kpts2d = k2ds k2dus = [] for nv in range(k2ds.shape[0]): camera = self.cameras[self.camnames[nv]] k2d = k2ds[nv].copy() k2du = Undistort.points(k2d, camera['K'], camera['dist']) k2dus.append(k2du) Pall = np.stack([self.cameras[cam]['P'] for cam in self.camnames]) k2dus = np.stack(k2dus) k3d = batch_triangulate(k2dus, Pall) if gt is not None: if gt.shape[0] < k3d.shape[0]: # gt少了点 gt = np.vstack([gt, np.zeros((k3d.shape[0]-gt.shape[0], 3))]) valid = np.where(k3d[:, -1] > 0.)[0] err3d = np.linalg.norm(k3d[valid, :3] - gt[valid], axis=1) print('[Check3D] mean error: {:.2f}mm'.format(err3d.mean()*1000)) return k3d def load2d_ground(path, nf=0, camnames=[]): k2ds = [] k3d = None MAX_POINTS = 0 for cam in sorted(camnames): annname = join(path, cam, '{:06d}.json'.format(nf)) if not os.path.exists(annname): mywarn(annname + ' not exists') data = read_json(annname) k2d = np.array(data['keypoints2d'], dtype=np.float32) k3d = np.array(data['keypoints3d'], dtype=np.float32) if k2d.shape[0] > MAX_POINTS: MAX_POINTS = k2d.shape[0] k2ds.append(k2d) for i, k2d in enumerate(k2ds): if k2d.shape[0] < MAX_POINTS: k2ds[i] = np.vstack([k2d, np.zeros((MAX_POINTS-k2d.shape[0], 3))]) k2ds = np.stack(k2ds) conf = k2ds[:, :, 2].sum(axis=1) if (conf>0).sum() < 2: return False, None, None return True, k2ds, k3d def read_match2d_file(file, camnames): points = read_json(file)['points_global'] match2d = np.zeros((len(camnames), len(points), 3)) for npo in range(match2d.shape[1]): for key, (x, y) in points[npo].items(): if key not in camnames: continue match2d[camnames.index(key), npo] = [x, y, 1.] return True, match2d, np.zeros((match2d.shape[1], 3)) def check_calib(path, out, vis=False, show=False, debug=False): if vis: out_dir = join(out, 'check') os.makedirs(out_dir, exist_ok=True) cameras = read_camera(join(out, 'intri.yml'), join(out, 'extri.yml')) cameras.pop('basenames') total_sum, cnt = 0, 0 for nf in tqdm(range(10000)): imgs = [] k2ds = [] for cam, camera in cameras.items(): if vis: for ext in ['jpg', 'png']: imgname = join(path, 'images', cam, '{:06d}.{}'.format(nf, ext)) if not os.path.exists(imgname): continue assert os.path.exists(imgname), imgname img = cv2.imread(imgname) img = Undistort.image(img, camera['K'], camera['dist']) imgs.append(img) annname = join(path, 'chessboard', cam, '{:06d}.json'.format(nf)) if not os.path.exists(annname): break data = read_json(annname) k2d = np.array(data['keypoints2d'], dtype=np.float32) k2d = Undistort.points(k2d, camera['K'], camera['dist']) k2ds.append(k2d) if len(k2ds) == 0: break Pall = np.stack([camera['P'] for camera in cameras.values()]) k2ds = np.stack(k2ds) k3d = batch_triangulate(k2ds, Pall) kpts_repro = projectN3(k3d, Pall) for nv in range(len(k2ds)): conf = k2ds[nv][:, -1] dist = conf * np.linalg.norm(kpts_repro[nv][:, :2] - k2ds[nv][:, :2], axis=1) total_sum += dist.sum() cnt += conf.sum() if debug: print('{:2d}-{:2d}: {:6.2f}/{:2d}'.format(nf, nv, dist.sum(), int(conf.sum()))) if vis: kpts_repro_vis = np.hstack((kpts_repro[nv][:, :2], conf[:, None])) plot_points2d(imgs[nv], kpts_repro_vis, [], col=(0, 0, 255), lw=1, putText=False) plot_points2d(imgs[nv], k2ds[nv], [], lw=1, putText=False) for i in range(kpts_repro_vis.shape[0]): cv2.line(imgs[nv], kpts_repro_vis[i], k2ds[nv][i], (0,0,0), thickness=1) if show: cv2.imshow('vis', imgs[nv]) cv2.waitKey(0) if vis: imgout = merge(imgs, resize=False) outname = join(out, 'check', '{:06d}.jpg'.format(nf)) cv2.imwrite(outname, imgout) print('{:.2f}/{} = {:.2f} pixel'.format(total_sum, int(cnt), total_sum/cnt)) 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) for i in range(kpts_repro_vis.shape[0]): cv2.line(imgs[nv], kpts_repro[i], points2d[cams.index(cam)][i], (0,0,0), thickness=1) outname = join(out, cam+'.jpg') cv2.imwrite(outname, img) if __name__ == "__main__": import argparse parser = argparse.ArgumentParser() parser.add_argument('path', type=str, help='the directory contains the extrinsic images') parser.add_argument('--sub', type=str, default=[], nargs='+') parser.add_argument('--out', type=str, help='with camera parameters') parser.add_argument('--mode', type=str, default='cube', help='with camera parameters') parser.add_argument('--ext', type=str, default='.jpg', choices=['.jpg', '.png']) parser.add_argument('--prefix', type=str, default=None) parser.add_argument('--grid_x', type=int, default=3) parser.add_argument('--grid_y', type=int, default=3) parser.add_argument('--grid_step', type=float, default=1.) parser.add_argument('--grid_two', action='store_true') parser.add_argument('--step', type=int, default=5) parser.add_argument('--show', action='store_true') parser.add_argument('--write', action='store_true') parser.add_argument('--debug', action='store_true') parser.add_argument('--human', action='store_true') parser.add_argument('--hand', action='store_true') parser.add_argument('--pid', type=int, default=0) parser.add_argument('--frame', type=int, default=0) parser.add_argument('--annot', type=str, default='annots') parser.add_argument('--calib', action='store_true') parser.add_argument('--text', action='store_true') parser.add_argument('--print3d', action='store_true') parser.add_argument('--gt', action='store_true') args = parser.parse_args() if args.mode in ['cube', 'grid']: points, lines = {'cube': load_cube, 'grid': load_grid}[args.mode]( xrange=args.grid_x, yrange=args.grid_y, step=args.grid_step, two=args.grid_two, grid_size=args.grid_step ) print('Check {} points'.format(points.shape)) checker = BaseCheck(args.path, args.out, args.mode, args.ext) checker.check(points, lines, args.frame, show=args.show, write=args.write) elif args.mode in ['gcp', 'match']: checker = QuanCheck(args.path, args.out, args.mode, args.ext) lines = [] if args.mode == 'match': for nf in range(0, 10000, args.step): # try: flag, k2ds, gt3d = load2d_ground(join(args.path, args.annot), nf=nf, camnames=checker.camnames) # except: # myerror('{} not exist'.format(join(args.path, args.annot, '{:06d}.json'.format(nf)))) # break if not flag:continue points = checker.triangulate(k2ds, gt=gt3d) if args.print3d: valid = points[:, -1] > 0.01 points_ = points[valid] np.savetxt(join(args.out, 'points3d.txt'), points_, fmt='%10.5f') print(myarray2string(points_, indent=0)) norm = np.linalg.norm(points_, axis=1) print('[calib] max norm={}, min norm={}'.format(norm.max(), norm.min())) checker.check(gt3d if args.gt else points, lines, nf, show=args.show, write=args.write) checker.summary() elif args.mode == 'gcp': flag, k2ds, gt3d = read_match2d_file(join(args.path, 'calib.json'), camnames=checker.camnames) points = checker.triangulate(k2ds, gt=gt3d) print(myarray2string(points, indent=4)) checker.check(gt3d if args.gt else points, lines, 0, show=args.show, write=args.write) else: flag, k2ds, gt3d = load2d_ground(join(args.path, 'chessboard'), camnames=checker.camnames) points = checker.triangulate(k2ds, gt=gt3d) checker.check(gt3d if args.gt else points, lines, 0, show=args.show, write=args.write) elif args.mode == 'human': checker = QuanCheck(args.path, args.out, args.mode, args.ext, sub=args.sub) points, lines = load_human(args.path, pid=args.pid, nf=args.frame, camnames=checker.camnames, annot=args.annot) points = checker.triangulate(points, gt=None) print('[calib] check human') print(myarray2string(points, indent=0)) print('[calib]limblength: {}'.format(np.linalg.norm(points[1, :3] - points[8, :3]))) checker.check(points, lines, args.frame, show=args.show, write=args.write) elif args.calib: check_match(args.path, args.out) else: check_calib(args.path, args.out, args.vis, args.show, args.debug)