def __init__(self, nMarkers=8, markerSize=3, markerLength=0.012, markerSeparation=0.003, randomSeed=10, size=(0, 0)):
self.aruco_dict = aruco.custom_dictionary(nMarkers, markerSize, randomSeed)
if size == (0, 0):
self.size = (1, nMarkers)
self.board = aruco.GridBoard_create(1, nMarkers, markerLength, markerSeparation, self.aruco_dict)
else:
self.size = size
self.board = aruco.GridBoard_create(size[0], size[1], markerLength, markerSeparation, self.aruco_dict)
def main(marker_length, marker_separation):
cam = cv2.VideoCapture(0)
ardict = aruco.Dictionary_get(aruco.DICT_4X4_1000)
parameters = aruco.DetectorParameters_create()
# create arUco board
board = aruco.GridBoard_create(4, 5, marker_length, marker_separation,
ardict)
shape = None
counter, corners_list, id_list = [], [], []
while cam.isOpened():
_, frame = cam.read()
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
shape = gray.shape
corners, ids, rejected_img_points = aruco.detectMarkers(
gray, ardict, parameters=parameters)
# rvecs, tvecs, trash = aruco.estimatePoseSingleMarkers(corners, size_of_marker, mtx, dist)
if corners is not None:
aruco.drawDetectedMarkers(frame, corners, ids)
cv2.imshow("123", frame)
k = cv2.waitKey(17)
if k != -1:
print(k)
if k == ord('p'):
if len(corners_list) == 0:
corners_list = corners
id_list = ids
else:
corners_list = np.vstack((corners, corners_list))
id_list = np.vstack((ids, id_list))
counter.append(len(ids))
if k == ord('q'):
cam.release()
break
counter = np.array(counter)
print(corners_list, id_list, counter, shape, sep='\n')
ret, mtx, dist, rvecs, tvecs = aruco.calibrateCameraAruco(
corners_list, id_list, counter, board, shape, None, None)
data = {
'camera_matrix': np.asarray(mtx).tolist(),
'dist_coeff': np.asarray(dist).tolist()
}
f = open("calib.json", 'w')
json.dump(data, f)
f.close()
def __init__(self, cam, cf):
super().__init__(cam, cf)
# Pour le multicast
self.sender = None
self.width = 1280
self.height = 720
self.cap.set(3, self.width)
self.cap.set(4, self.height)
# For validating results, show aruco board to camera.
self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000)
self.aruco_params = aruco.DetectorParameters_create()
# Provide length of the marker's side, measurement unit is centimetre.
self.marker_length = 3.80
# Provide separation between markers, measurement unit is centimetre.
self.marker_separation = 0.5
# create arUco board
self.board = aruco.GridBoard_create(4, 5, self.marker_length,
self.marker_separation,
self.aruco_dict)
# finir le path avec un /
self.path = "/media/data/3D/projets/aruco/cam_data/"
self.count = 0
self.loop = 1
self.t = time.time()
def calibrate_aruco(dirpath, image_format, marker_length, marker_separation):
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000)
arucoParams = aruco.DetectorParameters_create()
board = aruco.GridBoard_create(5, 7, marker_length, marker_separation,
aruco_dict)
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}'):
image = cv2.imread(str(img))
img_gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = aruco.detectMarkers(img_gray,
aruco_dict,
parameters=arucoParams)
if first == 0:
corners_list = corners
id_list = ids
else:
corners_list = np.vstack((corners_list, corners))
id_list = np.vstack((id_list, ids))
first = first + 1
counter.append(len(ids))
counter = np.array(counter)
# Actual calibration
ret, mtx, dist, rvecs, tvecs = aruco.calibrateCameraAruco(
corners_list, id_list, counter, board, img_gray.shape, None, None)
return [ret, mtx, dist, rvecs, tvecs]
def draw_axis_on_board(self):
board = aruco.GridBoard_create(self.MarkerX,self.MarkerY,0.5,0.01,self.aruco_dict)
cam_matrix,dist_coeff,rvecs,tvecs= self.read_cam_matrix(self.outputfile)
im = Imutils_Master()
while True:
img = im.getframe()
gray = im.gray(img)
corners,ids,rejectedImgPoints = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters)
corners1,ids1,rejectedImgPoints1,rec_index = aruco.refineDetectedMarkers(gray,board, corners, ids,rejectedImgPoints)
getcount = self.get_marker_count(ids1)
if getcount>0:
ret,rv,tv = aruco.estimatePoseBoard(corners1,ids1,board,cam_matrix,dist_coeff)
if ret==2:
# print(ret,rv,tv)
# obj_points,imgpoints = aruco.getBoardObjectAndImagePoints(board,corners1,ids1)
# ret,rv1,tv1,inline = cv2.solvePnPRansac(obj_points, imgpoints, cam_matrix, dist_coeff)
# ret,rv1,tv1 = cv2.solvePnP(obj_points, imgpoints, cam_matrix, dist_coeff)
aruco.drawAxis(img,cam_matrix,dist_coeff,rv,tv,0.3)
# imgpts = np.int32(imgpoints[1]).reshape(-1,2)
# # draw ground floor in green
# img = cv2.drawContours(img, [imgpts[:4]],-1,(0,255,0),-3)
# for cor in corners1:
# r,t ,_objpoints = aruco.estimatePoseSingleMarkers(cor,0.5,cam_matrix,dist_coeff)
# # aruco.drawDetectedMarkers(img, corners,ids,(0,255,0))
# if r is not None:
# aruco.drawAxis(img,cam_matrix,dist_coeff,r,t,0.3)
cv2.imshow("frame",img)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
def __init__(self):
# Load calibration file
cal_data = yaml.load(
open(CALIBRATION_FILE_DIR + DEFAULT_CALIB_FILE, 'r'))
self.cmatx = np.asarray(cal_data["camera_matrix"])
self.dist = np.asarray(cal_data["dist_coefficients"])
## GET RID OF ASSUMING LAUNCH FILE PARAMS WORK
# markerLength = .029 # m; determine later
# markerSeparation = .006 # m; determine later
##
# Set up aruco boards
self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
self.aruco_param = aruco.DetectorParameters_create(
) # default parameters
self.aruco_board = aruco.GridBoard_create(numMarkersX, numMarkersY,
markerLength,
markerSeparation,
self.aruco_dict)
self.image_sub = rospy.Subscriber('camera/image_raw', Image,
self.image_callback)
self.pose_pub = rospy.Publisher("aruco/pose_raw", Pose, queue_size=0)
self.bool_pub = rospy.Publisher("aruco/marker_detected",
Bool,
queue_size=0)
self.bridge = CvBridge()
def create_board(start_id):
num_x = 1
num_y = 2
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
board = aruco.GridBoard_create(num_x, num_y, 1, .2, dictionary, start_id)
img = board.draw((800 * num_x, 800 * num_y))
cv2.imwrite('boards/aruco_board%d.jpg' % start_id, img)
def calibration(images, obj_size=(7, 7), obj_dis=30):
""" 进行相机标定
images: RGB格式的一些图片数组
"""
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_50)
parameters = aruco.DetectorParameters_create()
board = aruco.GridBoard_create(2, 2, 0.08, 0.01, aruco_dict)
charucoCorners = []
charucoIds = []
marknums = []
for img in images:
img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
img, aruco_dict, parameters=parameters)
corners, ids, rejectedImgPoints, recoveryIdxs = aruco.refineDetectedMarkers(
img, board, corners, ids, rejectedImgPoints)
if ids is not None:
charucoCorners.extend(corners)
charucoIds.extend(ids)
marknums.append(len(ids))
charucoCorners = np.concatenate(charucoCorners)
charucoIds = np.concatenate(charucoIds)
marknums = np.array(marknums)
ret, mtx, dist, rvecs, tvecs, _, _, perViewErrors = aruco.calibrateCameraArucoExtended(
charucoCorners, charucoIds, marknums, board, img.shape[:2][::-1], None, None)
print(ret)
print("mtx:\n", mtx) # 内参数矩阵
print("dist:\n", dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
# print("rvecs:\n", rvecs[0]) # 旋转向量 # 外参数
# print("tvecs:\n", tvecs[0]) # 平移向量 # 外参数
mean_error = np.mean(perViewErrors)
print("total error: {:.4f}(pixel)".format(mean_error))
return mtx, dist, mean_error
def get_calibration_board(config):
board_size = config['calibration']['board_size']
board_type = config['calibration']['board_type'].lower()
if board_type in ['aruco', 'charuco']:
dkey = (config['calibration']['board_marker_bits'],
config['calibration']['board_marker_dict_number'])
dictionary = aruco.getPredefinedDictionary(ARUCO_DICTS[dkey])
if board_type == 'aruco':
board = aruco.GridBoard_create(
board_size[0], board_size[1],
config['calibration']['board_marker_length'],
config['calibration']['board_marker_separation_length'],
dictionary)
elif board_type == 'charuco':
board = aruco.CharucoBoard_create(
board_size[0], board_size[1],
config['calibration']['board_square_side_length'],
config['calibration']['board_marker_length'], dictionary)
elif board_type == 'checkerboard':
board = Checkerboard(board_size[0], board_size[1],
config['calibration']['board_square_side_length'])
else:
raise ValueError(
"board_type should be one of "
"'aruco', 'charuco', or 'checkerboard' not '{}'".format(
board_type))
return board
def example_calibrate_folder(folder_in, folder_out,marker_length_mm = 3.75, marker_space_mm = 0.5,dct = aruco.DICT_6X6_1000):
tools_IO.remove_files(folder_out)
num_cols, num_rows = 4, 5
board = aruco.GridBoard_create(num_cols, num_rows, marker_length_mm, marker_space_mm,aruco.getPredefinedDictionary(dct))
filenames = numpy.unique(tools_IO.get_filenames(folder_in, '*.jpg,*.png'))[:3]
counter, corners_list, id_list, first = [], [], [], True
for filename_in in filenames:
base_name, ext = filename_in.split('/')[-1].split('.')[0], filename_in.split('/')[-1].split('.')[1]
image = cv2.imread(folder_in + filename_in)
img_gray = cv2.cvtColor(image,cv2.COLOR_RGB2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(img_gray, aruco.getPredefinedDictionary(dct))
if first == True:
corners_list = corners
id_list = ids
first = False
else:
corners_list = numpy.vstack((corners_list, corners))
id_list = numpy.vstack((id_list,ids))
counter.append(len(ids))
image_temp = tools_image.desaturate(image.copy())
aruco.drawDetectedMarkers(image_temp, corners)
cv2.imwrite(folder_out+base_name+'.png',image_temp)
print(base_name)
counter = numpy.array(counter)
ret, camera_matrix, dist, rvecs, tvecs = aruco.calibrateCameraAruco(corners_list, id_list, counter, board, img_gray.shape, None, None )
print(camera_matrix)
return
def detect_board():
num_x = 2
num_y = 2
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
board = aruco.GridBoard_create(num_x, num_y, .0932, .01869, dictionary)
vs = VideoStream(usePiCamera=True, resolution=res).start()
fps = FPS().start()
time.sleep(1)
freq = 0
i = 0
target_x = []
target_y = []
while (i < 100):
tf = time.time()
# Capture frame-by-frame
frame = vs.read()
# Our operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
parameters = aruco.DetectorParameters_create()
# print(parameters)
status = "No Marker Detected"
position_dict = {}
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, dictionary, parameters=parameters)
if ids is not None:
frame = aruco.drawDetectedMarkers(frame, corners, ids)
l, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
camera_matrix, dist_coeff)
print(rvec)
print(tvec)
frame = aruco.drawAxis(frame, camera_matrix, dist_coeff, rvec,
tvec, .1)
camera_points = cv2.projectPoints(
np.array([np.array([0, 0, 1.0]).T]), rvec, tvec, camera_matrix,
dist_coeff)
camera_points = np.array(camera_points[0][0][0])
status = "Target detect, x: %.2f, y: %.2f" % (camera_points[0],
camera_points[1])
if np.linalg.norm(camera_points) <= 10e6:
cv2.circle(frame,
(int(camera_points[0]), int(camera_points[1])), 6,
(255, 0, 0))
# Display the resulting frame
cv2.putText(frame, status, (20, 30), cv2.FONT_HERSHEY_SIMPLEX, .5,
(0, 0, 255), 2)
out.write(frame)
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 talker():
# generate the aruco board
# note this function is deterministic so it will match the board
# generated by `create_board.py` as long as the parameters from
# `consts.py` don't change.
board = aruco.GridBoard_create(consts.ROWS, consts.COLS,
consts.markerLength,
consts.markerSeparation, consts.aruco_dict)
# init video capture
cap = cv2.VideoCapture(0)
# init ros publisher node
pub = rospy.Publisher('chatter', String, queue_size=10)
rospy.init_node('talker_1')
rate = rospy.Rate(10)
# get existing calibration data for pose estimation
mtx = extract_calibration.camera_matrix
dist = extract_calibration.dist_matrix
font = cv2.FONT_HERSHEY_SIMPLEX
while not rospy.is_shutdown():
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters_create()
# we need to detect the markers before trying to detect the board
corners, ids, rejected = aruco.detectMarkers(gray,
aruco_dict,
parameters=parameters)
aruco.refineDetectedMarkers(gray, board, corners, ids, rejected)
if np.all(ids != None):
# estimate pose
retval, rvec, tvec = aruco.estimatePoseBoard(
corners, ids, board, mtx, dist)
info = [ids, corners, rvec, tvec, retval]
pub.publish(str(info))
if DEBUG:
rospy.loginfo(str(info))
if DISPLAY:
aruco.drawAxis(frame, mtx, dist, rvec, tvec, 0.1)
aruco.drawDetectedmarkers(frame, corners, ids)
cv2.putText(frame, "ID: " + str(ids), (0, 64), font, 1,
(0, 255, 0), 2, cv2.LINE_AA)
cv2.imshow('frame', frame)
rate.sleep()
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# release the capture
cap.release()
cv2.destroyAllWindows()
def aruco():
print("getting data from file")
cam_matrix, dist_coefs, rvecs, tvecs = get_cam_matrix(
"camera_matrix_aruco.yaml")
# aruco data
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
parameters = aruco.DetectorParameters_create()
corner_points = []
count = 1
img = vs.read()
height, width, channels = img.shape
out = cv2.VideoWriter('outpy1.avi',
cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), 30,
(width, height))
while True:
count = 1
img = get_frame(vs)
gray = convert_gray(img)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
if ids is not None and corners is not None:
# obj = aruco.drawDetectedMarkers(img, corners,ids,(255,0,0))
for cor in corners:
r, t, _objpoints = aruco.estimatePoseSingleMarkers(
cor, 0.5, cam_matrix, dist_coefs)
# aruco.drawAxis(img,cam_matrix,dist_coefs,r,t,0.2)
# if len(corners) == len(ids):
board = aruco.GridBoard_create(6, 8, 0.05, 0.01, aruco_dict)
corners, ids, rejectedImgPoints, rec_idx = aruco.refineDetectedMarkers(
gray, board, corners, ids, rejectedImgPoints)
ret, rv, tv = aruco.estimatePoseBoard(corners, ids, board, cam_matrix,
dist_coefs)
if ret:
aruco.drawAxis(img, cam_matrix, dist_coefs, rv, tv, 0.2)
obj_points, imgpoints = aruco.getBoardObjectAndImagePoints(
board, corners, ids)
# imgpts = np.int32(imgpoints).reshape(-1,2)
# # draw ground floor in green
# img = cv2.drawContours(img, [imgpts[:4]],-1,(0,255,0),3)
# out.write(img)
cv2.imshow("Markers", img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
out.release()
cv2.destroyAllWindows()
def arucodetect(QueryImg):
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_6X6_1000)
# Create grid board object we're using in our stream
board = aruco.GridBoard_create(
markersX=2,
markersY=2,
markerLength=0.09,
markerSeparation=0.01,
dictionary=ARUCO_DICT)
gray = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
# Make sure all 5 markers were detected before printing them out
if ids is not None:
#pos0 = (corners[0][0][2]+corners[0][0][3])/2
areas = numpy.array(list(map(cv2.contourArea,corners[0])))
length = int(math.sqrt(areas))
perspective1 = numpy.float32([corners[0][0][3],corners[0][0][2],corners[0][0][1],corners[0][0][0]])
perspective2 = numpy.float32([[0, 0],[length, 0],[length, length],[0, length]])
pos = (length/2,0)
psp_matrix = cv2.getPerspectiveTransform(perspective1,perspective2)
rev_psp_matrix = numpy.linalg.inv(psp_matrix)
#pos.reshape((2,1))
pos1 = numpy.append (pos, [1])
pos2 = numpy.dot(rev_psp_matrix ,pos1)
pos2 = pos2 / pos2[2]
pos2 = numpy.delete(pos2,2,0)
pos2 = pos2.astype(numpy.int)
# Print corners and ids to the console
#for i, corner in zip(ids, corners):
#print('ID: {}; Corners: {}'.format(i, corner))
#print(pos2)
# Outline all of the markers detected in our image
QueryImg = aruco.drawDetectedMarkers(QueryImg, corners, borderColor=(0, 0, 255))
QueryImg = cv2.circle(QueryImg,tuple(pos2),5,(255,0,0),-1)
#QueryImg = cv2.circle(QueryImg,tuple(pos0),5,(255,0,255),-1)
else:
pos2 = (0,0)
return pos2, QueryImg
def __init__(self,
aoi_label,
aoi_id,
aruco_detector,
markerLength=0.1,
markerSeparation=0.1,
width=1920,
height=1080,
markers_x=3,
markers_y=2,
aruco_dict=aruco.Dictionary_get(aruco.DICT_4X4_100)):
self.aoi_label = aoi_label
self.aoi_id = aoi_id
self.markerLength = markerLength
self.markerSeparation = markerSeparation
self.__width__ = width
self.__height__ = height
self.__markers_x__ = markers_x
self.__markers_y__ = markers_y
self.__aruco_dict__ = aruco_dict
self.__margin_size__ = 40
if self.__markers_x__ > 1 and self.__markers_y__ > 1:
self.feature_3dpoints = np.array(
[[
self.markerLength, self.__markers_y__ * self.markerLength +
(self.__markers_y__ - 1) * self.markerSeparation, 0.0
],
[(self.__markers_x__ - 1) * self.markerLength +
(self.__markers_x__ - 1) * self.markerSeparation,
self.__markers_y__ * self.markerLength +
(self.__markers_y__ - 1) * self.markerSeparation, 0.0],
[self.markerLength, 0.0, 0.0],
[(self.__markers_x__ - 1) * self.markerLength +
(self.__markers_x__ - 1) * self.markerSeparation, 0.0, 0.0]])
elif self.__markers_x__ == 1 and self.__markers_y__ == 1:
self.feature_3dpoints = np.array(
[[0.0, self.markerLength, 0.0],
[self.markerLength, self.markerLength, 0.0], [0.0, 0.0, 0.0],
[self.markerLength, 0.0, 0.0]])
self.__aoi__ = aruco.GridBoard_create(
markersX=self.__markers_x__,
markersY=self.__markers_y__,
markerLength=self.markerLength,
markerSeparation=self.markerSeparation,
dictionary=self.__aruco_dict__,
firstMarker=aoi_id * self.__markers_x__ * self.__markers_y__)
corners, self.__ids__ = aruco_detector.getCorners(
self.getCVMat(), self.__aruco_dict__)
self.features_2dpoints = aruco_detector.getDetectedFeatures(
self, corners, self.__ids__)
def getParameter(self, configFile):
fs = cv2.FileStorage(configFile, cv2.FileStorage_READ)
self.marker_X = int(fs.getNode("marker_X").real())
self.marker_Y = int(fs.getNode("marker_Y").real())
self.markerSeparation = fs.getNode("markerSeparation").real()
self.tag_size = fs.getNode("tag_size").real()
self.dictionary = aruco.Dictionary_get(aruco.DICT_6X6_100)
self.board = aruco.GridBoard_create(self.marker_X, self.marker_Y,
self.tag_size,
self.markerSeparation,
self.dictionary, 0)
self.parameters = aruco.DetectorParameters_create()
def detect_markers(img, params, root_dir, file_name):
o_path = os.path.join(root_dir, "tmp")
if not os.path.exists(o_path):
os.makedirs(o_path)
#######
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000)
path_board_size = os.path.join(root_dir, "board_size.txt")
assert os.path.exists(path_board_size), path_board_size
board_size = np.loadtxt(path_board_size)
print("[*] board_size : {}".format(board_size))
markerLength = board_size[0]
markerSeparation = board_size[1]
board = aruco.GridBoard_create(5, 7, markerLength, markerSeparation, aruco_dict)
####
intrinsic = params["intrinsic"]
dist = params["dist"]
arucoParams = aruco.DetectorParameters_create()
img = cv2.undistort(img, intrinsic, dist)
img_gray = cv2.cvtColor(img[:, :, ::-1], cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(img_gray, aruco_dict, parameters=arucoParams)
corners, ids, rej, recovered_ids = aruco.refineDetectedMarkers(img_gray, board, corners, ids, rejectedImgPoints)
dist[:] = 0
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, intrinsic, dist)
R, _ = cv2.Rodrigues(rvec)
tmp_img = aruco.drawDetectedMarkers(img[:, :, ::-1].copy(), corners, ids, (0, 255, 0))
tmp_img = aruco.drawAxis(tmp_img, intrinsic, dist, rvec, tvec, 100)
cv2.imwrite(os.path.join(o_path, "{}_marker_detected.png".format(file_name)), tmp_img)
########
marker_coordinates = np.zeros(shape=(5 * 7, 4, 2))
for index in range(len(ids)):
try:
marker_coordinates[ids[index], :, :] = corners[index].reshape(4, 2)
except Exception as e:
print("ERROR: ", end=" ")
print(ids[index], corners[index])
board_objPoints = np.array(board.objPoints)
return marker_coordinates, board_objPoints, R, tvec
def generateArucoBoard(self, rows=3, columns=4):
# Create the board
board = aruco.GridBoard_create(
markersX=columns,
markersY=rows,
markerLength=0.1,
markerSeparation=0.1/2,
dictionary=self.arucoDict)
img = board.draw((175*columns, 175*rows))
# Show the image
cv2.imshow('ArUco Board', img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Save it to a file
cv2.imwrite('ArUcoBoard.png', img)
def ShowGridBoard():
# Create gridboard, which is a set of Aruco markers
# the following call gets a board of markers 5 wide X 7 tall
gridboard = aruco.GridBoard_create(markersX=1,
markersY=1,
markerLength=0.09,
markerSeparation=0.01,
dictionary=aruco.Dictionary_get(
aruco.DICT_6X6_1000))
# Create an image from the gridboard
#img = gridboard.draw(outSize=(988, 1400))
img = gridboard.draw(outSize=(800, 800))
#cv2.imwrite("test_gridboard.jpg", img)
## Display the image to us
ShowImage(img)
return img
def __init__(self):
self.cap = cv2.VideoCapture(0)
if self.cap:
h = 7
w = 6
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30,
0.001)
objp = np.zeros((w * h, 3), np.float32)
objp[:, :2] = np.mgrid[0:h, 0:w].T.reshape(-1, 2)
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
k = 1
count = 1
while (count < 11):
re, frame = self.cap.read()
img = frame
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, gray = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (h, w), None)
cv2.imshow('img', gray)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11),
(-1, -1), criteria)
imgpoints.append(corners2)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (h, w), corners2, ret)
cv2.imshow('img', img)
count += 1
k = cv2.waitKey(0)
self.ret, self.mtx, self.dist, self.rvecs, self.tvecs = cv2.calibrateCamera(
objpoints, imgpoints, gray.shape[::-1], None, None)
self.dictionary = aruco.Dictionary_get(aruco.DICT_6X6_250)
self.parameters = aruco.DetectorParameters_create()
self.board = aruco.GridBoard_create(5, 7, 0.04, 0.01,
self.dictionary)
img = self.board.draw((600, 500), marginSize=10, borderBits=1)
cv2.imshow('img', img)
cv2.waitKey(5)
def __init__(self):
rospy.init_node('demonstration_recorder')
rospy.Rate(150)
self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_1000)
#Provide length of the marker's side
markerLength = 3.7 # Here, measurement unit is centimetre.
# Provide separation between markers
markerSeparation = 0.48 # Here, measurement unit is centimetre.
# create arUco board
self.board = aruco.GridBoard_create(4, 5, markerLength,
markerSeparation, self.aruco_dict)
'''uncomment following block to draw and show the board'''
#img = board.draw((864,1080))
#cv2.imshow("aruco", img)
self.arucoParams = aruco.DetectorParameters_create()
self.tf = tf.TransformListener()
#check if camera is working or not
self.camera = cv2.VideoCapture(1)
ret, img = self.camera.read()
cv2.imshow("debug", img)
with open('../data/' + 'camera_calibration_asus.yaml') as f:
loadeddict = yaml.load(f)
mtx = loadeddict.get('camera_matrix')
dist = loadeddict.get('dist_coeff')
self.mtx = np.array(mtx)
self.dist = np.array(dist)
img_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
h, w = img_gray.shape[:2]
self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix(
self.mtx, self.dist, (w, h), 1, (w, h))
self.path_publisher = rospy.Publisher('/recorded_path',
Path,
queue_size=1)
rospy.Subscriber('/demonstration_recorder_event_in',
std_msgs.msg.String, self.event_in_cb)
self.event_in = None
self.state = 'INIT'
def __init__(self, targ, dictionary):
#generic properties
self.properties = targ
self.corners = None
self.r_mat = None
self.t_vec = None
self.position = None
self.yaw = 0
self.camera_frame_loc = None
self.d_cam_image = None
self.props_are_set = False
self.yaw = None
self.board = aruco.GridBoard_create(self.properties["num_x"],
self.properties["num_y"],
self.properties["size_square"],
self.properties["space_between"],
dictionary,
self.properties["marker_start"])
self.ids = self.board.ids
def example_calibrate_aruco_markers(filename_in, marker_length_mm = 3.75, marker_space_mm = 0.5, dct = aruco.DICT_6X6_1000):
image = cv2.imread(filename_in)
gray = tools_image.desaturate(image)
scale = (marker_length_mm / 2, marker_length_mm / 2, marker_length_mm / 2)
num_cols, num_rows = 4,5
board = aruco.GridBoard_create(num_cols, num_rows, marker_length_mm, marker_space_mm,aruco.getPredefinedDictionary(dct))
image_AR, image_cube = gray.copy(), gray.copy()
base_name, ext = filename_in.split('/')[-1].split('.')[0], filename_in.split('/')[-1].split('.')[1]
#board_width_px = int((num_cols * marker_length_mm + (num_cols - 1) * marker_space_mm))
#board_height_px= int((num_rows * marker_length_mm + (num_rows - 1) * marker_space_mm))
#image_board = aruco.drawPlanarBoard(board, (board_width_px, board_height_px))
camera_matrix = None#tools_pr_geom.compose_projection_mat_3x3(image.shape[1], image.shape[0])
corners, ids, _ = aruco.detectMarkers(cv2.cvtColor(image, cv2.COLOR_BGR2GRAY), aruco.getPredefinedDictionary(dct))
if len(corners)>0:
if len(corners)==1:corners,ids = numpy.array([corners]),numpy.array([ids])
else:corners, ids = numpy.array(corners), numpy.array(ids)
counters = numpy.array([len(ids)])
ret, camera_matrix, dist, rvecs, tvecs = aruco.calibrateCameraAruco(corners,ids,counters, board, gray.shape[:2],None, None)
image_markers = [tools_image.saturate(aruco.drawMarker(aruco.getPredefinedDictionary(dct), id, 100)) for id in ids]
#!!!
#camera_matrix = tools_pr_geom.compose_projection_mat_3x3(4200,4200)
for i,image_marker in enumerate(image_markers):
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners[i], marker_length_mm, camera_matrix, numpy.zeros(5))
image_AR = tools_render_CV.draw_image(image_AR,image_marker, camera_matrix, numpy.zeros(5), numpy.array(rvecs).flatten(), numpy.array(tvecs).flatten(),scale)
image_cube = tools_render_CV.draw_cube_numpy(image_cube, camera_matrix, numpy.zeros(5), numpy.array(rvecs).flatten(),numpy.array(tvecs).flatten(), scale)
cv2.imwrite(folder_out + base_name+'_AR.png', image_AR)
cv2.imwrite(folder_out + base_name+'_AR_cube.png', image_cube)
print(camera_matrix)
return camera_matrix
def update_frame(self, frame):
self.frame = frame
self.error = ''
self.corners, self.ids, self.rejectedImgPoints = aruco.detectMarkers(
self.frame, self.aruco_dict, parameters=self.parameters)
self.rvecs, self.tvecs, self._corners = aruco.estimatePoseSingleMarkers(
self.corners, self.markerLength, self.cameraMatrix,
self.distCoeffs)
self.corners_tracker, self.ids_tracker, _ = aruco.detectMarkers(
self.frame, self.tracker_dict, parameters=self.parameters)
self.rvecs_tracker, self.tvecs_tracker, _ = aruco.estimatePoseSingleMarkers(
self.corners_tracker, markerLength_tracker, self.cameraMatrix,
self.distCoeffs)
size = self.arucoTrackerBoard.getBoardSize()
board = aruco.GridBoard_create(size[0], size[1], markerLength_tracker,
markerSeparation_tracker,
self.tracker_dict)
_, self.rvec_tracker, self.tvec_tracker = aruco.estimatePoseBoard(
self.corners_tracker, self.ids_tracker, board, self.cameraMatrix,
self.distCoeffs)
def get_calibration_board(config):
board_size = config["calibration"]["board_size"]
board_type = config["calibration"]["board_type"].lower()
if board_type in ["aruco", "charuco"]:
dkey = (
config["calibration"]["board_marker_bits"],
config["calibration"]["board_marker_dict_number"],
)
dictionary = aruco.getPredefinedDictionary(ARUCO_DICTS[dkey])
if board_type == "aruco":
board = aruco.GridBoard_create(
board_size[0],
board_size[1],
config["calibration"]["board_marker_length"],
config["calibration"]["board_marker_separation_length"],
dictionary,
)
elif board_type == "charuco":
board = aruco.CharucoBoard_create(
board_size[0],
board_size[1],
config["calibration"]["board_square_side_length"],
config["calibration"]["board_marker_length"],
dictionary,
)
elif board_type == "checkerboard":
board = Checkerboard(
board_size[0],
board_size[1],
config["calibration"]["board_square_side_length"],
)
else:
raise ValueError(
"board_type should be one of "
"'aruco', 'charuco', or 'checkerboard' not '{}'".format(
board_type))
return board
def eye_arm_calibrate(images, urs, mtx, dist, eye_in_hand=False, show=True):
# 创建标定板模型
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_50)
parameters = aruco.DetectorParameters_create()
board = aruco.GridBoard_create(2, 2, 0.08, 0.01, aruco_dict)
# 从图像中得到标定板坐标系到相机坐标系的变换
cam = []
for img in images:
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, mtx,
dist, None, None)
cam.append(to_matrix(np.squeeze(np.r_[tvec, rvec])))
# urs是工具坐标系到基座坐标系的变换,在eye to hand的时候用的是基座坐标到手坐标的变换,要求逆
urs = [to_matrix(s) for s in urs]
if not eye_in_hand:
urs = [np.linalg.inv(s) for s in urs]
# calibrateHandEye函数的输入要分开旋转矩阵和平移向量
R_gripper2base = np.array([pos[:3, :3] for pos in urs])
t_gripper2base = np.array([pos[:3, 3] for pos in urs])
R_target2cam = np.array([pos[:3, :3] for pos in cam])
t_target2cam = np.array([pos[:3, 3] for pos in cam])
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
R_gripper2base,
t_gripper2base,
R_target2cam,
t_target2cam,
method=cv2.CALIB_HAND_EYE_PARK)
# 把输出还原为4x4变换矩阵
cam2base = np.eye(4)
cam2base[:3, :3] = np.squeeze(R_cam2gripper)
cam2base[:3, 3] = np.squeeze(t_cam2gripper)
print(cam2base)
if show:
display(cam2base, eye_in_hand)
return cam2base
def main(image_dir):
""" Main function
:image_dir: str
"""
# For validating results, show aruco board to camera.
aruco_dict = aruco.getPredefinedDictionary(ARUCO_DICT)
# create arUco board
board = aruco.GridBoard_create(4, 4, MARKERLENGTH, MARKERSEPARATION,
aruco_dict)
'''uncomment following block to draw and show the board'''
# img = board.draw((864,1080))
# cv2.imshow("aruco", img)
aruco_params = aruco.DetectorParameters_create()
if CALIBRATE_CAMERA:
calibrate_camera(image_dir, board, aruco_dict, aruco_params)
else:
validate_results(board, aruco_dict, aruco_params)
def eye_arm_calibrate(images, urs, mtx, dist, eye_in_hand=False):
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_50)
parameters = aruco.DetectorParameters_create()
board = aruco.GridBoard_create(2, 2, 0.08, 0.01, aruco_dict)
cam = []
for img in images:
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, mtx,
dist, None, None)
cam.append(to_matrix(np.squeeze(np.r_[tvec, rvec])))
urs = [to_matrix(s) for s in urs]
if not eye_in_hand:
urs = [np.linalg.inv(s) for s in urs]
# cam = [np.linalg.inv(s) for s in cam]
R_gripper2base = np.array([pos[:3, :3] for pos in urs])
t_gripper2base = np.array([pos[:3, 3] for pos in urs])
R_target2cam = np.array([pos[:3, :3] for pos in cam])
t_target2cam = np.array([pos[:3, 3] for pos in cam])
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
R_gripper2base,
t_gripper2base,
R_target2cam,
t_target2cam,
method=cv2.CALIB_HAND_EYE_PARK)
cam2base = np.eye(4)
cam2base[:3, :3] = np.squeeze(R_cam2gripper)
cam2base[:3, 3] = np.squeeze(t_cam2gripper)
print(cam2base)
cam2world = base2world.dot(cam2base)
print(cam2world)
zz = cam2world.dot([0, 0, 1, 1])[:3]
print(zz / np.linalg.norm(zz))
display(cam2world)
return cam2base
def make_gridboards(nx, ny, marker_size, between_markers, aruco_dict,
num_gridboards, save_dir):
gridboards = []
for i in range(num_gridboards):
offset = i * nx * ny
gridboards.append(
aruco.GridBoard_create(nx,
ny,
marker_size,
between_markers,
aruco_dict,
firstMarker=offset))
if save_dir:
imboard = gridboards[i].draw((500, 500))
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
title = f"{save_dir}/{nx}x{ny}_{offset}.png"
plt.title(
f"{title}\nmarker_size={marker_size}m, between_markers={between_markers}m"
)
ax.axis("off")
plt.imshow(imboard, cmap=mpl.cm.gray, interpolation="nearest")
plt.savefig(title)
return gridboards
def __init__(self):
# Check for camera calibration data
if not os.path.exists('./calibration/CameraCalibration.pckl'):
print(
"You need to calibrate the camera you'll be using. See calibration project directory for details."
)
exit()
else:
calib = open('./calibration/CameraCalibration.pckl', 'rb')
(camera_matrix, dist_coeffs, _, _) = pickle.load(calib)
calib.close()
if camera_matrix is None or dist_coeffs is None:
print(
"Calibration issue. Remove ./calibration/CameraCalibration.pckl and recalibrate your camera with calibration_ChAruco.py."
)
exit()
self.camera_matrix = camera_matrix
self.dist_coeffs = dist_coeffs
# Constant parameters used in Aruco methods
self.aruco_params = aruco.DetectorParameters_create()
self.aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_50)
# Create grid board object we're using in our stream
self.board = aruco.GridBoard_create(markersX=1,
markersY=1,
markerLength=0.09,
markerSeparation=0.01,
dictionary=self.aruco_dict)
# Create vectors we'll be using for rotations and translations for postures
self.rotation_vectors, self.translation_vectors = None, None
self.axis = np.float32([[-.5, -.5, 0], [-.5, .5, 0], [.5, .5, 0],
[.5, -.5, 0], [-.5, -.5, 1], [-.5, .5, 1],
[.5, .5, 1], [.5, -.5, 1]])
