def doCalibration(self):
print('doCalibration')
chessSize = (10, 6)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((chessSize[0] * chessSize[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessSize[0], 0:chessSize[1]].T.reshape(-1, 2)
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob('/Users/chengong.cg/Git/AR/calibration/png/*.png')
np.random.shuffle(images)
images = images[0:20]
images.sort()
for fname in images:
print(fname)
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, chessSize, None)
if ret:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
imgpoints.append(corners2)
# img = cv2.drawChessboardCorners(img, chessSize, corners2, ret)
# cv2.imshow('img', img)
cv2.waitKey(1)
ret, mtx, dist, rvecs, tvecs, devIn, devEx, err = cv2.calibrateCameraExtended(
objpoints, imgpoints, gray.shape[::-1], None, None,
flags=cv2.CALIB_FIX_ASPECT_RATIO | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5)
print("default mtx", mtx)
print("default dist", dist)
h, w = img.shape[:2]
print("image size", w, h)
nmtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 0.6, (w, h), centerPrincipalPoint=True)
print("new mtx", nmtx)
print("roi", roi)
# cv2.destroyAllWindows()
self.mapx, self.mapy = cv2.initUndistortRectifyMap(mtx, dist, None, mtx, (w, h), 5)
self.calDone = True
print('doCalibration done')
def calibCamera(objPts, imgPts):
# Image size in pixels
imSize = (512, 512)
# Run through calibration
# Outputs are: mean reproj error, intrinsic matrix, distortion coeff.,
# rotation vectors (radians), translation vectors (mm)
flags = cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_THIN_PRISM_MODEL + \
cv2.CALIB_TILTED_MODEL
out = cv2.calibrateCameraExtended(objPts,
imgPts,
imSize,
None,
None,
flags=flags)
meanErr = out[0]
mtx = out[1]
dist = out[2]
rvecs = out[3]
tvecs = out[4]
intStDev = out[5]
extStDev = out[6]
viewErr = out[7]
# Collect calibration information into a dictionary
camCal = {
'Intrinsics': mtx,
'Distortion': dist,
'Rotation': rvecs,
'Translation': tvecs
}
# Standard deviation values
errCal = {
'Intrinsics StDev': intStDev.flatten(),
'Extrinsics StDev': extStDev.flatten(),
'View Errors': viewErr.flatten()
}
return camCal, errCal
def intrinsic(self, imgpoints_list, objpoints_list, imgsize):
real_coors = []
img_points = []
n = len(imgpoints_list)
l = np.size(imgpoints_list[0], 0)
for i in range(len(imgpoints_list)):
a = np.ndarray([l, 1, 3], dtype=np.float32)
b = np.ndarray([l, 1, 2], dtype=np.float32)
for j in range(l):
a[j, 0, 0] = objpoints_list[i][j, 0]
a[j, 0, 1] = objpoints_list[i][j, 1]
a[j, 0, 2] = 0
b[j, 0, 0] = imgpoints_list[i][j, 0]
b[j, 0, 1] = imgpoints_list[i][j, 1]
real_coors.append(a)
img_points.append(b)
ret, mtx, dist, rvecs, tvec, stdDeviationsIntrinsics, stdDeviationsExtrinsics, rme = cv2.calibrateCameraExtended(
real_coors, img_points, imgsize, None, None)
return rme, mtx, dist
def main():
# parse arguments
parser = argparse.ArgumentParser(
description='Calibrate camera an image taken of a checker board')
parser.add_argument('--path',
'-f',
action='append',
help='path to the calibration images / video(s)')
parser.add_argument(
'--skip',
'-i',
type=int,
default=1,
help='frame interval (1=no skipping, 2=use every other frame, etc)')
parser.add_argument('--offset',
'-o',
type=int,
default=0,
help='skip these many images from the start')
parser.add_argument('--nx',
'-x',
type=int,
help='checker board corner count on x-axis')
parser.add_argument('--ny',
'-y',
type=int,
help='checker board corner count on y-axis')
parser.add_argument('--cell-size',
'-s',
type=float,
help='cell width and height in mm')
parser.add_argument('--dist-coef-np',
'-np',
type=int,
default=5,
help='how many distortion coeffs to estimate [0-5]')
parser.add_argument('--fix-pp',
action='store_true',
help='fix principal point')
parser.add_argument('--fix-aspect',
action='store_true',
help='fix aspect ratio')
parser.add_argument('--pause',
action='store_true',
help='pause after each image during corner detection')
parser.add_argument(
'--max-err',
type=float,
help='run calibration a second time after dropping out frames'
' with max repr err surpassing this value')
args = parser.parse_args()
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
detect_corner_flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((args.nx * args.ny, 3), np.float32)
objp[:, :2] = np.mgrid[0:args.nx, 0:args.ny].T.reshape(-1,
2) * args.cell_size
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
names = []
imgs = []
shape = None
def process_img(img, name):
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
shape = gray.shape
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (args.nx, args.ny),
None, detect_corner_flags)
# If found, add object points, image points (after refining them)
if ret:
objpoints.append(objp)
imgpoints.append(corners)
imgs.append(img)
names.append(name)
# Draw and display the corners
img_show = img.copy()
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
cv2.drawChessboardCorners(img_show, (args.nx, args.ny), corners2,
ret)
sc = 1024 / np.max(img_show.shape)
cv2.imshow('detected corners',
cv2.resize(img_show, None, fx=sc, fy=sc))
else:
print('cant find the corners from %s' % name)
sc = 1024 / np.max(img.shape)
cv2.imshow('detected corners', cv2.resize(img, None, fx=sc, fy=sc))
cv2.setWindowTitle('detected corners', name)
cv2.waitKey(0 if args.pause else 500)
return shape
for path in args.path:
if os.path.isdir(path):
files = [
fname for fname in os.listdir(path)
if fname[-4:] in ('.bmp', '.jpg', '.png')
]
files = sorted(files)
for i, fname in enumerate(files):
if i >= args.offset and (i - args.offset) % args.skip == 0:
img = cv2.imread(os.path.join(path, fname))
shape = process_img(img, fname)
else:
cap = cv2.VideoCapture(path)
i, ret = 0, True
while cap.isOpened() and ret:
ret, img = cap.read()
if i >= args.offset and (i -
args.offset) % args.skip == 0 and ret:
shape = process_img(img, 'frame-%d' % i)
i += 1
cap.release()
cv2.destroyAllWindows()
if len(imgs) == 0:
print('too few images found at %s (%d)' % (args.path, len(imgs)))
return
flags = 0
if args.dist_coef_n > 5:
flags |= cv2.CALIB_RATIONAL_MODEL
if args.dist_coef_n < 5 and args.dist_coef_n != 3:
flags |= cv2.CALIB_FIX_K3
if args.dist_coef_n < 4:
flags |= cv2.CALIB_FIX_TANGENT_DIST
if args.dist_coef_n < 2:
flags |= cv2.CALIB_FIX_K2
if args.dist_coef_n < 1:
flags |= cv2.CALIB_FIX_K1
if args.fix_pp:
flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
if args.fix_aspect:
flags |= cv2.CALIB_FIX_ASPECT_RATIO
print('estimating camera parameters using a total of %d images...' %
(len(imgs), ))
for iter in range(2):
rms, K, dist, rvecs, tvecs, stds, *_ = cv2.calibrateCameraExtended(
objpoints, imgpoints, shape[::-1], None, None, flags=flags)
intr = tuple(np.array([K[0, 0], K[1, 1], K[0, 2], K[1, 2]]))
dist = tuple(dist.flatten())
stds = stds.flatten()
intr_lbl = ('f_x', 'f_y', 'c_x', 'c_y')
dist_lbl = ('k_1', 'k_2', 'p_1', 'p_2', 'k_3')[:len(dist)]
img_errs = []
img_projs = []
for i in range(len(objpoints)):
if 1:
img_proj, _ = cv2.projectPoints(objpoints[i], rvecs[i],
tvecs[i], K, dist)
else:
from visnav.algo.odo.vis_gps_bundleadj import project
img_proj = project(
objpoints[i],
np.array([*rvecs[i], *tvecs[i]]).reshape((-1, 6)), K,
dist)[:, None, :]
errors = np.linalg.norm(imgpoints[i] - img_proj, axis=2).flatten()
img_projs.append(img_proj)
img_errs.append(errors)
max_errs = np.max(np.array(img_errs), axis=1)
I = np.argsort(-max_errs)
for i in I:
print('%s max repr err [px]: %.3f' % (names[i], max_errs[i]))
if args.max_err is not None and iter == 0 and np.any(
max_errs >= args.max_err):
print(
'recalibrating after dropping %d frames because their max repr err exceeded %f (%s)...'
% (np.sum(max_errs >= args.max_err), args.max_err,
max_errs[max_errs >= args.max_err]))
J = np.where(max_errs < args.max_err)[0]
objpoints = [objpoints[j] for j in J]
imgpoints = [imgpoints[j] for j in J]
imgs = [imgs[j] for j in J]
names = [names[j] for j in J]
else:
break
def to_str(lbls, vals, unit='', list_sep=', ', lbl_sep='): ', prec=3):
return list_sep.join(lbls) + lbl_sep + list_sep.join([
tools.fixed_precision(val, prec, True) + unit
for lbl, val in zip(lbls, vals)
])
# return list_sep.join([lbl + lbl_sep + tools.fixed_precision(val, prec, True) for lbl, val in zip(lbls, vals)])
print('intrinsics (' + to_str(intr_lbl, intr, prec=5))
print('dist coefs (' + to_str(dist_lbl, dist, prec=5))
print('stds (' + to_str(intr_lbl + dist_lbl, stds[:len(intr) + len(dist)]))
with warnings.catch_warnings():
warnings.filterwarnings(
action='ignore',
message='invalid value encountered in true_divide')
print('relative (' + to_str(intr_lbl + dist_lbl,
100 * stds[:len(intr) + len(dist)] /
np.abs(np.array(intr + dist)),
unit='%',
prec=2))
y = np.array(imgpoints).reshape((-1, 2)) - np.array(intr)[None, 2:4]
yh = np.array(img_projs).reshape((-1, 2)) - np.array(intr)[None, 2:4]
e = y - yh
u = y / np.linalg.norm(y, axis=1)[:, None]
e_radial = np.einsum('ij,ij->i', e, u)
e_cross = np.linalg.norm(e - e_radial[:, None] * u, axis=1)
r = np.linalg.norm(y, axis=1)
J = np.argsort(r)
print('repr err across all imgs q=0.99: %.3f, e_r: %.3f, e_c: %.3f' % (
np.quantile(np.array(img_errs).flatten(), 0.99),
np.quantile(e_radial, 0.99),
np.quantile(e_cross, 0.99),
))
from scipy import stats
plt.figure()
plt.plot(r[J], e_radial[J], '.')
bin_means, bin_edges, binnumber = stats.binned_statistic(r[J],
e_radial[J],
statistic='mean',
bins=20)
plt.hlines(bin_means, bin_edges[:-1], bin_edges[1:], lw=5, color='C1')
plt.title('radial errors vs distance from principal point')
plt.tight_layout()
plt.figure()
plt.plot(r[J], e_cross[J], '.')
bin_means, bin_edges, binnumber = stats.binned_statistic(r[J],
e_cross[J],
statistic='mean',
bins=20)
plt.hlines(bin_means, bin_edges[:-1], bin_edges[1:], lw=5, color='C1')
plt.title('tangential errors vs distance from principal point')
plt.tight_layout()
if 1:
# project
for i in I:
name, pts, img = names[i], img_projs[i], imgs[i]
plt.figure(figsize=(20, 14))
plt.imshow(np.flip(img, axis=2))
for pt in pts.squeeze():
plt.plot(*pt, 'oC1', mfc='none')
plt.title(name + ', max-err %.3f px' % max_errs[i])
plt.tight_layout()
plt.show()
else:
# undistort
h, w = imgs[0].shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
mtx, dist, (w, h), 1, (w, h))
print('new camera mx (P):\n%s' % (newcameramtx, ))
for img in imgs:
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
# x, y, w, h = roi
# dst = dst[y:y + h, x:x + w]
cv2.imshow('undistorted', dst)
cv2.waitKey()
def calibrate(self, framesPath='results/acquired'):
self.workingImages, objectPoints, imgPoints, imageSize = self.detectInImages(
framesPath)
# Implement calibrateCameraExtended here
ret, self.cameraMatrix, self.distCoeffs, self.rvecs, self.tvecs, stdDeviationsIntrinsics, stdDeviationsExtrinsics, self.perViewErrors = cv.calibrateCameraExtended(
objectPoints, imgPoints, imageSize, None, None)
self.rms = np.sqrt(np.mean((self.perViewErrors)**2))
print('\nRMS:', self.rms)
print('camera matrix:\n', self.cameraMatrix)
print('distortion coefficients: ', self.distCoeffs.ravel())
return self.rms, self.cameraMatrix, self.distCoeffs, imageSize
def intrinsics(self, prefix):
# from threading import Thread
# import matplotlib.pyplot as plt
name = prefix.split('\\')
name = name[-1]
self.set_blob_params(name)
out = self.IntrinsicsOut(self, prefix)
imgAr = []
img = []
ind = 0
for imPath in out.imgPaths:
img = self.read_gray_image(imPath)
imgAr.append(img)
ret, centers = cv2.findCirclesGrid(
img,
(self.nrows, self.ncols), # patternSize
None, # centers
flags=self.findCirclesGridFlags, # flags
blobDetector=self.findCirclesGridBlobDetector)
if ret:
out.imgPoints[ind, :, :] = centers
out.centers[:, :,
ind] = np.reshape(centers,
(self.nrows * self.ncols, 2))
out.imgValid[ind] = True
if self.verbose > 0:
img2 = cv2.drawChessboardCorners(img,
(self.nrows, self.ncols),
centers, ret)
cv2.circle(img2, tuple(centers[0][0]), 25, (128), -1)
if img2.shape[1] > 2000:
img2 = cv2.resize(img2,
None,
fx=0.4,
fy=0.4,
interpolation=cv2.INTER_AREA)
cv2.imshow('imgOk', img2)
if self.verbose == 1:
cv2.waitKey(self.displayTime)
if self.verbose > 1:
cv2.waitKey(0)
if self.verbose > 2:
print(str(out.centers[0:6, :, ind]))
cv2.imwrite(
self.imgPath + 'preview_' + name +
"_{0:02d}.png".format(ind), img2)
#cv2.destroyAllWindows()
else:
# print('Dot pattern not detected')
if self.verbose > 0:
img2 = img
if img.shape[1] > 2000:
img2 = cv2.resize(img2,
None,
fx=0.4,
fy=0.4,
interpolation=cv2.INTER_AREA)
if self.verbose > 1:
cv2.imwrite(
self.imgPath + 'discarded_' + name +
"_{0:02d}.png".format(ind), img2)
cv2.imshow('imgKo', img2)
print('Pattern not detected in img {:02d}'.format(ind))
if self.verbose == 1:
cv2.waitKey(self.displayTime)
elif self.verbose > 1:
print('No dots found in ' + str(ind))
cv2.waitKey(0)
ind += 1
out.CP.resolution = img.shape
out.objPoints = self.objectPoints
# objectPointsAr = \
# np.repeat(np.reshape(self.objectPoints, (1, self.nrows * self.ncols, 3)), 3, 0)
# [_, out.CP.K, _, _, _] = cv2.calibrateCamera(objectPointsAr,
# out.imgPoints[0:3, :, :],
# img.shape, out.CP.K, None, None, None,
# self.calibrateCameraFlags)
objectPointsAr = \
np.repeat(np.reshape(self.objectPoints, (1, self.nrows * self.ncols, 3)), len(np.where(out.imgValid)[0]), 0)
# [out.rms, out.CP.K, _, _, _] = cv2.calibrateCamera(objectPointsAr,
# out.imgPoints[np.where(out.imgValid)[0], :, :],
# img.shape, out.CP.K, None, None, None,
# cv2.CALIB_FIX_ASPECT_RATIO)
[out.rms, out.CP.K, D, out.CP.Rot, out.CP.Tras, _, _,
perViewErr] = cv2.calibrateCameraExtended(
objectPointsAr,
out.imgPoints[np.where(out.imgValid)[0], :, :],
img.shape,
out.CP.K,
None,
None,
None, # rvecs, tvecs
None,
None,
None, # stdDevInt, stDevExt, perViewErr
cv2.CALIB_FIX_ASPECT_RATIO)
# cv2.CALIB_USE_INTRINSIC_GUESS)
rotMat = np.zeros((3, 3, len(out.CP.Rot)))
for i in range(len(out.CP.Rot)):
rotMat[:, :, i] = cv2.Rodrigues(out.CP.Rot[i][:].T[0])[0]
out.CP.Rot = rotMat
imgSelected = np.where(out.imgValid)[0]
# if name=='ToF':
imgSelected = imgSelected[np.where(
perViewErr < np.mean(perViewErr) + 2 * np.std(perViewErr))[0]]
objectPointsAr = \
np.repeat(np.reshape(self.objectPoints, (1, self.nrows * self.ncols, 3)), len(imgSelected), 0)
[out.rms, out.CP.K, D, _, _, _, _, _] = cv2.calibrateCameraExtended(
objectPointsAr,
out.imgPoints[imgSelected, :, :],
img.shape,
out.CP.K,
D,
None,
None, # rvecs, tvecs
None,
None,
None,
# stdDevInt, stDevExt, perViewErr
cv2.CALIB_USE_INTRINSIC_GUESS)
imgShape = tuple(np.flip(img.shape, 0))
validInd = np.where(out.imgValid)[0]
for i in validInd:
tmp = np.reshape(out.imgPoints[i, :, :, :],
(1, out.imgPoints.shape[1], 2))
objPointsUndistorted = cv2.undistortPoints(tmp, out.CP.K, D)
imgUndistorted = cv2.undistort(imgAr[i], out.CP.K, D)
out.imgPointsUnd[i, :, :, :] = np.reshape(
objPointsUndistorted[:, :, :], (out.imgPoints.shape[1], 1, 2))
out.imgUnd[i] = imgUndistorted
[out.CP.newK,
out.CP.roi] = cv2.getOptimalNewCameraMatrix(out.CP.K, D, imgShape, 1,
imgShape)
out.CP.invMapX, out.CP.invMapY = cv2.initUndistortRectifyMap(
out.CP.K, D, None, out.CP.newK, imgShape, 5)
out.validInd = imgSelected
u = out.CP.resolution[1]
v = out.CP.resolution[0]
tmp = np.meshgrid([float(i) for i in range(u)],
[float(i) for i in range(v)])
u = tmp[0] + 0.5
v = tmp[1] + 0.5
uCol = np.reshape(u.T, [u.size, 1])
vCol = np.reshape(v.T, [v.size, 1])
uv = np.hstack((uCol, vCol))
uv = np.array([uv])
# pts = cv2.undistortPoints(np.array([[[1.0, 1.0], [944, 504]]]), out.CP.K, D)
pts = cv2.undistortPoints(uv, out.CP.K, D)
u = np.reshape(pts[0][:, 0],
(out.CP.resolution[1], out.CP.resolution[0])).T
v = np.reshape(pts[0][:, 1],
(out.CP.resolution[1], out.CP.resolution[0])).T
out.CP.mapX = u * out.CP.K[0, 0] + out.CP.K[0, 2]
out.CP.mapY = v * out.CP.K[1, 1] + out.CP.K[1, 2]
D = np.array(D[0])
out.CP.R = D[[0, 1, 4]]
out.CP.D = D[[2, 3]]
if self.verbose >= 0:
print(str(out))
return out
imageSize = img.shape # [::-1]
# %% calibracion rational Extended, con incertezas
objectPoints = [chessboardModel] * nIm
# Parametros de entrada/salida de la calibracion
cameraMatrix = np.eye(4)
distCoeffs = np.zeros((14, 1))
flags = cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_ZERO_TANGENT_DIST
retAll = cv2.calibrateCameraExtended(objectPoints,
imagePoints,
imageSize,
cameraMatrix,
distCoeffs,
flags=flags)
#[, rvecs[, tvecs[, stdDeviationsIntrinsics[, stdDeviationsExtrinsics[, perViewErrors[, flags[, criteria]]]]]]])
(retval, cameraMatrix, distCoeffs, rvecs, tvecs, stdDeviationsIntrinsics,
stdDeviationsExtrinsics, perViewErrors) = retAll
# stdDeviationsIntrinsics
# (f_x, f_y, c_x, c_y, k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 , s_1, s_2, s_3, s_4, \tau_x, \tau_y)
rvecs = np.array(rvecs)
tvecs = np.array(tvecs)
intrinsics = np.zeros((18, 1))
intrinsics[:4, 0] = cameraMatrix[[0, 1, 0, 1], [0, 1, 2, 2]]
def calibrateIntrinsics(self,
single_path,
single_detected_path,
cibles,
cibles_l,
cibles_r,
fisheye=False):
"""
||Public method||
Calibration individuelle de 2 caméras AVEC DES CIBLES
Args:
single_path (str): "path_to_single_images/"
single_detected_path (str): "path_to_single_images_detected/"
fisheye (Bool): True pour caméra fisheye
"""
self.single_path = single_path
self.single_detected_path = single_detected_path
clean_folders([single_detected_path])
self.objpoints_l, self.imgpoints_l, self.imageSize1 = self.__read_single(
'left')
self.objpoints_r, self.imgpoints_r, self.imageSize2 = self.__read_single(
'right')
if fisheye == True:
flags = self.fisheye_flags
else:
flags = self.not_fisheye_flags
# GAUCHE
self.err1, self.M1, self.d1, self.r1, self.t1, stdDeviationsIntrinsics1, stdDeviationsExtrinsics1, self.perViewErrors1 = cv.calibrateCameraExtended(
self.objpoints_l,
self.imgpoints_l,
self.imageSize1,
None,
None,
flags=flags)
# DROITE
self.err2, self.M2, self.d2, self.r2, self.t2, stdDeviationsIntrinsics2, stdDeviationsExtrinsics2, self.perViewErrors2 = cv.calibrateCameraExtended(
self.objpoints_r,
self.imgpoints_r,
self.imageSize2,
None,
None,
flags=flags)
# Enlever les outliers -------------------------------------------------
# gauche
objpoints_l = np.array(self.objpoints_l.copy())
imgpoints_l = np.array(self.imgpoints_l.copy())
indices = np.indices(self.perViewErrors1.shape)[0]
self.indexes_l = indices[self.perViewErrors1 < self.err1 * 1]
if len(self.indexes_l) > 0:
objpoints_l = objpoints_l[self.indexes_l]
imgpoints_l = imgpoints_l[self.indexes_l]
# droite
objpoints_r = np.array(self.objpoints_r.copy())
imgpoints_r = np.array(self.imgpoints_r.copy())
indices = np.indices(self.perViewErrors2.shape)[0]
self.indexes_r = indices[self.perViewErrors2 < self.err2 * 1]
if len(self.indexes_r) > 0:
objpoints_r = objpoints_r[self.indexes_r]
imgpoints_r = imgpoints_r[self.indexes_r]
# ----------------------------------------------------------------------
# Ajouter les points des cibles
objpoints_l = list(objpoints_l)
objpoints_r = list(objpoints_r)
imgpoints_l = list(imgpoints_l)
imgpoints_r = list(imgpoints_r)
objpoints_l.append(cibles)
objpoints_r.append(cibles)
imgpoints_l.append(cibles_l)
imgpoints_r.append(cibles_r)
self.err1, self.M1, self.d1, self.r1, self.t1, stdDeviationsIntrinsics1, stdDeviationsExtrinsics1, self.perViewErrors1 = cv.calibrateCameraExtended(
objpoints_l, imgpoints_l, self.imageSize1, None, None, flags=flags)
self.err2, self.M2, self.d2, self.r2, self.t2, stdDeviationsIntrinsics2, stdDeviationsExtrinsics2, self.perViewErrors2 = cv.calibrateCameraExtended(
objpoints_r, imgpoints_r, self.imageSize2, None, None, flags=flags)
# Print erreur de reprojection
print('Erreur de reprojection RMS calibration individuelle')
print(self.err1, self.err2)
def calibrateSingle(self,
single_path,
single_detected_path,
fisheye=False):
"""
||Public method||
Calibration individuelle de 2 caméras
Args:
single_path (str): "path_to_single_images/"
single_detected_path (str): "path_to_single_images_detected/"
fisheye (Bool): True pour caméra fisheye
"""
self.single_path = single_path
self.single_detected_path = single_detected_path
clean_folders([single_detected_path])
self.objpoints_l, self.imgpoints_l, self.imageSize1 = self.__read_single(
'left')
self.objpoints_r, self.imgpoints_r, self.imageSize2 = self.__read_single(
'right')
if fisheye == True:
self.err1, self.M1, self.d1, self.r1, self.t1, stdDeviationsIntrinsics1, stdDeviationsExtrinsics1, self.perViewErrors1 = cv.calibrateCameraExtended(
self.objpoints_l,
self.imgpoints_l,
self.imageSize1,
None,
None,
flags=self.fisheye_flags)
self.err2, self.M2, self.d2, self.r2, self.t2, stdDeviationsIntrinsics2, stdDeviationsExtrinsics2, self.perViewErrors2 = cv.calibrateCameraExtended(
self.objpoints_r,
self.imgpoints_r,
self.imageSize2,
None,
None,
flags=self.fisheye_flags)
else:
self.err1, self.M1, self.d1, self.r1, self.t1, stdDeviationsIntrinsics1, stdDeviationsExtrinsics1, self.perViewErrors1 = cv.calibrateCameraExtended(
self.objpoints_l,
self.imgpoints_l,
self.imageSize1,
None,
None,
flags=self.not_fisheye_flags)
self.err2, self.M2, self.d2, self.r2, self.t2, stdDeviationsIntrinsics2, stdDeviationsExtrinsics2, self.perViewErrors2 = cv.calibrateCameraExtended(
self.objpoints_r,
self.imgpoints_r,
self.imageSize2,
None,
None,
flags=self.not_fisheye_flags)
# Print erreur de reprojection
print('Erreur de reprojection RMS calibration individuelle')
print(self.err1, self.err2)
intrinsic_data_folder =""
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1e-4)
cols, rows = (6,9)
cell_size = 10
objp = np.zeros((rows * cols, 3), np.float32)
objp[:, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
objp *= cell_size
objpoints = []
imgpoints = []
imgs_paths = glob(path.join(intrinsic_data_folder, '*.png'))
h, w = cv2.imread(imgs_paths[0]).shape[:2]
for fname in imgs_paths:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCornersSB(gray, (cols, rows), cv2.CALIB_CB_ACCURACY)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
imgpoints.append(corners)
_, camera_matrix, dist, _, _, _, _, _ = cv2.calibrateCameraExtended(
objpoints,
imgpoints, (w, h),
None,
None,
flags=cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_RATIONAL_MODEL,
criteria=criteria)
np.save(cfg.M_int_path, camera_matrix)
print('calibration completed')
print(camera_matrix)
def read_images(self, folder):
i = 0
images = []
images_fail = []
for file in os.listdir(folder):
if re.search(r'.*_left', file) == None:
continue
image1 = cv2.imread(folder + "/" + file)
if image1 is None:
break
file_right = re.sub(r'_left', '_right', file)
image2 = cv2.imread(folder + "/" + file_right)
if image2 is None:
break
gray_l = cv2.extractChannel(image1, 1)
gray_r = cv2.extractChannel(image2, 1)
# Find the chess board corners
ret_l, corners_l = cv2.findChessboardCorners(
gray_l, (8, 6), None, cv2.CALIB_CB_ADAPTIVE_THRESH)
if not ret_l:
print("left fail")
images_fail.append(file)
continue
ret_r, corners_r = cv2.findChessboardCorners(
gray_r, (8, 6), None, cv2.CALIB_CB_ADAPTIVE_THRESH)
if not ret_r:
print("right fail")
images_fail.append(file)
continue
images.append(file)
if ret_l and ret_r:
# If found, add object points, image points (after refining them)
self.objpoints.append(self.objp)
rt = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1),
self.criteria)
self.imgpoints_l.append(corners_l)
# Draw and display the corners
cv2.drawChessboardCorners(gray_l, (8, 6), corners_l, ret_l)
rt = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1),
self.criteria)
self.imgpoints_r.append(corners_r)
# Draw and display the corners
cv2.drawChessboardCorners(gray_r, (8, 6), corners_r, ret_r)
cv2.imshow("Image Left", gray_l)
cv2.imshow("Image Right", gray_r)
key = cv2.waitKey(1)
if key == ord('q'):
break
if key == ord('a'):
return
img_shape = gray_r.shape
self.shape = img_shape
print(f"Fails: {images_fail}", file=sys.stderr)
print("Starting camera calibration", file=sys.stderr)
flags = 0
# flags |= cv2.CALIB_FIX_INTRINSIC
# flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
# flags |= cv2.CALIB_USE_INTRINSIC_GUESS
# flags |= cv2.CALIB_FIX_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_ASPECT_RATIO
# flags |= cv2.CALIB_ZERO_TANGENT_DIST
# flags |= cv2.CALIB_RATIONAL_MODEL
# flags |= cv2.CALIB_SAME_FOCAL_LENGTH
#flags |= cv2.CALIB_FIX_K3
#flags |= cv2.CALIB_FIX_K4
#flags |= cv2.CALIB_FIX_K5
#flags |= cv2.CALIB_FIX_K6
rt, self.M1, self.d1, self.r1, self.t1, sdi, sde, pve = cv2.calibrateCameraExtended(
self.objpoints, self.imgpoints_l, img_shape, None, None)
print("Reprojection error left: " + str(rt), file=sys.stderr)
j = 0
for image in images:
print(f"{image}: {pve[j,0]}", file=sys.stderr)
j += 1
rt, self.M2, self.d2, self.r2, self.t2, sid, sde, pve = cv2.calibrateCameraExtended(
self.objpoints, self.imgpoints_r, img_shape, None, None)
print("Reprojection error right: " + str(rt), file=sys.stderr)
j = 0
for image in images:
print(f"{image}: {pve[j,0]}", file=sys.stderr)
j += 1
print("Starting stereo camrea calibration", file=sys.stderr)
self.camera_model = self.stereo_calibrate(img_shape)
def calibrate(self, images, board, flags=None):
"""
images: an array of grayscale images, all assumed to be the same size.
If images are not grayscale, then assumed to be in BGR format.
board: an object that represents your target, i.e., Chessboard
marker_scale: how big are your markers in the real world, example:
checkerboard with sides 2 cm, set marker_scale=0.02 so your T matrix
comes out in meters
"""
# self.save_cal_imgs = []
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
max_corners = board.marker_size[0] * board.marker_size[1]
bad_images = []
# for cnt, gray in enumerate(tqdm(images)):
for cnt, gray in enumerate(images):
if len(gray.shape) > 2:
gray = bgr2gray(gray)
# ret, corners = self.findMarkers(gray)
ok, corners, objp = board.find(gray)
# If found, add object points, image points (after refining them)
if ok:
# imgpoints.append(corners.reshape(-1, 2))
# get the real-world pattern of points
# objp = board.objectPoints()
objpoints.append(objp)
# print('[{}] + found {} of {} corners'.format(
# cnt, corners.size / 2, max_corners))
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30,
0.001)
corners = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
term)
imgpoints.append(corners.reshape(-1, 2))
# Draw the corners
# tmp = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
# cv2.drawChessboardCorners(tmp, board.marker_size, corners, True)
# tmp = board.draw(gray, corners)
# self.save_cal_imgs.append(tmp)
else:
bad_images.append(cnt)
# print(f'{Fore.RED}*** Image[{cnt}] - Could not find markers ***{Fore.RESET}')
if len(bad_images) > 0:
print(
f'{Fore.RED}>> Could not find markers in images: {bad_images}{Fore.RESET}'
)
# images size here is backwards: w,h
h, w = images[0].shape[:2]
# initial guess for camera matrix
# K = None # FIXME
f = 0.8 * w
cx, cy = w // 2, h // 2
K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]])
# not sure how much these really help
if flags is None:
flags = 0
# flags |= cv2.CALIB_THIN_PRISM_MODEL
# flags |= cv2.CALIB_TILTED_MODEL
# flags |= cv2.CALIB_RATIONAL_MODEL
# rms, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
# objpoints, imgpoints, (w, h), K, None, flags=flags)
rms, mtx, dist, rvecs, tvecs, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors = cv2.calibrateCameraExtended(
objpoints, imgpoints, (w, h), K, None)
data = {
'date': time.strftime("%a, %d %b %Y %H:%M:%S", time.localtime()),
'markerType': board.type,
'markerSize': board.marker_size,
'imageSize': images[0].shape,
'K': mtx,
'd': dist, #DistortionCoefficients(dist),
'rms': rms,
'rvecs': rvecs,
'tvecs': tvecs,
"objpoints": objpoints,
"imgpoints": imgpoints,
"badImages": bad_images,
"stdint": stdDeviationsIntrinsics,
"stdext": stdDeviationsExtrinsics,
"perViewErr": perViewErrors
}
cam = Camera(mtx, dist, images[0].shape[:2])
print(f"{Fore.GREEN}>> RMS: {rms:0.3f}px{Fore.RESET}")
print("\n", cam)
return cam, data
