def detect_and_save_markers(rgb_img, ir_img):
corners, ids, rejected = aruco.detectMarkers(rgb_img, BoardInfo.aurcoDict)
if len(ids) < 5:
return
_, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(corners, ids, rgb_img, BoardInfo.charucoBoard)
if len(charucoIds)<5:
return
corners, ids, rejected = aruco.detectMarkers(ir_img, BoardInfo.aurcoDict)
if len(ids) < 5:
return
_, charucoCorners_ir, charucoIds_ir = aruco.interpolateCornersCharuco(corners, ids, ir_img, BoardInfo.charucoBoard)
if len(charucoIds_ir)<5:
return
selectionArray, selectionArray_ir, selected_ids = [], [], []
for index, testId in enumerate(charucoIds):
if testId not in charucoIds_ir:
continue
indexInIr = np.where(charucoIds_ir == testId)[0][0]
selectionArray.append(index)
selectionArray_ir.append(indexInIr)
selected_ids.append(testId[0])
if len(selected_ids) < 5:
return
all_charco_corners_camera.append(charucoCorners[selectionArray])
all_charco_corners_ir.append(charucoCorners_ir[selectionArray_ir])
all_real_points.append(BoardInfo.charucoBoard.chessboardCorners[selected_ids])
def getChArucoPose(self, input_image):
corners, ids, _ = aruco.detectMarkers(image=input_image,
dictionary=self.charuco_dict,
parameters=self.aruco_params)
response, chararuco_corners, chararuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=input_image,
board=self.CHARUCO_BOARD)
while response < 4:
raw_input(
'Checkerboard not found, manually update ur pose and try again'
)
color_img, input_image = self.getRosImage()
corners, ids, _ = aruco.detectMarkers(image=input_image,
dictionary=self.charuco_dict,
parameters=self.aruco_params)
response, chararuco_corners, chararuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=input_image,
board=self.CHARUCO_BOARD)
def detect_pts(self, img):
self.corners, ids, self.rejected = detectMarkers(img, self.ardict)
self.N_pts = 0
self.mean_flow = None
if ids is None or ids.size == 0:
self.last_ccorners = None
self.last_cids = None
return
res = interpolateCornersCharuco(self.corners,
ids,
img,
self.board,
minMarkers=self.pt_min_markers)
self.N_pts, self.ccorners, self.cids = res
if self.N_pts == 0:
return
if not np.array_equal(self.last_cids, self.cids):
self.last_ccorners = self.ccorners.reshape(-1, 2)
self.last_cids = self.cids
return
diff = self.last_ccorners - self.ccorners.reshape(-1, 2)
self.mean_flow = np.mean(la.norm(diff, axis=1))
self.last_ccorners = self.ccorners.reshape(-1, 2)
self.last_cids = self.cids
def extractCalibPoints(self, image):
'''
Supports multiple boards.
In reality, only ONE normal board supported.
For multiple boards, "charuco_diamond" is used
'''
all_detected_corners = []
# relative to robot's coordinates
all_board_corners = np.empty((3, 0))
all_ids = []
# This will be needed when optimizing for board pose
points_per_board = [0] * len(self.boards)
# TODO Decide on idx after profiling against enumerate(self.boards)
for idx, board in enumerate(self.boards):
if board.pattern_type == BoardProperties.PatternType.CHARUCO_BOARD or\
board.pattern_type == BoardProperties.PatternType.CHARUCO_DIAMOND:
gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY)
res = ar.detectMarkers(gray, board.dictionary)
if len(res[0]) > 0:
res2 = ar.interpolateCornersCharuco(
res[0], res[1], gray, board.board)
if res2[1] is not None and res2[2] is not None and len(
res2[1]) > 3:
ids = np.array(res2[2]).flatten()
if np.amax(ids) < len(board.board.chessboardCorners):
# make a copy of board 3D points
tempBoardPoints = np.matrix(
board.board.chessboardCorners,
copy=True).transpose()
# we are deleting the none-detected columns using a mask
# id offset may come handy in future ;)
mask = np.zeros(tempBoardPoints.shape[1],
dtype=bool)
mask[:] = False
mask[ids] = True
transformedPoints = Transforms.transformHomography(
board.pose_matrix, tempBoardPoints[:, mask])
# add the data.
all_board_corners = np.concatenate(
(all_board_corners, transformedPoints), axis=1)
points_per_board[idx] = len(ids)
all_detected_corners.append(res2[1])
all_ids.append(res2[2])
else:
raise ValueError(
"Id cannot have higher index than chessboard corners!",
len(ids), len(board.board.chessboardCorners))
else:
raise NotImplementedError("Not implemented for OTHER patterns")
# TODO Decide on idx after profiling against enumerate(self.boards)
return all_detected_corners, all_board_corners, all_ids, points_per_board
def getCharucoData(self, img):
# Grayscale the image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=self.ARUCO_DICT)
if len(corners) > 0:
# Get charuco corners and ids from detected aruco markers
#response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
return aruco.interpolateCornersCharuco(markerCorners=corners,
markerIds=ids,
image=gray,
board=self.CHARUCO_BOARD)
## If a Charuco board was found, let's collect image/corner points
## Requiring the exact number of AR markers
#if response == minNumberOfData:
## Outline the aruco markers found in our query image
#img = aruco.drawDetectedMarkers(
# image=img,
# corners=corners)
## Draw the Charuco board we've detected to show our calibrator the board was properly detected
#img = aruco.drawDetectedCornersCharuco(
# image=img,
# charucoCorners=charuco_corners,
# charucoIds=charuco_ids)
## Reproportion the image, maxing width or height at 1000
#proportion = max(img.shape) / 1000.0
#img = cv2.resize(img, (int(img.shape[1]/proportion), int(img.shape[0]/proportion)))
## Pause to display each image, waiting for key press
#cv2.imshow('Charuco board', img)
#cv2.waitKey(0)
else:
return 0, None, None
def get_calibration_matrices(board, img_arr):
"""
:param board: Charucoboard object to calibrate against
:param img_arr: array of images (from different viewpoints)
:return ret_val: unknown usage
:return camera_matrix: 3X3 camera calibration matrix
:return dist_coeffs: camera's distortion coefficients
"""
dictionary = AeroCubeMarker.get_dictionary()
all_charuco_corners = []
all_charuco_IDs = []
img_size = None
for img in img_arr:
# Convert to grayscale before performing operations
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
corners, IDs, _ = aruco.detectMarkers(gray, dictionary)
_, charuco_corners, charuco_IDs = aruco.interpolateCornersCharuco(
corners, IDs, gray, board)
all_charuco_corners.append(charuco_corners)
all_charuco_IDs.append(charuco_IDs)
# Get matrix shape of grayscale image
img_size = gray.shape
ret_val, camera_matrix, dist_coeffs, _, _ = aruco.calibrateCameraCharuco(
all_charuco_corners, all_charuco_IDs, board, img_size, None, None)
return ret_val, camera_matrix, dist_coeffs
def calibrateWithBoard(self, imgs, sensor, draw=False):
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_4X4_50)
# Create grid board object we're using in our stream
CHARUCO_BOARD = aruco.CharucoBoard_create(
squaresX=10,
squaresY=6,
squareLength=0.04, #in meters
markerLength=0.03, #in meters
dictionary=ARUCO_DICT)
# grayscale image
gray = cv2.cvtColor(imgs[sensor], cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
# Refine detected markers
# Eliminates markers not part of our board, adds missing markers to the board
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(image = gray,
board = CHARUCO_BOARD,
detectedCorners = corners,
detectedIds = ids,
rejectedCorners = rejectedImgPoints,
cameraMatrix = self.intrinsic[sensor],
distCoeffs = self.distCoeffs)
#print('Found {} corners in C{} sensor {}'.format(len(corners), self.camId, sensor))
imgs[sensor] = aruco.drawDetectedMarkers(imgs[sensor], corners, ids=ids, borderColor=(0, 0, 255))
# Only try to find CharucoBoard if we found markers
if ids is not None and len(ids) > 10:
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(markerCorners=corners,
markerIds=ids,
image=gray,
board=CHARUCO_BOARD)
# Require more than 20 squares
if response is not None and response > 20:
# Estimate the posture of the charuco board, which is a construction of 3D space based on the 2D video
pose, self.extrinsic[sensor]['rvec'], self.extrinsic[sensor]['tvec'] = aruco.estimatePoseCharucoBoard(charucoCorners=charuco_corners,
charucoIds=charuco_ids,
board=CHARUCO_BOARD,
cameraMatrix=self.intrinsic[sensor],
distCoeffs=self.distCoeffs)
if draw:
imgs[sensor] = aruco.drawAxis(imgs[sensor], self.intrinsic[sensor], self.distCoeffs, self.extrinsic[sensor]['rvec'], self.extrinsic[sensor]['tvec'], 2)
cv2.imwrite(rospack.get_path('corsmal_benchmark_s2')+'/data/out/calib_C{}_{}.png'.format(self.camId,sensor), imgs[sensor])
else:
print('Calibration board is not fully visible for C{} sensor: {}'.format(self.camId, sensor))
assert 1==0
self.extrinsic[sensor]['rvec'] = cv2.Rodrigues(self.extrinsic[sensor]['rvec'])[0]
self.extrinsic[sensor]['projMatrix'] = np.matmul(self.intrinsic[sensor], np.concatenate((self.extrinsic[sensor]['rvec'],self.extrinsic[sensor]['tvec']), axis=1))
def estimatePoseToBoard(_img, cameraMatrix, distCoeffs):
img = _img.copy()
# grayscale image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
# Refine detected markers
# Eliminates markers not part of our board, adds missing markers to the board
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image=gray,
board=CHARUCO_BOARD,
detectedCorners=corners,
detectedIds=ids,
rejectedCorners=rejectedImgPoints,
cameraMatrix=cameraMatrix,
distCoeffs=distCoeffs)
## REMOVE ID 49 (the robot marker)
corners, ids = removeMarkerById(corners, ids, 49)
img = aruco.drawDetectedMarkers(img,
corners,
ids=ids,
borderColor=(0, 0, 255))
rvec, tvec = None, None
# Only try to find CharucoBoard if we found markers
if ids is not None and len(ids) > 10:
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=CHARUCO_BOARD)
# Require more than 20 squares
if response is not None and response > 20:
# Estimate the posture of the charuco board
pose, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners=charuco_corners,
charucoIds=charuco_ids,
board=CHARUCO_BOARD,
cameraMatrix=cameraMatrix,
distCoeffs=distCoeffs)
img = aruco.drawAxis(img, cameraMatrix, distCoeffs, rvec, tvec, 2)
cv2.imwrite('calib_board.png', img)
else:
print('Calibration board is not fully visible')
assert 1 == 0
return cv2.Rodrigues(rvec)[0], tvec
def calibrate_charucoboard(self, image, display_image):
# ChAruco board variables
CHARUCOBOARD_ROWCOUNT = 5
CHARUCOBOARD_COLCOUNT = 5
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_50)
SQUARE_LENGTH = (1 + 5.0 / 8) * 0.0254
MARKER_LENGTH = (1 + 9.0 / 32) * 0.0254
# Create constants to be passed into OpenCV and Aruco methods
print(CHARUCOBOARD_COLCOUNT > 1, CHARUCOBOARD_ROWCOUNT > 1,
SQUARE_LENGTH > MARKER_LENGTH)
CHARUCO_BOARD = aruco.CharucoBoard_create(
squaresX=CHARUCOBOARD_COLCOUNT,
squaresY=CHARUCOBOARD_ROWCOUNT,
squareLength=SQUARE_LENGTH,
markerLength=MARKER_LENGTH,
dictionary=ARUCO_DICT)
# Create the arrays and variables we'll use to store info like corners and IDs from images processed
corners_all = [] # Corners discovered in all images processed
ids_all = [] # Aruco ids corresponding to corners discovered
image_size = None # Determined at runtime
# Grayscale the image
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=ARUCO_DICT)
# Outline the aruco markers found in our query image
display_image = aruco.drawDetectedMarkers(image=display_image,
corners=corners)
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=CHARUCO_BOARD)
# If a Charuco board was found, let's collect image/corner points
# Requiring at least 20 squares
if response > 5:
# Add these corners and ids to our calibration arrays
corners_all.append(charuco_corners)
ids_all.append(charuco_ids)
# Draw the Charuco board we've detected to show our calibrator the board was properly detected
display_image = aruco.drawDetectedCornersCharuco(
image=display_image,
charucoCorners=charuco_corners,
charucoIds=charuco_ids)
return [], corners_all, display_image
else:
print("Not able to detect a charuco board in image")
return [], [], display_image
def get_corners_aruco(fname, board, skip=20):
cap = cv2.VideoCapture(fname)
length = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
allCorners = []
allIds = []
go = int(skip / 2)
board_type = get_board_type(board)
board_size = get_board_size(board)
max_size = get_expected_corners(board)
for framenum in trange(length, ncols=70):
ret, frame = cap.read()
if not ret:
break
if framenum % skip != 0 and go <= 0:
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
params = aruco.DetectorParameters_create()
params.cornerRefinementMethod = aruco.CORNER_REFINE_CONTOUR
params.adaptiveThreshWinSizeMin = 100
params.adaptiveThreshWinSizeMax = 700
params.adaptiveThreshWinSizeStep = 50
params.adaptiveThreshConstant = 5
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, board.dictionary, parameters=params)
if corners is None or len(corners) <= 2:
go = max(0, go - 1)
continue
detectedCorners, detectedIds, rejectedCorners, recoveredIdxs = \
aruco.refineDetectedMarkers(gray, board, corners, ids,
rejectedImgPoints, parameters=params)
if board_type == 'charuco' and len(detectedCorners) > 0:
ret, detectedCorners, detectedIds = aruco.interpolateCornersCharuco(
detectedCorners, detectedIds, gray, board)
if detectedCorners is not None and \
len(detectedCorners) >= 2 and len(detectedCorners) <= max_size:
allCorners.append(detectedCorners)
allIds.append(detectedIds)
go = int(skip / 2)
go = max(0, go - 1)
cap.release()
return allCorners, allIds
def get_corners(self, cv2_img, cam_id):
gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
if ids is not None and len(ids) > 5:
_, Chcorners, Chids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
self.current_markers[cam_id] = len(Chcorners)
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
_, Chcorners, Chids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
QueryImg = aruco.drawDetectedCornersCharuco(
gray, Chcorners, Chids, (0, 0, 255))
# Display our image
#cv2.imshow('QueryImage', QueryImg)
print("{} markers detected".format(len(ids)))
a = raw_input()
def get_corners(self, cv2_img):
gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
if ids is not None and len(ids) > 5:
_, Chcorners, Chids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
self.current_markers = len(Chcorners)
self.corners_all.append(Chcorners)
self.ids_all.append(Chids)
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
if cols > 60 and rows > 60:
corners, ids, rejectedImgPoints = aruco.detectMarkers(
cv_image, self.aruco_dict, parameters=self.parameters)
#print("aruco")
#print(corners)
if len(corners) > 0:
retval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, cv_image, self.board)
#print(diamondCorners)
img = aruco.drawDetectedCornersCharuco(cv_image,
charucoCorners)
if self.camera_info_read and (not charucoCorners is None):
retval, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners, charucoIds, self.board,
self.camera_matrix, self.dist_coeffs)
#print(retval)
if not rvec is None:
img = aruco.drawAxis(img, self.camera_matrix,
self.dist_coeffs, rvec, tvec, 0.1)
marker_msg = MarkerArray()
marker_msg.header = data.header
marker_msg.markers = []
#print(corners)
if not ids is None:
if len(ids) != 0:
for i in range(len(ids)):
marker_msg.markers.append(Marker())
marker_msg.markers[i].id = int(ids[i])
marker_msg.markers[i].corners = []
for j in range((corners[i]).shape[1]):
marker_msg.markers[i].corners.append(Point2D())
marker_msg.markers[i].corners[j].x = corners[i][0,
j,
0]
marker_msg.markers[i].corners[j].y = corners[i][0,
j,
1]
#cv2.imshow("Image window", cv_image)
#cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
self.marker_pub.publish(marker_msg)
except CvBridgeError as e:
print(e)
def get_corners(cv2_img):
gray_img = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(image=gray_img,
dictionary=aruco_dict)
if ids is not None and len(ids) > 5:
_, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
corners, ids, gray_img, charuco_board)
return charuco_corners, charuco_ids
else:
print("Too few markers detected, skip...")
return None, None
def detect_board():
vs = VideoStream(usePiCamera=True, resolution=res).start()
fps = FPS().start()
time.sleep(1)
freq = 0
i=0
target_x = []
target_y = []
while(i<5000):
tf = time.time()
# Capture frame-by-frame
frame = vs.read()
# Our operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
#print(parameters))
status = "No Marker Detected"
position_dict = {}
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
if ids is not None:
ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board )
# if there are enough corners to get a reasonable result
if( ret > 5 ):
aruco.drawDetectedCornersCharuco(frame,ch_corners,ch_ids,(0,0,255) )
retval, rvec, tvec = aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, board, camera_matrix, dist_coeff )
# if a pose could be estimated
if( retval ) :
frame = aruco.drawAxis(frame,camera_matrix,dist_coeff,rvec,tvec,0.1)
rmat = cv2.Rodrigues(rvec)[0]
position_camera = -np.matrix(rmat).T * np.matrix(tvec)
position_dict[ids[0][0]] = position_camera
status = ("x: %.2f, y: %.2f, z: %.2f")%(position_camera[0],position_camera[1],position_camera[2])
# Display the resulting frame
cv2.putText(frame, status, (20,30), cv2.FONT_HERSHEY_SIMPLEX, .5, (0,0,255),2)
out.write(frame)
#cv2.imshow('frame',gray)
#if cv2.waitKey(1) & 0xFF == ord('q'):
# break
i += 1
freq = .7*freq + .3 * (1/(time.time()-tf))
fps.update()
#print("Operating frequency: %.2f Hz"%freq)
# When everything done, release the capture
out.release()
vs.stop()
cv2.destroyAllWindows()
def getPose(im, board, m, d):
if len(im.shape) > 2:
gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
else:
gray = im
# parameters = aruco.DetectorParameters_create()
# corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary, parameters=parameters)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary)
ret, chcorners, chids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
ret, rvecs, tvecs = aruco.estimatePoseCharucoBoard(chcorners, chids, board,m,d)
im = aruco.drawAxis(im, m, d, rvecs, tvecs,0.5)
cv2.imshow('markers', im)
cv2.waitKey()
def calibrate_camera():
"""
Calibrate our Camera
"""
required_count = 50
resolution = (960, 720)
stream = VideoStream(usePiCamera=True, resolution=resolution).start()
time.sleep(2) # Warm up the camera
all_corners = []
all_ids = []
frame_idx = 0
frame_spacing = 5
success = False
calibration_board = charucoBoard.draw((1680, 1050))
show_calibration_frame(calibration_board)
while True:
frame = stream.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
marker_corners, marker_ids, _ = aruco.detectMarkers(
gray,
charucoDictionary,
parameters=detectorParams)
if len(marker_corners) > 0 and frame_idx % frame_spacing == 0:
ret, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
marker_corners,
marker_ids,
gray,
charucoBoard
)
if charuco_corners is not None and charuco_ids is not None and len(charuco_corners) > 3:
all_corners.append(charuco_corners)
all_ids.append(charuco_ids)
aruco.drawDetectedMarkers(gray, marker_corners, marker_ids)
if cv2.waitKey(1) & 255 == ord('q'):
break
frame_idx += 1
print("Found: " + str(len(all_ids)) + " / " + str(required_count))
if len(all_ids) >= required_count:
success = True
break
def detect_aruco(gray, intrinsics, board):
board_type = get_board_type(board)
# grayb = gray
grayb = cv2.GaussianBlur(gray, (5, 5), 0)
params = aruco.DetectorParameters_create()
params.cornerRefinementMethod = aruco.CORNER_REFINE_CONTOUR
params.adaptiveThreshWinSizeMin = 100
params.adaptiveThreshWinSizeMax = 600
params.adaptiveThreshWinSizeStep = 50
params.adaptiveThreshConstant = 5
corners, ids, rejectedImgPoints = aruco.detectMarkers(
grayb, board.dictionary, parameters=params
)
if intrinsics is None:
INTRINSICS_K = INTRINSICS_D = None
else:
INTRINSICS_K = np.array(intrinsics["camera_mat"])
INTRINSICS_D = np.array(intrinsics["dist_coeff"])
if ids is None:
return [], []
elif len(ids) < 2:
return corners, ids
detectedCorners, detectedIds, rejectedCorners, recoveredIdxs = aruco.refineDetectedMarkers(
gray,
board,
corners,
ids,
rejectedImgPoints,
INTRINSICS_K,
INTRINSICS_D,
parameters=params,
)
if board_type == "charuco" and len(detectedCorners) > 0:
ret, detectedCorners, detectedIds = aruco.interpolateCornersCharuco(
detectedCorners, detectedIds, gray, board
)
if detectedIds is None:
detectedCorners = detectedIds = []
return detectedCorners, detectedIds
def detect_image(self, image, camera=None):
if len(image.shape) == 3:
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
else:
gray = image
corners, ids = self.detect_markers(image, camera, refine=True)
if len(corners) > 0:
ret, detectedCorners, detectedIds = aruco.interpolateCornersCharuco(
corners, ids, gray, self.board)
if detectedIds is None:
detectedCorners = detectedIds = np.float64([])
else:
detectedCorners = detectedIds = np.float64([])
return detectedCorners, detectedIds
def findPointsAndSaveImages(img, img_save_prefex, img_nr):
imgName = img_save_prefex+str(img_nr)
cv2.imwrite(save_path+imgName+".png", img)
corners, ids, rejected = aruco.detectMarkers(img, BoardInfo.charucoBoard.dictionary)
cimg = aruco.drawDetectedMarkers(img.copy(), corners, ids)
cv2.imwrite(save_path+"detected__"+imgName+".png", cimg)
if len(ids) < 10:
return cimg, None, None
numCorners, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(corners, ids, img, BoardInfo.charucoBoard)
cimg = aruco.drawDetectedCornersCharuco(img.copy(), charucoCorners, charucoIds)
cv2.imwrite(save_path+"corners__"+imgName+".png", cimg)
if numCorners <= 0:
return cimg, None, None
return cimg, charucoCorners, charucoIds
def scan(self):
# Get the ROI
roi = self.grayimage[self.roiPt0[1]:self.roiPt1[1],
self.roiPt0[0]:self.roiPt1[0]]
# Detect markers in the image
self.corners, self.ids, self.rejectedImgPoints = aruco.detectMarkers(
roi, self.aruco_dict, parameters=self.parameters)
# If no markers were found.
if self.corners is None or self.ids is None:
self.ids = np.array([])
self.corners = np.array([])
self.charucoCorners = np.array([])
self.charucoIds = np.array([])
else:
retval, self.charucoCorners, self.charucoIds = aruco.interpolateCornersCharuco(
self.corners, self.ids, self.grayimage, self.board)
return
def detect_charuco(frame, charuco_board, aruco_dict):
# grayscale the image
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=aruco_dict)
try:
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=charuco_board)
return charuco_corners, charuco_ids
except:
return None, None
def est_board_pose(self, cv2_img):
gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
if ids is not None and len(ids) > 5:
_, Chcorners, Chids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
retval, rvecs, tvecs = aruco.estimatePoseCharucoBoard(
Chcorners, Chids, charuco_board, self.camera_matrix[:, :],
self.distCoeffs[:, :])
#######################
X_C_M = np.eye(4)
X_C_M[0, 3] = tvecs[0]
X_C_M[1, 3] = tvecs[1]
X_C_M[2, 3] = tvecs[2]
R = cv2.Rodrigues(rvecs[:])[0]
X_C_M[0:3, 0:3] = R
X_C_E = X_C_M.dot(self.X_M_E)
tvecs_new = X_C_E[:3, 3]
rvecs_new = cv2.Rodrigues(X_C_E[0:3, 0:3])[0]
# #####################
QueryImg = aruco.drawDetectedCornersCharuco(
cv2_img, Chcorners, Chids, (0, 0, 255))
if (retval):
QueryImg = aruco.drawAxis(cv2_img, self.camera_matrix[:, :],
self.distCoeffs[:, :], rvecs, tvecs,
0.032)
QueryImg = aruco.drawAxis(QueryImg, self.camera_matrix[:, :],
self.distCoeffs[:, :], rvecs_new,
tvecs_new, 0.032)
# Display our image
# cv2.imshow('QueryImage', QueryImg)
# cv2.waitKey(0)
else:
rvecs = np.zeros((3, 1))
tvecs = np.zeros((3, 1))
cv2.imshow('QueryImage', cv2_img)
print("board not detected")
cv2.waitKey(1)
#print("{} markers detected".format(len(ids)))
self.tvecs = tvecs
self.rvecs = rvecs
def get_fake_calibration_parameters() -> CalibrationParameters:
"""
HACK: Generate fake calibration parameters
"""
dictionary = aruco.getPredefinedDictionary(MarkerType.ARUCO_6X6)
seen_corners = []
seen_ids = []
image_size = (200, 200)
board = aruco.CharucoBoard_create(6, 6, 0.025, 0.0125, dictionary)
image = board.draw(image_size)
for _ in range(15):
corners, ids, _ = aruco.detectMarkers(image, dictionary)
_, corners, ids = aruco.interpolateCornersCharuco(corners, ids, image, board)
seen_corners.append(corners)
seen_ids.append(ids)
ret, mtx, dist, _, _ = aruco.calibrateCameraCharuco(
seen_corners, seen_ids, board, image_size, None, None
)
return CalibrationParameters(mtx, dist, (1280, 720))
def detect_charuco_board_corners(image, CHARUCO_BOARD_SETTINGS_FILE):
"""
:param image:
:param CHARUCO_BOARD_SETTINGS_FILE:
:return: ret, ch_corners, ch_ids of interpolated charuco board detected in image
"""
# read aruco dict num from config file
config_reader = cv2.FileStorage(CHARUCO_BOARD_SETTINGS_FILE,
cv2.FileStorage_READ)
assert config_reader.isOpened()
aruco_dict_num = int(config_reader.getNode('aruco_dict_num').real())
config_reader.release()
# generate aruco dictonary
dictionary = cv2.aruco.getPredefinedDictionary(aruco_dict_num)
# aruco parameters TODO: find out what ever this means
aruco_parameters = aruco.DetectorParameters_create()
# convert image to grayscale
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# detect aruco marker corners
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, dictionary, parameters=aruco_parameters)
# if at least one corner was detected interpolate corners for more acurate result
if (ids is not None):
# load board from settings file
board = load_charuco_board(CHARUCO_BOARD_SETTINGS_FILE)
# interpolate corners
ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(
corners, ids, gray, board)
return ret, ch_corners, ch_ids
# otherwise
logger.info("No corners Detected")
return None, None, None
def get_fake_calibration_parameters(size: int,
iterations: int = 15
) -> CalibrationParameters:
"""
HACK: Generate fake calibration parameters
"""
dictionary = aruco.getPredefinedDictionary(MarkerDict.DICT_6X6_1000)
seen_corners = []
seen_ids = []
image_size = (size, size)
board = aruco.CharucoBoard_create(6, 6, 0.025, 0.0125, dictionary)
for i in range(iterations):
image = board.draw(image_size)
corners, ids, _ = aruco.detectMarkers(image, dictionary)
_, corners, ids = aruco.interpolateCornersCharuco(
corners, ids, image, board)
seen_corners.append(corners)
seen_ids.append(ids)
ret, mtx, dist, _, _ = aruco.calibrateCameraCharuco(
seen_corners, seen_ids, board, image_size, None, None)
return CalibrationParameters(mtx, dist)
def _detect_and_localize_board(self, img, cam):
# detect markers
self.corners, self.ids, rejectedImgPoints = cv2.aruco.detectMarkers(self.img, self.ar_dict)
# refine markers detection using board description
res = aruco.refineDetectedMarkers(self.img, self.gridboard, self.corners, self.ids, rejectedImgPoints, cam.K, cam.D)
detectedCorners, detectedIds, rejectedCorners, recoveredIdxs = res
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=detectedCorners,
markerIds=detectedIds,
image=self.img,
board=self.gridboard)
ret, self.rvec, self.tvec = cv2.aruco.estimatePoseCharucoBoard(charuco_corners, charuco_ids, self.gridboard,
cam.K, cam.D, None, None, False)
self.board_2_cam_T = cv_u.T_of_t_r(self.tvec.squeeze(), self.rvec)
self.ref_2_board_t = [0., 0., -0.005] # ref (track frame) to ar board transform
self.ref_2_board_R = [[1, 0, 0],[0, 1, 0],[0, 0, 1]]
self.ref_2_board_T = cv_u.T_of_t_R(self.ref_2_board_t, self.ref_2_board_R)
self.ref_2_cam_T = np.dot(self.board_2_cam_T, self.ref_2_board_T)
cam.set_pose_T(self.ref_2_cam_T)
rospy.loginfo('found world to cam pose')
def get_charuco_corners(stream):
"""
Given an VideoStream with a charuco calibration board in view,
return a list of the charuco board corners and their corresponding
ids
"""
window_name = 'Calibrating...'
corners = []
ids = []
frame_idx = 0
# Read Images Loop
while True:
#cap_success, frame = stream.read()
frame = stream.read_rgb()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
marker_corners, marker_ids, _ = aruco.detectMarkers(
gray, charucoDictionary, parameters=detectorParams)
if marker_corners and frame_idx % FRAME_SPACING == 0:
_, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
marker_corners, marker_ids, gray, charucoBoard)
if charuco_corners is not None and charuco_ids is not None:
if len(charuco_corners) > 3:
corners.append(charuco_corners)
ids.append(charuco_ids)
print("Found: %d / %d" % (len(ids), NUM_VALID_THRESHOLD))
aruco.drawDetectedMarkers(gray, marker_corners, marker_ids)
cv2.imshow(window_name, gray)
frame_idx += 1
if cv2.waitKey(1) & 255 == ord('q'):
break
if len(ids) >= NUM_VALID_THRESHOLD:
cv2.destroyWindow(window_name)
return corners, ids
def calibrate_charuco(dirpath, image_format, marker_length, square_length):
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000)
board = aruco.CharucoBoard_create(5, 7, square_length, marker_length,
aruco_dict)
arucoParams = aruco.DetectorParameters_create()
counter, corners_list, id_list = [], [], []
img_dir = pathlib.Path(dirpath)
first = 0
# Find the ArUco markers inside each image
for img in img_dir.glob(f'*{image_format}'):
print(f'using image {img}')
image = cv2.imread(str(img))
img_gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = aruco.detectMarkers(img_gray,
aruco_dict,
parameters=arucoParams)
resp, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners, markerIds=ids, image=img_gray, board=board)
# If a Charuco board was found, let's collect image/corner points
# Requiring at least 20 squares
if resp > 20:
# Add these corners and ids to our calibration arrays
corners_list.append(charuco_corners)
id_list.append(charuco_ids)
# Actual calibration
ret, mtx, dist, rvecs, tvecs = aruco.calibrateCameraCharuco(
charucoCorners=corners_list,
charucoIds=id_list,
board=board,
imageSize=img_gray.shape,
cameraMatrix=None,
distCoeffs=None)
return [ret, mtx, dist, rvecs, tvecs]
def get_cam_poses(images, intr, save_path=None):
aruco_dict, board, distCoeffs, cameraMatrix = get_board(intr)
cam_poses = []
for image in images:
gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
gray, aruco_dict)
if len(corners) > 0:
charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, gray, board)
rvec = None
tvec = None
retval, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners, charucoIds, board, cameraMatrix, distCoeffs,
rvec, tvec)
rmat = cv2.Rodrigues(rvec)[0]
trans_mat = np.zeros((4, 4))
trans_mat[3, 3] = 1
trans_mat[:3, :3] = rmat
trans_mat[:3, 3] = tvec[:, 0]
cam_poses.append(trans_mat.flatten())
cam_poses = np.array(cam_poses)
if not save_path:
print(cam_poses)
else:
if not os.path.exists(save_path):
os.makedirs(save_path)
save_path = os.path.join(save_path, 'cam_poses.yaml')
fs_write = cv2.FileStorage(save_path, cv2.FILE_STORAGE_WRITE)
fs_write.write("poses", cam_poses)
fs_write.release()
return cam_poses
def stream_cam_pose(stop_after_image=False):
cam = DepthCam(depth_frame_height=720,
depth_frame_width=1280,
color_frame_height=720,
color_frame_width=1280)
intr = cam.get_intrinsics()
aruco_dict, board, distCoeffs, cameraMatrix = get_board(intr)
fps = 2
while True:
start = time.time()
intr = cam.get_intrinsics()
out = cam.get_frames()
gray = cv2.cvtColor(out['image'], cv2.COLOR_RGB2GRAY)
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
gray, aruco_dict)
imsize = gray.shape
charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, gray, board)
rvec = None
tvec = None
retval, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners, charucoIds, board, cameraMatrix, distCoeffs, rvec,
tvec)
rmat = cv2.Rodrigues(rvec)[0]
trans_mat = np.zeros((4, 4))
trans_mat[3, 3] = 1
trans_mat[:3, :3] = rmat
trans_mat[:3, 3] = tvec[:, 0]
print(trans_mat)
eps = time.time() - start
if eps < 1 / fps:
time.sleep(1 / fps - eps)
if stop_after_image:
input()
board = aruco.CharucoBoard_create(x,y,sqr,mrk,dictionary)
imgs = get_images("cal-imgs/*.png", gray=False)
calcorners = []
calids = []
for im in imgs:
gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
# corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary, parameters=parameters)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary)
# print('ids 1:', len(ids))
# if ids were found, then
if len(ids) > 0:
# print(corners, ids, rejectedImgPoints)
ret, chcorners, chids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
calcorners.append(chcorners)
calids.append(chids)
im2 = im.copy()
aruco.drawDetectedCornersCharuco(im2, chcorners, chids)
# aruco.drawDetectedMarkers(im2, rejectedImgPoints, borderColor=(100, 0, 240))
cv2.imshow('markers', im2)
cv2.waitKey()
ret, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco(calcorners, calids, board, gray.shape[::-1], None, None)
print('rms', ret)
print(cameraMatrix)
print(distCoeffs)
