210 lines
7.3 KiB
Python
210 lines
7.3 KiB
Python
'''
|
|
@ 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
|
|
'''
|
|
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():
|
|
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.]
|
|
])
|
|
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.int)
|
|
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')
|
|
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:
|
|
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'])
|
|
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:
|
|
plot_points2d(imgs[nv], kpts_repro[nv], [], col=(0, 0, 255), lw=1, putText=False)
|
|
plot_points2d(imgs[nv], k2ds[nv], [], lw=1, putText=False)
|
|
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_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'))
|
|
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()
|
|
parser.add_argument('path', type=str,
|
|
help='the directory contains the extrinsic images')
|
|
parser.add_argument('--out', type=str,
|
|
help='with camera parameters')
|
|
parser.add_argument('--vis', action='store_true')
|
|
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:
|
|
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) |