From 368ab6092e15f8bff1749faf101f7921b8d744d0 Mon Sep 17 00:00:00 2001 From: shuaiqing Date: Fri, 24 Mar 2023 21:27:39 +0800 Subject: [PATCH] add check calib --- apps/calibration/check_calib.py | 361 ++++++++++++++++++++++++++------ 1 file changed, 299 insertions(+), 62 deletions(-) diff --git a/apps/calibration/check_calib.py b/apps/calibration/check_calib.py index 00b606b..a0a998d 100644 --- a/apps/calibration/check_calib.py +++ b/apps/calibration/check_calib.py @@ -2,9 +2,11 @@ @ Date: 2021-03-27 19:13:50 @ Author: Qing Shuai @ LastEditors: Qing Shuai - @ LastEditTime: 2021-04-15 22:53:23 - @ FilePath: /EasyMocap/apps/calibration/check_calib.py + @ 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 @@ -28,16 +30,29 @@ LINES_SQUARE = np.array([ [3, 0] ]) -def load_cube(): +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([ - [0., 0., 0.], - [1., 0., 0.], - [1., 1., 0.], - [0., 1., 0.], - [0., 0., 1.], - [1., 0., 1.], - [1., 1., 1.], - [0., 1., 1.] + [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], @@ -52,7 +67,7 @@ def load_cube(): [1, 5], [2, 6], [3, 7] - ], dtype=int) + ], dtype=np.int64) points3d = np.hstack((points3d, np.ones((points3d.shape[0], 1)))) return points3d, lines @@ -73,36 +88,211 @@ def merge_points_lines(points3d, lines): mapid[mapid==newi] = i return newpoints, mapid[lines] -def load_grid(xrange=10, yrange=10): +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 = 1. - stepy = 1. + stepx = step + stepy = step points3d, lines = [], [] - for i in range(xrange): - for j in range(yrange): + 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*yrange+j)) + 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_axes(): - points3d = np.array([ - [0., 0., 0.], - [1., 0., 0.], - [0., 1., 0.], - [0., 0., -1.] - ]) - lines = np.array([ - [0,1], - [0,2], - [0,3] - ], dtype=int) - points3d = np.hstack((points3d, np.ones((points3d.shape[0], 1)))) - return 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: @@ -116,7 +306,10 @@ def check_calib(path, out, vis=False, show=False, debug=False): k2ds = [] for cam, camera in cameras.items(): if vis: - imgname = join(path, 'images', cam, '{:06d}.jpg'.format(nf)) + 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']) @@ -142,8 +335,11 @@ def check_calib(path, out, vis=False, show=False, debug=False): if debug: print('{:2d}-{:2d}: {:6.2f}/{:2d}'.format(nf, nv, dist.sum(), int(conf.sum()))) if vis: - plot_points2d(imgs[nv], kpts_repro[nv], [], col=(0, 0, 255), lw=1, putText=False) + 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) @@ -153,20 +349,6 @@ 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, points3d, lines): - cameras = read_camera(join(out, 'intri.yml'), join(out, 'extri.yml')) - cameras.pop('basenames') - 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) - 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')) @@ -195,6 +377,8 @@ def check_match(path, out): 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) @@ -203,26 +387,79 @@ if __name__ == "__main__": 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('--vis', action='store_true') + 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('--cube', action='store_true') - parser.add_argument('--grid', action='store_true') - parser.add_argument('--axes', 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.cube: - 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.axes: - points, lines = load_axes() - check_scene(args.path, args.out, points, lines) + 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: