170 lines
6.9 KiB
Python
170 lines
6.9 KiB
Python
'''
|
|
@ Date: 2021-03-02 16:13:03
|
|
@ Author: Qing Shuai
|
|
@ LastEditors: Qing Shuai
|
|
@ LastEditTime: 2022-08-03 17:35:16
|
|
@ FilePath: /EasyMocapPublic/apps/calibration/calib_extri.py
|
|
'''
|
|
from easymocap.mytools.camera_utils import write_intri
|
|
import os
|
|
from glob import glob
|
|
from os.path import join
|
|
import numpy as np
|
|
import cv2
|
|
from easymocap.mytools import read_intri, write_extri, read_json
|
|
from easymocap.mytools.debug_utils import mywarn
|
|
|
|
def init_intri(path, image):
|
|
camnames = sorted(os.listdir(join(path, image)))
|
|
cameras = {}
|
|
for ic, cam in enumerate(camnames):
|
|
imagenames = sorted(glob(join(path, image, cam, '*.jpg')))
|
|
assert len(imagenames) > 0
|
|
imgname = imagenames[0]
|
|
img = cv2.imread(imgname)
|
|
height, width = img.shape[0], img.shape[1]
|
|
focal = 1.2*max(height, width) # as colmap
|
|
K = np.array([focal, 0., width/2, 0., focal, height/2, 0. ,0., 1.]).reshape(3, 3)
|
|
dist = np.zeros((1, 5))
|
|
cameras[cam] = {
|
|
'K': K,
|
|
'dist': dist
|
|
}
|
|
return cameras
|
|
|
|
def solvePnP(k3d, k2d, K, dist, flag, tryextri=False):
|
|
k2d = np.ascontiguousarray(k2d[:, :2])
|
|
# try different initial values:
|
|
if tryextri:
|
|
def closure(rvec, tvec):
|
|
ret, rvec, tvec = cv2.solvePnP(k3d, k2d, K, dist, rvec, tvec, True, flags=flag)
|
|
points2d_repro, xxx = cv2.projectPoints(k3d, rvec, tvec, K, dist)
|
|
kpts_repro = points2d_repro.squeeze()
|
|
err = np.linalg.norm(points2d_repro.squeeze() - k2d, axis=1).mean()
|
|
return err, rvec, tvec, kpts_repro
|
|
# create a series of extrinsic parameters looking at the origin
|
|
height_guess = 2.1
|
|
radius_guess = 7.
|
|
infos = []
|
|
for theta in np.linspace(0, 2*np.pi, 180):
|
|
st = np.sin(theta)
|
|
ct = np.cos(theta)
|
|
center = np.array([radius_guess*ct, radius_guess*st, height_guess]).reshape(3, 1)
|
|
R = np.array([
|
|
[-st, ct, 0],
|
|
[0, 0, -1],
|
|
[-ct, -st, 0]
|
|
])
|
|
tvec = - R @ center
|
|
rvec = cv2.Rodrigues(R)[0]
|
|
err, rvec, tvec, kpts_repro = closure(rvec, tvec)
|
|
infos.append({
|
|
'err': err,
|
|
'repro': kpts_repro,
|
|
'rvec': rvec,
|
|
'tvec': tvec
|
|
})
|
|
infos.sort(key=lambda x:x['err'])
|
|
err, rvec, tvec, kpts_repro = infos[0]['err'], infos[0]['rvec'], infos[0]['tvec'], infos[0]['repro']
|
|
else:
|
|
ret, rvec, tvec = cv2.solvePnP(k3d, k2d, K, dist, flags=flag)
|
|
points2d_repro, xxx = cv2.projectPoints(k3d, rvec, tvec, K, dist)
|
|
kpts_repro = points2d_repro.squeeze()
|
|
err = np.linalg.norm(points2d_repro.squeeze() - k2d, axis=1).mean()
|
|
# print(err)
|
|
return err, rvec, tvec, kpts_repro
|
|
|
|
def calib_extri(path, image, intriname, image_id):
|
|
camnames = sorted(os.listdir(join(path, image)))
|
|
camnames = [c for c in camnames if os.path.isdir(join(path, image, c))]
|
|
if intriname is None:
|
|
# initialize intrinsic parameters
|
|
intri = init_intri(path, image)
|
|
else:
|
|
assert os.path.exists(intriname), intriname
|
|
intri = read_intri(intriname)
|
|
if len(intri.keys()) == 1:
|
|
key0 = list(intri.keys())[0]
|
|
for cam in camnames:
|
|
intri[cam] = intri[key0].copy()
|
|
extri = {}
|
|
# methods = [cv2.SOLVEPNP_ITERATIVE, cv2.SOLVEPNP_P3P, cv2.SOLVEPNP_AP3P, cv2.SOLVEPNP_EPNP, cv2.SOLVEPNP_DLS, cv2.SOLVEPNP_IPPE, cv2.SOLVEPNP_SQPNP]
|
|
methods = [cv2.SOLVEPNP_ITERATIVE]
|
|
for ic, cam in enumerate(camnames):
|
|
imagenames = sorted(glob(join(path, image, cam, '*{}'.format(args.ext))))
|
|
chessnames = sorted(glob(join(path, 'chessboard', cam, '*.json')))
|
|
# chessname = chessnames[0]
|
|
assert len(chessnames) > 0, cam
|
|
chessname = chessnames[image_id]
|
|
|
|
data = read_json(chessname)
|
|
k3d = np.array(data['keypoints3d'], dtype=np.float32)
|
|
k2d = np.array(data['keypoints2d'], dtype=np.float32)
|
|
if k3d.shape[0] != k2d.shape[0]:
|
|
mywarn('k3d {} doesnot match k2d {}'.format(k3d.shape, k2d.shape))
|
|
length = min(k3d.shape[0], k2d.shape[0])
|
|
k3d = k3d[:length]
|
|
k2d = k2d[:length]
|
|
#k3d[:, 0] *= -1
|
|
valididx = k2d[:, 2] > 0
|
|
if valididx.sum() < 4:
|
|
extri[cam] = {}
|
|
rvec = np.zeros((1, 3))
|
|
tvec = np.zeros((3, 1))
|
|
extri[cam]['Rvec'] = rvec
|
|
extri[cam]['R'] = cv2.Rodrigues(rvec)[0]
|
|
extri[cam]['T'] = tvec
|
|
print('[ERROR] Failed to initialize the extrinsic parameters')
|
|
extri.pop(cam)
|
|
continue
|
|
k3d = k3d[valididx]
|
|
k2d = k2d[valididx]
|
|
if args.tryfocal:
|
|
infos = []
|
|
for focal in range(500, 5000, 10):
|
|
dist = intri[cam]['dist']
|
|
K = intri[cam]['K']
|
|
K[0, 0] = focal
|
|
K[1, 1] = focal
|
|
for flag in methods:
|
|
err, rvec, tvec, kpts_repro = solvePnP(k3d, k2d, K, dist, flag)
|
|
infos.append({
|
|
'focal': focal,
|
|
'err': err,
|
|
'repro': kpts_repro,
|
|
'rvec': rvec,
|
|
'tvec': tvec
|
|
})
|
|
infos.sort(key=lambda x:x['err'])
|
|
err, rvec, tvec = infos[0]['err'], infos[0]['rvec'], infos[0]['tvec']
|
|
kpts_repro = infos[0]['repro']
|
|
focal = infos[0]['focal']
|
|
intri[cam]['K'][0, 0] = focal
|
|
intri[cam]['K'][1, 1] = focal
|
|
else:
|
|
K, dist = intri[cam]['K'], intri[cam]['dist']
|
|
err, rvec, tvec, kpts_repro = solvePnP(k3d, k2d, K, dist, flag=cv2.SOLVEPNP_ITERATIVE)
|
|
extri[cam] = {}
|
|
extri[cam]['Rvec'] = rvec
|
|
extri[cam]['R'] = cv2.Rodrigues(rvec)[0]
|
|
extri[cam]['T'] = tvec
|
|
center = - extri[cam]['R'].T @ tvec
|
|
print('{} center => {}, err = {:.3f}'.format(cam, center.squeeze(), err))
|
|
write_intri(join(path, 'intri.yml'), intri)
|
|
write_extri(join(path, 'extri.yml'), extri)
|
|
|
|
if __name__ == "__main__":
|
|
import argparse
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument('path', type=str)
|
|
parser.add_argument('--image', type=str, default='images')
|
|
parser.add_argument('--intri', type=str, default=None)
|
|
parser.add_argument('--ext', type=str, default='.jpg')
|
|
parser.add_argument('--step', type=int, default=1)
|
|
parser.add_argument('--debug', action='store_true')
|
|
parser.add_argument('--tryfocal', action='store_true')
|
|
parser.add_argument('--tryextri', action='store_true')
|
|
parser.add_argument('--image_id', type=int, default=0, help='Image id used for extrinsic calibration')
|
|
|
|
args = parser.parse_args()
|
|
calib_extri(args.path, args.image, intriname=args.intri, image_id=args.image_id) |