def calibrate_distortion(
images: ImageSequence, checkerboard_size: Tuple[int, int]
) -> Tuple[np.ndarray, np.ndarray, np.ndarray, Tuple[int, int, int, int]]:
"""Perform lens calibration
Args:
images (ImageSequence): Sequence of calibration shots
checkerboard_size (Tuple[int, int]): Size of the checkerboard (outer corners do not count)
Returns:
mtx: Matrix of intrinsic camera parameters
dist: Vectors of distortion coefficients (k1, k2, p1, p2, k3)
newcameramtx: Matrix that performs additional scaling to account for black borders
roi: ROI of valid pixels
"""
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((checkerboard_size[0] * checkerboard_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[
0 : checkerboard_size[0], 0 : checkerboard_size[1]
].T.reshape(-1, 2)
# 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.
for img in tqdm(images):
p = np.percentile(img.data, [5, 95])
img = rescale_intensity(
img.data, in_range=tuple(p.tolist()), out_range=(0, 255)
).astype(np.uint8)
# Find the chess board corners
ret, corners = cv2.findChessboardCornersSB(img, checkerboard_size)
if ret == True:
objpoints.append(objp)
imgpoints.append(corners)
# sanity check
if len(objpoints) < 0.8 * len(images):
logging.warn(
"Checkerboard detection failed on more than 20% percent of the images (only {:d} succeeded). Please consider preprocessing the images, such that the checkerboard has a better contrast.".format(
len(objpoints)
)
)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, images[0].shape[::-1], None, None
)
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
mtx, dist, img.shape, 1, img.shape
)
return mtx, dist, newcameramtx, roi
def find_corners_sb(img):
"""
查找棋盘格角点函数 SB升级款
:param img: 处理原图
"""
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# 查找棋盘格角点;
ret, corners = cv2.findChessboardCornersSB(
gray, pattern_size, cv2.CALIB_CB_EXHAUSTIVE + cv2.CALIB_CB_ACCURACY)
if ret:
# 显示角点
cv2.drawChessboardCorners(img, pattern_size, corners, ret)
def extract_corners(self, img):
img = cv2.resize(img, (0,0), fx=self.img_resize_fac, fy=self.img_resize_fac)
if self.board_type == "radon":
retval, corners, meta = cv2.findChessboardCornersSB(img, self.pattern_size,
cv2.CALIB_CB_NORMALIZE_IMAGE| cv2.CALIB_CB_EXHAUSTIVE)
elif self.board_type == "aruco":
print("detectaruco")
else:
print("Not supported board type. Choices: radon, aruco")
pass
return pts3d, corners
def camera_chessboard_corner_detection(img):
# 转换为灰度图
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# 找到棋盘格角点
ret, corners = cv2.findChessboardCornersSB(gray, (W, H), None)
# ret, corners = cv2.findChessboardCorners(gray, (W, H), None)
# 如果找到足够点对，将其存储起来
if ret:
# 消除多余的dimension
corners = np.squeeze(corners)
# 确保索引是从左下方的角点开始的
if corners[0, 1] < corners[-1, 1]:
# 保证首点在末点下方
corners = corners[::-1, :]
elif corners[0, 1] == corners[-1, 1]:
# 保证首点在末点左侧
if corners[0, 0] > corners[-1, 0]:
corners = corners[::-1, :]
if DEBUG > 1:
vis = np.copy(img)
# 将角点在图像上显示
vis = cv2.drawChessboardCorners(vis, (W, H), corners, ret)
for corner_id in range(len(corners)):
cv2.circle(vis, tuple(corners[corner_id]), 5, (0, 0, 255), 10)
cv2.putText(vis, '{}'.format(corner_id),
tuple(corners[corner_id] + 3),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 255), 1)
cv2.imshow('findCorners', vis)
cv2.waitKey(0)
return corners
else:
print('ERROR: Corners not found!!!', file=sys.stderr)
return None
)
logger = logging.getLogger("CAMERA CALIBRATION")
real_points = numpy.array(
[[numpy.float32(j), numpy.float32(i),
numpy.float32(0)] for i in range(0, 14, 2) for j in range(0, 14, 2)])
width, height, channel = (0, 0, 0)
p3d, p2d = [], []
final_path = CURRENT_PATH + PATH_BEST
for filename in os.listdir(final_path):
if filename.endswith(".jpg"):
img = cv2.imread(final_path + '/' + filename)
if width == 0 or height == 0 or channel == 0:
width, height, channel = img.shape
ret, corners = cv2.findChessboardCornersSB(img, (7, 7))
if ret:
logger.info("{} - Chessboard found in file : {}".format(
final_path, filename))
p3d.append(real_points)
p2d.append(corners)
else:
logger.info("{} - Chessboard not found in file : {}".format(
final_path, filename))
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(p3d, p2d, (height, width),
None, None)
logger.info("CALIBRATION RESULT: {}, RET: {}".format(final_path, ret))
img = cv2.imread(final_path + '/' + 'IMG_20200405_203414.jpg')[..., ::-1]
img_undistorted = cv2.undistort(img, mtx, dist)
console.print(Rule())
ncols = Prompt.ask(Text("Enter the number of inner corners in the horizontal direction", style="green"), default="9")
ncols = int(ncols)
nrows = Prompt.ask(Text("Enter the number of inner corners in the vertical direction", style="green"), default="7")
nrows = int(nrows)
grid_delta = Prompt.ask(Text("Enter the size of each square of the checkerboard in mm", style="green"), default="2")
grid_delta = float(grid_delta)
# Read image and convert to BW
im = cv.imread(imfile)
imbw = cv.cvtColor(im, cv.COLOR_BGR2GRAY)
# Detect checkerboard
console.print(Rule())
console.print("Detecting checkerboard...", style="yellow bold")
detected, corners = cv.findChessboardCornersSB(imbw, (nrows, ncols))
if not detected:
sys.exit("ERROR: checkerboard detection failed")
cv.drawChessboardCorners(im, (nrows, ncols), corners, detected)
# Draw checkerboard
cv.namedWindow("Checkerboard", cv.WINDOW_NORMAL)
utils.imshow_scaled("Checkerboard", im)
console.print("Press ENTER to validate and write the calibration data, else press any other key to abort.", style="yellow bold blink")
key = cv.waitKey(0)
cv.destroyAllWindows()
if key == 13: # if ENTER is pressed, continue
# Reshape corners as matrices
valid_img_ids = []
width_l, height_l, channel_l = (0, 0, 0)
width_r, height_r, channel_r = (0, 0, 0)
for i in img_ids:
img_left = cv2.imread(CURRENT_PATH + PATHs[1] + "/" + img_left[i])
img_right = cv2.imread(CURRENT_PATH + PATHs[0] + "/" + img_right[i])
if width_l == 0 or height_l == 0 or channel_l == 0:
width_l, height_l, channel_l = img_left.shape
if width_r == 0 or height_r == 0 or channel_r == 0:
width_r, height_r, channel_r = img_right.shape
ret_left, left_corners = cv2.findChessboardCornersSB(img_left, (7, 7))
ret_right, right_corners = cv2.findChessboardCornersSB(
img_right, (7, 7))
if ret_left and ret_right:
valid_img_ids.append(i)
p2d_right.append(right_corners)
p2d_left.append(left_corners)
p3d.append(real_points)
ret_r, mtx_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(
p3d, p2d_right, (height_r, width_r), None, None)
ret_l, mtx_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(
p3d, p2d_left, (height_l, width_l), None, None)
retval, _, _, _, _, R, T, E, F = (cv2.stereoCalibrate(
import glob
import cv2
import numpy as np
PATTERN_SIZE = (9, 6)
objp = np.zeros((PATTERN_SIZE[0] * PATTERN_SIZE[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:PATTERN_SIZE[0], 0:PATTERN_SIZE[1]].T.reshape(-1, 2)
obj_points = []
img_points = []
filelist = glob.glob('images/*.jpg')
for filename in filelist:
img = cv2.imread(filename, cv2.IMREAD_COLOR)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
found, corners = cv2.findChessboardCornersSB(gray, PATTERN_SIZE)
if found == True:
img_points.append(corners)
obj_points.append(objp)
ret, camera_matrix, distortion, rvecs, tvecs, _ = cv2.calibrateCameraRO(
obj_points, img_points, gray.shape[::-1], -1, None, None)
print('reprojection error:\n', ret)
print('camera matrix:\n', camera_matrix)
print('distortion:\n', distortion)
print('rvecs:\n', rvecs[0].shape)
print('tvecs:\n', tvecs[0].shape)
def process_calibration(self, images: Sequence[Image]):
"""
Calibrate 2+ images: find intrinsic + extrinsic matrices and R/T between the camera pairs
"""
outputs = {str(i): image for (i, image) in enumerate(images)}
# step 1: press space to start
if self.stage == StereoVision.STAGES["CALIBRATE_WAIT"]:
print("[calibration] Grab your chessboard pattern, and press space to start taking snapshots.")
print(" The chessboard needs to be fully visible in *all* images.")
# step 2: take snapshots every 3s
elif self.snapshot_count < 8:
# step 2a: waiting for next snapshot
if time.time() - self.last_snapshot < 3.000:
# waiting for next snapshot moment
outputs = {str(i): image for (i, image) in enumerate(images)}
# step 2b: take snapshot (if delay passed and the pattern was found for all images)
else:
all_corners = {}
all_snapshots = {}
for i, image in enumerate(images):
key = str(i)
img = image.get(ImageType.OPENCV)
print(i, img.shape)
img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCornersSB(
img_gray, StereoVision.CHESSBOARD_SIZE, None)
if ret:
all_corners[key] = corners
all_snapshots[key] = img
img = cv2.drawChessboardCorners(
img, StereoVision.CHESSBOARD_SIZE, corners, ret)
img = cv2.rectangle(img, (1, 1), (img.shape[1]-2, img.shape[0]-2), (0, 255, 0), 2)
else:
img = cv2.rectangle(img, (1, 1), (img.shape[1]-2, img.shape[0]-2), (0, 0, 255), 2)
outputs[key] = Image(img, opencv=True)
if len(all_corners) == len(images):
# found corners in all images; saving
self.snapshot_count += 1
self.last_snapshot = time.time()
for key in all_corners:
self.corners[key].append(all_corners[key])
self.snapshots[key].append(all_snapshots[key])
print("[calibration] Snapshot {}! Waiting 3s for the next snapshot..".format(
self.snapshot_count))
# step 3: calculate calibration parameters
elif self.snapshot_count >= 8:
print("[calibration] Calculating calibration parameters")
# calibrate individual cameras
SIZEX, SIZEY = StereoVision.CHESSBOARD_SIZE
world_points_frame = np.zeros((SIZEX*SIZEY, 3), np.float32)
world_points_frame[:, :2] = np.mgrid[0:SIZEX, 0:SIZEY].T.reshape(-1, 2) * StereoVision.SQUARE_SIZE
world_points = [world_points_frame] * 8
for key in self.corners:
img_points = self.corners[key]
img_size = self.snapshots[key][0].shape[1::-1]
camparams = mvutils.calibrate_camera(
world_points, img_points, img_size)
self.camparams[key] = camparams
pprint(camparams)
# calibrate camera pairs (pairwise, chained in given order)
for index_l, index_r in zip(range(0, len(images)-1), range(1, len(images))):
key_l, key_r = str(index_l), str(index_r)
camparams_img_l = self.camparams[key_l]
camparams_img_r = self.camparams[key_r]
img_size = self.snapshots[key_l][0].shape[1::-1]
img_size_r = self.snapshots[key_r][0].shape[1::-1]
if img_size != img_size_r:
print("ERROR: images should be of the same size; current sizes:", img_size, img_size_r)
pairparams = mvutils.calibrate_camera_pair(
world_points, self.corners[key_l], self.corners[key_r],
camparams_img_l, camparams_img_r, img_size)
self.pairparams[(key_l, key_r)] = pairparams
print((key_l, key_r))
pprint(pairparams)
# save to disk
self.save_params()
self.stage = StereoVision.STAGES["RUNNING"]
return outputs
def test_calibrate():
flags = (cv2.CALIB_CB_EXHAUSTIVE + cv2.CALIB_CB_ACCURACY +
cv2.CALIB_CB_MARKER) # for findChessboardCornersSB
obj_points = []
img_points_r = []
img_points_l = []
img_shape = None
chess_img_r = None
chess_img_l = None
corners_l = None
corners_r = None
objp = np.zeros((np.prod(CHESSBOARD_SIZE), 3), dtype=np.float32)
objp[:, :2] = np.mgrid[0:CHESSBOARD_SIZE[0],
0:CHESSBOARD_SIZE[1]].T.reshape(-1, 2)
for i in range(1, 113):
t = str(i)
chessboard_l_found = False
chessboard_r_found = False
corners_l = None
corners_r = None
chess_img_l = cv2.imread('{0}/chessboard-L{1}.png'.format(DIR_NAME, t),
0)
chess_img_r = cv2.imread('{0}/chessboard-R{1}.png'.format(DIR_NAME, t),
0)
img_shape = chess_img_r.shape[:2]
try:
chessboard_l_found, corners_l = cv2.findChessboardCornersSB(
chess_img_l, CHESSBOARD_SIZE, flags=flags)
chessboard_r_found, corners_r = cv2.findChessboardCornersSB(
chess_img_r, CHESSBOARD_SIZE, flags=flags)
except Exception as ex:
print('error finding chessboard {0}'.format(t))
if chessboard_l_found == True & chessboard_r_found == True:
print('Found {0}'.format(t))
obj_points.append(objp)
img_points_l.append(corners_l)
img_points_r.append(corners_r)
# calibrate cameras
ret_l, K_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(
obj_points, img_points_l, chess_img_l.shape[::-1], None, None)
ret_r, K_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(
obj_points, img_points_r, chess_img_r.shape[::-1], None, None)
new_camera_matrix_l, roi_l = cv2.getOptimalNewCameraMatrix(
K_l, dist_l, img_shape, 1, img_shape)
new_camera_matrix_r, roi_r = cv2.getOptimalNewCameraMatrix(
K_r, dist_r, img_shape, 1, img_shape)
left_camera = (new_camera_matrix_l, K_l, dist_l)
with open('camera_matrix_l.pickle', 'wb') as handle:
pickle.dump(left_camera, handle, protocol=HIGHEST_PROTOCOL)
right_camera = (new_camera_matrix_r, K_r, dist_r)
with open('camera_matrix_r.pickle', 'wb') as handle:
pickle.dump(right_camera, handle, protocol=HIGHEST_PROTOCOL)
# stereo calibrate
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS,
100, 1e-5)
retS, matrix_l, dist_coef_l, matrix_r, dist_coef_r, R, T, E, F = cv2.stereoCalibrate(
obj_points,
img_points_l,
img_points_r,
new_camera_matrix_l,
dist_l,
new_camera_matrix_r,
dist_r,
img_shape,
criteria=stereocalib_criteria,
flags=cv2.CALIB_FIX_INTRINSIC)
rot_l, rot_r, proj_l, proj_r, Q, roi_l, roi_r = cv2.stereoRectify(
matrix_l, dist_coef_l, matrix_r, dist_coef_r, img_shape, R, T)
print('retS.stereoCalibrate - ', retS, ' retL - ', ret_l, ' retR - ',
ret_r)
print('R: ', R)
print('T:', T)
left_map = cv2.initUndistortRectifyMap(matrix_l, dist_coef_l, rot_l,
proj_l, img_shape, cv2.CV_16SC2)
right_map = cv2.initUndistortRectifyMap(matrix_r, dist_coef_r, rot_r,
proj_r, img_shape, cv2.CV_16SC2)
stereo_map = (left_map, right_map)
with open('stereo_map.pickle', 'wb') as handle:
pickle.dump(stereo_map, handle, protocol=HIGHEST_PROTOCOL)
cameras = Cameras()
while True:
frame_l, frame_r = cameras.grab_one()
res = np.hstack((frame_l, frame_r))
cv2.imshow('original', res)
print('roi_l:', roi_l, ' - roi_r:', roi_r)
ret = cv2.getValidDisparityROI(roi_l, roi_r, 1, 10, 5)
print(ret)
left_rectified = cv2.remap(frame_l,
left_map[0],
left_map[1],
interpolation=cv2.INTER_LANCZOS4,
borderMode=cv2.BORDER_CONSTANT)
right_rectified = cv2.remap(frame_r,
right_map[0],
right_map[1],
interpolation=cv2.INTER_LANCZOS4,
borderMode=cv2.BORDER_CONSTANT)
cv2.imshow('left_rectified', left_rectified)
cv2.imshow('right_rectified', right_rectified)
img_to_use_l = left_rectified
img_to_use_r = right_rectified
chessboard_l_found, corners_l = cv2.findChessboardCorners(
img_to_use_l, CHESSBOARD_SIZE, None)
chessboard_r_found, corners_r = cv2.findChessboardCorners(
img_to_use_r, CHESSBOARD_SIZE, None)
print(chessboard_l_found, chessboard_r_found)
if chessboard_l_found == True & chessboard_r_found == True:
print('point_l: ', corners_l[0][0][0])
print('point_r: ', corners_r[0][0][0])
disparity_maybe = corners_l[0][0][0] - corners_r[0][0][0]
disp_may = (corners_l[0][0][0], corners_l[0][0][1],
disparity_maybe)
np_disp = np.array(
[[[corners_l[0][0][0], corners_l[0][0][1], disparity_maybe]]],
dtype=np.float32)
new_coord = cv2.perspectiveTransform(np_disp, Q)
print('new_coord: ', new_coord)
k = cv2.waitKey(0) & 0xFF
if k == 115:
cv2.imwrite('{0}/img-L.png'.format(DIR_NAME), frame_l)
cv2.imwrite('{0}/img-R.png'.format(DIR_NAME), frame_r)
if k == 27:
break
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)
import cv2
import numpy as np
import matplotlib.pyplot as plt
chessImg = cv2.imread("../../../gallery/chessBoard.jpg", cv2.IMREAD_COLOR)
# grayChess = cv2.cvtColor(chessImg, cv2.COLOR_BGR2GRAY)
found, corners = cv2.findChessboardCornersSB(chessImg, (8, 8))
found, corners = cv2.findCirclesGrid(chessImg, (8, 8),
cv2.CALIB_CB_SYMMETRIC_GRID)
print(found)
cv2.drawChessboardCorners(chessImg, (8, 8), corners, found)
cv2.imshow("detected-corners", chessImg)
cv2.waitKey(0)
cv2.destroyAllWindows()