def estimate_camera_pose(camera_matrix: list[list[float]],
dist_matrix: list[float], image: Image.Image,
marker_size: float) -> dict[int, Pose]:
camera_matrix_arr, dist_matrix_arr, gray, corners, ids = detect_corners(
camera_matrix, dist_matrix, image, refine=True)
ret: dict[int, Pose] = {}
if np.all(ids is None):
return ret
# TODO do not perform estimation for un-configured markers
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix_arr,
dist_matrix_arr)
rvec = rvec.reshape(len(ids), 3)
tvec = tvec.reshape(len(ids), 3)
if __debug__:
backtorgb = cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB)
aruco.drawDetectedMarkers(backtorgb,
corners) # Draw A square around the markers
for idx in range(len(ids)):
aruco.drawAxis(backtorgb, camera_matrix_arr, dist_matrix_arr,
rvec[idx], tvec[idx], 0.15)
cv2.imwrite("marker.jpg", backtorgb)
for idx, mid in enumerate(ids):
# convert pose of the marker wrt camera to pose of camera wrt marker
# based on https://stackoverflow.com/a/51515560/3142796
marker_rot_matrix, _ = cv2.Rodrigues(rvec[idx])
assert np.allclose(np.linalg.inv(marker_rot_matrix),
marker_rot_matrix.transpose())
assert math.isclose(np.linalg.det(marker_rot_matrix), 1)
camera_rot_matrix = marker_rot_matrix.transpose()
camera_trans_vector = np.matmul(-camera_rot_matrix,
tvec[idx].reshape(3, 1)).flatten()
o = Orientation.from_quaternion(
quaternion.from_rotation_matrix(camera_rot_matrix))
ret[mid[0]] = Pose(
Position(camera_trans_vector[0], camera_trans_vector[1],
camera_trans_vector[2]), o)
return ret
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 image_processing_callback(self, msg):
frame = self.bridge.imgmsg_to_cv2(msg, "rgb8")
height = msg.height
width = msg.width
self.number_of_pixels = height
matrix_coefficients = np.mat([[1.0, 0.0, height / 2.0],
[0.0, 1.0, width / 2.0],
[0.0, 0.0, 1.0]])
distortion_coefficients = np.mat([0.0, 0.0, 0.0, 0.0])
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Change grayscale
# Use 5x5 dictionary to find markers
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
# Marker detection parameters
parameters = aruco.DetectorParameters_create()
# lists of ids and the corners beloning to each id
corners, ids, _ = aruco.detectMarkers(
gray,
aruco_dict,
parameters=parameters,
cameraMatrix=matrix_coefficients,
distCoeff=distortion_coefficients)
print(ids, ": ID of AR-tag")
if np.all(ids is not None): # If there are markers found by detector
for i in range(0, len(ids)): # Iterate in markers
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners[i],
0.1,
matrix_coefficients,
distortion_coefficients)
# Draw A square around the markers
aruco.drawDetectedMarkers(frame, corners)
aruco.drawAxis(frame, matrix_coefficients,
distortion_coefficients,
rvec,
tvec,
0.1) # Draw Axis
top = corners[0][0][0][0]
bottom = corners[0][0][2][0]
if abs(top - bottom) > self.reach_threshold:
self.reached = True
self.get_logger().info('Reached the goal')
else:
self.reached = False
self.translation_x = tvec[0][0][0]
self.stop = False
else:
self.translation_x = 0
self.stop = True
self.processedImage_publish.publish(self.bridge.cv2_to_imgmsg(frame,
"rgb8"))
self.regulate_direction()
def talker():
pub2 = rospy.Publisher('aruco_pose', Pose, queue_size=30)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(30) # 30hz
j = 0
while not rospy.is_shutdown():
ret, frame = cap.read()
j = j + 1
print(j, "j")
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
font = cv2.FONT_HERSHEY_SIMPLEX
if np.all(ids != None):
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners, marker_length, mtx, dist)
position_data[j, :] = tvec[0].flatten()
for i in range(0, ids.size):
aruco.drawAxis(frame, mtx, dist, rvec[i], tvec[i], 5)
aruco.drawDetectedMarkers(frame, corners)
strg = ''
for i in range(0, ids.size):
strg += str(ids[i][0]) + ', '
cv2.putText(frame, "Id: " + strg, (0, 64), font, 1, (0, 255, 0), 2,
cv2.LINE_AA)
aruco_pose = Pose()
aruco_pose.position = Point(x=tvec[0][0][0],
y=tvec[0][0][1],
z=tvec[0][0][2])
aruco_pose.orientation = Quaternion(x=1, y=0, z=0, w=0)
rospy.loginfo(aruco_pose)
pub2.publish(aruco_pose)
else:
cv2.putText(frame, "No Ids", (0, 64), font, 1, (0, 255, 0), 2,
cv2.LINE_AA)
# out.write(frame)
# display the resulting frame
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
rate.sleep()
def inspectFrame(corners, ids, theID, markerLength, camMatrix, distCoef,
frame):
n = len(ids) - 1
for i in (0, n):
if (ids[i] == theID):
Rvec, Tvec = aruco.estimatePoseSingleMarkers(
corners[i], markerLength, camMatrix, distCoef)
aruco.drawDetectedMarkers(frame, [corners[i]])
aruco.drawAxis(frame, camMatrix, distCoef, Rvec, Tvec, 5)
R_ct = np.matrix(cv.Rodrigues(Rvec)[0])
R_tc = R_ct.T
#x_rot,y_rot,z_rot = rotationMatrixToEulerAngles(np.eye(3,dtype=np.float32)*R_tc)
cv.imshow('frame', frame)
def get_M_Cmp(gray, M_Cm0, M_m0mP, visualize=False):
markerLength = 0.063
M_Cmp = []
for tmp_M in M_m0mP:
tmp_M_Cmp = np.dot(M_Cm0, tmp_M)
if visualize:
rvec, _ = cv2.Rodrigues(tmp_M_Cmp[:3, :3])
tvec = tmp_M_Cmp[:3, 3]
aruco.drawAxis(gray, cameraMatrix, distCoeffs, rvec, tvec,
markerLength)
M_Cmp.append(tmp_M_Cmp)
if OBJ_POSE_ONLY: break
return M_Cmp
def acha_aruco(gray):
#Função cujo objetivo é identificar os ids dos arucos da pista
gray = cv2.cvtColor(gray, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
print(ids)
if ids is not None:
#-- ret = [rvec, tvec, ?]
#-- rvec = [[rvec_1], [rvec_2], ...] vetor de rotação
#-- tvec = [[tvec_1], [tvec_2], ...] vetor de translação
ret = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix, camera_distortion)
rvec, tvec = ret[0][0, 0, :], ret[1][0, 0, :]
#-- Desenha um retanculo e exibe Id do marker encontrado
aruco.drawDetectedMarkers(temp_image, corners, ids)
aruco.drawAxis(temp_image, camera_matrix, camera_distortion, rvec,
tvec, 1)
#-- Print tvec vetor de tanslação em x y z
str_position = "Marker x=%4.0f y=%4.0f z=%4.0f" % (tvec[0], tvec[1],
tvec[2])
print(str_position)
cv2.putText(temp_image, str_position, (0, 100), font, 1, (0, 255, 0),
1, cv2.LINE_AA)
##############----- Referencia dos Eixos------###########################
# Linha referencia em X
cv2.line(temp_image,
(int(temp_image.shape[1] / 2), int(temp_image.shape[0] / 2)),
((int(temp_image.shape[1] / 2) + 50),
(int(temp_image.shape[0] / 2))), (0, 0, 255), 5)
# Linha referencia em Y
cv2.line(temp_image,
(int(temp_image.shape[1] / 2), int(temp_image.shape[0] / 2)),
((int(temp_image.shape[1] / 2)),
(int(temp_image.shape[0] / 2) + 50)), (0, 255, 0), 5)
#####################---- Distancia Euclidiana ----#####################
# Calcula a distancia usando apenas a matriz tvec, matriz de tanslação
# Pode usar qualquer uma das duas formas
distance = np.sqrt(tvec[0]**2 + tvec[1]**2 + tvec[2]**2)
distancenp = np.linalg.norm(tvec)
#-- Print distance
str_dist = "Dist aruco=%4.0f dis.np=%4.0f" % (distance, distancenp)
print(str_dist)
cv2.putText(temp_image, str_dist, (0, 15), font, 1, (0, 255, 0), 1,
cv2.LINE_AA)
return distance, ids
return 1000, [[-1]]
def track_aruco(frame):
# operations on the frame
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# set dictionary size depending on the aruco marker selected
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
# detector parameters can be set here (List of detection parameters[3])
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# font for displaying text (below)
font = cv2.FONT_HERSHEY_SIMPLEX
# check if the ids list is not empty
# if no check is added the code will crash
if np.all(ids != None):
# estimate pose of each marker and return the values
# rvet and tvec-different from camera coefficients
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners, 0.05, mtx, dist)
for i in range(0, ids.size):
# draw axis for the aruco markers
aruco.drawAxis(frame, mtx, dist, rvec[i], tvec[i], 0.1)
# draw a square around the markers
aruco.drawDetectedMarkers(frame, corners)
# code to show ids of the marker found
strg = ''
for i in range(0, ids.size):
strg += str(ids[i][0]) + ', '
cv2.putText(frame, "Id: " + strg, (0, 64), font, 1, (0, 255, 0), 2,
cv2.LINE_AA)
else:
# code to show 'No Ids' when no markers are found
cv2.putText(frame, "No Ids", (0, 64), font, 1, (0, 255, 0), 2,
cv2.LINE_AA)
# display the resulting frame
cv2.imshow('frame', frame)
cv2.waitKey(1)
return rvec, tvec
def aruco_routine(img, drone_tvec, drone_rvec):
valid_ids = set(range(250))
corners, ids, rvecs, tvecs = utils.find_markers(img, camera_matrix, dist_coef) #3d
print(len(corners))
aruc3d = frame.copy()
if len(corners) > 0:
utils.process_markers(drone_tvec, drone_rvec, corners, ids, rvecs, tvecs, valid_ids) #store
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, 0.33, camera_matrix, dist_coef)
# Draw it
for rvecs, tvecs in zip(rvec, tvec):
aruco.drawAxis(aruc3d, camera_matrix, dist_coef, rvecs, tvecs, 0.33)
cv2.imwrite("detection.jpg", aruc3d)
def run(self):
global dist_2_target
while self._running:
#-- Read the camera frame
frame = self.vs.read()
#frame = cv2.flip(image, 1) # Flip the picture horizontally
#-- Convert in gray scale
gray = cv2.cvtColor(
frame, cv2.COLOR_BGR2GRAY
) #-- remember, OpenCV stores color images in Blue, Green, Red
#-- Find all the aruco markers in the image
corners, ids, rejected = aruco.detectMarkers(
image=gray,
dictionary=self.aruco_dict,
parameters=self.parameters,
cameraMatrix=self.camera_matrix,
distCoeff=self.camera_distortion)
if ids is not None and ids[0] == self.id_to_find:
ret = aruco.estimatePoseSingleMarkers(corners,
self.marker_size,
self.camera_matrix,
self.camera_distortion)
#-- Unpack the output, get only the first
rvec, tvec = ret[0][0, 0, :], ret[1][0, 0, :]
#-- Draw the detected marker and put a reference frame over it
aruco.drawDetectedMarkers(frame, corners)
aruco.drawAxis(frame, camera_matrix, camera_distortion, rvec,
tvec, 10)
#-- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T
#-- Now get Position and attitude f the camera respect to the marker
pos_camera = -R_tc * np.matrix(tvec).T
str_position = "CAMERA Position x=%4.0f y=%4.0f z=%4.0f" % (
pos_camera[0], pos_camera[1], pos_camera[2])
cv2.putText(frame, str_position, (0, 200), font, 1,
(0, 255, 0), 2, cv2.LINE_AA)
dist_2_target = int(
pos_camera[2][0, 0]) # Distance to maker in cm
#--- Display the frame
cv2.imshow('frame', frame)
#--- use 'q' to quit
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cv2.destroyAllWindows()
break
def estimate_aruco_marker_pose(img_list,
calibration_data,
marker_size=50,
aruco_dict=aruco.DICT_4X4_250):
"""
:param img_list:
:param calibration_data: could be a yamlfilpath or a list [mtx, dist, rvecs, tvecs]
:param marker_size:
:param aruco_dict:
:return:
author: weiwei
date: 2018, 20210516
"""
if isinstance(calibration_data, str):
mtx, dist, _, _, _ = yaml.load(open(calibration_data, 'r'),
Loader=yaml.UnsafeLoader)
elif isinstance(calibration_data, list):
if len(calibration_data) == 4:
mtx, dist, _, _ = calibration_data
elif len(calibration_data) == 2:
mtx, dist = calibration_data
else:
raise ValueError(
"Calibration data must include focus-centeraxis matrix and distortion."
)
else:
raise ValueError(
"Calibration data must be a str (file path) or a list.")
aruco_dict = aruco.Dictionary_get(aruco_dict)
parameters = aruco.DetectorParameters_create()
pos_list = []
rotmat_list = []
for img in img_list:
corners, ids, rejectedImgPoints = aruco.detectMarkers(
img, aruco_dict, parameters=parameters)
if ids is not None:
aruco.drawDetectedMarkers(img, corners, borderColor=[255, 255, 0])
marker_rvecs, marker_tvecs, _objPoints = aruco.estimatePoseSingleMarkers(
corners, marker_size, mtx, dist)
aruco.drawAxis(img, mtx, dist, marker_rvecs[0],
marker_tvecs[0] / 1000.0, 0.1)
pos = marker_tvecs[0][0].ravel()
rotmat = cv2.Rodrigues(marker_rvecs[0])[0]
pos_list.append(pos)
rotmat_list.append(rotmat)
average_pos = rm.posvec_average(pos_list)
average_rotmat = rm.rotmat_average(rotmat_list)
return average_pos, average_rotmat
def real_time_poseEstimation(self):
"""
Method that display real time detection of the aruco markers with the pose estimation and id of each
Returns:
"""
cv2.namedWindow("Aruco")
#frame = self.kinect.get_color()
frame = self.kinect.get_color() #[270:900,640:1400]
rval = True
while rval:
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
parameters = aruco.DetectorParameters_create()
aruco_dict = aruco.Dictionary_get(self.aruco_dict)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
if ids is not None:
frame = aruco.drawDetectedMarkers(frame, corners, ids)
# side lenght of the marker in meter
rvecs, tvecs, trash = aruco.estimatePoseSingleMarkers(
corners, self._size_of_marker, self.mtx, self.dist)
for i in range(len(tvecs)):
frame = aruco.drawAxis(frame, self.mtx, self.dist,
rvecs[i], tvecs[i],
self._length_of_axis)
cv2.imshow("Aruco", cv2.cvtColor(frame, cv2.COLOR_RGB2BGR))
#frame = self.kinect.get_color()
frame = self.kinect.get_color() #[270:900,640:1400]
key = cv2.waitKey(20)
if key == 27: # exit on ESC
break
cv2.destroyWindow("Aruco")
def detect():
cameraMatrix = np.array([[8.2177218160147447e+02, 0., 3.4694289806342221e+02],[0., 8.2177218160147447e+02, 2.4795144956871457e+02],[0., 0., 1.]])
distCoeffs = np.array([4.4824308523616324e-02, -7.4951985348854000e-01, 3.7539708742088725e-03, 8.8931335565222442e-03, 3.7214188475390984e+00])
#型宣言
rvecs = np.array([])
tvecs = np.array([])
# Capture frame-by-frame
ret, frame = cap.read()
#print(frame.shape) #480x640
# 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)
''' detectMarkers(...)
detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedI
mgPoints]]]]) -> corners, ids, rejectedImgPoints
'''
#lists of ids and the corners beloning to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
#print(corners)
#It's working.
# my problem was that the cellphone put black all around it. The alrogithm
# depends very much upon finding rectangular black blobs
if (corners != []):
rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs)
frame = aruco.drawAxis(frame, cameraMatrix, distCoeffs, rvecs, tvecs, 0.1)
frame = aruco.drawDetectedMarkers(frame, corners, ids)
return frame, rvecs, tvecs
def est_marker_pose(self, cv2_img):
gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(gray,
aruco_dict,
parameters=parameters)
vis_img = aruco.drawDetectedMarkers(cv2_img,
corners,
ids,
borderColor=(0, 0, 255))
if ids is not None and len(ids) > 3:
n_ids = len(ids)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, PAD_CFG[ARGS.target]["marker_size"], K, DIST)
rvecs, tvecs = rvecs.reshape((n_ids, 3)), tvecs.reshape((n_ids, 3))
Ccv_X_Ts = np.zeros((4, 4, 4))
X_Ts = np.zeros((4, 4, 4))
n_pads = 0
for i in range(4):
Ccv_X_T_temp = self.cmpt_target_pose(rvecs, tvecs, ids,
self.pad_ids[str(i)])
if Ccv_X_T_temp is not None:
n_pads += 1
X_T_temp = X_Cr.dot(Cr_X_Ccv).dot(Ccv_X_T_temp)
# print(X_T_temp[2, 3])
vis_img = aruco.drawAxis(
vis_img, K, DIST,
cv2.Rodrigues(Ccv_X_T_temp[0:3, 0:3])[0],
Ccv_X_T_temp[0:3, 3], 0.04)
vis_img = draw_BB(vis_img, X_T_temp)
#
Ccv_X_Ts[i, :, :] = Ccv_X_T_temp
X_Ts[i, :, :] = X_T_temp
rospy.sleep(0.05)
# Display our image
return bridge.cv2_to_imgmsg(vis_img, 'rgb8')
def estimatePoseToMarker(_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)
## Keep ID 49 (the robot marker)
corners, ids = keepMarkerById(corners, ids, 49)
img = aruco.drawDetectedMarkers(img,
corners,
ids=ids,
borderColor=(0, 0, 255))
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners=corners,
markerLength=0.1965,
cameraMatrix=cameraMatrix,
distCoeffs=distCoeffs)
img = aruco.drawAxis(img, cameraMatrix, distCoeffs, rvec, tvec, 2)
cv2.imwrite('calib_marker.png', img)
return cv2.Rodrigues(rvec)[0], tvec.reshape(3, 1)
def update(self, frame):
# transform image to grayscale
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, self.dict, parameters=self.ar_params)
markers = aruco.drawDetectedMarkers(gray, corners, ids)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, self.markerLength, self.calibration_params[0],
self.calibration_params[1])
if rvecs is not None and tvecs is not None:
print rvecs
print tvecs
self.debug_img = aruco.drawAxis(markers,
self.calibration_params[0],
self.calibration_params[1], rvecs,
tvecs, 1)
self.markerPose.append(tvecs)
else:
self.debug_img = markers
# clear the stream in preparation for the next frame
self.cap.truncate(0)
def main():
# Read Built-in Camera Feed
cap = cv2.VideoCapture(0)
while(cap.isOpened()):
ret, im = cap.read()
if (im is not None):
# im = cv2.imread('images/marker.jpg', cv2.IMREAD_COLOR)
marker_sz = 11.1 #cm (same units used to report tvecs)
camera_matrix = np.array([[1776,0,762],[0,1780,1025],[0,0,1]],dtype=float) #cx,cy ~= im.shape[1],im.shape[0]
dist_coeffs = np.array([[0,0,0,0]],dtype=float) #need to be float type
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
aruco_params = aruco.DetectorParameters_create()
corners, ids, rejectedPts = aruco.detectMarkers(im, aruco_dict, parameters=aruco_params) #print np.squeeze(corners[0]).shape
im = aruco.drawDetectedMarkers(im, corners, ids=ids, borderColor=1)
rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners, marker_sz, camera_matrix, dist_coeffs); #print rvecs
if rvecs is not None:
im = aruco.drawAxis(im, camera_matrix, dist_coeffs, rvecs, tvecs, marker_sz)
cvDrawWindow('im',im, 1)
if cv2.waitKey(1)==27:
break
cv2.destroyAllWindows()
def table_callback(self, rgb_img,ros_cloud):
try:
rgb_img = self.bridge.imgmsg_to_cv2(rgb_img,'bgr8')
gray = cv2.cvtColor(rgb_img, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict, parameters=parameters)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.0001)
for corner in corners:
cv2.cornerSubPix(gray, corner, winSize = (3,3), zeroZone = (-1,-1), criteria = criteria)
frame_markers = aruco.drawDetectedMarkers(rgb_img.copy(), corners, ids)
markerLength = 0.8
flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, markerLength, cameramatrix, dist_coeffs)
# result_matrix = np.zeros(shape=[4,4])
# rotation_matrix = eulerAnglesToRotationMatrix(rvecs[0][0])
# result_matrix[:3,:3] = rotation_matrix
# result_matrix[:3,3] = tvecs[0]
# result_matrix[3,3] = 1.
length_of_axis = 0.6
total_center = np.zeros(shape = [4,1,2])
for index in range(len(ids)):
center = np.array(np.mean(corners[index][0],axis = 0), dtype = np.int)
total_center[ids[index]-1] = center
imaxis = aruco.drawDetectedMarkers(rgb_img.copy(), corners, ids)
for i in range(len(tvecs)):
imaxis = aruco.drawAxis(imaxis, cameramatrix, dist_coeffs, rvecs[i], tvecs[i], length_of_axis)
# Display the resulting frame
self.ar_pub.publish(self.bridge.cv2_to_imgmsg(imaxis,'bgr8'))
print('height: {}'.format(ros_cloud.height))
print('width: {}'.format(ros_cloud.width))
def draw_axes(self, frame, detections: ArucoPoseDetections):
if self.camera is None:
raise MissingParametersError()
rvecs = detections.rvecs
tvecs = detections.tvecs
if rvecs is None:
return frame
for i in range(len(rvecs)):
rvec = rvecs[i]
tvec = tvecs[i]
try:
frame = aruco.drawAxis(image=frame,
cameraMatrix=self.camera.camera_matrix,
distCoeffs=self.camera.dist_coeffs,
rvec=rvec,
tvec=tvec,
length=self.context.marker_length)
except cv2.error:
# print('error drawing axis')
pass
return frame
def detect(frame, camera_matrix, dist_coefs):
frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame_gray, aruco_dict, parameters=parameters)
rendered_img = frame
rvecs = []
tvecs = []
rvec = []
tvec = []
if corners != []:
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, size_marker, camera_matrix, dist_coefs)
# Hypothesis: we have only the chessboard marker, so we take only the first result
rvec = rvecs[0]
tvec = tvecs[0]
rendered_img = aruco.drawAxis(rendered_img, camera_matrix, dist_coefs,
rvec, tvec, 10.0)
rvec, tvec = getCorrectVectors(rvec, tvec)
else:
rvec, tvec = getCorrectVectors(rvec, tvec)
return (np.array([rvec]), np.array([tvec]), rendered_img)
def detect_marker_pose(img, corners, ids, camera_type, display=False):
distCoeffs = np.zeros((5,1))
if camera_type == "D415":
# default camera_matrix from realsense ros wrapper
camera_mtx = np.array([[931.7362060546875, 0.0, 622.6597900390625],
[0.0, 931.1814575195312, 354.47479248046875],
[0.0, 0.0, 1.0]])
if camera_type == "D435":
camera_mtx = np.array([[617.0361328125, 0.0, 327.0294189453125],
[0.0, 617.2791137695312, 237.9000701904297],
[0.0, 0.0, 1.0]])
size_of_marker = 0.04 # side lenght of the marker in meters
length_of_axis = 0.1
rvecs,tvecs = aruco.estimatePoseSingleMarkers(corners, size_of_marker, camera_mtx, distCoeffs)
img_axis = aruco.drawDetectedMarkers(img.copy(), corners, ids)
for i in range(len(tvecs)):
img_axis = aruco.drawAxis(img_axis, camera_mtx, distCoeffs, rvecs[i], tvecs[i], length_of_axis)
if display==True:
plt.figure()
plt.imshow(img_axis)
plt.grid()
plt.show()
return img_axis, rvecs, tvecs
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
global camera_angle_x, camera_angle_y
markerLength = 5
#(rows,cols,channels) = cv_image.shape
# if cols > 60 and rows > 60 :
# cv2.circle(cv_image, (50,50), 10, 255)
#cv2.imshow("Image window", cv_image)
# cv2.waitKey(3)
# Load the camera coefficients etc
with np.load('B.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
# Make the image gray for ArUco tag detection
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# Assign the dictionary we wish to use (6 bit, with 250 separate tags)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
#aruco.drawDetectedMarkers(cv_image, corners, ids)
# this if statement lets us check if we have detected a tag by checking the size
# of the corners list to see if it's greater than 0. If it is, then we want to
# find the center position of the tag and then correct the camera angle to center
# it by publishing back a new angle to the control_camera topic.
if (len(corners) != 0):
# Draw on the markers so we can see they've been detected
gray = aruco.drawDetectedMarkers(cv_image, corners, ids)
rvec, tvec = aruco.estimatePoseSingleMarkers(
corners, markerLength, mtx, dist) # For a single marker
gray = aruco.drawAxis(gray, mtx, dist, rvec, tvec, 10)
aruco_code_center_pixel = self.tag_center(corners)
camera_angle_x, camera_angle_y = self.camera_angle_correction(
aruco_code_center_pixel, camera_angle_x, camera_angle_y)
self.publish_the_message(
camera_angle_x, camera_angle_y
) # use this method to publish the desired camera angle the the /bebop/camera_control topic
# Display the video feed frames every 3 ms.
cv2.imshow("Image window", cv_image)
cv2.imshow("Gray window", gray)
cv2.waitKey(5)
# Publish the image back into ROS image message type (not sure why, I guess it's if you
# want to do something else with it after using OpenCV).
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def draw_pose(self, image, matrix, distortion):
if self.set:
image_draw = aruco.drawAxis(image, matrix, distortion, self.rvec, self.tvec, 0.1)
else:
image_draw = image
return image_draw
def image_callback(ros_image):
global initialized, initial_rot, initial_trans
bridge = CvBridge()
image = bridge.imgmsg_to_cv2(ros_image, desired_encoding='bgr8')
#converting to grayscale
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
corners = []
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
if corners:
frame_markers = aruco.drawDetectedMarkers(image.copy(), corners, ids)
size_of_marker = 0.181 # side lenght of the marker in meter
rvecs, tvecs, trash = aruco.estimatePoseSingleMarkers(
corners, size_of_marker, mtx, dist)
frame_markers = aruco.drawAxis(frame_markers, mtx, dist, rvecs, tvecs,
size_of_marker)
aruco_image = bridge.cv2_to_imgmsg(frame_markers, encoding="bgr8")
aruco_image_publisher.publish(aruco_image)
rvecs = np.reshape(rvecs, (3, ))
tvecs = np.reshape(tvecs, (3, ))
cam_to_aruco_R = Rotation.from_rotvec(rvecs)
cam_to_aruco_dcm = cam_to_aruco_R.as_dcm()
aruco_to_cam_dcm = cam_to_aruco_dcm.transpose()
aruco_to_cam_position = -np.matmul(aruco_to_cam_dcm, tvecs)
aruco_to_cam_R = Rotation.from_dcm(aruco_to_cam_dcm)
if not initialized:
initial_trans = aruco_to_cam_position
initial_rot = aruco_to_cam_dcm
aruco_to_cam_position = np.matmul(
aruco_to_cam_dcm, aruco_to_cam_position - initial_trans)
aruco_to_cam_R = Rotation.from_dcm(
np.matmul(initial_rot, aruco_to_cam_dcm.transpose()))
aruco_to_cam_quat = aruco_to_cam_R.as_quat()
camera_pose.header.frame_id = "aruco_link"
camera_pose.header.stamp = rospy.Time.now()
camera_pose.pose.position.x = aruco_to_cam_position[0]
camera_pose.pose.position.y = aruco_to_cam_position[1]
camera_pose.pose.position.z = aruco_to_cam_position[2]
camera_pose.pose.orientation.x = aruco_to_cam_quat[0]
camera_pose.pose.orientation.y = aruco_to_cam_quat[1]
camera_pose.pose.orientation.z = aruco_to_cam_quat[2]
camera_pose.pose.orientation.w = aruco_to_cam_quat[3]
initialized = True
return initialized
def aruco(self, frame):
#gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame, self.aruco_dict, parameters=self.parameters)
frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
rvecs, tvecs, trash = aruco.estimatePoseSingleMarkers(
corners, self.size_of_marker, self.mtx, self.dist)
imaxis = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
if tvecs is not None:
for i in range(len(tvecs)):
imaxis = aruco.drawAxis(imaxis, self.mtx, self.dist, rvecs[i],
tvecs[i], self.length_of_axis)
rvec = np.squeeze(rvecs[0], axis=None)
tvec = np.squeeze(tvecs[0], axis=None)
tvec = np.expand_dims(tvec, axis=1)
rvec_matrix = cv2.Rodrigues(rvec)[0]
proj_matrix = np.hstack((rvec_matrix, tvec))
euler_angles = cv2.decomposeProjectionMatrix(proj_matrix)[6]
cv2.putText(imaxis, 'X: ' + str(int(euler_angles[0])),
(10, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 255))
cv2.putText(imaxis, 'Y: ' + str(int(euler_angles[1])),
(115, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 255, 0))
cv2.putText(imaxis, 'Z: ' + str(int(euler_angles[2])),
(200, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(255, 0, 0))
cv2.imshow('Aruco', imaxis)
def main():
im = cv2.imread('images/IMG_0081.jpg', cv2.IMREAD_COLOR)
marker_sz = 11.1 #cm (same units used to report tvecs)
camera_matrix = np.array([[1776, 0, 762], [0, 1780, 1025], [0, 0, 1]],
dtype=float) #cx,cy ~= im.shape[1],im.shape[0]
dist_coeffs = np.array([[0, 0, 0, 0]], dtype=float) #need to be float type
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
aruco_params = aruco.DetectorParameters_create()
corners, ids, rejectedPts = aruco.detectMarkers(
im, aruco_dict,
parameters=aruco_params) #print np.squeeze(corners[0]).shape
im = aruco.drawDetectedMarkers(im, corners, ids=ids, borderColor=1)
rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners, marker_sz,
camera_matrix, dist_coeffs)
#print rvecs
if rvecs is not None:
im = aruco.drawAxis(im, camera_matrix, dist_coeffs, rvecs, tvecs,
marker_sz)
cvDrawWindow('im', im, .3, 1)
cv2.destroyAllWindows()
def get_board_range(self, board):
detected_board_ids = list()
detected_board_corners = list()
if(self.get_visible_ids() is not None):
for i in range(len(self.get_visible_ids())):
detected_id = self.get_visible_ids()[i]
detected_corner = self.corners[i]
if(detected_id in board.ids.tolist()):
detected_board_ids.append(detected_id)
detected_board_corners.append(detected_corner)
if(detected_board_ids):
detected_board_ids = np.asarray(detected_board_ids)
detected_board_corners = np.asarray(detected_board_corners)
retval, rvec, tvec = aruco.estimatePoseBoard(detected_board_corners,
detected_board_ids, board,
self.camera_matrix, self.dist_coeffs)
self.frame = aruco.drawAxis(
self.frame,
self.camera_matrix,
self.dist_coeffs,
rvec,
tvec,
self.axis_length_inches)
distance_to_board = np.linalg.norm(tvec)
return distance_to_board
# board not visible, distance unknown
return None
def gen():
"""Video streaming generator function."""
while True:
# read camera frame
ret, frame = cap.read()
# convert gray scale
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) #-- remember, OpenCV stores color images in Blue, Green, Red
#-- Find all the aruco markers in the image
corners, ids, rejected = aruco.detectMarkers(image=gray, dictionary=aruco_dict, parameters=parameters,
cameraMatrix=camera_matrix, distCoeff=camera_distortion)
#-- Unpack the output, get only the first
if ids is not None and ids[0] == id_to_find:
ret1 = aruco.estimatePoseSingleMarkers(corners, marker_size, camera_matrix, camera_distortion)
#-- Unpack the output, get only the first
rvec, tvec = ret1[0][0,0,:], ret1[1][0,0,:]
#-- Get the attitude
aruco.drawDetectedMarkers(frame, corners)
aruco.drawAxis(frame, camera_matrix, camera_distortion, rvec, tvec, 10)
str_position = "MARKER Position x=%4.0f y=%4.0f z=%4.0f"%(tvec[0], tvec[1], tvec[2])
cv2.putText(frame, str_position, (0, 100), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
#-- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T
pos_camera = -R_tc*np.matrix(tvec).T
str_position = "CAMERA Position x=%4.0f y=%4.0f z=%4.0f"%(pos_camera[0], pos_camera[1], pos_camera[2])
cv2.putText(frame, str_position, (0, 200), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
cv2.putText(frame, str_position, (0, 200), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
encode_return_code, image_buffer = cv2.imencode('.jpg', frame)
io_buf = io.BytesIO(image_buffer)
yield (b'--frame\r\n'
b'Content-Type: image/jpeg\r\n\r\n' + io_buf.read() + b'\r\n')
def trackAruco(self):
corners, ids, rejected = aruco.detectMarkers(image=self.gray, dictionary=self.aruco_dict, parameters=self.parameters)
self.pose.marker_id = -1
self.pose.distance = 0
self.pose.roll = 0
self.pose.pitch = 0
self.pose.yaw = 0
if not ids is None:
rvec = []
tvec = []
ret = aruco.estimatePoseSingleMarkers(corners, self.marker_size, self.camera_matrix, self.camera_distortion)
for markers in ret[0]:
rvec.append(markers[0])
for markers in ret[1]:
tvec.append(markers[0])
#print(rvec)
# only for video
aruco.drawDetectedMarkers(self.frame, corners)
for i in range(len(rvec)):
aruco.drawAxis(self.frame, self.camera_matrix, self.camera_distortion, rvec[i], tvec[i], 5)
for i in range(len(ids)):
x = tvec[i][0]
z = tvec[i][2]
R_ct = np.matrix(cv2.Rodrigues(rvec[i])[0])
R_tc = R_ct.T
roll_camera, pitch_camera, yaw_camera = self.rotationMatrixToEulerAngles(self.R_flip*R_tc)
self.pose.marker_id = int(ids[i][0])
self.pose.distance = int(z)
self.pose.roll = int(math.degrees(roll_camera))
self.pose.pitch = int(math.degrees(pitch_camera))
self.pose.yaw = int(math.degrees(yaw_camera))
str_attitude = 'CAMERA Attitude r=%4.0f p=%4.0f y=%4.0f'%(math.degrees(roll_camera),math.degrees(pitch_camera), math.degrees(yaw_camera))
cv2.putText(self.frame, str_attitude, (0,250),self.font,1,(0,255,0),2,cv2.LINE_AA)
str_position = 'Marker Position x=%4.0f y=%4.0f z=%4.0f'%(tvec[0][0], tvec[0][1], tvec[0][2])
cv2.putText(self.frame, str_position, (0,150),self.font,1,(0,255,0),2,cv2.LINE_AA)
marker = 'Marker ID %4.0f'%(ids[i][0])
cv2.putText(self.frame, marker, (0,200), self.font,1,(0,255,0),2,cv2.LINE_AA)
def run_flann_matcher(self):
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50) # or pass empty dictionary
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(np.asarray(self.__marker_desc, np.float32),
np.asarray(self.__input_desc, np.float32),
k=2)
# Need to draw only good matches, so create a mask
matchesMask = [[0, 0] for i in range(len(matches))]
# store all the good matches as per Lowe's ratio test.
good = []
for m, n in matches:
if m.distance < 0.7 * n.distance:
good.append(m)
if len(good) > MIN_MATCH_COUNT:
src_pts = np.float32([
self.__marker_kp[m.queryIdx].pt for m in good
]).reshape(-1, 1, 2)
dst_pts = np.float32([
self.__input_kp[m.trainIdx].pt for m in good
]).reshape(-1, 1, 2)
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
h, w, _ = self.in_image.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, H)
num, self.r_vec, self.t_vec, Ns = cv2.decomposeHomographyMat(
H, self.__cam_mat)
print(self.__in_image.shape)
if (self.__json_params["debug_draw"] == True):
aruco.drawAxis(self.in_image, self.__cam_mat, self.__dist_mat,
self.r_vec[1], self.t_vec[1], 0.1) # Draw Axis
self.in_image = cv2.polylines(self.in_image, [np.int32(dst)],
True, 255, 3, cv2.LINE_AA)
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 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()
