def estimate_pose_checkerboard(grayf, intrinsics, board):
ratio = 400.0 / grayf.shape[0]
gray = cv2.resize(grayf, (0, 0),
fx=ratio,
fy=ratio,
interpolation=cv2.INTER_CUBIC)
board_size = board.getChessboardSize()
corners, check_score = detect_checkerboard(gray, board_size)
if corners is None:
return False, None
INTRINSICS_K = np.array(intrinsics['camera_mat'])
INTRINSICS_D = np.array(intrinsics['dist_coeff'])
retval, rvec, tvec, inliers = cv2.solvePnPRansac(board.objPoints,
corners,
INTRINSICS_K,
INTRINSICS_D,
confidence=0.9,
reprojectionError=10)
return True, (corners, None, rvec, tvec)
def extract_points_2D(self, img_msg, now, proc_results, calibrator_instance, rectify=False):
# print("board_points:", len(board_points), ":", [x.shape for x in board_points])
# Log PID
rospy.loginfo('2D Picker PID: [%d]' % os.getpid())
# Read image using CV bridge
try:
if (IMAGE_COMPRESSED):
img = CV_BRIDGE.compressed_imgmsg_to_cv2(img_msg, 'bgr8')
else:
img = CV_BRIDGE.imgmsg_to_cv2(img_msg, 'bgr8')
except CvBridgeError as e:
rospy.logerr(e)
return None
rospy.loginfo('image msg converted.')
# Rectify image
if rectify and CAMERA_MODEL_INITED: CAMERA_MODEL.rectifyImage(img, img)
cv2.imwrite(os.path.join(PKG_PATH, CALIB_PATH, self.camera_name+"_pcd", str(self.board_points_num)+".jpg"), img)
# gamma = 2.
# invGamma = 1.0 / gamma
# table = np.array([((i / 255.0) ** invGamma) * 255 for i in np.arange(0, 256)]).astype("uint8")
# # apply gamma correction using the lookup table
# img = cv2.LUT(np.array(img, dtype = np.uint8), table)
# cv2.namedWindow('gamma_transform', 0)
# cv2.imshow("gamma_transform", img)
# cv2.waitKey(0)
# detect corners
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, self.chessboard_size, None)
# corners2 = None
if ret == True:
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),self.chessboard_corner_criteria)
# Draw and display the corners
cv2.drawChessboardCorners(img, self.chessboard_size, corners, ret)
cv2.namedWindow('chessboard corner detect', 0)
cv2.imshow('chessboard corner detect', img)
key = cv2.waitKey(0)
if (key == ord('q')):
cv2.destroyAllWindows()
proc_results.put([self.chessboard_corner_coords, np.squeeze(corners)])
else:
rospy.logwarn("No chessboard corners found in the current image! Try another chessboard corners detection method...")
corners, score = detect_checkerboard(gray, self.chessboard_size[::-1])
if (corners is None):
rospy.logerr("Chessboard corner detection failed! Skip this frame.")
return
corners = corners.astype(np.float32)
proc_results.put([self.chessboard_corner_coords, np.squeeze(corners)])
cv2.drawChessboardCorners(img, self.chessboard_size, corners, True)
cv2.namedWindow('chessboard', 0)
cv2.imshow('chessboard', img)
cv2.resizeWindow('chessboard', 640,480)
cv2.moveWindow('chessboard', 500,0)
key = cv2.waitKey(0)
if (key==13 or key==32): #按空格和回车键退出
# cv2.destroyAllWindows()
pass
cv2.destroyWindow('chessboard')
def extract_points_2D(self, img_file, proc_results, rectify=False):
# print("board_points:", len(board_points), ":", [x.shape for x in board_points])
# Log PID
rospy.loginfo('2D Picker PID: [%d] file name: %s' %
(os.getpid(), os.path.basename(img_file)))
# Read image
img = cv2.imread(img_file)
# Rectify image
if rectify and CAMERA_MODEL_INITED: CAMERA_MODEL.rectifyImage(img, img)
# detect corners
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, self.chessboard_size,
None)
# corners2 = None
if ret == True:
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
self.chessboard_corner_criteria)
# Draw and display the corners
cv2.drawChessboardCorners(img, self.chessboard_size, corners, ret)
cv2.namedWindow('chessboard', 0)
cv2.imshow('chessboard', img)
cv2.resizeWindow('chessboard', 640, 480)
cv2.moveWindow('chessboard', 500, 0)
key = cv2.waitKey(0)
if (key == ord('q')):
# cv2.destroyAllWindows()
pass
cv2.destroyWindow('chessboard')
proc_results.put(
[self.chessboard_corner_coords,
np.squeeze(corners)])
else:
rospy.logwarn(
"No chessboard corners found in the current image! Try another chessboard corners detection method..."
)
corners, score = detect_checkerboard(gray,
self.chessboard_size[::-1])
if (corners is None):
rospy.logerr(
"Chessboard corner detection failed! Skip this frame.")
return
corners = corners.astype(np.float32)
proc_results.put(
[self.chessboard_corner_coords,
np.squeeze(corners)])
cv2.drawChessboardCorners(img, self.chessboard_size, corners, True)
cv2.namedWindow('chessboard', 0)
cv2.imshow('chessboard', img)
cv2.resizeWindow('chessboard', 640, 480)
cv2.moveWindow('chessboard', 500, 0)
key = cv2.waitKey(0)
if (key == 13 or key == 32): #按空格和回车键退出
# cv2.destroyAllWindows()
pass
cv2.destroyWindow('chessboard')
def get_corners_checkerboard(fname, board, skip=20):
cap = cv2.VideoCapture(fname)
length = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
allCorners = []
allScores = []
go = int(skip / 2)
board_size = board.getChessboardSize()
for framenum in trange(length, ncols=70):
ret, frame = cap.read()
if not ret:
break
if framenum % skip != 0 and go <= 0:
continue
grayf = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
## TODO: adjust the checkerboard library to handle more general ratios
## TODO: make this ratio and trimming configurable
ratio = 400.0 / grayf.shape[0]
gray = cv2.resize(grayf, (0, 0),
fx=ratio,
fy=ratio,
interpolation=cv2.INTER_CUBIC)
corners, check_score = detect_checkerboard(gray, board_size)
if corners is not None and len(corners) > 0:
corners_new = corners / ratio
allCorners.append(corners_new)
allScores.append(check_score)
go = int(skip / 2)
go = max(0, go - 1)
cap.release()
return allCorners, allScores
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob('/home/user/ACSC/calibration_data/**/**/*.png')
# path = 'results'
# pathlib.Path(path).mkdir(parents=True, exist_ok=True)
found = 0
for fname in images: # Here, 10 can be changed to whatever number you like to choose
orig = cv2.imread(fname) # Capture frame-by-frame
img = cv2.cvtColor(orig, cv2.COLOR_BGR2GRAY)
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8,8))
img = clahe.apply(img)
# Find the chess board corners
corners, score = detect_checkerboard(img, (7,5))
# If found, add object points, image points (after refining them)
if corners is not None:
corners = corners.astype(np.float32)
objpoints.append(objp) # Certainly, every loop objp is the same, in 3D.
imgpoints.append(corners)
# Draw and display the corners\
#print(len(corners), score, corners.shape)
#rint(orig.shape)
#image = cv2.drawChessboardCorners(orig, (7,5), corners, True)
found += 1
# cv2.imshow('image', image)
# cv2.waitKey(500)
# if you want to save images with detected corners
# uncomment following 2 lines and lines 5, 18 and 19
# cv2.imshow('thresh', thresh)
cv2.imshow('image', img)
cv2.waitKey()
# maybe template matching is gonna work better
from checkerboard import detect_checkerboard
size = (7, 7) # size of checkerboard
img = cv2.imread('screenshot.png') # obtain checkerboard
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
sharpen_kernel = np.array([[-1, -1, -1], [-1, 9, -1], [-1, -1, -1]])
sharpen = cv2.filter2D(gray, -1, sharpen_kernel)
thresh = cv2.threshold(sharpen, 160, 255, cv2.THRESH_BINARY_INV)[1]
corners, score = detect_checkerboard(thresh, size)
score
corners_int = corners.astype(int)
for i in corners_int:
x, y = i.ravel()
cv2.circle(img, (x, y), 11, (0, 0, 255), -1)
cv2.imshow("image", img)
cv2.waitKey()
# this works but is super slow
# approaches we can use to make it faster => use the thresholded image for cv2
# rescale image to a tiny size and upscale results
def example_two(picture):
"""
Skew correction using homography and corner detection using contour points
Returns: None
"""
print("Identifying boad and Correcting skew")
#image = cv2.imread('images/example_2.jpg')
image = cv2.imread(picture)
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
plt.imshow(image)
plt.title('Original Image')
plt.show()
filtered_image = apply_filter(image)
threshold_image = apply_threshold(filtered_image)
cnv, largest_contour = detect_contour(threshold_image, image.shape)
corners = detect_corners_from_contour(cnv, largest_contour)
destination_points, h, w = get_destination_points(corners)
un_warped = unwarp(threshold_image, np.float32(corners), destination_points)
cropped = un_warped[0:h, 0:w]
#f, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 8))
# f.subplots_adjust(hspace=.2, wspace=.05)
#ax1.imshow(un_warped)
#ax2.imshow(cropped)
#plt.show()
plt.imshow(cropped)
plt.axis('off')
plt.savefig('images/processed.jpg')
plt.show()
print("looking for checkerboard")
filename='images/processed.jpg'
size = (5, 5) # size of checkerboard
image = cv2.imread(filename)
#plt.imshow(image)
#plt.title('Original Image')
#plt.show()
start=time.time()
corners, score = detect_checkerboard(image, size)
print(filename)
if score > 0.7:
print("Obstructed", score)
messagebox.showinfo('Result', 'Board is obstructed')
elif score > 0.3:
print("Not sure", score)
messagebox.showinfo('Result', 'Not Sure')
else:
print("Clear", score)
messagebox.showinfo('Result', 'Board is clear')
elapsed=time.time()-start
print("Elapsed time: {} s".format(elapsed))
